基于旋量理論的7自由度機(jī)械臂運動學(xué)建模與分析

李冠琦，武建德，李瑞琴

(中北大學(xué) 機(jī)械工程學(xué)院，山西 太原 030051)

0 引言

機(jī)械臂是機(jī)器人技術(shù)領(lǐng)域中應(yīng)用最為廣泛的自動化裝置，在工業(yè)制造、醫(yī)療康復(fù)等領(lǐng)域都有其應(yīng)用實例，擬人化機(jī)械臂技術(shù)也逐漸成熟[1-4]。匹茲堡大學(xué)生物醫(yī)學(xué)團(tuán)隊在BCI機(jī)械臂假肢里引入實時觸覺反饋，使完成任務(wù)的時間減少一半。波士頓動力開發(fā)Stretch移動式7R機(jī)械臂已應(yīng)用于倉庫運輸。

20世紀(jì)80年代學(xué)者們開始討論將旋轉(zhuǎn)軸從數(shù)學(xué)中引入機(jī)器人研究的可行性，旋量理論[5-9]日益成熟，然而對其的應(yīng)用介紹卻鮮見報道。本文建模7自由度串聯(lián)機(jī)械臂，著重對螺旋軸這一單位矢量在運動學(xué)的應(yīng)用加以詳細(xì)描述。

6自由度逆解有通用公式，7R逆解包含32個實根，出現(xiàn)虛空間或自運動歧。7R逆解可結(jié)合Newton-Raphson數(shù)值迭代法求解[10]。

使用Matlab進(jìn)行正解中的化簡與直接展開等運算過程的比較，并且同時使用開源Python對結(jié)果進(jìn)行比較。使用基于空間(space簡稱s)坐標(biāo)系的空間雅可比矩陣，找出數(shù)值迭代法求逆解的適用范圍，基于正解的前提下，驗證逆解的準(zhǔn)確性。

1 旋量理論

螺旋(旋量——旋轉(zhuǎn)向量)理論在正運動學(xué)中的應(yīng)用。

1.1 M矩陣(0位置、起始位置)

M矩陣為當(dāng)所有的關(guān)節(jié)轉(zhuǎn)角都為0的時候，操作空間坐標(biāo)系(body簡稱b)在s坐標(biāo)系中的位置和姿態(tài)矩陣。

1.2 旋轉(zhuǎn)向量與指數(shù)積公式

(1)

(2)

T07=eS7θ7M

(3)

特殊正交群是所有有效的3×3旋轉(zhuǎn)矩陣的集合群SO(3)：包括R、ω、eωθ。特殊的歐幾里得集合群或剛體運動群或R3中的同質(zhì)變換矩陣Se(3)表示位姿：包括M、eSθ、T07。

(4)

式(4)中，可利用特性ω3=-ω來化簡，且eωθ有Rodrigues公式：

f(θ)=Iθ+(1-cosθ)ω+(θ-sinθ)ω2

(5)

eωθ=I+sinθω+(1-cosθ)ω2

(6)

依次解鎖一個角度，往前代值，得到

T07=eS1θ1…eS7θ7M

(7)

2 數(shù)值法逆解

使用非線性尋根的Newton-Raphson方法，有尋根、不存在根時尋找近似解、存在多個解時尋找最優(yōu)解的優(yōu)勢。給定一個初始值，然后代入迭代式求解直到出現(xiàn)誤差范圍內(nèi)的解。

設(shè)正向運動學(xué)函數(shù)為f(θd)，末端執(zhí)行器的位置向量為ν，非線性尋根Newton-Raphson是找到目標(biāo)函數(shù)的解。幾何視角如圖1所示；式(8)是解析視角。式(9)-式(11)是計算雅可比矩陣。

圖1 迭代法幾何過程

Δθ=J-1(θ0)[vd-f(θi)]

(8)

(9)

(10)

(11)

式中*為其伴隨矩陣。

對這種算法的進(jìn)一步改進(jìn)：

1)末端執(zhí)行器的位置描述f(θd)變更為正運動學(xué)計算出的矩陣T07。

3)引進(jìn)偽逆矩陣J?避免求解奇異時無解的情況。在Matlab里編程為pinv(J)。

