排水管道自適應清理機器人支撐機構運動學分析

摘要：""隨著城市排水系統規模增長，排水管道堵塞問題愈發復雜，傳統人工清堵方式存在操作難、效率低的問題。采用排水管道自適應清理機器人代替人工進行管道清堵，提高清堵工作的效率和安全性，確保城市排水系統順暢。通過對自適應清理機器人支撐機構進行正逆運動學分析和正逆運動分析完成了自適應清理機器人的支撐機構的設計和結構優化，為支撐機構的設計、優化、控制和驗證提供了基礎理論和方法；并通過仿真驗證了機構的可行性，對支撐機構進行軌跡規劃使其能夠在排水管道復雜的環境中實現避障功能。

關鍵詞： "管道清理機器人； 支撐機構； 正逆運動學； 軌跡規劃

中圖分類號： TP242""文獻標識碼： A

中國城市化逐漸地加快，城市排水系統逐年擴大，排水管道中出現堵塞的問題也越來越復雜多變。在城市道路施工過程中會有部分建筑廢料或者道路廢料掉落的管道內部造成更嚴重的堵塞^［1］，城市排水系統通暢與否影響著人民的生活和出行安全，但是排水管道中存在多種有害有毒氣體，排水管道尺寸狹窄，人工清堵的難度大，管道的清理急需用機器人代替人工進行作業。羅繼曼等^［24］設計的管道清淤機器人實現污水管道的清理，整體結構比較完善，行走結構擁有較高的通過能力。但是清淤結構尺寸固定，只適合特定尺寸的管道適用范圍較小。李文章等^［5］設計的全氣動管道檢測機器人能實現油氣管道的檢測，機器人整體采用非對稱設計的自適應結構（自適應范圍600～1 050 mm），依靠氣缸驅動支撐機構與管道壁之間產生壓力，依靠底部的輪子行走，保證機器人重心低，更加穩定。SABER KAZEMINASAB等^［6］設計的輪式自適應管道機器人在管網中能夠進行管道狀態評估、泄漏檢測、水質檢測等操作，屬于欠驅動系統，結構簡單可靠性高，但與管道壁之間的壓力僅依靠彈簧的預緊力，支撐不穩定。PASALA VENKATA SATISH等^［7］設計的履帶式自適應管道機器人能夠實現管道的檢測和維護等功能，結構簡單可靠，使用氣缸驅動支撐機構能夠獲得較大的支撐力，非常穩定，能夠在液體中運動，但是氣缸體積較大，無法折疊，對較小的管道適應性較差。P. DHANANCHEZHIYAN等^［8］設計的輪式自適應管道機器人整體是由三個支撐機構和前端的清理機構組成，結構單薄，兩輪的設計提供的支撐僅能完成對軟質地的污垢的清理工作。支撐機構的穩定性對于管道清理機器人至關重要，確保機器在受到外力時能夠正常發揮其功能的前提^［9］，使機構能夠適應不同的管道環境條件。針對中大型排水管道中硬質廢物清理，設定管道機器人參數，對自適應支撐機構進行優化設計^［10］，使管道清理機器人能在不同尺寸的管道中具備高強度和足夠的支撐力。綜上所述，康復機器人的結構設計仍存在問題，需要繼續的優化，本文通過對支撐機構進行運動學^［11］分析，確定支撐機構的各個關節在運動時的最大和最小角度或位置，從而確定機構的運動范圍，根據運動學分析的結果為控制系統提供數據來計算和調整關節的移動。

1"自適應支撐機構設計

為了確保排水管道工程的高效性和可靠性，通過研究分析，確定行走速度3～10 m/min；適應排水管道直徑600～1 500 mm；運行距離50 m；整體尺寸小于550 mm× 1 500 mm，整體質量不超過80 kg，越障能力大于80 mm；主要用于中大型排水管道中的硬質廢物（如混凝土）進行破碎和清理。

