姚海峰，劉偉，鄭曉雯，張?zhí)m勝2，，劉福新2，，吳淼

1.中國礦業(yè)大學(xué)(北京) 機(jī)電與信息工程學(xué)院，北京 100083；2.河北省煤礦機(jī)械工程技術(shù)研究中心，河北石家莊 051431；3.石家莊煤礦機(jī)械有限責(zé)任公司，河北石家莊 051431

機(jī)器人工作時需要對各關(guān)節(jié)參數(shù)進(jìn)行實時控制，對各關(guān)節(jié)的運動進(jìn)行精度補償，因此機(jī)器人各關(guān)節(jié)的逆運動學(xué)求解十分重要[1-3]。工業(yè)機(jī)器人多為6個自由度機(jī)器人，前端肩部3個自由度用于確定位置，后端腕部3個自由度用于確定姿態(tài)，可以在運動范圍內(nèi)任意定位與定向[4]。

已證明只有2種構(gòu)型機(jī)器人的運動學(xué)逆解是封閉的，一類是存在3個連續(xù)相交軸的Pieper型機(jī)器人，一類是存在3個連續(xù)平行軸的Duffy型機(jī)器人[5]。機(jī)器人逆運動學(xué)求解一般有3種方法：代數(shù)法、幾何法和DH法[6-8]。代數(shù)法和幾何法適用于少自由度機(jī)器人，其關(guān)節(jié)之間要滿足一定條件[9]；對于6個自由度機(jī)器人當(dāng)前應(yīng)用較廣的是DH方法。

為得到簡易的封閉逆解，多數(shù)工業(yè)機(jī)器人構(gòu)型滿足Pieper條件，這類機(jī)器人即球型腕部機(jī)器人。在國內(nèi)外的相關(guān)文獻(xiàn)[2，10-13]中討論的都是這種球形腕部機(jī)器人，這種構(gòu)型存在不能承受較大負(fù)載的缺陷。為解決這一問題，采用腕部偏置機(jī)器人的結(jié)構(gòu)形式[14]。當(dāng)前對腕部偏置型機(jī)器人運動學(xué)求解的研究多借助于復(fù)雜的數(shù)值計算求解，文獻(xiàn)[14-19]中認(rèn)為不存在解析解，須通過優(yōu)化、迭代或神經(jīng)網(wǎng)絡(luò)等方法求取數(shù)值解。這些方法往往計算時間長且精度較差，不利于工業(yè)性應(yīng)用。

本文對復(fù)雜的腕部偏置型機(jī)器人進(jìn)行研究，該機(jī)器人用于掘進(jìn)機(jī)截割頭焊接和井下鑿巖，為了增大工作范圍和靈活性，在執(zhí)行端采用滑動構(gòu)件使得其運動學(xué)求解更為復(fù)雜。筆者結(jié)合迭代搜索求解的思路采用一種有別于傳統(tǒng)DH方法的全局坐標(biāo)系遞次歸位的算法進(jìn)行求解，該方法計算耗時短、精度高，可促進(jìn)腕部偏置型機(jī)器人工業(yè)應(yīng)用的實現(xiàn)。

1 建立全局坐標(biāo)系

各種文獻(xiàn)中常見的PUMA560機(jī)器人和斯坦福機(jī)器人都屬于典型的球型腕部機(jī)器人，其構(gòu)型如圖1所示。

腕部偏置伸縮型Pieper機(jī)器人構(gòu)型和結(jié)構(gòu)如圖2所示，圖2(a)為構(gòu)型，圖2(b)中用不同顏色箭頭表示各矢量軸。其中紅色構(gòu)件為動件1，繞矢量軸1旋轉(zhuǎn)，即第1自由度，其參數(shù)值為θ1；橙色構(gòu)件為動件2，繞矢量軸2旋轉(zhuǎn)，即第2自由度，其參數(shù)值為θ2；以此類推。藍(lán)色構(gòu)件為動件6，沿矢量軸6滑動，即第6自由度，其參數(shù)值為L6。

圖1 PUMA560機(jī)器人和斯坦福機(jī)器人構(gòu)型Fig.1 Architecture of PUMA560 and Stanford robot

圖2 腕部偏置伸縮Pieper機(jī)器人構(gòu)型和結(jié)構(gòu)Fig.2 Architecture and structure of wrist offset Pieper robot with prismatic link

