窨井蓋更換機器人的運動學分析與其工作裝置控制系統研究

吳卓良WU Zhuo-liang

（重慶交通大學車輛與機電工程學院，重慶400067）

0 引言

近年來，隨著各個地區城市智能化進度的不斷發展，電氣與燃氣運輸、電信網絡線路、給排水等線路管道鋪設的增多，窨井的使用數量也越來越多。因此，在市政養護工作中，窨井蓋的更換變得更為頻繁。現存在的更換方式均為人工操作器械且還需不同工人進行相互配合方能完成，程序繁瑣、更換時危險系數較大且效率不高。在窨井蓋更換上，出現了由凱斯SR250 滑移裝載機裝配HS-57II 型井蓋銑刨機附件構成的機械；也出現了由徐工筑路公司推出的XM50 型瀝青路面銑刨機；另有VISTA 公司推出的集路面銑刨、井蓋提拉一體的井蓋銑刨機。窨井蓋更換是市政養護工作的重點部分，現擁有的各種機械均為人工操作，無法達到智能化操作，高效更換的目的。因此，本文基于對路面銑刨之后進行井蓋更換的方式設計一款具有較高穩定性、響應快速的兩自由度窨井蓋更換機器人。

1 機器人結構設計

窨井蓋更換機器人主要包括機器人主體、末端執行器、定位器組成，如圖1 所示。

圖1 窨井蓋更換機器人

末端執行器主要包括刀頭、刀盤、提拉設備等，末端執行器是該機器人功能的直接體現，主要功能是對需更換井蓋的路面進行旋轉銑刨，配合窨井定位器進行定位并提拉井座。依據《城市道路工程設計規范》CJJ 37-2012：面層水泥混凝土的抗彎拉強度不得低于4.5MPa，快速路、主干路、重交通等不得低于5.0MPa。為此采用高強度碳鋼合金刀頭設計，共計18 枚，另刀頭與刀片為可拆卸式設計，更換損壞刀頭更具高效性與靈活性。同時為加大機器人靈活性及降低末端執行器重量，采用圓形刀片式切割。為適應不同尺寸窨井蓋更換作業的要求刀片與刀盤之間為可調式伸縮結構設計。其原理是刀片通過高速旋轉對路面進行銑削工作，到達銑削深度要求時，停止銑削，通過中部定位軸與定位器相互配合對窨井蓋及其周邊路面進行提拉作業，進而實行更換。

由于城市窨井蓋的各式各樣，其尺寸也因實際使用情況而不同，生活中常見的尺寸為600mm、700mm、800mm等，因此定位器的設計需滿足能對各種尺寸窨井定位從而更換工作的要求，如圖2 所示。

圖2 窨井蓋定位器

為了適應窨井圓形形狀，采用三靜臂呈Y 字形圓弧排列設計，各靜臂夾角為120°，靜臂與定位器中心及動頂頭與靜頂頭均為可調式螺栓連接，依據窨井尺寸及井座高度進行調節達到定位穩固的工作要求。同時在靜臂與定位器中心增設了彈簧接口，提高其工作時的穩定性。

為了能迅速移動至目的地進行快速更換工作要求，由圖可知，機器人整體采用車輛式設計，末端執行器的驅動形式為液壓馬達驅動，實現對窨井蓋周圍路面的銑削功能；兩動臂的驅動形式為液壓缸驅動，相互配合實現對窨井蓋的定位、井蓋及井座整體的提拉。工作裝置各關節均以鉸接方式連接，另增添了備用人工操作室，預防機器人意外故障的發生。

2 機器人運動學分析

窨井蓋更換機器人工作裝置為大動臂、小動臂故而可簡化為一個二自由度機器人。在機器人運動學中，分為正向運動學分析與反向運動學分析，另外描述各桿件間的相對關系時，通常采用D-H 參數法根據連桿坐標系{i}與連桿i 的位置關系，D-H 參數法有坐標系前置與坐標系后置兩種，二者研究等價[1]，本文研究采用坐標系后置的方法，建立的連桿坐標系如圖3 所示。