輪式^［11］或履帶式^［12］的末端行走結構都可滿足正常的運動要求。輪式結構輕便但越障能力弱，履帶式結構越障能力強但結構復雜且重。整體控制方案成本低，適應性強，但越障能力有限，不適合惡劣環境，單個控制方案越障能力好，但成本高，載重小，不適合大直徑管道。自適應管道機器人的支撐位置和穩定性會因展開程度和管道直徑不同而變化，如圖1。

自適應支撐機構無論是輪式還是履帶式，整體控制或單個控制，每個方案都有各自的優缺點，無法滿足用于中大型排水管道、能夠承受較大載荷和沖擊以及越障的要求。為了使管道機器人完全能夠進入直徑為600 mm的管道中，滿足管道的最小尺寸要求，能夠在直徑為1 500 mm的管道中獲得穩定支撐。

為單自由度連桿機構在支撐位置固定方面的不足，通過模仿四足動物的腿部結構，參考仿生MIT機械狗^［13］和Ascento^［14］的腿部結構。MIT機械狗展現了卓越的平衡能力和越障能力，但控制方案復雜且行走速度較慢。而Ascento則結合了輪式和腿式機器人的優勢，具有較快的行走速度和跳躍越障能力，但僅能越過較小障礙物。在融合了MIT機械狗和Ascento的優勢后，開發了一種新型自適應支撐機構如圖2。該機構由小臂電機、大臂電機、末端輪驅動電機以及三根連桿等部件構成，通過并排布局電機以減少徑向尺寸，使用低副連桿機構來減少運動過程中的振動，提高控制穩定性。

為保證機器人的平穩運行，在清理工作時能夠保持穩定，電機模組^［15］性能強的eRob90I。優化過后的支撐機構，折疊尺寸更小。展開后臂展更長，軸向也有一定的承載能力。一個帶有編碼器的減速電機、錐齒輪和輪胎組成末端輪結構。減速電機通過驅動錐齒輪帶動輪胎運動，減速電機具有自鎖功能。在折疊狀態下尺寸為530 mm×1 110 mm，可進入直徑600 mm的管道；展開后尺寸為1 600 mm×1 110 mm，能在直徑1 500 mm的管道中穩定工作。采用6061鋁合金材料^［16］，整體質量約為66.127 7 kg。機械臂長820 mm，有4個自由度，在管道內的靈活運動，高效清理能力，如圖3和圖4所示。

2"自適應支撐機構運動學分析

機器人運動學進行機構設計、軌跡規劃和控制是實現機構設計和控制的理論基礎^［17］。正逆運動學^［1819］在機器人學中扮演著核心角色，是軌跡規劃的先決條件。通過建立自適應支撐機構的運動學模型，為其軌跡規劃提供了必要的理論和方法。為了方便數學計算，自適應支撐機構的模型簡化為以下二自由度的連桿模型如圖5。其中O和O₁分別為小臂驅動電機和大臂驅動電機。小臂由連桿l_OA、連桿l_AB、直角連桿l_BO1C、連桿l_CD、l_DEF組成，大臂由l_O1E組成。末端位置F（x，y）。l_OA=l_BO=l_CO=l_DE；l_CD=l_EF；l_EF=l₁；l_O1E=l₂。為方便計算設置參數l₂與x軸夾角為β，l₁與l₂夾角為α，l₁與x軸夾角為γ。

2.1"適應支撐機構運動學正解

運動學正解是已知自適應支撐機構各個關節的位置參數求解末端F（x，y）的坐標，如圖9所示，點E（x₁，y₁）表示為

x₁=l₂cosβ

y₁=l₂sinβ（1）

點F（x，y）表示為

x=x₁-l₁cosγ=l₂cosβ-l₁cosγ

y=y₁+l₁sinγ=l₂sinβ+l₁sinγ（2）

求解得

x=l₂cosβ+l₁cos（α+β）

y=l₂sinβ+l₁sin（α+β）（3）

方程（3）為自適應支撐機構的運動學正解結果。通過已知的機構的位姿（α和β）求得機構末端的位置F（x，y）

