基于旋量和自運動的七自由度機械臂逆解算法

吳 亮，李憲華，2

(1.安徽理工大學機械工程學院 安徽 淮南 232001； 2.安徽理工大學人工智能學院 安徽 淮南 232001)

0 引言

傳統(tǒng)串聯(lián)機械臂通常采用D-H參數(shù)法[1]和旋量法[2]來建立機器人運動學模型。D-H參數(shù)法需要在每個連桿處建立坐標系，然后，得出每一個連桿坐標系相對于前一連桿坐標系的位姿轉(zhuǎn)換矩陣，即得到機械臂運動學方程。D-H參數(shù)法相對于旋量法更加復雜，但目前用來分析機械臂正逆運動學十分成熟，且應用廣泛[3,4]。而采用旋量法建立機器人運動學模型，只需要建立基坐標系和末端執(zhí)行器坐標系，通過確定各個連桿之間的旋轉(zhuǎn)或平移運動，得到機械臂末端坐標系相對于基坐標系的運動關(guān)系，旋量法相對于D-H參數(shù)法更加簡明清晰，便于理解，在機器人運動學建模中使用旋量法更加方便[5,6]。

相較于傳統(tǒng)六自由度機械臂而言，七自由度機械手臂有著顯著的優(yōu)點：更高的工作性能、更廣泛的應用空間，因自由度數(shù)目的增加使得機械臂的操作性能有了一個質(zhì)的飛躍。但是也因為比六自由度多出一個冗余關(guān)節(jié)，而增加了計算逆解的難度，七自由度機械臂肘部會產(chǎn)生自運動,其軌跡為圓，由自運動產(chǎn)生的運動變量被定義為臂角[7]，作為求取逆解的一個關(guān)鍵參數(shù)，對于臂角的研究至關(guān)重要。文獻[8]將冗余關(guān)節(jié)角作為參數(shù)，用解析法求得七自由度機械臂逆解，然而該方法依賴于機械臂構(gòu)型，如果冗余角選擇不合適將導致計算十分復雜。文獻[9]和文獻[10]分別采用神經(jīng)網(wǎng)絡和遺傳算法計算七自由度冗余機械臂逆解的解析解，然而，兩種算法都很復雜，并且需要大量的計算，同時難以對機械臂進行實時控制。文獻[11]通過固定七自由度機械臂中的3個關(guān)節(jié)角度將其轉(zhuǎn)換四自由度，并結(jié)合代數(shù)迭代法和位姿分離法兩種方法求解逆解，簡化了逆解的求解過程，但由于固定了關(guān)節(jié)角度使求出的逆解不夠準確。

本文以七自由度模塊化機械臂為研究對象，提出一種七自由度機械臂逆解算法。首先，基于SolidWorks軟件建立七自由度模塊化機械臂模型，采用旋量法建立機械臂正運動學方程；然后，采用矢量法和位姿分離法兩者結(jié)合求解機械臂逆解；最后，在MATLAB軟件編制逆解算法程序，得出機械臂八組逆解，將八組逆解代入正運動學方程，并在Robotics Toolbox中進行仿真，便可得到八組逆解的機械臂末端位姿。仿真結(jié)果驗證了運動學建模的正確性，使機械臂逆解求解更加簡明，為后續(xù)機械臂動力學研究和軌跡規(guī)劃提供研究基礎(chǔ)。

1 機器人運動學模型建立

1.1 旋量理論

(1)

剛體的旋量運動可采用矩陣指數(shù)形式來表示，則有

(2)

(1-cosθ)

將坐標系T固定到機械臂末端上，則坐標系T相對于基坐標系S的位姿可由gst表示，初始狀態(tài)下位姿為gst(0)，當各關(guān)節(jié)轉(zhuǎn)動θi后的位姿為

(3)

1.2 機械臂運動學建模

該七自由度模塊化機械臂有7個轉(zhuǎn)動關(guān)節(jié)，并且后3個關(guān)節(jié)軸線交于一點(如圖1)。根據(jù)該機械臂構(gòu)型特點，及該機械臂某一位姿建立基坐標系S和固定在末端執(zhí)行器坐標系T，如圖2所示，其中d1=0.3 m，d3=0.328 m，d5=0.2765 m，d7=0.2916 m。

圖1 七自由度模塊化機械臂模型 圖2 初始姿態(tài)機械臂構(gòu)型及坐標系建立

機械臂運動學方程建立步驟為：

(1)在初始位姿下，機械臂末端執(zhí)行器坐標系T相對于基坐標系S的位姿矩陣為

(4)