圖3 機器人工作裝置連桿坐標系

圖中θ1表示動臂1 的關節角，θ2表示動臂2 的關節角；ai表示沿xi方向，zi到zi+1的距離，工作裝置的各個連桿由連桿長度ai，連桿的扭轉角αi，關節轉角θi和連桿偏距di四個參數描述。

研究對象，窨井蓋更換機器人工作裝置的關節參數如表1 所示。

表1 工作裝置D-H 坐標系參數表

機器人連桿坐標系{i-1}到坐標系{i}的齊次變換矩陣，表示為

對該機器人的正向運動學進行求解，計算公式為

在該機器人中，其工具坐標系與連桿2 的坐標系重合，因此

根據表1 中的數據及式（3）依次代入式（2）中，得到

其中，θ12是θ1+θ2的縮寫；

cθ12=cos（θ1+θ2）；sθ12=sin（θ1+θ2）。由上式可知機器人末端的位姿，用向量[x，y，z，φ]T的形式來表示：

由此可知，機器人末端執行器的姿態角僅與兩動臂轉角有關。正向運動學的解是唯一確定的，即機器人的末端執行器的位姿是唯一確定的；反向運動學的解則往往具有多重解[2]，需根據特定情況而言選擇其最優解。

3 液壓臂控制系統設計

PID 控制器結構簡單，在生產過程中是一種最普遍采用的控制方法冶金、化工、機械等行業中獲得了廣泛應用。

PID 的控制規律為：

其中，u（t）為控制器的輸出，e（t）為控制器的輸入，Kp為比例增益，Ti為積分時間常數，Td為微分時間常數。根據Kp、Td、Ti三種參數對系統相應的影響，經過反復調節整定，最終確定PID 的各控制參數。

經計算，大臂控制子系統開、閉環傳遞函數為

本文以大臂為例進行控制仿真分析，利用Matlab 對大臂閉環系統仿真，分別輸入階躍信號和正弦信號，分得到其響應曲線如圖4、圖5 所示。

圖4 大臂系統單位階躍響應曲線

圖5 大臂系統正弦跟蹤曲線

由圖4 可得到峰值yp=1，即無超調，峰值時間tp=10.9s，調整時間ts=5.6s。由圖5 可得正弦跟蹤信號具有較大相位延遲，在短時間內不能達到預設值；由以上分析可知，大動臂控制系統在無PID 控制時響應時間較長且正弦響應誤差較大，因此為縮短響應時間及提高響應精度，為此引入PID 控制器。

運用試湊法對PID 控制器參數進行整定[3]，通過大量實驗仿真，對比之后得到了一組令人較為滿意的系統PID參數：Kp=4.02，Ki=2.93，Kd=0.23。運用Simulink 建立加入PID 控制器之后進行大臂控制系統仿真模擬。

由圖6 可得到，加入PID 控制器后大動臂系統階躍響應曲線峰值yp=1，即無超調，峰值時間tp=1.73s，調整時間ts=1.3s，上升時間tr=0.79s，加入PID 控制器后的大臂系統響應時間及調整明顯極大縮短；由圖7 可得到加入PID 控制后，大動臂系統正弦跟蹤信號基本沒有衰減和相位延遲，能在較短的時間內達到設定值，PID 校正效果良好；綜上所述，加入PID 控制器后的大動臂系統在保持良好的穩定性基礎上，響應速度明顯加快，跟蹤輸出能力增強，能快速達到設定控制要求。

圖6 大臂PID 控制系統階躍響應曲線

圖7 大臂PID 控制系統正弦跟蹤曲線

4 結語

針對目前窨井蓋更換繁瑣、效率低下設計一款便捷、靈敏的兩自由度窨井蓋更換機器人。采用D-H 參數法建立了機器人連桿坐標系并進行了運動學分析；運用PID 控制理論設計了機器人機械臂的控制系統并利用Matlab 仿真，結果表明了機器人工作裝置控制系統設計的合理性及穩定性，為后續該類機器人的改進提供了參考。