蔣坤, 劉承君, 戴義祥, 張峰

(陸軍軍官學(xué)院　a.裝甲兵系;　b.訓(xùn)練部, 安徽 合肥　230031)

?

一般耦合6R機(jī)器人實(shí)時(shí)高精度逆運(yùn)動(dòng)學(xué)算法及應(yīng)用

蔣坤a, 劉承君a, 戴義祥b, 張峰a

(陸軍軍官學(xué)院a.裝甲兵系;b.訓(xùn)練部, 安徽 合肥230031)

對(duì)于一般6R機(jī)器人，包括耦合和解耦機(jī)械臂，提出一種通用的與常見(jiàn)工業(yè)機(jī)器人有區(qū)別的能夠?qū)崿F(xiàn)實(shí)時(shí)高精度控制的機(jī)械臂逆運(yùn)動(dòng)學(xué)算法，并且應(yīng)用在特種多關(guān)節(jié)機(jī)械臂上?；诰仃嚩囗?xiàng)式理論及其性質(zhì)，從12個(gè)等式方程中重新提取6個(gè)獨(dú)立的等式方程。對(duì)多變量等式方程組進(jìn)行消元后轉(zhuǎn)換成單變量方程，將問(wèn)題轉(zhuǎn)換為求解單變量方程根的問(wèn)題，最后利用矩陣多項(xiàng)式的性質(zhì)將多項(xiàng)式的求根問(wèn)題轉(zhuǎn)化成為矩陣的特征值求解問(wèn)題，由此大大降低了求解難度，并且保證了算法的穩(wěn)定性和精度問(wèn)題。求解實(shí)例表明，提出的該算法能在毫秒級(jí)別內(nèi)得到某種耦合機(jī)械臂的所有運(yùn)動(dòng)學(xué)逆解，并且得到的解能夠精確到小數(shù)點(diǎn)后11位，從而實(shí)現(xiàn)實(shí)時(shí)高精度控制。

耦合機(jī)械臂；矩陣多項(xiàng)式；逆運(yùn)動(dòng)學(xué)；實(shí)時(shí)；高精度

0　引　言

自上世紀(jì)70年代開(kāi)始機(jī)器人領(lǐng)域的相關(guān)專家和學(xué)者一直在密切關(guān)注和研究一般6R機(jī)器人的逆運(yùn)動(dòng)學(xué)算法問(wèn)題，研究逆解算法在機(jī)器人學(xué)研究領(lǐng)域中的意義非常重大。Pieper[1]在1968年最早嘗試了將6自由度機(jī)械臂逆運(yùn)動(dòng)學(xué)求解問(wèn)題轉(zhuǎn)換為單變量多項(xiàng)式問(wèn)題，提出了解耦機(jī)械臂能夠得到封閉解。之后Duffy和Derby等[1]相繼嘗試從6自由多機(jī)械臂方程組中提取最簡(jiǎn)化的特征多項(xiàng)式方程，但是只能達(dá)到32次單變量多項(xiàng)式，并且猜想到次數(shù)仍可降低。1985年Tsai和Morgan[2]使用多項(xiàng)式連續(xù)方法在數(shù)值上對(duì)非線性方程進(jìn)行替換，得到了二次多項(xiàng)式方程組的形式。上述研究者也都實(shí)踐證明了機(jī)械臂最多可以得到16組實(shí)數(shù)逆解問(wèn)題。在1988年Lee和Liang[1]提出將相同的問(wèn)題轉(zhuǎn)換成16次單變量多項(xiàng)式問(wèn)題。1990年和1993年Li以及Raghavan和Roth[3]相繼提出了不同的單變量多項(xiàng)式系數(shù)計(jì)算的過(guò)程。1990年Manocha[3]提出了多項(xiàng)式方程的求解方法，將單變量方程組根的求解問(wèn)題轉(zhuǎn)換成求解矩陣特征值的問(wèn)題。至此逆運(yùn)動(dòng)學(xué)問(wèn)題雖然可以認(rèn)為基本解決，但是關(guān)于如何更加穩(wěn)定和快速地求解出其所有逆解這一問(wèn)題至今一直在研究中。