腕部偏置伸縮型Pieper機(jī)器人矢量軸1、矢量軸2和矢量軸3相交于一點，符合Pieper條件；矢量軸4、矢量軸5和矢量軸6處于偏置交錯狀態(tài)，且第6自由度為伸縮動作，因此其運動學(xué)逆向求解不同于傳統(tǒng)結(jié)構(gòu)。

1.1 全局坐標(biāo)系

機(jī)器人初始位置全局坐標(biāo)系如圖3所示，其中a1為矢量軸2與矢量軸4之間距離，a2為矢量軸4與矢量軸5之間距離，d1為矢量軸3與矢量軸6之間距離，這3個值為固定結(jié)構(gòu)尺寸。a3為動件6相對于動件5未伸縮時動件6前端尖點與矢量軸5之間距離，該尺寸值根據(jù)機(jī)器人工作狀況可以調(diào)整，但調(diào)整好后在機(jī)器人作業(yè)過程中該值固定不變。

圖3 全局坐標(biāo)系和關(guān)鍵結(jié)構(gòu)參數(shù)Fig.3 Global coordinate system and the key structure parameters

各矢量軸由各自軸上空間兩點坐標(biāo)值確定，因此在起始位置有：矢量軸1由原點指向點(0，0，1)，矢量軸2由原點指向點(0，1，0)，矢量軸3由原點指向點(1，0，0)，矢量軸4由點(-a1，0，0)指向點(-a1，1，0)，矢量軸5由點(-a1-a2，0，0)指向點(-a1-a2，0，1)，矢量軸6由點(-a1-a2-a3，0，d1)指向點(-a1-a2，0，d1)。

1.2 基礎(chǔ)公式

在全局坐標(biāo)系中進(jìn)行點的空間變換求解有 2種：一是計算空間一點繞空間矢量軸旋轉(zhuǎn)給定角度后該點坐標(biāo)；二是計算空間一點沿空間矢量軸移動給定距離后該點坐標(biāo)。

假定空間點為I(x，y，z)，空間矢量軸由點J(x1，y1，z1)指向點K(x2，y2，z2)，進(jìn)行上述2種運算。令

點I沿矢量rJK移動距離l變?yōu)镮′(x′，y′，z′)，有

(1)

點I繞矢量rJK轉(zhuǎn)動θ變?yōu)镮″(x″，y″，z″)，有

(2)

式中，cθ為余弦cosθ；sθ為正弦sinθ。

2 全局坐標(biāo)系求解

2.1 正向求解

在執(zhí)行末端(動件6)選定3點A、B、C，以其空間坐標(biāo)值表述執(zhí)行端位姿。A點設(shè)定在尖端處，初始位置坐標(biāo)值為(-a1-a2-a3，0，d1)，B點坐標(biāo)為(-a1-a2-a3，0，d1+t)，C點坐標(biāo)為(-a1-a2-a3，t，d1)，其中t值任意給定，給定后其值固定。

正向求解是已知各關(guān)節(jié)角度值求解A、B、C三點最終坐標(biāo)。計算時先求第6自由度完成后各點坐標(biāo)值，然后計算第5自由度完成后各點坐標(biāo)值，以此類推即可計算出第4至第1自由度完成后各點坐標(biāo)值。現(xiàn)以點A為例，其計算公式如下：

(3)

2.2 逆向求解

2.2.1 迭代求解L6值和θ5值

直接通過式(3)進(jìn)行逆向求解是無法實現(xiàn)的，必須結(jié)合腕部偏置伸縮Pieper機(jī)器人空間構(gòu)型才能進(jìn)行求解。

圖4 逆向求解關(guān)鍵點Fig.4 Key points of inverse kinematics

如圖4所示，在執(zhí)行端(動件6)上再選定D、E、F三點。D點為矢量軸5與矢量軸6的交點，在初始位置時AD之間距離為a3；E點為矢量軸3與矢量軸5的交點，DE之間距離恒定為d1；F點為矢量軸3與矢量軸4的交點，EF之間距離恒定為a2；O點為坐標(biāo)原點，OF之間距離恒定為a1。

由圖4可知，F(xiàn)點為求解關(guān)鍵點，當(dāng)該點空間坐標(biāo)確定時，θ1、θ2、θ3即可求出，因此逆向求解思路第一步是求解該點空間坐標(biāo)值。

給定6個自由度參數(shù)值的機(jī)器人空間狀態(tài)如圖5所示。由圖5可知，由A、D、E三點確定的平面Ⅰ和由原點O和矢量軸5所確定的平面Ⅱ之間夾角(區(qū)分正負(fù))為第5自由度參數(shù)值θ5，因此求解該兩平面之間夾角轉(zhuǎn)化為求解線AD與平面Ⅱ法線之間夾角。