2.2"自適應支撐機構運動學逆解

運動學逆解是已知末端F（x，y）的坐標求各個關節的位置參數，是軌跡規劃的前提條件。將規劃的軌跡即F（x，y）的軌跡方程帶入到逆解結果中，便可以的到各個關節的位置函數，以及電機轉動的控制函數。

將正解方程代入x²+y²整理可得

x²+y²=l²₁+l²₂+2l₁l₂cosαsin²β+cosαcos²β=l²₁+l²₂+2l₁l₂cosα "（4）

求解α得

α=arccosx²+y²-l²₁+l²₂2l₁l₂（5）

正解方程展開得

x=l₂cosβ+l₁cosαcosβ-l₁sinαsinβ

y=l₂sinβ+l₁sinαcosβ+l₁cosαsinβ（6）

假設Fa，b到原點O₁的距離是s，則末端Fa，b可以表示為

a=scosθ=l₂-l₁cosγ=l₂+l₁cosα

b=ssinθ=l₁sinγ=l₁sinα（7）

求得

β=arctanyx-θ=arctanyx-l₁sinαl₂+l₁cosα（8）

綜上，得自適應支撐機構的運動學逆解結果為

α=arccos x²+y²-（l²₁+l²₂）2l₁l₂

β=arctan（yx）-l₁sinαl₂+l₁cosα（9）

通過機構末端F（x，y）的位置求得機構的位姿，通過逆解公式解機構各個關節和電機的運動情況，解出各關節的旋轉角度；與通過Robotics Toolbox工具箱中的求逆函數求解得出的結果，相同表明逆運動學方程求解正確，驗證了正運動學方程的正確性。證實支撐機構關節的運動距離、角度的理論值與實際的結果一致。正逆運動學方程的建立幫助支撐機構實現軌跡規劃、跟蹤以及處理限制和約束，實現支撐機構關節得精準定位，保證運動的穩定性。

3"自適應支撐機構軌跡規劃

排水管道自適應清理機器人工作時，首先自適應支撐機構展開，末端輪與管道壁接觸以獲得穩定支撐，機器人末端輪滾動至工作位置，面對小障礙物時繼續滾動，遇到大障礙物則鎖定末端輪，通過爬行運動越過障礙。到達工作區域后，鎖定支撐機構，機械臂調整至適當位姿進行清理，對支撐機構的展開和越障運動進行軌跡規劃，以適應復雜的管道環境。

3.1"自適應支撐機構自適應展開運動軌跡

本文設計的自適應支撐機構為二自由度結構，能夠保持末端輪在折疊狀態和展開狀態的支撐位置不變，即支撐機構末端F在x方向上保持靜止，y方向上做勻速直線運動，可表示為F點坐標為F（0，y），其中y=t，如圖6。

得到末端點F的y方向

y=l₂sinβ+"l²₁-l²₂cos²β（10）

y=l₁sinγ+"l₂-l²₁cos²γ（11）

逆解得

β=arcsin l²₁-l²₂-y²-2yl₂=arcsin y²-l²₁+l²₂2yl₂（12）

當以速度勻速展開時，y=t，得到支撐機構在展開過程的軌跡方程

β=arcsin ²t²-l²₁+l²₂2t·l₂（13）

α=180°-arcsin ²t²-l²₁+l²₂2t·l₂-arcsin ²t²+l²₁-l²₂2t·l₁（14）

3.1.1"多項式軌跡

為保證自適應支撐機構運行平穩減少沖擊需要對擺動相和支撐相進行約束。開始運動時t=0，末端的X和Y方向的位移、速度、加速度均為0，使開始運動時的沖擊為0。0～T/2為抬起動作。當t=T/2時，末端運動到最高位置，即末端Y方向的位移達到H。T/2～T為放下動作，應減小兩個運動間的沖擊，所以末端Y方向的速度和加速度均為0。結束擺動動作t=T，末端完成擺動相運動，X方向的位移為S，X方向的速度和加速度為0，Y方向的位移、速度、加速度均為0，可以使得結束運動時的沖擊為0。在支撐相中末端與支撐面接觸，所以Y方向并不運動。支撐相開始運動t=T，此時X方向的位移為S，X方向的速度和加速度為0。支撐相結束運動t=2T，此時X方向的位移、速度、加速度均為0。可以減少支撐相開始和結束時的沖擊力。