本文中采用以符號(hào)推演和數(shù)值計(jì)算方法為基礎(chǔ)的逆解算法，從多項(xiàng)式方程組中消除相關(guān)變量，將非線性方程組進(jìn)行線性組合，將多項(xiàng)式方程等式以矩陣的形式表達(dá)出來(lái)，最后將求解多項(xiàng)式方程根問(wèn)題轉(zhuǎn)化成矩陣特征值問(wèn)題，并且利用特征向量和方程組其他未知數(shù)之間的對(duì)應(yīng)關(guān)系求解出其它未知數(shù)。為了更加方便該算法的應(yīng)用，依托于MATLAB平臺(tái)將求解代碼程序封裝在GUI(圖形交互界面)，輸入已知量，直觀快速的解出未知量。

1　一般6R機(jī)械臂運(yùn)動(dòng)學(xué)方程的建立

基于D-H模型，一般6R機(jī)器人運(yùn)動(dòng)學(xué)方程[4]可以改寫(xiě)成如下形式：

(1)

式中Ai(i=1～6)表示一個(gè)連桿相對(duì)與下一個(gè)連桿間相對(duì)關(guān)系的齊次變換矩陣，T6表示機(jī)器人末端執(zhí)行器相對(duì)于基坐標(biāo)系之間的相對(duì)關(guān)系，Ai和T6可分別表示如下：

(2)

ci=cosθi,si=sinθi,μi=sinαi,λi=cosαi，其中θi，di，ai，αi分別表示關(guān)節(jié)兩連桿法線夾角，兩連桿相對(duì)位置、公共法線距離和扭角參數(shù)。di，ai，αi表示機(jī)械臂的關(guān)節(jié)參數(shù)，一般6R機(jī)器人運(yùn)動(dòng)學(xué)逆解問(wèn)題就是在已知di，ai，αi和T6的條件下求解關(guān)節(jié)變量θi。

由于Dinesh Manocha和John F. Canny[3]構(gòu)建等式方程的過(guò)程復(fù)雜，且計(jì)算方式繁瑣，本文基于位姿矩陣的特征，通過(guò)將位姿矩陣分解為位置和姿態(tài)分矩陣來(lái)重新構(gòu)建方程中的所需的矢量U,V,W和Y：

(3)

(4)

W=X2Z3X3Q4Q5e

(5)

(6)

式中:

且為正交陣。

由此可以先得到如下兩個(gè)等式:

U=V

(7)

W=Y

(8)

根據(jù)空間向量的投影性質(zhì)和幾何意義，尋找和上述兩個(gè)矢量式相同的冪乘積(單變量消去法的核心思路)，采用符號(hào)運(yùn)算可以得到以下14個(gè)運(yùn)動(dòng)學(xué)方程。

Ux(θ3,θ4,θ5)=Vx(θ1,θ2)

(9a)

Uy(θ3,θ4,θ5)=Vy(θ1,θ2)

(9b)

Uz(θ3,θ4,θ5)=Vz(θ1,θ2)

(9c)

Wx(θ3,θ4,θ5)=Yx(θ1,θ2)

(9d)

Wy(θ3,θ4,θ5)=Yy(θ1,θ2)

(9e)

Wz(θ3,θ4,θ5)=Yz(θ1,θ2)

(9f)

U·U(θ3,θ4,θ5)=V·V(θ1,θ2)

(9g)

U·W(θ3,θ4,θ5)=V·Y(θ1,θ2)

(9h)

(U×W)x(θ3,θ4,θ5)=(V×Y)x(θ1,θ2)

(9i)

(U×W)y(θ3,θ4,θ5)=(V×Y)y(θ1,θ2)

(9j)

(U×W)z(θ3,θ4,θ5)=(V×Y)z(θ1,θ2)

(9k)

[(U·U)W-2(U·W)U]x(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]x(θ1,θ2)

