煤礦抓管機器人冗余機械臂逆運動學求解算法

朱明鎏 ，李 梁

（1.國家能源集團寧夏煤業公司 棗泉煤礦，寧夏 靈武 750499；2.中煤科工機器人科技有限公司，廣東 深圳 518000）

現階段，煤礦井下管道的常規安裝仍是采用人力和輔助設備相結合的傳統方法[1]。首先把起吊架固定在梭礦車上，用手拉葫蘆起吊煤礦管道，然后工人爬到固定好的管路上進行連接施工。此種接管工藝完全依靠人力，導致安全性差，工作效率低，具有很大的制約性及安全隱患。煤礦抓管機器人是近年來興起的一種新型的井下輔助作業機器人[2-3]，主要由履帶式移動底盤搭載重載液壓機械臂，通過車-臂協同控制，來實現管路的搬運、碼放、安裝、拆卸等多種功能，作業對象可以為煤礦的瓦斯管、水管、風管等。

機械臂作為抓管機器人目標物抓取作業的重要組成，其有效的連桿坐標系間的運動關系決定了機械臂的運動控制精度和平穩可靠運行。機械臂的逆運動學求解是已知末端坐標系在慣性參考系下的位置和姿態描述，求解各個關節的變量值。目前求機械臂運動學逆解的方法主要有解析法和數值法[4-6]。串聯機械臂用逆運動學解析解的充分條件是滿足Pieper 準則。解析法比數值方法求解精度高、速度快，而且容易區分所有可能解，是機器人逆運動學求解的首選方法。因此，一般只有在機器人構型不滿足封閉解求解條件時，才會采用數值方法求解。對于數值解法來說，目前國內研究熱點是設計基于優化算法的逆運動學求解算法，并取得了大量成果[7-10]。然而數值解法存在的主要問題是求解效率低、不穩定、可能求解失敗等，顯然與煤礦應用的高可靠性原則相悖。

為了適應煤礦管路安裝作業的特點以及液壓驅動機構的動作特性，管路抓取作業機械臂的結構設計無法滿足Pieper 準則，因此不存在封閉解；另一方面，為了增加機械臂的操作靈活性，抓管機器人的液壓機械臂還具有1 個冗余自由度，導致在逆運動學求解時存在無窮多種解。為了解決上述難題，基于固定角度法和搜索方法，設計了具有冗余自由度的煤礦抓管機器人液壓機械臂的逆運動學解析解法，保證了求解過程的高可靠性、高效率和結果的準確性。同時，解析解因具有顯式數學表達式，相比于雅克比矩陣法、遺傳算法等數值算法更易于部署到單片機、PLC 等嵌入式控制器，極大地降低了算法部署的成本和難度。

1 煤礦抓管機器人

針對國內某煤礦輔助作業場景需求，研制出了一款煤礦抓管機器人。該機器人具有遙控行走、電子圍欄等功能，能夠有效輔助人員完成煤礦巷道內的不同規格尺寸的管道搬運、提升、安裝等工序，且可根據現場實際情況選擇半自主作業，遠程遙控作業、人工就地作業等多種作業模式，可有效降低井下作業人員的人身安全，同時大大提高工作效率，實現了煤礦井下巷道危重場景全方位的智能化、無人化輔助作業，保障煤礦安全生產。

煤礦抓管機器人的系統構成如圖1。

履帶式移動底盤上搭載了抓管機械臂，抓管機械臂為液壓機械臂，通過履帶式移動底盤上搭載的液壓泵站驅動。煤礦抓管機器人通過自身搭載的激光測距儀的引導實現在井下巷道的自主導航和運動，并且通過自身搭載的視覺系統對管道進行識別并引導抓管機械臂進行管道安裝作業。

煤礦抓管機器人能夠在空間狹小、傾角大等復雜井下條件下實現管道的安全、高效安裝。抓管機械臂主要技術參數為：①抓管機械臂末端最大速度：0.1 m/s；②作業半徑：3 m；③最大抓舉質量：0.5 t；④機械臂自由度數量：6；⑤遙控距離：30 m；⑥管道安裝精度：≤6 mm；⑦管道識別角度誤差：≤±3°；⑧標志物識別率：≥95%。除了管路的抓取、搬運及安裝等作業功能外，煤礦抓管機器人還具有如下功能：

