袁得春

(東北林業(yè)大學(xué)，哈爾濱，150040)

六自由度解耦機(jī)械臂的逆運(yùn)動(dòng)學(xué)通解

袁得春

(東北林業(yè)大學(xué)，哈爾濱，150040)

為求解六自由度解耦機(jī)械臂逆運(yùn)動(dòng)學(xué)通解，提出一種基于歐幾里得范數(shù)的位姿分解逆運(yùn)動(dòng)學(xué)求解方法。先推導(dǎo)前3個(gè)關(guān)節(jié)角的解析解，快速、準(zhǔn)確的獲取機(jī)械臂的腕心位置；再利用已知的執(zhí)行器姿態(tài)，計(jì)算后3個(gè)關(guān)節(jié)角，經(jīng)六自由度機(jī)械臂的正逆運(yùn)動(dòng)學(xué)仿真互驗(yàn)，驗(yàn)證了方法的有效性。

六自由度，解耦機(jī)械臂，逆運(yùn)動(dòng)學(xué)通解

六自由度解耦機(jī)械臂是工業(yè)中常見的機(jī)械臂。其前3個(gè)關(guān)節(jié)控制機(jī)械臂腕心的位置，后3個(gè)關(guān)節(jié)的軸線交于腕心，控制機(jī)械臂末端姿態(tài)。該構(gòu)型具有控制簡潔、定位精度高等優(yōu)點(diǎn)。正確建立機(jī)械臂正逆運(yùn)動(dòng)學(xué)模型是精確控制六自由度解耦機(jī)械臂的基礎(chǔ)，逆運(yùn)動(dòng)學(xué)求解析解是其中最重要的問題。

國內(nèi)外許多科研人員針對六自由度機(jī)械臂求逆解提出許多方法。Fu et al[1]78-82提出幾何法求解機(jī)械臂的逆解，該方法需要繪制各桿件間的關(guān)系，計(jì)算效率偏低，不方便利用計(jì)算機(jī)求機(jī)械臂逆解。Paul et al[2]提出解析法求解六自由度機(jī)械臂的解析解，但其解法適用于機(jī)械臂各桿件相互平行或垂直的特殊構(gòu)型機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，是一種理想情況下的求解，在實(shí)際工程中，由于存在安裝誤差，無法保證機(jī)械臂桿件間的平行或垂直關(guān)系。故該方法不適于實(shí)際機(jī)械臂求解。徐文福等[3]針對空間漂浮機(jī)械臂的笛卡爾空間連續(xù)路徑規(guī)劃問題，提出基于速度級逆運(yùn)動(dòng)學(xué)求解方法。該方法利用雅可比迭代求機(jī)械臂逆運(yùn)動(dòng)學(xué)解，計(jì)算復(fù)雜度較高，效率偏低，不適于工程中對于高速機(jī)械臂逆運(yùn)動(dòng)學(xué)求解的要求。MIT講義中給出了六自由度解耦機(jī)械臂逆解通解的結(jié)果，但并未給出證明過程。

本文提出一種六自由度解耦機(jī)械臂逆運(yùn)動(dòng)學(xué)通解的方法，它適用于任何非奇異構(gòu)型的解耦機(jī)械臂求解，其速度和效率可以得到保證。并通過某型號著陸器機(jī)械臂的運(yùn)動(dòng)學(xué)實(shí)驗(yàn)驗(yàn)證該方法的正確性。

1 機(jī)械臂運(yùn)動(dòng)學(xué)原理

1.1 機(jī)械臂DH系建模

本文研究六自由度解耦機(jī)械臂，采用關(guān)節(jié)型串聯(lián)構(gòu)型，機(jī)械臂末端安裝載荷(見圖1)。采用Denavit-Hartenburg(D-H)方法對該機(jī)械臂的結(jié)構(gòu)參數(shù)及各關(guān)節(jié)的運(yùn)動(dòng)學(xué)參數(shù)進(jìn)行描述，建立相鄰桿件固接坐標(biāo)系之間的齊次變換陣。機(jī)械臂D-H系如圖2所示，其中0系為底座系；機(jī)械臂D-H參數(shù)見表1。

表1 機(jī)械臂D-H參數(shù)

1.2 機(jī)械臂正運(yùn)動(dòng)學(xué)

機(jī)械臂正運(yùn)動(dòng)學(xué)，即給定機(jī)械臂6個(gè)關(guān)節(jié)角度的情況下，獲得機(jī)械臂任意桿件上1點(diǎn)在其他坐標(biāo)系下的位置。

圖1 機(jī)械臂結(jié)構(gòu)圖

圖2 機(jī)械臂D-H系

設(shè)λi=cosαi、μi=sinαi，可得D-H系的旋轉(zhuǎn)變換陣(Qi)。

(1)

與投影關(guān)系得齊次變換陣(ri)。

ri=[aiC(θi)aiS(θi)bi]T。

(2)

可得D-H系的齊次變換陣(Ti)。

(3)

