王其軍，杜建軍

(哈爾濱工業(yè)大學(xué) 深圳研究生院，廣東 深圳518055，jjdu@hit.edu.cn)

機(jī)器人現(xiàn)有的逆解求解方法主要有解析法［1-3］、幾何法［4］、符號及數(shù)值方法［5］及近年出現(xiàn)的神經(jīng)網(wǎng)絡(luò)法［6-8］等.在機(jī)器人逆解過程中，一般在360°范圍內(nèi)計算機(jī)器人關(guān)節(jié)角度，但文獻(xiàn)［2-3］在解關(guān)節(jié)角時采用單變量反正切函數(shù)，一個反正切函數(shù)的解可能被丟失.機(jī)器人反解有多解，但控制機(jī)器人只能有一組解，所以必須選取一組解作為最后確定的解，文獻(xiàn)［2-3］中反解存在8 組解，沒有給出怎樣選取一組解作為最后反解的結(jié)果.

針對以上問題，本文在求關(guān)節(jié)變量時采用雙變量正切函數(shù)，這樣通過自變量的符號確定關(guān)節(jié)角度所在的象限，避免了解被丟失的可能性，針對反解多解，采用“最短行程”準(zhǔn)則選取一組最接近于當(dāng)前操作臂的解.

1 機(jī)器人運(yùn)動學(xué)模型的建立

機(jī)器人運(yùn)動學(xué)傳統(tǒng)的描述方法是基于D-H方法［9］，用D-H 法建立了各桿坐標(biāo)系如圖1 所示，第i-1 系和i 系間的關(guān)系可以用坐標(biāo)系的平移和旋轉(zhuǎn)來實(shí)現(xiàn).這種關(guān)系可由表示連桿i 對連桿i-1 相對位置的4 個齊次變換來描述，并叫做矩陣Ai.此關(guān)系式為

式中:θi為是關(guān)節(jié)i 的旋轉(zhuǎn)角;βi為是關(guān)節(jié)i 和i+1 間的扭轉(zhuǎn)角;ai為是關(guān)節(jié)i+1 的長度;di為是關(guān)節(jié)i 的偏距;i=0，1，2，…，6.

由六自由度機(jī)器人參考坐標(biāo)系圖，得到六自由度機(jī)器人的D-H 參數(shù)表如表1.

圖1 機(jī)器人D-H 表示法參考坐標(biāo)系

表1 D－H 參數(shù)

依照D-H 表示方法原理，可以得到坐標(biāo)系X0-Z0到坐標(biāo)系X1-Z1的變化矩陣為A1，同理相鄰坐標(biāo)系間的變化矩陣計為A2，A3，A4，A5，A6.這6 個坐標(biāo)變化矩陣的表達(dá)式如下:

式中:Si=sin θi;Ci=cos θi，則

式中:

這樣，由T 可以得到機(jī)器人末端的位姿(位置和姿態(tài)).

2 運(yùn)動學(xué)反解

逆運(yùn)動學(xué)問題是已知機(jī)器人末端執(zhí)行器的位姿，求相應(yīng)的關(guān)節(jié)變量.

將式(1)變形為

式(2)的左邊

式(2)右邊

式中:

1)求解θ1.由式(3)與式(4)中的矩陣元素第三行第四列元素相等得到

θ1=Atan2(-ayL6+py，-axL6+px)或

θ1=Atan2(ayL6-py，axL6-px).

2)求解θ2.依據(jù)兩邊矩陣(3)，(4)中的第一行第四列元素和第二行第四列元素分別相等得到等式:

令

把u，v 代入式(5)、(6)得到

令

將式(7)與與(8)兩邊平方然后相加化簡得到

3)求解θ3.式(7)與(8)是關(guān)于S3與C3的方程組.所以可以解得

4)求解θ5.依據(jù)兩邊矩陣(2)，(3)中的第一行第三列元素和第二行第三列元素分別相等得到等式:

假設(shè):

式(9)與(10)兩邊分別乘以S3與C3，然后相減，可以得到

5)求解θ4.式(9)與(10)兩邊分別乘以C3與S3，然后相加得到

6)求解θ6.依據(jù)兩邊矩陣(2)，(3)中的第三行第一列元素和第三行第二列元素分別相等，可以得到