1）避障功能。機器人搭載超聲避障傳感器，當傳感器探測到障礙物時，機器人按照預先設定好的包含速度和加速度約束的軌跡平滑地實現停車。

2）通信功能。機器人通過無線路由器與本地設備的有線通信，通過OPC 系統實現了部分參數井上管控平臺和本地通信。

3）路徑規劃導航功能。機器人搭載3D 激光測距傳感器，通過糾偏算法實現移動過程中機器人本體與目標側巷道壁的距離始終保持不變。

4）車輛姿態感知功能。機器人通過傾角傳感器、編碼器獲得自身的當前位姿，超過安全限度即時報警。

5）視覺功能。機器人通過視覺傳感器識別管路，完成管路坐標的識別，將坐標數據發送給控制系統。

6）語音提示功能。機器人具有語音提示功能，可以合理對故障及隱患進行預報警，提高作業時的安全性。

2 冗余液壓機械臂逆運動學

機械臂的控制系統采用層級模塊化設計，具備關節編碼器信號采集、運動學解算、控制指令接收與下達及液壓伺服控制等功能。工作原理具體為：控制系統采用機器視覺等傳感技術獲得目標物體的位姿，并將其通過以太網（TCP）發送給運動控制器，控制器自主完成軌跡規劃；當機械臂末端處于軌跡上某點時，逆運動學算法將從編碼器讀取機械臂的當前關節角序列作為搜索算法的初始值，并根據軌跡上的下1 個運動點對應的末端位姿完成逆運動學解算，獲得下1 個運動點對應的機械臂關節角序列，并將其發送給液壓伺服控制系統來驅動機械臂末端運動到軌跡上的下1 點。

2.1 基于固定角度法的封閉解

機械臂的各關節旋轉軸分布如圖2，基于標準D-H 方法建立的機械臂連桿坐標系如圖3。

圖2 機械臂的各關節旋轉軸分布Fig.2 Configuration of each joint rotation axis of robotic arm

圖3 機械臂坐標系Fig.3 Robotic arm coordinate system

由圖2 可知：液壓機械臂共有6 個自由度，分別是基座繞垂直軸的旋轉自由度（軸1）、4 個連桿繞關節軸的旋轉自由度（軸2～軸5），以及機械臂末端的法蘭盤旋轉自由度（軸6）。

由圖3 可知：機械臂各關節旋轉角qj繞zj-1旋轉，旋轉方向遵守右手法則。記抓手中心點T相對于坐標系 {0}的 坐標為（xOT,yOT,zOT）， 即坐標系 {6}相對于 {0}的 平移。圖中： θi為關節旋轉角，是xi-1沿zi轉向xi軸 的有向轉角；di為 連桿偏距，是xi-1與zi的交點到連桿i坐標系的原點沿zi方向的有向距離；ai為 連桿長度，是zi軸 與zi+1軸 的公法線長度； αi為連桿扭轉角，是zi軸 沿xi軸 轉向zi+1軸的有向轉角。

抓管機械臂的標準D-H 參數模型各參數為：①桿1：θ1=q1rad，d1=0.135 mm，a1=0.190 mm，α1=π/2；②桿2：θ2=q2rad，d2=0 mm，a2=1.150 mm，α2=p2rad；③桿3：θ3=q3rad，d3=0 mm，a3=1.450 mm，α3=0；④桿4：θ4=q4rad，d4=0 mm，a4=0.970 mm，α4=0；⑤桿5：θ5=（q5-π/2） rad，d5=0.466 mm，a5=0.138 mm，α1=-π/2；⑥桿6：θ6=q6rad，d6=0.215 mm，a6=0 mm，α6=0。

基于上述坐標系和D-H 模型參數，建立的煤礦抓管機器人的前向運動學公式如式（1）：

式中：0AT為從機器人機座到機械臂末端的坐標變換矩陣；0A1、1A2、2A3、3A4、4A5、5A6為4×4階坐標變換矩陣。

令：sin（x）簡寫為Sx、 cos（x）簡寫為Cx（以下皆采用簡寫），則0A1、1A2、2A3、3A4、4A5、5A6坐標變換矩陣可表示為：

