城市管道清淤機(jī)器人數(shù)學(xué)建模與MATLAB仿真

嵇鵬程，嵇鑫宇

（1.常州市河道湖泊管理處，江蘇 常州213022；2.中國(guó)礦業(yè)大學(xué)，江蘇 徐州221116）

我國(guó)地下排水管道的清淤工作仍普遍采用人工作業(yè)的落后方式［1］，這種清淤作業(yè)方式整體效率低［2］。近年來(lái)，水下機(jī)器人技術(shù)的快速發(fā)展為水下機(jī)器人在各行各業(yè)中的應(yīng)用提供了條件，本文研制的城市排水管道清淤機(jī)器人，采用PID控制技術(shù)，使得機(jī)器人具有實(shí)用性、可靠性、智能性和可操行，可完全代替人工下井作業(yè)，使工人完全脫離了惡劣的作業(yè)環(huán)境。

1 機(jī)器人機(jī)構(gòu)系統(tǒng)建模

本文研究的機(jī)器人機(jī)構(gòu)由機(jī)器人本體、管徑適應(yīng)機(jī)構(gòu)、行走機(jī)構(gòu)、絞刀機(jī)構(gòu)等組成［3］，見(jiàn)圖1。

1.1 管徑適應(yīng)機(jī)構(gòu)系統(tǒng)的數(shù)學(xué)建模

以管徑適應(yīng)機(jī)構(gòu)后面的搖桿與機(jī)架的鉸鏈點(diǎn)A為坐標(biāo)系原點(diǎn)，建立如圖2所示的坐標(biāo)系XOY。

圖1 管道清淤機(jī)器人結(jié)構(gòu)模型

圖2 鉸鏈四桿力封閉機(jī)構(gòu)調(diào)節(jié)方式示意圖

圖中：Νo為管道內(nèi)壁作用在適徑機(jī)構(gòu)頂輪上的壓力，F(xiàn)是螺母受到的水平拉力，Tm1是適徑機(jī)構(gòu)驅(qū)動(dòng)電機(jī)軸上的輸出扭矩。

通過(guò)對(duì)鉸鏈四桿機(jī)構(gòu)幾何尺寸關(guān)系式求導(dǎo)和做功守恒原理得出螺母的軸向力F：

通過(guò)對(duì)滾珠絲杠移動(dòng)距離求導(dǎo)和做功守恒原理得出滾珠絲杠扭矩與機(jī)器人對(duì)管壁壓力關(guān)系：

機(jī)器人輸出的牽引力F牽按600 N計(jì)算，驅(qū)動(dòng)輪和管道內(nèi)壁之間的摩擦系數(shù)μ取0.5，則管道內(nèi)壁作用在車輪上的壓力N=1200 N。所以選取滾珠絲杠公稱直徑為20 mm，選用的型號(hào)為FC1B20×4-5-E2，其額定負(fù)載是5393 N，摩擦角為φ=10′；螺旋升角γ=3°25′，則其傳動(dòng)效率：

當(dāng)機(jī)器人行走在φ400 mm的管道中時(shí)，α=28.7°、β=10.6°，代入式（1）得出F=4895 N；當(dāng)機(jī)器人行走在φ500 mm的管道中時(shí)，α=64.2°、β=32.7°，代入式（1）得出F=1121.5 N。

把已知數(shù)據(jù)代入式（2）可求出滾珠絲杠的扭矩Tn1=4.4515 N·m，可見(jiàn)采用鉸鏈四桿力封閉調(diào)節(jié)方式時(shí)，清淤機(jī)器人對(duì)管壁壓力設(shè)定為1200 N的情況下，當(dāng)管徑在400 mm～500 mm范圍內(nèi)變化時(shí)，滾珠絲杠上所需的最大扭矩為4.4515 N·m，目前大功率的直流伺服電機(jī)都能提供，為此，伺服電機(jī)可選用型號(hào)為CN-400-10，其額定輸出扭矩達(dá)到4.5 N·m，最高轉(zhuǎn)矩可達(dá)40 N·m，滿足該系統(tǒng)的要求。

根據(jù)管徑適應(yīng)機(jī)構(gòu)的傳動(dòng)過(guò)程，建立的模型結(jié)構(gòu)見(jiàn)圖3。

圖3 管徑適應(yīng)機(jī)構(gòu)模型結(jié)構(gòu)

圖中：C1為電機(jī)及滾軸絲杠傳動(dòng)的粘性摩擦系數(shù)；TL1（t）為滾珠絲杠上的轉(zhuǎn)矩；θi1（t）為電機(jī)軸的轉(zhuǎn)角；θo1（t）為滾珠絲杠的轉(zhuǎn)角；Jm1為電機(jī)軸的轉(zhuǎn)動(dòng)慣量；Tm1（t）為電動(dòng)機(jī)的轉(zhuǎn)矩；JL1為負(fù)載的轉(zhuǎn)動(dòng)慣量。