圖5 各動作完成后空間狀態(tài)Fig.5 Ultimate spatial state

如果將動件6沿矢量rAD后移合適的距離l使得DE與矢量軸5重合，此時將F點繞DE軸線旋轉(zhuǎn)-θ5使F點與矢量軸3、矢量軸4交點重合。動件6逐漸后移時采用前述方法計算所得的點F到O點之間距離的絕對值|OF|是先縮小后增大的，后移的目的是使F點與矢量軸3、矢量軸4交點重合，此時F點與O點之間距離為定值a1，因此可采用迭代方法計算出L6值和θ5值。

迭代求解L6值和θ5值流程如下：

A、D、E、F四點均位于執(zhí)行端上，在逆向求解時執(zhí)行端位姿是確定的，因此四點在全局坐標(biāo)系內(nèi)坐標(biāo)值也是確定的，分別為A0(A0x，A0y，A0z)，D0(D0x，D0y，D0z)，E0(E0x，E0y，E0z)，F(xiàn)0(F0x，F(xiàn)0y，F(xiàn)0z)。

各點沿矢量rA0D0移動l的變換矩陣為

(4)

動件后移完成各點坐標(biāo)如下：

由于直線DE為兩平面交線，因此將求平面Ⅰ和平面Ⅱ之間夾角轉(zhuǎn)化為求空間直線AD與平面Ⅱ法線的夾角，直線AD平行向量與平面Ⅱ法線向量分別為

(5)

平面Ⅰ與平面Ⅱ法線的夾角為

(6)

將點F繞矢量E1D1轉(zhuǎn)動-θⅠ-Ⅱ得

(7)

此時F點到O點的距離：

(8)

將上述計算流程整合為函數(shù)f_iteration，返回值為

f_iteration(l)=|OF2|-a1

通過迭代方法重復(fù)以上過程，lmin初始值為 -500，lmax初始值為0，設(shè)定判定條件為10-8。迭代流程如下：

fmin=f_iteration(lmin)

fmid=f_iteration(lmid)

fmax=f_iteration(lmax)

該循環(huán)完成后判斷|fmid|<10-8是否成立，如成立則循環(huán)結(jié)束，如不成立則重新對lmin和lmax賦值并進(jìn)行下一步循環(huán)。賦值判別條件為：如果本循環(huán)所得fminfmid<0，則將當(dāng)前的lmid值賦予下一步的lmax初始值；如果fmidfmax<0，則將當(dāng)前的lmid值賦予下一步的lmin初始值；當(dāng)|fmid|<10-8時計算完成，得到L6=lmid，θ5=θlmid。

2.2.2 遞次歸位方法求解θ1、θ2、θ3和θ4

第6自由度L6和第5自由度θ5求解完成后，先后將第6和第5自由度歸至0位，有

第6和第5自由度歸至0位后機(jī)器人空間狀態(tài)如圖6(a)所示，由其俯視圖可計算出θ1，有

(9)

將第1自由度歸至0位，有

第6、第5和第1自由度歸至0位后機(jī)器人空間狀態(tài)如圖6(b)所示，由其主視圖可計算出θ2，有

(10)

將第2自由度歸至0位，有

圖6 歸位過程中機(jī)器人空間狀態(tài)Fig.6 Spatial states of the robot in returning process

第6、第5、第1和第2自由度歸至0位后機(jī)器人空間狀態(tài)如圖6(c)所示，由其左視圖可計算出θ3，有

θ3=arctan 2(D4z，D4y)

(11)

將第3自由度歸至0位，有

第6、第5、第1、第2和第3自由度歸至0位機(jī)器人空間狀態(tài)如圖6(d)所示，由其主視圖可計算出θ4，有

(12)

3 驗證和應(yīng)用

3.1 正向驗證

各動作參數(shù)值設(shè)定為：θ1=5°、θ2=-10°、θ3=15°、θ4=-20°，由其左視圖可計算出θ5=25°、L6=30，由式(3)得

三維模型驅(qū)動結(jié)果如圖7所示，與計算結(jié)果一致。

圖7 正向結(jié)果驗證Fig.7 Test data for forward kinematics

3.2 逆向驗證