1）"擺動相。由于在x方向上有六個約束，建立五次多項式

x=At⁵+Bt⁴+Ct³+Dt²+Et+F

=5At⁴+4Bt³+3Ct²+2Dt+E

=20At³+12Bt²+6Ct+2D（15）

求解代入方程得到X方向的位移、速度、加速度隨時間變化函數

x=6ST⁵t⁵-15ST⁴t⁴+10ST³t³

=30ST⁵t⁴-60ST⁴t³+30ST³t²

=120ST⁵t³-180ST⁴t²+60ST³t（16）

t在0～T，取T=1 s，S₁=20 mm，S₂=40 mm，S₃=60 mm，S₄=80 mm，S₅=100 mm。函數圖如圖7、圖9、圖11。

由于在Y方向上有九個約束，建立八次多項式

y=at⁸+bt⁷+ct⁶+dt⁵+et⁴+ft³+gt²+ht+i

=8at⁷+7bt⁶+6ct⁵+5dt⁴+4et³+3ft²+2gt+h

=56at⁶+42bt⁵+30ct⁴+20dt³+12et²+6ft+2g（17）

代入約束可得多項式方程，求解得帶入方程得到Y方向的位移、速度、加速度隨時間變化函數：

y=-768HT⁸t⁸+3072HT⁷t⁷-4864HT⁶t⁶+3480HT⁵t⁵-1536HT⁴t⁴+256HT³t³

=-6144HT⁸t⁷+21504HT⁷t⁶-29184HT⁶t⁵+19200HT⁵t⁴-6144HT⁴t³+768HT³t²

=-43008HT⁸t⁶+129024HT⁷t⁵-145920HT⁶t⁴+76800HT⁵t³-18432HT⁴t²+1536HT³t（18）

t在0～T，取T=1 s，H₁=20 mm，H₂=40 mm，H₃=60 mm，H₄=80 mm，H₅=100 mm。函數圖如圖8、圖10、圖12。

2）"支撐相。在x方向上有六個約束，建立五次多項式。

x=Gt⁵+Ht⁴+It³+Jt²+Kt+L

=5Gt⁴+4Ht³+3It²+2Jt+K

=20Gt³+12Ht²+6It+2J（19）

代入約束可得多項式方程，求解方程得：

x=-6ST⁵t⁵+45ST⁴t⁴-130ST³t³+180ST²t²-120STt+32S

=-30ST⁵t⁴+180ST⁴t³-390ST³t²+360ST²t-120ST

=-120ST⁵t³+540ST⁴t²-780ST³t+360ST²（20）

t在T～2T，取T=1 s，S₁=20 mm，S₂=40 mm，S₃=60 mm，S₄=80 mm，S₅=100 mm。得到支撐相X方向位移、速度和加速度如圖13～圖15。

得出支撐相在X方向是擺動相關于t=T對稱的函數，在Y方向末端始終與地面接觸保持靜止。

3.1.2"復合擺線軌跡

使用復合擺線方程進行軌跡規劃能夠有效減少自適應支撐機構末端在擺動相和支撐相之間切換時以及在擺動相過程中速度突變時產生的沖擊。

1）"擺動相。復合擺線的軌跡方程為：

x（t）=S（tT-12πsin （2πtT））

y（t）=H（12-12cos （2πtT））（21）

對x、y進行求導和求二階導，得到末端速度方程和加速度方程。

（t）=S（1T-cos2πtTT）

（t）=Hπsin2πtTT（22）

（t）=2Sπsin （2πtT）T

（t）=2Hπ²cos（2πtT）T²""（23）