根據(jù)模型結(jié)構(gòu)圖求得：

式中：Um1為電動(dòng)機(jī)的電樞電壓；CM1為電動(dòng)機(jī)電磁力矩系數(shù)；Rm1為電動(dòng)機(jī)的電樞電阻；ωi1為電動(dòng)機(jī)軸的角速度；im1為電動(dòng)機(jī)電樞電流；Lm1為電樞電感。

由式（1）～式（4）得到電機(jī)傳動(dòng)模型方框圖，見(jiàn)圖4。

圖4 管徑適應(yīng)機(jī)構(gòu)驅(qū)動(dòng)電機(jī)傳動(dòng)模型方框圖

由圖4可求出電機(jī)角速度的表達(dá)式：

1.2 行走機(jī)構(gòu)系統(tǒng)的數(shù)學(xué)建模

由于該機(jī)器人要求中低速行駛，而電動(dòng)機(jī)具有較高的額定轉(zhuǎn)速，因此，電動(dòng)機(jī)和輪子之間必須裝設(shè)減速機(jī)構(gòu)，行走機(jī)構(gòu)示意圖見(jiàn)圖5。

圖5 行走機(jī)構(gòu)示意圖

1.2.1 齒輪相關(guān)參數(shù)計(jì)算

齒輪傳動(dòng)比i=4，故取Z1=12，Z2=48，m=2，α=20°，則有：

1.2.2 負(fù)載轉(zhuǎn)矩的折算

為便于選擇電機(jī)，先將各轉(zhuǎn)動(dòng)部分的轉(zhuǎn)矩和轉(zhuǎn)動(dòng)慣量或直線運(yùn)動(dòng)部分的質(zhì)量都折算到電機(jī)軸上。

選定輪子的直徑D=120 mm，則輪子的周長(zhǎng)C≈0.38 m，清淤機(jī)器人的正常行駛速度v機(jī)設(shè)定為0.1 m/s，當(dāng)牽引力F牽=600 N時(shí)需要電機(jī)的功率為PL=F牽v機(jī)=60 W，按平均傳動(dòng)效率η為95%，則輸入功率PM≈63.15 W，實(shí)際我們選的直流電機(jī)的額定功率可選擇為85 W，如果取機(jī)器人正常清淤時(shí)電機(jī)的最高轉(zhuǎn)速為1000 r/min，則其負(fù)載轉(zhuǎn)矩為：

式中：ηC為電動(dòng)機(jī)拖動(dòng)機(jī)械運(yùn)動(dòng)時(shí)的傳動(dòng)效率，這里ηC取95%；nM為電動(dòng)機(jī)軸的轉(zhuǎn)速。

根據(jù)上面計(jì)算出的功率和扭矩，選擇直流伺服電機(jī)的型號(hào)為CN-800-10，其額定輸出扭矩達(dá)到8.3 N·m，最高轉(zhuǎn)矩可達(dá)80 N·m，可以滿足本設(shè)計(jì)所需的8.2 N·m。

1.2.3 轉(zhuǎn)動(dòng)慣量的折算

轉(zhuǎn)動(dòng)慣量對(duì)運(yùn)動(dòng)特性有很大的影響，不但與加速能力、加速時(shí)驅(qū)動(dòng)力矩及動(dòng)態(tài)的快速反應(yīng)有關(guān)，在閉環(huán)或半閉環(huán)系統(tǒng)中還影響系統(tǒng)的穩(wěn)定性，因此，須核算轉(zhuǎn)動(dòng)慣量。清淤機(jī)器人的質(zhì)量為48 kg，折算到電動(dòng)機(jī)軸上的總轉(zhuǎn)動(dòng)慣量公式為：

式中：JM、JZ為電動(dòng)機(jī)軸、輪子軸上的轉(zhuǎn)動(dòng)慣量；j1=ωM/ω1為電動(dòng)機(jī)與輪子軸之間的速比；ωM、ω1為電動(dòng)機(jī)軸、輪子軸上的角速度。

外徑為D、內(nèi)徑為d的圓環(huán)轉(zhuǎn)動(dòng)慣量公式為：

（1）對(duì)于電機(jī)軸上的齒輪，D1=24 mm，d1=12 mm，寬L1=20 mm，代入式（7）可求出其轉(zhuǎn)動(dòng)慣量為：

（2）對(duì)于輪子軸上的齒輪，D2=96 mm，d2=16 mm，寬L2=20 mm，同樣代入式（7）求出其轉(zhuǎn)動(dòng)慣量為：