逆向驗證選用腕部偏置伸縮Pieper機(jī)器人在掘進(jìn)機(jī)截割頭組對焊接作業(yè)實例來說明。懸臂式掘進(jìn)機(jī)是煤礦井下巷道掘進(jìn)的核心關(guān)鍵裝備[21-23]，截割頭是掘進(jìn)機(jī)關(guān)鍵件之一。在截割頭自動化組對焊接時先將齒座調(diào)整到位，然后將齒座伸至截割頭體貼面，如圖8所示。

圖8 掘進(jìn)機(jī)截割頭組對焊接Fig.8 Application in welding of cutting head

圖8連桿尖點A(a，b，c)坐標(biāo)為(-507.406 3，89.807 9，69.440 2)，θx為20°，θy為-15°，θz為25°，位姿矩陣為

計算可得

(D0x，D0y，D0z)=(-367.338 1，155.122 8，110.851 2)

(E0x，E0y，E0z)=(-359.370 7，198.462 7，15.545 5)

(F0x，F(xiàn)0y，F(xiàn)0z)=(-284.959 5，233.161 2，37.545 2)

按照第2節(jié)所述方法編寫程序，用時8 ms迭代7次得到計算結(jié)果為L6=-51.556，θ5=62.246 2°，逐步歸位，由式(12)、式(15)得θ1=-35.595 4°，θ2=2.728 9°，θ3=22.539 2°，θ4=7.765 7°。

無論是在虛擬樣機(jī)模擬還是在實體產(chǎn)品生產(chǎn)中，以上計算結(jié)果都極為精準(zhǔn)。

3.3 產(chǎn)品應(yīng)用

中國礦業(yè)大學(xué)(北京)與石家莊煤礦機(jī)械有限責(zé)任公司合作研發(fā)的新構(gòu)型鑿巖機(jī)器人采用腕部偏置伸縮Pieper構(gòu)型，如圖9所示。

圖9 鑿巖機(jī)器人虛擬樣機(jī)和產(chǎn)品Fig.9 Virtual prototype and product of rock drilling robot

目前對鑿巖機(jī)器人進(jìn)行了地面廠內(nèi)試驗，如圖10所示。首先構(gòu)建模擬巷道及前端工作面，按照兗礦集團(tuán)東灘煤礦提供的86孔打眼數(shù)據(jù)在工作面上標(biāo)出各孔位置，將各孔數(shù)據(jù)輸入鑿巖機(jī)器人；然后，進(jìn)行地面試驗，機(jī)器人自行完成各孔定位。

圖10 鑿巖機(jī)器人地面試驗Fig.10 Drilling robot in ground test

地面試驗所得實際定位結(jié)果與理論定位結(jié)果偏移量如圖11所示。由圖11可知，最大偏移為27.6 mm。鑿巖機(jī)器人定位精準(zhǔn)快速，符合礦方±30 mm打眼誤差的要求。

圖11 加工和裝配造成的定位誤差Fig.11 Positioning error caused by machining and assembly

4 結(jié) 論

(1) 腕部偏置伸縮Pieper機(jī)器人采用偏置形式具有更強的結(jié)構(gòu)剛度和更高的靈活性，動件6的伸縮使其有更好的應(yīng)用性。

(2) 腕部偏置伸縮Pieper機(jī)器人存在3個兩兩相交矢量軸，可以實現(xiàn)逆向求解，由于腕部除了偏置還帶有伸縮動作，其求解需要采用迭代搜索的求解形式。

(3) 腕部偏置伸縮Pieper機(jī)器人結(jié)構(gòu)有2個特性：一是對第5自由度的求解可以轉(zhuǎn)化為對特定直線與特定平面法線之間夾角的求解；二是矢量軸3和矢量軸4繞軸交點與原點O的距離為定值。這2個特性表明可以對第5和第6自由度進(jìn)行迭代搜索求解。

(4) 數(shù)據(jù)結(jié)果顯示迭代搜索過程僅迭代7或8次，耗時10 ms，精度誤差可達(dá)10-8，求解準(zhǔn)確、快速、簡便。

(5) 基于全局坐標(biāo)系內(nèi)機(jī)器人空間狀態(tài)關(guān)系的算法耗時短、精度高，解決了傳統(tǒng)數(shù)值算法計算量大、耗時長且精度差的缺點，推動了腕部偏置伸縮Pieper構(gòu)型在截割頭組對焊接機(jī)器人及井下鑿巖機(jī)器人等多種設(shè)備上的應(yīng)用。地面試驗表明，打眼誤差在±30 mm內(nèi)，效果良好，可滿足精準(zhǔn)和快速要求。

致謝：本課題得到了石家莊煤礦機(jī)械有限責(zé)任公司的大力支持，在此深表感謝！