t在0～T，取T=1 s，S₁=20 mm，S₂=40 mm，S₃=60 mm，S₄=80 mm，S₅=100 mm，H₁=20 mm，H₂=40 mm，H₃=60 mm，H₄=80 mm，H₅=100 mm。得到復合擺線擺動相X、Y方向的位移、速度和加速度函數圖如圖16～圖21。

2）"支撐相。與多項式同理，支撐相x方向取擺動相關于t=T對稱函數表示：

x（t）=S（2-tT-12πsin （2π2-tT））

（t）=ST（cos2π2-tT-1）

（t）=2πSTsin（2π2-tT）（24）

t在T～2T，取T=1 s，S₁=20 mm，S₂=40 mm，S₃=60 mm，S₄=80 mm，S₅=100 mm。畫出得到結果的函數圖，如圖22～圖24。

3.2"結果與討論

多項式軌跡中開始運動時末端X和Y方向速度均為0，此處的軌跡平臺表示Y方向的速度變為0，所以在前T/2時間中多項式軌跡Y方向需要先加速再加速到0，加速度有較大變化如圖12所示。在復合擺線軌跡中整個運動周期過程中除開始和結束時，末端X和Y方向速度均不為0，速度變化并不大，Y方向加速如圖22有3個峰值。對比多項式軌跡和復合擺線軌跡運動過程的最大值如表1，可以看出對于相同的位移多項式軌跡和復合擺線軌跡均能夠滿足約束要求并完成運動，X和Y方向上的速度兩種軌跡差別不大，X方向上加速度最大值差距較小，但是擺動相Y方向加速度最大值差距較大，多項式軌跡4 064.6 mm/s²，復合擺線軌跡 1 973.9 mm/s²。加速度表示支撐機構在運動過程中受到沖擊的大小，在運動過程中應該盡量減小沖擊，顯然復合擺線軌跡的最大加速較小更加符合設計目的，所以本文將復合擺線軌跡作為最終的運動軌跡方案。

通過圖7和圖24可以看到多項式軌跡和復合擺線軌跡運動在擺動相的末端軌跡，通過調節參數S，H，T可以獲得不同的軌跡和速度。其中（S4，H2）可以用來跨越距離長高度低的障礙，（S2，H4）可應用于跨越距離短高度高的障礙。對于不同的障礙物通過調節參數就可以跨越不同種類的障礙物。

4"結論

參考仿生MIT機械狗和Ascento結構設計了自適應管道機器人的支撐機構，并進行了正逆運動學的分析，選擇性能更強的eRob90I電機模組，使支撐機構折疊尺寸更小，展開臂展更長，提升了管道自適應機器人的支撐性能的同時增強了整體結構的承載性能。通過對比多項式軌跡和復合擺線軌跡運動過程的最大值，得到復合擺線軌跡的最大加速度相對于多項式擺動軌跡較小的結論，最終以復合擺線軌跡作為最終的運動軌跡規劃方案。未來可將正逆運動學與人工智能、機器學習等領域相結合，利用數據驅動的方法提高運動學模型的性能。

參考文獻

［1］"楊家岐. 排水管道自適應清理機器人設計與研究［D］. 青島： 青島大學， 2023.

［2］"羅繼曼， 都闖， 郭松濤， 等. 管道機器人輪爪式行走裝置運動學和力學特性分析［J］. 沈陽建筑大學學報（自然科學版）， 2020， 36（2）： 344351.

［3］"羅繼曼， 戴璐璐， 印輝， 等. 基于模糊PI的管道清淤機器人控制系統研究［J］. 機械與電子， 2020， 38（4）： 5156.

［4］"羅繼曼， 印輝， 郭松濤， 等. 基于流固耦合的管道機器人清淤裝置［J］. 科學技術與工程， 2020， 20（9）： 36353639.

［5］"李文章， 周明連. 全氣動管道檢測機器人的研制［J］. 液壓與氣動， 2020（8）： 161166.