（3）清淤機(jī)器人折算到電機(jī)軸上，m=48 kg，則：

將上述數(shù)據(jù)代入式（6）中求出J=0.012 kg·m2。

行走機(jī)構(gòu)模型方塊圖見(jiàn)圖6，其中，C2為剛性聯(lián)軸器的阻尼系數(shù)，TL2（t）為負(fù)載力矩；Jm2、JL2分別為直流電機(jī)軸和車輪軸的轉(zhuǎn)動(dòng)慣量；θm2（t）和θo2（t）分別為電機(jī)軸和車輪軸的轉(zhuǎn)角；i為齒輪傳動(dòng)比；Td（t）為折算到在電動(dòng)機(jī)軸上的負(fù)載轉(zhuǎn)矩；Tm2（t）為電機(jī)力矩。

圖6 行走機(jī)構(gòu)驅(qū)動(dòng)系統(tǒng)模型方框圖

根據(jù)模型結(jié)構(gòu)圖求得：

式中：Um2為電樞電壓；CM2為電動(dòng)機(jī)電磁力矩系數(shù)；im2為電樞電流；Rm2為電樞回路電阻。

又因?yàn)殡姍C(jī)軸與車輪軸單位時(shí)間內(nèi)的轉(zhuǎn)角之比為：

機(jī)器人的牽引力與其它負(fù)載力的關(guān)系為：

式中：μ為機(jī)器人與管道內(nèi)壁的摩擦系數(shù)；N為機(jī)器人適徑機(jī)構(gòu)對(duì)管道內(nèi)壁的正壓力；G為機(jī)器人的重量；N負(fù)載為機(jī)器人拖動(dòng)負(fù)載（電纜線）的拉力。

由式（8）～式（11）得到行走機(jī)構(gòu)系統(tǒng)方框圖，見(jiàn)圖7。

圖7 行走機(jī)構(gòu)系統(tǒng)模型方框圖

式中：C為電動(dòng)機(jī)拖動(dòng)機(jī)械運(yùn)動(dòng)時(shí)的傳動(dòng)效率，取95%；nM為行走機(jī)構(gòu)驅(qū)動(dòng)電機(jī)軸的轉(zhuǎn)速；v機(jī)為機(jī)器人的行走速度。

在建立管道機(jī)器人動(dòng)力傳動(dòng)系統(tǒng)模型后，可得到行走機(jī)構(gòu)輪子輸出轉(zhuǎn)角的表達(dá)式為：

2 機(jī)器人機(jī)構(gòu)控制算法與仿真

為了確保城市管道清淤機(jī)器人能夠平穩(wěn)、可靠地運(yùn)動(dòng)，需要對(duì)機(jī)構(gòu)協(xié)調(diào)運(yùn)動(dòng)控制策略進(jìn)行深入研究［4］，城市管道清淤機(jī)器人系統(tǒng)的核心是智能控制算法，因此，關(guān)鍵技術(shù)就是要選取合適的智能控制算法。

2.1 行走機(jī)構(gòu)閉環(huán)控制及仿真

行走機(jī)構(gòu)采用位置-電流二環(huán)控制，二環(huán)參數(shù)調(diào)試時(shí)遵循從內(nèi)環(huán)到外環(huán)的順序，先調(diào)試電流環(huán)，最后是位置環(huán)，電流環(huán)處于內(nèi)環(huán)，調(diào)試時(shí)首先考慮其快速跟隨性，即輸出電流能夠跟隨給定電流的變化；電流環(huán)采用PI控制算法，位置環(huán)作為外環(huán)，直接決定了伺服系統(tǒng)的動(dòng)態(tài)和靜態(tài)性能，采用PD控制算法。

2.1.1 電流環(huán)參數(shù)的整定

電流環(huán)采用PI控制，其傳遞函數(shù)框見(jiàn)圖8。

圖8 電流環(huán)傳遞函數(shù)框圖

電流環(huán)的傳遞函數(shù)為：

令式（13）參考函數(shù)模型為：

表1 CN-800-10的性能參數(shù)

選擇電機(jī)為CN-800-10，主要性能參數(shù)見(jiàn)表1，由于需要較快的響應(yīng)速度，使Io1（s）≈Ip1（s），根據(jù)實(shí)際情況這里取ζ1=0.707、ωn1=2500 rad/s代入相關(guān)參數(shù)可求出：KI1=23125，Kp1=17.72。

2.1.2 位置環(huán)參數(shù)的整定

位置環(huán)采用PD控制，位置環(huán)的數(shù)學(xué)模型可由圖9所示傳遞函數(shù)方塊圖表示，這里已經(jīng)簡(jiǎn)化為單位反饋模型，因?yàn)槲恢梅答佅禂?shù)Ks為純?cè)鲆?。圖中Θp（s）=vs/Ks是給定的位置，也是期望的位置值，T1為電機(jī)有效轉(zhuǎn)矩。