3 運(yùn)動學(xué)反解不唯一性

從解的表達(dá)式中可以看到θ1，θ2，θ5分別有兩個根，所以機(jī)器人有8 組根.在實(shí)際運(yùn)行中，如何從機(jī)器人運(yùn)動學(xué)的多組反解中選擇一組合適的解是一個值得研究的問題.一般地，具有多組反解的機(jī)器人運(yùn)動學(xué)問題可以采用優(yōu)化算法來解決，定義一個優(yōu)化準(zhǔn)則，選擇滿足優(yōu)化準(zhǔn)則的最優(yōu)解作為機(jī)器人運(yùn)動學(xué)的確定反解.本文采用“最短行程”準(zhǔn)則選取一組最接近于當(dāng)前操作臂的解.算法流程圖如圖2 所示.

圖2 最短行程準(zhǔn)則流程圖

4 驗(yàn) 證

按照如圖3 所示的具體步驟，利用計算機(jī)程序進(jìn)行計算，可以驗(yàn)證運(yùn)動學(xué)解的正確性.

圖3 運(yùn)動學(xué)驗(yàn)證流程圖

已知輸入各關(guān)節(jié)角分別為-18.495 4，-37.618 3，-59.786 4，19.860 0，110.967 2，7.364 8，正解得到機(jī)器人位姿為387.217 0，-99.421 0，-104.717 0，180.000 0，-89.999 9，0.

逆解得到的關(guān)節(jié)角為-18.495 3，-37.618 3，-59.786 4，19.859 8，110.967 1，7.364 7.

計算結(jié)果表明，所得的解完全符合所賦初值.

5 結(jié) 論

2)在機(jī)器人逆解求關(guān)節(jié)角過程中，采用雙變量反正切函數(shù)，通過自變量的符號就可以確定關(guān)節(jié)角度所在的象限，避免了解被丟失的可能性.

3)給出了六自由度機(jī)器人對應(yīng)某一位姿的8個位行，采用“最短行程“準(zhǔn)則選取一組最接近于當(dāng)前操作臂的解.

4)本文提出的方法已經(jīng)在深圳市元創(chuàng)興科技有限公司自主研發(fā)的工業(yè)六自由度機(jī)器人中成功應(yīng)用.

［1］PAUL R P，SHIMANO B E，MAYER G.Kinematic control equations for simple manipulations［J］.IEEE Trans SMC，1981，11(6):449-455.

［2］王雪松，許世范，郝繼飛.MOTOMAN 機(jī)械手運(yùn)動方程新的推導(dǎo)方法及求解［J］.中國礦業(yè)大學(xué)學(xué)報，2001，30(1):73-76.

［3］王奇志，徐心和.PUMA 機(jī)械手逆運(yùn)動方程新的推導(dǎo)方法及求解.機(jī)器人.1998，20(2):81-87.

［4］FU K S，GONZALEZ R C，LEE C S G.Robotics Control Sensins Vision and Intelligence［M］.New York:McGrawHill，1987:78-82.

［5］MANOCHA D，CANNY J F.Efficient inverse kinematics for general 6R manipulators［J］.IEEE Transactions on Robotics and Automation，1994，10(5):648-657.

［6］KARLIK B，AYDIN S.An improved approach to the solution of inverse kinematics problems for robot manipulators［J］.Engineering Applications of Artificial Intelligence，2000(13):159-164.

［7］陳學(xué)生，陳在禮，謝濤.基于神經(jīng)網(wǎng)絡(luò)的機(jī)器人操作手IKP 精確求解［J］.機(jī)器人，2002，24(2):130-133.

［8］MAYROGA R V，SANONGBOOM.A radial basis function network approach for geometrically bounded manipulator inverse kinematics computation［J］//2003 IEEE/RSJ International Conference on Intelligent robots and system，2003，4(12):3564-3569.

［9］DENAVIT J，HARTENBERG R S.A kinematic notation for lower-pair mechanisms based on matrices［J］.ASM Journal of Applied Mechanics，1955，22(2):215-221.

［10］TAROKH M，KIM M.Inverse Kinematics of 7-DOF Robots and Limbs by Decomposition and Approximation［J］.IEEE Transaction on Robotics，2007，23(3):6-11.