固定大臂旋轉角，即令q2為已知量，對式（7）方程組進行求解得到封閉解如下：

2.2 基于搜索方法的逆運動學求解

逆運動學算法流程圖如圖4。

圖4 逆運動學算法流程圖Fig.4 Flow chart of inverse kinematics algorithm

逆運動學算法基本思想是固定大臂傾角q2，令其值等于運動前的初始值q2_original，假如由式（8）求得的封閉解滿足機械臂的各個角度約束，則無需搜索新的q2，直接得到1 組解，供運動控制系統使用。簡單來說就是在滿足末端位姿要求時，先保持大臂不動，動其他臂。如果上述情況不成立，則在大臂傾角的初始值q2_original附近搜索1 個離其最近的q2，使得q1、q2、q3、q4、q5同時滿足各自角度約束,并由（q1,q2,q3,q4,q5）構成1 組可行解。搜索步長為0.1°（步長可根據機器人現場調試效果在程序中調整）。簡單來說，就是在保證大臂動得最小的前提下，求取封閉解，來使得滿足末端位姿要求時各個運動軸的位移量總體來說盡量最小。

3 仿真與實驗

基于上述給出的機械臂逆運動學求解公式和算法流程，采用MATLAB 進行了逆運動學算法的仿真驗證。根據圖4 所示算法流程圖，編寫出MATLAB 函數[ik_q1,ik_q2,ik_q3,ik_q4,ik_q5]=

pipe_grab_ikine（x_oT,y_oT,z_oT,beta_T,q2_original）。函數的輸入參數為抓手中心點三維坐標x_oT,y_oT,z_oT、抓手傾角 beta_T及大臂傾角的初始值q2_original， 輸出參數為各旋轉軸的角度ik_q1，ik_q2，ik_q3，ik_q4，ik_q5。

假設目標末端位姿為T_in，當大臂傾角初始值為q2_original 時，通過逆運動學算法求得1 組逆解ik_q。再以逆解ik_q作為輸入，進行前向運動學計算得到末端位姿T_ik。由仿真結果可知T_ik=T_in，即根據目標位姿進行逆運動學求解得到的關節角序列能夠驅動機械臂末端運動到目標位姿，說明逆運動學算法有效。

基于該逆運動學函數，在笛卡爾空間內規劃1 條直線軌跡，令機械臂末端從T1運動到T2，基于MATLAB 的機械臂逆運動學仿真實驗如圖5。圖中的T1點與T2點之間的直線段為機械臂在仿真環境中實際劃過的軌跡，說明基于本文設計的逆運動學算法，管路安裝機器人能夠實現機械臂末端的精準軌跡控制。

圖5 基于MATLAB 的機械臂逆運動學仿真實驗Fig.5 Inverse kinematics simulation experiment of robotic arm based on MATLAB

此外，該算法已在煤礦抓管機器人樣機上部署，并在煤礦井下進行管路抓取作業測試。額定負載管路直徑300 mm，質量500 kg，設定機械臂初始位姿為（1 860，900，0），目標位姿（650，0，0），進行機械臂重復運行管路抓取動作定位精度測試。試驗結果顯示：冗余機械臂可以實現重負載抓管操作，抓取過程中機械臂操控性良好，運行穩定、可靠，絕對誤差最大值為7.81 mm，滿足煤礦井下機械臂輔助作業精度設計要求。

4 結 語

針對現有煤礦井下管路安裝作業需求痛點，完成了基于模塊化技術的煤礦抓管機器人整體系統方案設計，并完成機器人的系統組成、核心參數和主要功能分析；研究了基于解析法的機械臂逆運動學求解算法，推導了冗余機械臂的逆運動學求解原理，給出了算法的具體實現流程；采用matlab 軟件完成了冗余機械臂的運動學仿真模擬。仿真結果表明，逆運動學求解得到的關節角序列能夠驅動機械臂末端運動到目標位姿，能夠實現機械臂的高精度作業控制，驗證了本研究算法的有效性。煤礦抓管機器人機械臂的運動控制精度測試結果表明：機械臂運行平穩、可靠，抓管精度絕對誤差最大值為7.81 mm，滿足設計需求。