當(dāng)前基于b坐標(biāo)系，變換為基于s坐標(biāo)系：

(12)

(13)

式中：Tsb是坐標(biāo)轉(zhuǎn)移矩陣；*為求其伴隨矩陣。

改進(jìn)算法的流程圖如圖2所示。

圖2 改進(jìn)的數(shù)值迭代法流程圖

3 驗證正逆解

3.1 正運動學(xué)方程

(14)

(15)

圖3 三維模型圖及7R示意圖

根據(jù)式(3)，使用矩陣分塊后化簡的運算過程，編程并計算結(jié)果。

(16)

根據(jù)式(3)，發(fā)現(xiàn)將矩陣指數(shù)直接泰勒展開(采用了3種計算方法：Pada法、特征值法、6次的泰勒展開)得出的結(jié)果雖然一致，但是它是虛數(shù)形式，增加了計算量。結(jié)果：

(17)

使用先進(jìn)行矩陣分塊然后利用特性化簡的運算方法，該結(jié)果更準(zhǔn)確、后期的運算量更小。代入式(7)，使用該方法分別在Matlab和Python中運算，得出的結(jié)果一致：

r1=150s1234-5-1 350s12347-300s123-675s123457-6-

675s123467-5-150s12345-1 350s1234-7+600s1234+300s12+

675s12345-6-7+675s12346-5-7-300s1+750s12345-6-750s12346-5

r2=150c1234-5+1 350c12347+300c123+675c123457-6+

675c123467-5-150c1234-5+1 350c1234-7-600c1234-300c12-

675c12345-6-7-675c12346-5-7+300c1-750c12345-6+750c12346-5

r3=2 700s7c5s6-s5c6-1 500s5s6-c5(1 500c6-1 500)-

1 200c5+1 200

3.2 逆運動學(xué)方程

1)第一次驗證

設(shè)初始位置為正解的0位置，轉(zhuǎn)動角度為θlist。

[3.142;1.571;1.047;0.785;0.628;0.524;0.449]

給逆解的初始值[3,1.5,1,0.6,0.5,0.4,0.3]。解得[3.161,1.563,1.059,0.762,0.628,0.523,0.449]。

2)第二次驗證

因發(fā)現(xiàn)誤差較大：1)改用角度制；2)迭代次數(shù)增加到1 000次；3)精度調(diào)整為eω<0.000 1，ev<0.000 1。經(jīng)過多組數(shù)據(jù)實驗發(fā)現(xiàn)，相差3°以內(nèi)，位置完全重現(xiàn)；相差10°以內(nèi)，第一角度有0.1°的偏差；相差15°以上的逆運算，第一個角度有1°以上的偏差。所以，該方法求逆解有完全重現(xiàn)的適用范圍，需要把初始解猜測在真實解的附近15°以內(nèi)(表1)。

表1 正逆解互相驗證 單位：(°)

4 結(jié)語

本文重點介紹了如何使用形如螺旋樓梯的轉(zhuǎn)向量計算正逆解。結(jié)合旋量與Newton-Raphson數(shù)值法求逆解。為涵蓋轉(zhuǎn)動機(jī)構(gòu)可能出現(xiàn)的問題，采用目前串聯(lián)機(jī)構(gòu)中最復(fù)雜的7R機(jī)構(gòu)并且使機(jī)構(gòu)盡可能復(fù)雜，但是設(shè)計中沒有涵蓋helical螺旋和cylindrical圓筒等機(jī)構(gòu)。在比較了不同的運算方法后，計算正解，得出先將矩陣分塊、再利用特性化簡的方法更好的結(jié)論。在計算逆解的過程中，發(fā)現(xiàn)當(dāng)初始猜測的第一個角度超過真實解20°，Newton-Raphson數(shù)值法不會重現(xiàn)正解。驗證結(jié)果表明旋量形式美觀，建?？旖?，編程明了，適用于機(jī)械臂中的串聯(lián)機(jī)構(gòu)、并聯(lián)機(jī)構(gòu)、轉(zhuǎn)動關(guān)節(jié)、平動關(guān)節(jié)、螺旋關(guān)節(jié)等的運動學(xué)、動力學(xué)特性建模和分析。