李冠琦，武建德，李瑞琴

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

0 引言

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

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

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

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

1 旋量理論

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

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

M矩陣為當(dāng)所有的關(guān)節(jié)轉(zhuǎn)角都為0的時(shí)候，操作空間坐標(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ωθ。特殊的歐幾里得集合群或剛體運(yùn)動(dòng)群或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)

依次解鎖一個(gè)角度，往前代值，得到

T07=eS1θ1…eS7θ7M

(7)

2 數(shù)值法逆解

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

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

圖1 迭代法幾何過程

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

(8)

(9)

(10)

(11)

式中*為其伴隨矩陣。

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

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

3)引進(jìn)偽逆矩陣J?避免求解奇異時(shí)無解的情況。在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 驗(yàn)證正逆解

3.1 正運(yùn)動(dòng)學(xué)方程

(14)

(15)

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

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

(16)

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

(17)

使用先進(jìn)行矩陣分塊然后利用特性化簡的運(yùn)算方法，該結(jié)果更準(zhǔn)確、后期的運(yùn)算量更小。代入式(7)，使用該方法分別在Matlab和Python中運(yùn)算，得出的結(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 逆運(yùn)動(dòng)學(xué)方程

1)第一次驗(yàn)證

設(shè)初始位置為正解的0位置，轉(zhuǎn)動(dòng)角度為θ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)第二次驗(yàn)證

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

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

4 結(jié)語

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