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

2021-06-21 05:51:36嵇鵬程嵇鑫宇

陜西水利 2021年5期

嵇鵬程，嵇鑫宇

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

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

1 機器人機構系統(tǒng)建模

本文研究的機器人機構由機器人本體、管徑適應機構、行走機構、絞刀機構等組成［3］，見圖1。

1.1 管徑適應機構系統(tǒng)的數(shù)學建模

以管徑適應機構后面的搖桿與機架的鉸鏈點A為坐標系原點，建立如圖2所示的坐標系XOY。

圖1 管道清淤機器人結構模型

圖2 鉸鏈四桿力封閉機構調節(jié)方式示意圖

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

通過對鉸鏈四桿機構幾何尺寸關系式求導和做功守恒原理得出螺母的軸向力F：

通過對滾珠絲杠移動距離求導和做功守恒原理得出滾珠絲杠扭矩與機器人對管壁壓力關系：

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

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

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

根據(jù)管徑適應機構的傳動過程，建立的模型結構見圖3。

圖3 管徑適應機構模型結構

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

根據(jù)模型結構圖求得：

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

由式（1）～式（4）得到電機傳動模型方框圖，見圖4。

圖4 管徑適應機構驅動電機傳動模型方框圖

由圖4可求出電機角速度的表達式：

1.2 行走機構系統(tǒng)的數(shù)學建模

由于該機器人要求中低速行駛，而電動機具有較高的額定轉速，因此，電動機和輪子之間必須裝設減速機構，行走機構示意圖見圖5。

圖5 行走機構示意圖

1.2.1 齒輪相關參數(shù)計算

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

1.2.2 負載轉矩的折算

為便于選擇電機，先將各轉動部分的轉矩和轉動慣量或直線運動部分的質量都折算到電機軸上。

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

式中：ηC為電動機拖動機械運動時的傳動效率，這里ηC取95%；nM為電動機軸的轉速。

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

1.2.3 轉動慣量的折算

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

式中：JM、JZ為電動機軸、輪子軸上的轉動慣量；j1=ωM/ω1為電動機與輪子軸之間的速比；ωM、ω1為電動機軸、輪子軸上的角速度。

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

（1）對于電機軸上的齒輪，D1=24 mm，d1=12 mm，寬L1=20 mm，代入式（7）可求出其轉動慣量為：

（2）對于輪子軸上的齒輪，D2=96 mm，d2=16 mm，寬L2=20 mm，同樣代入式（7）求出其轉動慣量為：

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

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

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

圖6 行走機構驅動系統(tǒng)模型方框圖

根據(jù)模型結構圖求得：

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

又因為電機軸與車輪軸單位時間內(nèi)的轉角之比為：

機器人的牽引力與其它負載力的關系為：

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

由式（8）～式（11）得到行走機構系統(tǒng)方框圖，見圖7。

圖7 行走機構系統(tǒng)模型方框圖

式中：C為電動機拖動機械運動時的傳動效率，取95%；nM為行走機構驅動電機軸的轉速；v機為機器人的行走速度。

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

2 機器人機構控制算法與仿真

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

2.1 行走機構閉環(huán)控制及仿真

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

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

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

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

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

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

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

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

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

位置環(huán)采用PD控制，位置環(huán)的數(shù)學模型可由圖9所示傳遞函數(shù)方塊圖表示，這里已經(jīng)簡化為單位反饋模型，因為位置反饋系數(shù)Ks為純增益。圖中Θp（s）=vs/Ks是給定的位置，也是期望的位置值，T1為電機有效轉矩。

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

傳遞函數(shù)為：

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

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

2.1.3 仿真分析

當輸入信號分別為階躍信號、斜坡信號、正弦信號時，系統(tǒng)的輸出響應信號和輸入信號對比見圖10。

圖10 分別輸入階躍信號、斜坡信號、正弦信號與輸出響應曲線對比圖

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

2.2 管徑適應機構壓力閉環(huán)控制及仿真

通過建立管徑適應機構動力模型，給出期望牽引力F牽，計算出來對管壁的期望壓力值N，通過比較實時反饋來由壓力傳感器測試的實際壓力值No與輸入的壓力期望值N得出的差值ΔN，PID控制器調整適徑驅動電機保持實際拉力No趨向到N。壓力傳感器的反饋系數(shù)KV=200，電壓放大倍數(shù)KU=5。PID控制的傳遞函數(shù)框圖見圖11。