朱明鎏 ，李 梁

（1.國(guó)家能源集團(tuán)寧夏煤業(yè)公司 棗泉煤礦，寧夏 靈武 750499；2.中煤科工機(jī)器人科技有限公司，廣東 深圳 518000）

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

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

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

1 煤礦抓管機(jī)器人

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

煤礦抓管機(jī)器人的系統(tǒng)構(gòu)成如圖1。

履帶式移動(dòng)底盤上搭載了抓管機(jī)械臂，抓管機(jī)械臂為液壓機(jī)械臂，通過履帶式移動(dòng)底盤上搭載的液壓泵站驅(qū)動(dòng)。煤礦抓管機(jī)器人通過自身搭載的激光測(cè)距儀的引導(dǎo)實(shí)現(xiàn)在井下巷道的自主導(dǎo)航和運(yùn)動(dòng)，并且通過自身搭載的視覺系統(tǒng)對(duì)管道進(jìn)行識(shí)別并引導(dǎo)抓管機(jī)械臂進(jìn)行管道安裝作業(yè)。

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

1）避障功能。機(jī)器人搭載超聲避障傳感器，當(dāng)傳感器探測(cè)到障礙物時(shí)，機(jī)器人按照預(yù)先設(shè)定好的包含速度和加速度約束的軌跡平滑地實(shí)現(xiàn)停車。

2）通信功能。機(jī)器人通過無線路由器與本地設(shè)備的有線通信，通過OPC 系統(tǒng)實(shí)現(xiàn)了部分參數(shù)井上管控平臺(tái)和本地通信。

3）路徑規(guī)劃導(dǎo)航功能。機(jī)器人搭載3D 激光測(cè)距傳感器，通過糾偏算法實(shí)現(xiàn)移動(dòng)過程中機(jī)器人本體與目標(biāo)側(cè)巷道壁的距離始終保持不變。

4）車輛姿態(tài)感知功能。機(jī)器人通過傾角傳感器、編碼器獲得自身的當(dāng)前位姿，超過安全限度即時(shí)報(bào)警。

5）視覺功能。機(jī)器人通過視覺傳感器識(shí)別管路，完成管路坐標(biāo)的識(shí)別，將坐標(biāo)數(shù)據(jù)發(fā)送給控制系統(tǒng)。

6）語音提示功能。機(jī)器人具有語音提示功能，可以合理對(duì)故障及隱患進(jìn)行預(yù)報(bào)警，提高作業(yè)時(shí)的安全性。

2 冗余液壓機(jī)械臂逆運(yùn)動(dòng)學(xué)

機(jī)械臂的控制系統(tǒng)采用層級(jí)模塊化設(shè)計(jì)，具備關(guān)節(jié)編碼器信號(hào)采集、運(yùn)動(dòng)學(xué)解算、控制指令接收與下達(dá)及液壓伺服控制等功能。工作原理具體為：控制系統(tǒng)采用機(jī)器視覺等傳感技術(shù)獲得目標(biāo)物體的位姿，并將其通過以太網(wǎng)（TCP）發(fā)送給運(yùn)動(dòng)控制器，控制器自主完成軌跡規(guī)劃；當(dāng)機(jī)械臂末端處于軌跡上某點(diǎn)時(shí)，逆運(yùn)動(dòng)學(xué)算法將從編碼器讀取機(jī)械臂的當(dāng)前關(guān)節(jié)角序列作為搜索算法的初始值，并根據(jù)軌跡上的下1 個(gè)運(yùn)動(dòng)點(diǎn)對(duì)應(yīng)的末端位姿完成逆運(yùn)動(dòng)學(xué)解算，獲得下1 個(gè)運(yùn)動(dòng)點(diǎn)對(duì)應(yīng)的機(jī)械臂關(guān)節(jié)角序列，并將其發(fā)送給液壓伺服控制系統(tǒng)來驅(qū)動(dòng)機(jī)械臂末端運(yùn)動(dòng)到軌跡上的下1 點(diǎn)。

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

機(jī)械臂的各關(guān)節(jié)旋轉(zhuǎn)軸分布如圖2，基于標(biāo)準(zhǔn)D-H 方法建立的機(jī)械臂連桿坐標(biāo)系如圖3。

圖2 機(jī)械臂的各關(guān)節(jié)旋轉(zhuǎn)軸分布Fig.2 Configuration of each joint rotation axis of robotic arm

圖3 機(jī)械臂坐標(biāo)系Fig.3 Robotic arm coordinate system

由圖2 可知：液壓機(jī)械臂共有6 個(gè)自由度，分別是基座繞垂直軸的旋轉(zhuǎn)自由度（軸1）、4 個(gè)連桿繞關(guān)節(jié)軸的旋轉(zhuǎn)自由度（軸2～軸5），以及機(jī)械臂末端的法蘭盤旋轉(zhuǎn)自由度（軸6）。

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

抓管機(jī)械臂的標(biāo)準(zhǔn)D-H 參數(shù)模型各參數(shù)為：①桿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。

基于上述坐標(biāo)系和D-H 模型參數(shù)，建立的煤礦抓管機(jī)器人的前向運(yùn)動(dòng)學(xué)公式如式（1）：

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

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

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