(2)由圖2可知，各關(guān)節(jié)軸線的單位方向向量為

(3)取各關(guān)節(jié)軸線上一點q，

(4)基于旋量理論計算機器人運動學正解，結(jié)合式(1)至(3)即可得機械臂運動學正解為

(5)

圖3 機械臂模型簡圖

2 七自由度機械臂逆運動學求解

(6)

(7)

同時，可以通過給定一個臂角φ值，求得E點位置。

(8)

(1)關(guān)節(jié)角θ4的求解

在ΔESW中，根據(jù)余弦定理有，可求出

(9)

式中：LSE=d3，LEW=d5。再由∠SEW與θ4的關(guān)系可以求出：θ4=±[π-∠ESW]。

(2)關(guān)節(jié)角θ2的求解

由圖3可知，θ2可由以下式(10)求出

(10)

(3)關(guān)節(jié)角θ1的求解

(11)

(4)關(guān)節(jié)θ3的求解

θ3的求解與θ1是類似的，在關(guān)節(jié)2建立坐標系o2-x2y2z2，如圖2所示，θ3可由式(12)求出

(12)

(5)關(guān)節(jié)角θ5、θ6、θ7的求解

已知θ1、θ2、θ3、θ4，根據(jù)旋量理論公式(3)，根據(jù)等式相等原則可以求出

θ5=arcsin(R1/s6)

(13)

θ6=±arccos(R2)

(14)

θ7=arcsin(R3/s6)

(15)

式中

R1=-r13(s3c2c1+s1c3)+r23(c1c3-s1s3c2)+s2s3r33/c2

R2=r13(s4c3c2c1+s2c1-s1s3s4)+r23(s1s4c2c3+s1s2+c1c3s4)+r33(c2-s2s4c3)/c2

R3=r12(s4c3c2c1+s2c1-s1s3s4)+r22(s1s4c2c3+s1s2+c1c3s4)+r32(c2-s2s4c3)/c2

3 逆解算法驗證

以上述正逆運動學分析為基礎(chǔ)，在MATLAB軟件中編制逆解算法，用來求解機械臂逆解。由于臂角的確定要結(jié)合避障來討論，該機械臂運動分析暫不涉及避障，所以為求解逆解時，臂角用隨機的值確定。

在機器人工作空間范圍內(nèi)，任意給出7個關(guān)節(jié)角度，這里取θ1=π/10，θ2=π/3，θ3=π/6，θ4=π/3，θ5=π/12，θ6=π/4，θ7=π/3，并給定臂角為φ=π/6。

(1)將各關(guān)節(jié)角度帶入式(3)，即可求得機械臂末端位姿矩陣為

表1 機械臂8組逆解

(2)利用本文提出的逆解算法，共得到 8 組逆運動學的解，如表1所示。

圖4 機械臂8組逆解位姿圖

將上述求得的8組逆解帶入正運動學方程式(3)中，并在MATLAB Robotics Toolbox中進行仿真，設置時間t=1 s，便可得到機械臂末端位姿和末端軌跡線，如圖4所示。

通過表格1和圖4可以發(fā)現(xiàn)，第一組解就是仿真前給出的7個關(guān)節(jié)角度，并且8幅圖的末端執(zhí)行器位姿與正運動學方程所得到的位姿是相同的，但是8幅位姿圖的末端軌跡線卻有所不同，表明機械臂可以通過8組逆解角度反求機械臂末端位姿，也證明了本文所提出的正逆運動學建模和逆解算法的正確性。本文所提出的逆解算法意義鮮明，運算速度快，準確性高，可以用于七自由度冗余機械臂的實時控制。

4 結(jié)論

本文通過旋量法和自運動對七自由度機械臂逆解進行分析，得到以下結(jié)論。

(1)以七自由度模塊化機械臂為研究對象，基于SolidWorks建立七自由度模塊化機械臂模型，采用旋量理論建立機械臂正運動學方程。

(2)基于矢量法和位姿分離法兩者結(jié)合求解機械臂逆解，在MATLAB軟件中編寫逆解算法程序，得到8組逆解，將8組逆解代入正運動學方程并結(jié)合MATLAB Robotics Toolbox仿真驗證，得出末端執(zhí)行器位姿與正運動學所求位姿相符合。

(3)該逆解算法能夠準確清晰表達八組逆解的幾何意義，降低了運算過程，使機械臂逆解求解更加簡明。MATLAB Robotics Toolbox仿真結(jié)果驗證了逆解算法的正確性，為后續(xù)機械臂動力學研究和軌跡規(guī)劃提供研究基礎(chǔ)。