應(yīng)用齊次變換陣的串接性質(zhì)，可得機(jī)械臂末端相對于機(jī)械臂底座的齊次變換陣(T)。

T=T1·T2·T3·T4·T5·T6。

(4)

已知機(jī)械臂執(zhí)行器l上任一點(diǎn)s位置rls，計(jì)算其在機(jī)械臂底座系下的位置。

(5)

1.3 機(jī)械臂逆運(yùn)動(dòng)學(xué)

1.3.1 由腕心位置求前3軸角度

工程中，機(jī)械臂末端的位置和姿態(tài)已知，故解耦機(jī)械臂逆運(yùn)動(dòng)學(xué)求解，需先根據(jù)機(jī)械臂末端姿態(tài)求出機(jī)械臂腕心位置，通過解析法求機(jī)械臂前3關(guān)節(jié)解，再求后3個(gè)關(guān)節(jié)的角度。

因坐標(biāo)系0與坐標(biāo)系1重合(見圖2)，故r0=0、Q0=1。c表示腕心位置矢量在底座系下的表示。其定位方程為式(6)。

(6)

式中：腕心位置c=[xc，yc，zc]T為已知量。

1.3.1.1求θ3

對式(6)兩端取歐幾里得范數(shù)，

(7)

式(7)右式范數(shù)表示為：

定位方程(7)左式范數(shù)為：

(8)

圖2所示的解耦機(jī)械臂，有a4=0，故有：

即：

2b2b3λ2+2b2b4λ2λ3+2b3b4λ3。

(9)

由式(8)及式(9)且左右范數(shù)相等得：

即：

2a1xcC(θ1)-2a1ycS(θ1)+(2a2a3-2b2b4μ2μ3)C(θ3)+

(10)

將式(10)表示為：

A·C(θ1)+B·S(θ1)+C·C(θ3)+D·S(θ3)+E=0。

(11)

因齊次變換陣第三行不含θ2，故有：

ycμ1C(θ1)-xcμ1S(θ1)-b4μ2μ3C(θ3)+a3μ2S(θ3)+

(12)

將式(12)表示為：

F·C(θ1)+G·S(θ1)+H·C(θ3)+I·S(θ3)+J=0。

(13)

其中衍生的結(jié)構(gòu)參數(shù)：F=ycμ1；G=-xcμ1；H=-b4μ2μ3；I=a3μ2；J=b2+b3λ2+b4λ2λ3-(zc-b1)λ1。

聯(lián)列式(11)、(13)得：

(14)

簡化為：

(15)

(16)

簡化為：

(17)

1.3.1.2求θ1

因C2(θ1)+S2(θ1)=1，故可消元簡化式(17)，得僅關(guān)于θ3的函數(shù)。

[-G(C·C(θ3)+D·S(θ3)+E)+B(H·C(θ3)+

I·S(θ3)+J)]2+[F(C·C(θ3)+D·S(θ3)+E)-

A(H·C(θ3)+I·S(θ3)+J)]2=(A·G-F·B)2。

簡化為：

[(B·H-G·C)C(θ3)+(B·I-G·D)S(θ3)+

(B·J-G·E)]2+[(F·C-A·H)C(θ3)+

(F·D-A·I)S(θ3)+(F·E-A·J)]2=

(A·G-F·B)2。

(18)

對式(18)進(jìn)一步處理得：

[(B·H-G·C)2+(F·C-A·H)2]C2(θ3)+[(B·I-G·

D)2+(F·D-A·I)2]S2(θ3)+2[(B·H-G·C)·

(B·I-G·D)+(F·C-A·H)(F·D-A·I)]·

C(θ3)·S(θ3)+2[(B·H-G·C)(B·J-G·E)+

(F·C-A·H)(F·E-A·J)]C(θ3)+2[(B·I-

G·D)(B·J-G·E)+(F·D-A·I)(F·E-A·

J)]S(θ3)+[(B·J-G·E)2+(F·E-A·J)2-

(A·G-F·B)2]=0。

(19)

將式(19)表示為：

K·C2(θ3)+L·S2(θ3)+M·C(θ3)·S(θ3)+

N·C(θ3)+P·S(θ3)+Q=0。

(20)

其中衍生的結(jié)構(gòu)參數(shù)：

K=(B·H-G·C)2+(F·C-A·H)2；

L=(B·I-G·D)2+(F·D-A·I)2；

M=2[(B·H-G·C)(B·I-G·D)+(F·C-A·H)(F·D-A·I)]；

N=2[(B·H-G·C)(B·J-G·E)+(F·C-A·H)(F·E-A·J)]；

P=2[(B·I-G·D)(B·J-G·E)+(F·D-A·I)(F·E-A·J)]；

Q=(B·J-G·E)2+(F·E-A·J)2-(A·G-F·B)2。

簡化為：

(21)

由式(21)得到關(guān)于τ3的4次方程：

(22)

其中τ3為式(22)的Gr?bner基[4]：

由Cardano及其助手給出的一元三次和四次方程的根式解[5]，可以求得θ3的4個(gè)可能解。

若R≠0得：