2.2 基于搜索方法的逆運(yùn)動(dòng)學(xué)求解

逆運(yùn)動(dòng)學(xué)算法流程圖如圖4。

圖4 逆運(yùn)動(dòng)學(xué)算法流程圖Fig.4 Flow chart of inverse kinematics algorithm

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

3 仿真與實(shí)驗(yàn)

基于上述給出的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解公式和算法流程，采用MATLAB 進(jìn)行了逆運(yùn)動(dòng)學(xué)算法的仿真驗(yàn)證。根據(jù)圖4 所示算法流程圖，編寫出MATLAB 函數(shù)[ik_q1,ik_q2,ik_q3,ik_q4,ik_q5]=

pipe_grab_ikine（x_oT,y_oT,z_oT,beta_T,q2_original）。函數(shù)的輸入?yún)?shù)為抓手中心點(diǎn)三維坐標(biāo)x_oT,y_oT,z_oT、抓手傾角 beta_T及大臂傾角的初始值q2_original， 輸出參數(shù)為各旋轉(zhuǎn)軸的角度ik_q1，ik_q2，ik_q3，ik_q4，ik_q5。

假設(shè)目標(biāo)末端位姿為T_in，當(dāng)大臂傾角初始值為q2_original 時(shí)，通過逆運(yùn)動(dòng)學(xué)算法求得1 組逆解ik_q。再以逆解ik_q作為輸入，進(jìn)行前向運(yùn)動(dòng)學(xué)計(jì)算得到末端位姿T_ik。由仿真結(jié)果可知T_ik=T_in，即根據(jù)目標(biāo)位姿進(jìn)行逆運(yùn)動(dòng)學(xué)求解得到的關(guān)節(jié)角序列能夠驅(qū)動(dòng)機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)位姿，說明逆運(yùn)動(dòng)學(xué)算法有效。

基于該逆運(yùn)動(dòng)學(xué)函數(shù)，在笛卡爾空間內(nèi)規(guī)劃1 條直線軌跡，令機(jī)械臂末端從T1運(yùn)動(dòng)到T2，基于MATLAB 的機(jī)械臂逆運(yùn)動(dòng)學(xué)仿真實(shí)驗(yàn)如圖5。圖中的T1點(diǎn)與T2點(diǎn)之間的直線段為機(jī)械臂在仿真環(huán)境中實(shí)際劃過的軌跡，說明基于本文設(shè)計(jì)的逆運(yùn)動(dòng)學(xué)算法，管路安裝機(jī)器人能夠?qū)崿F(xiàn)機(jī)械臂末端的精準(zhǔn)軌跡控制。

圖5 基于MATLAB 的機(jī)械臂逆運(yùn)動(dòng)學(xué)仿真實(shí)驗(yàn)Fig.5 Inverse kinematics simulation experiment of robotic arm based on MATLAB

此外，該算法已在煤礦抓管機(jī)器人樣機(jī)上部署，并在煤礦井下進(jìn)行管路抓取作業(yè)測(cè)試。額定負(fù)載管路直徑300 mm，質(zhì)量500 kg，設(shè)定機(jī)械臂初始位姿為（1 860，900，0），目標(biāo)位姿（650，0，0），進(jìn)行機(jī)械臂重復(fù)運(yùn)行管路抓取動(dòng)作定位精度測(cè)試。試驗(yàn)結(jié)果顯示：冗余機(jī)械臂可以實(shí)現(xiàn)重負(fù)載抓管操作，抓取過程中機(jī)械臂操控性良好，運(yùn)行穩(wěn)定、可靠，絕對(duì)誤差最大值為7.81 mm，滿足煤礦井下機(jī)械臂輔助作業(yè)精度設(shè)計(jì)要求。

4 結(jié) 語

針對(duì)現(xiàn)有煤礦井下管路安裝作業(yè)需求痛點(diǎn)，完成了基于模塊化技術(shù)的煤礦抓管機(jī)器人整體系統(tǒng)方案設(shè)計(jì)，并完成機(jī)器人的系統(tǒng)組成、核心參數(shù)和主要功能分析；研究了基于解析法的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解算法，推導(dǎo)了冗余機(jī)械臂的逆運(yùn)動(dòng)學(xué)求解原理，給出了算法的具體實(shí)現(xiàn)流程；采用matlab 軟件完成了冗余機(jī)械臂的運(yùn)動(dòng)學(xué)仿真模擬。仿真結(jié)果表明，逆運(yùn)動(dòng)學(xué)求解得到的關(guān)節(jié)角序列能夠驅(qū)動(dòng)機(jī)械臂末端運(yùn)動(dòng)到目標(biāo)位姿，能夠?qū)崿F(xiàn)機(jī)械臂的高精度作業(yè)控制，驗(yàn)證了本研究算法的有效性。煤礦抓管機(jī)器人機(jī)械臂的運(yùn)動(dòng)控制精度測(cè)試結(jié)果表明：機(jī)械臂運(yùn)行平穩(wěn)、可靠，抓管精度絕對(duì)誤差最大值為7.81 mm，滿足設(shè)計(jì)需求。