(9l)

[(U·U)W-2(U·W)U]y(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]y(θ1,θ2)

(9m)

[(U·U)W-2(U·W)U]z(θ3,θ4,θ5)=

[(V·V)Y-2(V·Y)V]z(θ1,θ2)

(9n)

式中下標(biāo)x,y和z分別表示矢量等式的第一部分、第二部分以及第三部分。為了消去變量，首先應(yīng)進(jìn)行變量分離，將等式左邊和右邊分別提取冪乘積x45、x12，可以得到：

Px45=Qx12

(10)

其中x45=[s4s5s4c5c4s5c4c5s4c4s5c51]T，

x12=[s1s2s1c2c1s2c1c2s1c1s2c2]T。

式中P為元素是θ3線性組合的14×9矩陣，而Q則為14×8常系數(shù)矩陣。消元θ1,θ2：

從Q矩陣較為特殊的結(jié)構(gòu)可知，式(10)中的6個(gè)等式右邊只含有S1和C1，將其中任意兩個(gè)等式帶入到其余四個(gè)等式中，如下：

C1x1=n1該式表示上述任意兩個(gè)等式，其中C1為2×2矩陣，且要求可逆。

C2x1=n2表示余下的四個(gè)等式，x1=[cosθ1sinθ1]T。

通過(guò)帶入消元法消去S1，C1，S2和C2，即可得到只與θ3,θ4,θ5有關(guān)的4個(gè)等式，寫(xiě)成矩陣形式：

(11)

式中T1表示元素為與θ3有關(guān)的4×9矩陣。P2為與上式(10)對(duì)應(yīng)的P矩陣的2×9分塊矩陣，同樣P4為對(duì)應(yīng)的4×9矩陣，01為2×1零矩陣。

除此之外，對(duì)上述剩余的8個(gè)等式進(jìn)行線性變換得到兩個(gè)等式右邊恒為0的等式，通過(guò)線性組合消元法將θ1,θ2消去，在Raghavan和Roth[2]構(gòu)建等式的基礎(chǔ)上進(jìn)一步進(jìn)行簡(jiǎn)化修改和驗(yàn)證，重新構(gòu)造了只和θ3,θ4,θ5有關(guān)的兩個(gè)等式，即：

-λi(U×W)x+ωUy-(r-d1)Wy=0

(12a)

-λi(U×W)y+ωUx-(r-d1)Wx=0

(12b)

其中ω=ozμ6+azλ6，r=-L6az+pz。至此總共得到6個(gè)只和變量θ3,θ4,θ5有關(guān)的等式方程，列成矩陣形式，得:

T2x45=O2

(13)

式中T2表示元素為與C3或S3線性相關(guān)有關(guān)多項(xiàng)式的矩陣，O2為6×1零矩陣。

參數(shù)代換：

T3(x3)x45=02

(14)

15)

T(x3)x″45(x4,x5)=03

(16)

(17)

式中O63為6×3階0矩陣，同時(shí)矩陣T(x3)可表示為:

(18)

A、B、C皆為12×12階常系數(shù)矩陣。

2　高精度求解

2.1符號(hào)推演和數(shù)值計(jì)算

一般6R機(jī)器人方程組求解過(guò)程運(yùn)算量非常之大，且數(shù)值計(jì)算大多是借助機(jī)器浮點(diǎn)運(yùn)算完成的，由于存在浮點(diǎn)計(jì)算的累計(jì)誤差問(wèn)題，浮點(diǎn)計(jì)算誤差不僅僅是位數(shù)的問(wèn)題，且在方程組求解過(guò)程中往往存在矩陣條件數(shù)大小問(wèn)題，累計(jì)誤差可能會(huì)直接誤導(dǎo)判斷的正確性。利用MATLAB軟件對(duì)上述方程組或者矩陣進(jìn)行符號(hào)預(yù)處理，避免了浮點(diǎn)舍入誤差的問(wèn)題。符號(hào)運(yùn)算預(yù)處理只需執(zhí)行一次，每次計(jì)算時(shí)只需要將數(shù)值帶入符號(hào)運(yùn)算結(jié)果中即可。