若R=0、S≠0，式(22)退化為3次多項(xiàng)式方程。

若R=0、S=0，式(22)退化為2次多項(xiàng)式方程。

再將θ3的解帶入(17)，得θ1解。

ifW+μ2·Z-yC·μ1=0∩xC·μ1>0∩W+μ2·Z+yC·μ1≠0;

ifW=0∩xC·μ1=0;

θ1=±π/2。

式中：W=b2+λ2(b3+b4λ3)-(zc-b1)λ1；Z=a3S(θ3)-b4μ3C(θ3)。

1.3.1.3求θ2

通過式(8)的前兩個(gè)標(biāo)量方程導(dǎo)出關(guān)于cosθ2、sinθ2的方程，得到θ2的解析解：

C(θ2)=(1/Δ22)[A11(xcC(θ1)+ycS(θ1)-a1)-A12(-xcλ1S(θ1)+ycλ1C(θ1)+(zc-b1)μ1)]；

S(θ2)=(1/Δ22)[A12(xcC(θ1)+ycS(θ1)-a1)+A11(-xcλ1S(θ1)+ycλ1C(θ1)+(zc-b1)μ1)]；

θ2=arctan(2(C(θ2),S(θ2)))。

式中：A11=a2+a3C(θ3)+b4μ3S(θ3)；A12=-a3λ2S(θ3)+b3μ2+b4μ3λ2C(θ3)+b4μ2λ3。

1.3.2 由末端姿態(tài)求后三關(guān)節(jié)角度

1.3.2.1求θ4

1.3.2.2求θ5

(23)

因式(23)第三列的第一與第二個(gè)分量獨(dú)立于θ6，可得：

θ5=arctan2(C(θ5)，S(θ5))。

1.3.2.3求θ6

θ6=arctan2(C(θ6),S(θ6))。

2 機(jī)械臂正逆運(yùn)動(dòng)學(xué)互驗(yàn)

機(jī)械臂正運(yùn)動(dòng)學(xué)與逆運(yùn)動(dòng)學(xué)是互逆的行為。表2和表3中的數(shù)據(jù)顯示：正逆運(yùn)動(dòng)學(xué)互驗(yàn)結(jié)果誤差范圍小于0.001°。

表2 腕心正逆運(yùn)動(dòng)學(xué)結(jié)果校驗(yàn)

表3 機(jī)械臂末端正逆運(yùn)動(dòng)學(xué)結(jié)果校驗(yàn)

3 結(jié)束語

森林作業(yè)機(jī)器人一直是國內(nèi)外機(jī)器人研究的熱點(diǎn)之一，本文提出一種基于歐幾里得范數(shù)的位姿分解逆運(yùn)動(dòng)學(xué)求解方法，用來解決六自由度解耦機(jī)械臂通解問題。首先，推導(dǎo)前3個(gè)關(guān)節(jié)角的解析解，快速、準(zhǔn)確的獲取機(jī)械臂腕心的位置。之后，通過采樣器的姿態(tài)，計(jì)算機(jī)械臂的后3個(gè)關(guān)節(jié)角。最后，通過自治機(jī)械臂仿真模型的正逆運(yùn)動(dòng)學(xué)互驗(yàn)結(jié)果證明該方法的正確性，為林業(yè)機(jī)器人的發(fā)展提供參考。

[1] FU K S， GONZALEZ R C， LEE C S G. Robotics control sensors vision and intelligence[M]. New York： McGrawHill，1987：78-82.

[2] PAUL R P， SHIMANO B， MAYER G E. Kinematic control equations for simple manipulators[J]. IEEE Transactions on Systems， Man， and Cybernetics，1981，11(6)：449-445.

[3] 徐文福，劉宇，強(qiáng)文義，等.自由漂浮空間機(jī)器人的笛卡爾連續(xù)路徑規(guī)劃[J].控制與決策，2008，23(3)：278-282，287.

[4] FATHI A， SHARIFAN N. A classic new method to solve quartic equations[J]. Applied and Computational Mathematics，2013，2(2)：24-27.

GeneralSolutionofInverseKinematicsofSixDegree-of-freedomDecouplingManipulator

Yuan Dechun

(Northeast Forestry University, Harbin 150040, P. R. China)Journal of Northeast Forestry University，2017,45(12)：88-92.

Six degree-of-freedom； Decoupling manipulator； General solution of inverse kinematics

袁得春，男，1985年4月生，東北林業(yè)大學(xué)機(jī)電工程學(xué)院，工程師，博士研究生。E-mail：8500284@qq.com。

2017年9月2日。

張 玉。

TH132.4；O311

In order to solve the general solution of inverse kinematics of the six degree-of-freedom (6DOF) decoupling manipulator, a decomposed position-orientation method based on Euclid Norm was presented. Firstly, the analytical solution of first three joint angles was deduced to calculate the wrist position of manipulator quickly and precisely. Secondly, the last three joint angles were solved by known executor’s orientation. By the inverse kinematic simulative simulation of the 6DOF manipulator, the effectiveness of method was verified.