袁得春

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

六自由度解耦機械臂的逆運動學(xué)通解

袁得春

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

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

六自由度，解耦機械臂，逆運動學(xué)通解

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

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

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

1 機械臂運動學(xué)原理

1.1 機械臂DH系建模

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

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

1.2 機械臂正運動學(xué)

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

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

圖2 機械臂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ì)，可得機械臂末端相對于機械臂底座的齊次變換陣(T)。

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

(4)

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

(5)

1.3 機械臂逆運動學(xué)

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

工程中，機械臂末端的位置和姿態(tài)已知，故解耦機械臂逆運動學(xué)求解，需先根據(jù)機械臂末端姿態(tài)求出機械臂腕心位置，通過解析法求機械臂前3關(guān)節(jié)解，再求后3個關(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所示的解耦機械臂，有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)進一步處理得：

[(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個可能解。

若R≠0得：

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

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

再將θ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)的前兩個標(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)第三列的第一與第二個分量獨立于θ6，可得：

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

1.3.2.3求θ6

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

2 機械臂正逆運動學(xué)互驗

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

表2 腕心正逆運動學(xué)結(jié)果校驗

表3 機械臂末端正逆運動學(xué)結(jié)果校驗

3 結(jié)束語

森林作業(yè)機器人一直是國內(nèi)外機器人研究的熱點之一，本文提出一種基于歐幾里得范數(shù)的位姿分解逆運動學(xué)求解方法，用來解決六自由度解耦機械臂通解問題。首先，推導(dǎo)前3個關(guān)節(jié)角的解析解，快速、準(zhǔn)確的獲取機械臂腕心的位置。之后，通過采樣器的姿態(tài)，計算機械臂的后3個關(guān)節(jié)角。最后，通過自治機械臂仿真模型的正逆運動學(xué)互驗結(jié)果證明該方法的正確性，為林業(yè)機器人的發(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] 徐文福，劉宇，強文義，等.自由漂浮空間機器人的笛卡爾連續(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é)機電工程學(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.