2.2矩陣特征值求根

根據(jù)線性方程組解性質(zhì)，可知若式子(16)有解，則有rank(T(x3))<12，即：

det(T(x3))=0

(19)

式子(19)是關(guān)于x3的一元16次方程，就單解變量x3的運(yùn)算量就相當(dāng)大，計(jì)算出x3之后，還需帶入線性方程組求解其他變量值，首先浮點(diǎn)計(jì)算的累計(jì)誤差會(huì)很大程度上影響解的精度，其次很容易出現(xiàn)增根、漏解的情況，更重要的是該計(jì)算方式只能在線下求解，且計(jì)算時(shí)間較長(zhǎng)，總言之無(wú)法實(shí)現(xiàn)實(shí)時(shí)高精度。與Raghavan和Roth等[2]采用的較為復(fù)雜繁瑣的計(jì)算方法不同，利用矩陣特征值求解的方法能較好的解決上述問(wèn)題。

若A矩陣可逆，將式(18)兩邊同時(shí)左乘A-1，得：

(20)

其中E為單位陣，根據(jù)矩陣多項(xiàng)式理論可知式(1.19)的根對(duì)應(yīng)如下矩陣M的特征值：

該矩陣的特征值x3所對(duì)應(yīng)的特征向量即為x″45(x4,x5)，即可求解出x4和x5，將求解出來(lái)的三個(gè)未知數(shù)帶入式(1)求解出x1和x2，最終再求解出x6。

然而在很多情況下，A矩陣的條件數(shù)是非常大的，即A矩陣接近不可逆，如此就無(wú)法轉(zhuǎn)換為可以求解的莫尼矩陣多項(xiàng)式，因此利用簡(jiǎn)單的元素線性代換法[5]，將A矩陣調(diào)整為可逆。將式(18)中的x3線性替換成：

(21)

2.3提高求解精度

每個(gè)特征值對(duì)應(yīng)的矩陣條件數(shù)的大小各不相同，即求解精度也各不相同。如果需要進(jìn)一步提高求解精度，將最初始計(jì)算出來(lái)的數(shù)值利用牛頓迭代法進(jìn)行一到兩次迭代可以進(jìn)一步提高求解精度(前提是要形成局部收斂情況)。

3　算法應(yīng)用于某種型號(hào)耦合機(jī)械臂

依托于MATLAB數(shù)學(xué)計(jì)算軟件，選擇具有代表性的6個(gè)旋轉(zhuǎn)自由度關(guān)節(jié)的某種內(nèi)窺機(jī)械臂作為求解實(shí)例，驗(yàn)證算法的可行性和有效性。關(guān)節(jié)參數(shù)如表1與圖1所示。

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

表1中θ為兩連桿法線夾角，d為兩連桿相對(duì)位置，a為公共法線距離，α為扭角參數(shù)。

圖1　運(yùn)動(dòng)學(xué)研究軟件界面

將機(jī)械臂模型導(dǎo)入至托卡馬克真空室內(nèi)部，將每個(gè)關(guān)節(jié)旋轉(zhuǎn)至一定的角度(例θ1=60°，θ2=15°，θ3=20°，θ4=25°，θ5=25°，θ6=20°θ1=60°，θ2=15°，θ3=20°，θ4=25°，θ5=25°，θ6=20°)，測(cè)量機(jī)械臂末端執(zhí)行器末端位姿矩陣(假設(shè)為目標(biāo)點(diǎn)位姿)，可表示為:

(22)

在MATLAB軟件中以符號(hào)運(yùn)算為基礎(chǔ)進(jìn)行運(yùn)算，之后帶入12位精度的各關(guān)節(jié)參數(shù)和目標(biāo)點(diǎn)的位姿，最終求解出各關(guān)節(jié)轉(zhuǎn)角。

表2　求解結(jié)果