圖9 位置環(huán)傳遞函數(shù)框圖

傳遞函數(shù)為：

令式（15）的參考模型為：

這里取ζ2=0.707、ωn2=125 rad/s，設(shè)當(dāng)機(jī)器人行走在直徑為400 mm管道中時(shí)，α=28.7°、β=10.6°。代入相關(guān)參數(shù)可求出：Kp2=203.804，Kd2=3。

2.1.3 仿真分析

當(dāng)輸入信號(hào)分別為階躍信號(hào)、斜坡信號(hào)、正弦信號(hào)時(shí)，系統(tǒng)的輸出響應(yīng)信號(hào)和輸入信號(hào)對(duì)比見(jiàn)圖10。

圖10 分別輸入階躍信號(hào)、斜坡信號(hào)、正弦信號(hào)與輸出響應(yīng)曲線對(duì)比圖

從圖10可以看出系統(tǒng)在PID參數(shù)整定后，具有較小超調(diào)量，較短調(diào)節(jié)時(shí)間和較好可控性。

2.2 管徑適應(yīng)機(jī)構(gòu)壓力閉環(huán)控制及仿真

通過(guò)建立管徑適應(yīng)機(jī)構(gòu)動(dòng)力模型，給出期望牽引力F牽，計(jì)算出來(lái)對(duì)管壁的期望壓力值N，通過(guò)比較實(shí)時(shí)反饋來(lái)由壓力傳感器測(cè)試的實(shí)際壓力值No與輸入的壓力期望值N得出的差值ΔN，PID控制器調(diào)整適徑驅(qū)動(dòng)電機(jī)保持實(shí)際拉力No趨向到N。壓力傳感器的反饋系數(shù)KV=200，電壓放大倍數(shù)KU=5。PID控制的傳遞函數(shù)框圖見(jiàn)圖11。

圖11 管徑適應(yīng)機(jī)構(gòu)閉環(huán)系統(tǒng)PID控制傳遞函數(shù)框圖

2.2.1 電流環(huán)參數(shù)的整定

為了得到較快的響應(yīng)時(shí)間，電流環(huán)仍采用PI控制，使Io2（s）≈Ip2（s），對(duì)PI參數(shù)進(jìn)行設(shè)計(jì)時(shí)，仍按經(jīng)驗(yàn)值求取，得到電流環(huán)傳遞函數(shù)為：

令式（17）的參考模型為：

選擇的電機(jī)為CN-400-10，主要參數(shù)見(jiàn)表2。

根據(jù)管徑適應(yīng)機(jī)構(gòu)的實(shí)際工作情況，這里取ζ3=0.707、ωn3=2500 rad/s代入相關(guān)參數(shù)可求出：Kp3=40081.5，KI3=21.1。

2.2.2 壓力環(huán)參數(shù)的整定

壓力環(huán)作為外環(huán)，決定伺服系統(tǒng)的動(dòng)態(tài)和靜態(tài)性能，所以

采用PD控制技術(shù)。

由圖11可得到輸出角與壓力的傳遞函數(shù)為：

表2 CN-400-10的性能參數(shù)

令式（19）的參考模型為：

這里取ζ4=0.707、ωn4=12.5 rad/s代入相關(guān)參數(shù)解得τ=0.5，KD4=12，KP4=8。

所以系統(tǒng)的傳遞函數(shù)為：

2.2.3 仿真分析

繪出管徑適應(yīng)機(jī)構(gòu)閉環(huán)系統(tǒng)的階躍響應(yīng)曲線和Bode圖，見(jiàn)圖12和圖13。

圖12 PID控制的閉環(huán)系統(tǒng)階躍響應(yīng)曲線

圖13 PID控制的閉環(huán)系統(tǒng)Bode圖

從圖12～圖13可以看出系統(tǒng)的響應(yīng)是穩(wěn)定的，超調(diào)量較小，過(guò)渡過(guò)程時(shí)間為6 s左右。

3 結(jié)語(yǔ)

本文通過(guò)對(duì)機(jī)器人行走機(jī)構(gòu)、管道適徑機(jī)構(gòu)進(jìn)行數(shù)學(xué)建模和計(jì)算來(lái)選取主要部件，并根據(jù)機(jī)器人的各機(jī)構(gòu)運(yùn)行特點(diǎn)選取相應(yīng)的控制策略，并通過(guò)MATLAB軟件進(jìn)行仿真表明，該系統(tǒng)運(yùn)行穩(wěn)定，可替代性強(qiáng)，能夠滿足城市排水管道清淤工程的需要。