［6］"KAZEMINASAB S， JANFAZA V， RAZAVI M， et al. Smart navigation for an in-pipe robot through multi-phase motion control and particle filtering method［C］∥2021 IEEE International Conference on Electro Information Technology （EIT）， Mt. Pleasant， 2021： 342349.

［7］"SATISH P V， KUMAR S N. Design of anin-pipe inspecion robot［J］. International Journal of Engineering Research amp; Technology （IJERT）， 2017， "6（3）： 6368.

［8］"DHANANCHEZHIYAN P， HIREMATH S S， SINGAPERUMAL M， et al. Design and development of a reconfigurable type autonomous sewage cleaning mobile manipulator［J］. Procedia Engineering， 2013， 64： 14641473.

［9］"李忠唐， 徐如斌， 陸文龍， 等. 熔融沉積成型自適應柱狀支撐算法設計與研究［J］. 機床與液壓， 2021， 49（10）： 7680.

［10］"李劍鋒， 趙朋波， 張雷雨， 等. 一種新型自適應康復外骨骼機構及重力平衡優化［J］. 工程科學與技術， 2018， 50（5）： 263270.

［11］"劉東， 羅亮， 盧丙舉， 等. 輪式變徑管道機器人結構分析及控制系統設計［J］. 艦船科學技術， 2023， 45（22）： 6771.

［12］"周龍. 關節履帶式核應急機器人越障分析與自適應控制研究［D］. 衡陽： 南華大學， 2023.

［13］"KATZ B G， A low cost modular actuator for dynamic robots［D］. Massachusetts： The MIT Press Cambridge， 2018.

［14］"KLEMM V， MORRA A， SALZMANN C， et al. Ascento： a two-wheeled jumping robot［C］∥2019 International Conference on Robotics and Automation （ICRA）， Montreal， QC， Canada： ICRA， 2019： 75157521.

［15］"吳文文. 腿足式機器人用關節電機設計的研究［D］. 南京： 東南大學， 2022.

［16］"徐良， 谷世偉， 楊海鋒， 等. 碳纖維增強復合材料與6061鋁合金激光連接仿真［J］. 焊接學報， 2023， 44（11）： 4251， 131.

［17］"陳濤. 五自由度自動加油機械手臂的設計與研究［D］. 西安： 西京學院， 2023.

［18］"毛怡琳， 李悅， 魏銘辰. 六自由度垃圾分揀機器人正逆運動學分析及構建工作空間的研究［J］. 現代機械， 2023（6）： 5559.

［19］"姜文軍， 呂東波， 陳正隆， 等. 手術機器人萬向關節組的正逆運動學分析［J］. 安徽科技學院學報， 2023， 37（6）： 103110.

Kinematics Analysis of Support Mechanism of Adaptive Cleaning Robot for Drainage Pipe

XU Wei^a， HUANG Qinghui^a， LI Baokang^a， YANG Jiaqi^a， WANG Jirong^a，b

（a. College of Mechanical and Electrical Engineering， Qingdao 266071， China;

b. Weihai Innovation Research Institute， Qingdao University， Weihai 264200， China）

Abstract：

With the advancement of urbanization， the scale of urban drainage system increases， and the problem of drainage pipe blockage becomes more and more complicated. Due to the limitation of pipeline space， the traditional manual blockage clearing method has the problems of difficult operation and low efficiency. The efficiency and safety of blocking removal can be improved by means of adaptive cleaning robot instead of manual entry into the pipeline， and the smooth operation of urban drainage system can be ensured.Through forward and inverse kinematics analysis and forward and inverse motion analysis of the support mechanism of the adaptive cleaning robot， the design and structure optimization of the support mechanism of the adaptive cleaning robot are completed， which provides the basic theory and method for the design， optimization， control and verification of the support mechanism. The feasibility of the mechanism is verified by simulation， and the trajectory planning of the support mechanism makes it realize the obstacle avoidance function in the complex environment of the drainage pipeline.

Keywords： pipeline cleaning robot; supporting mechanism; forward and inverse kinematics; trajectory planning