實(shí)驗(yàn)數(shù)據(jù)表明，該算法解出的第三組角度值相對(duì)理論值能夠精確到小數(shù)點(diǎn)后11位，求解全過(guò)程充分驗(yàn)證了算法的穩(wěn)定性、實(shí)時(shí)性和精度，能夠完全滿足一般6R機(jī)器人的實(shí)時(shí)高精度控制的需要，滿足了對(duì)某些必須使用耦合機(jī)械臂上的應(yīng)用。

4　結(jié)束語(yǔ)

(1) 根據(jù)矩陣多項(xiàng)式理論和矩陣性質(zhì)構(gòu)建6個(gè)相互獨(dú)立的等式方程，簡(jiǎn)化了計(jì)算過(guò)程的復(fù)雜程度。

(2) 通過(guò)符號(hào)運(yùn)算避免了一般浮點(diǎn)運(yùn)算帶來(lái)的累計(jì)誤差從

而提高算法的精度，并且通過(guò)轉(zhuǎn)換成求解矩陣特征值的途徑提高了算法的穩(wěn)定性和效率。

(3) 通過(guò)MATLAB自帶的GUI界面，將算法過(guò)程轉(zhuǎn)換成程序語(yǔ)言并將其封裝在其內(nèi)部，將已知量輸入之后可以在毫秒級(jí)別快速得到高精度解，將一般6R機(jī)器人逆運(yùn)動(dòng)學(xué)算法成功應(yīng)用于AIA多關(guān)節(jié)機(jī)械臂，滿足了實(shí)時(shí)高精度控制的需要。

[1] JORGE ANGELES.Fundamentals of robotic mechanical system. theory, methods and algorithms, second edition[M].New York:Springer,2003.

[2] MADHUSUDAN RAGHAVAN, BERNARD ROTH.Kinematic analysis of the 6R manipulator of general geometry[J].Journal of Mechanical Design,1990,115(3):502-508.

[3] DINESH MANOCHA,JOHN F CANNY.Efficient inverse kinematics for general 6R manipulators[J]. IEEE Transactions on Robotics and Automation, 1994,5(10):648-657.

[4] 蔡自興.機(jī)器人學(xué)[M].北京：清華大學(xué)出版社，2000.

[5] 劉松國(guó),朱世強(qiáng),王宣銀. 基于矩陣分解的一般6R機(jī)器人實(shí)時(shí)高精度逆運(yùn)動(dòng)學(xué)算法[J].機(jī)械工程學(xué)報(bào)，2008，44(11):304-309.

Algorithm and Application of Real-time Highly-accurate Inverse Dynamics of General Coupling 6R Robots

JIANG Kuna, LIU Cheng-juna, DAI Yi-xiangb, ZHANG Fenga

(a. Department of Armored Reconnaissance, b. Training Division, Academy of Army Officer, Hefei Anhui 230031, China)

For general 6R robots including coupling and decoupling robot arms, this paper presents an inverse dynamics algorithm of the robot arm with real-time and highly accurate control which differs from common industrial robots, and applies it to special robot arms with multiple joints. Based on the theory and property of matrix polynomials, 6 independent equations are drawn from 12 equations. Then, multi-variable equation groups are transferred into a single-variable equation through element elimination, so that we are now faced with the problem of finding the root of the single-variable equation. Finally, using the properties of the matrix polynomial, we transform the problem of extracting the root of a polynomial to finding the proper value of a matrix, thus making the solution much easier and ensuring the stability and accuracy of the algorithm. Solution examples indicate that the proposed algorithm can obtain all inverse kinematics of a specific coupling robot arm on the ms level, and the solution can be correct up to 11 decimal places, thus realizing real-time and highly accurate control.

coupling robot arm;matrix polynomial;inverse kinematics; real time;high accuracy

10.3969/j.issn.1000-3886.2016.02.031

TP242

A

1000-3886(2016)02-0102-04

蔣坤(1981-)，男，安徽渦陽(yáng)人，講師，碩士，電氣工程專業(yè)方向。

定稿日期: 2015-09-16