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

曹 行, 何 俊*, 段文杰, 張海博

1. 上海交通大學 機械與動力工程學院, 上海 200240

2. 北京控制工程研究所, 北京 100094

0 引 言

為高效開發利用軌道資源和清理軌道碎片,美國提出MEV(mission extension vehicle)計劃,德國提出DOES(deutsche orbitale service mission)計劃[1-3]開展基于空間七自由度機械臂的在軌維修、拼接組裝和燃料加注等問題的技術研究和探索[4-6].七自由度機械臂通常有兩種結構:1)具有俯仰運動副的七自由度機械臂,由二自由度肩關節、三自由度肘關節和二自由度腕關節組成,其中第3、4和5關節互相平行; 2)SRS (spherical-roll-spherical-joint)結構的七自由度機械臂,具有3個相互垂直自由度的肩、腕關節和單自由度的肘關節[7].本文主要研究具有SRS結構機械臂的逆運動學求解問題.

空間機械臂控制的實時性要求高,逆運動學通常采用解析法[8].對于一個確定的末端位姿,SRS機械臂的關節空間內存在無數組解,因此需要設定冗余參數求出逆運動學解析解[9].時凱飛等[10]將七自由度機械臂的關節3固定為0°和180°,從而等效為六自由度機械臂求解出解析逆解.徐俊虎等[11]選取關節3作為冗余角,通過冗余差分角法確定冗余角并實現運動學的優化.HOLLERBACH[12]首次利用肩、肘和腕關節構成的平面和參考平面之間的夾角來描述機械臂構型,而參考平面表示SRS機械臂自運動的0°平面.SHIMIZU等[13]基于臂型角參數化的方法求出SRS機械臂的逆運動學解析解,此方法同樣適用于關節極限位置.ZHOU等[14]對各關節角進行約束求出臂型角的變化范圍,根據末端姿態確定最佳臂型角,最后求解出機械臂的逆運動性解析解.徐文福等[15]采用參數化臂型角的方法,將非偏置的SRS機械臂擴展到具有關節偏置結構的SRS機械臂,求解偏置七自由度機械臂逆運動學,但求解結果會存在誤差.針對這一問題,MA等[16]結合關節角參數化的逆運動學求解方法,對基于臂型角參數化的偏置SRS機械臂的解析解進行修正,進而提高求解的精度.上述研究都為基于臂型角的SRS型機械臂的逆運動學求解奠定了理論基礎.

本文以全物理仿真工況為背景,研究SRS機械臂處于吊絲卸載重力環境下水平方向的抓取運動.此背景下SRS機械臂末端通過奇異點,且要求在豎直方向運動較小,已有的臂型角定義方法難以適應此約束條件.本文根據目標位置設置臂型角,提出了一種基于臂型角規劃的逆運動學求解方法,通過UG-Simulink聯合仿真以及包含重力卸載裝置的物理試驗,驗證了所提方法的有效性.

1 運動學建模

1.1 正運動學建模

SRS機械臂的DH (denavit-hartenberg)坐標系如圖1所示,DH參數如表1所示. 本文采用S、E和W來表示SRS機械臂的肩關節、肘關節和腕關節中心,肘部偏置中心用E′表示. 圖1中O為基坐標的原點,ai表示連桿長度,αi表示相鄰2個關節軸線在連桿長度ai的法平面內投影角度,di表示相鄰2個關節之間的公垂線度,θi表示關節轉角.

表1 SRS機械臂DH參數表Tab.1 The DH parameters of SRS manipulator

圖1 SRS機械臂的DH坐標系Fig.1 The DH coordinate system of SRS manipulator

1.2 臂型角定義

如圖2所示,O-xyz為機械臂的基坐標系,記向量OS=s,SW=w,SE=e.臂型角為參考平面和臂平面之間的夾角,其中參考平面是由w和任意單位向量v構成的平面,向量v根據目標位置進行設置.臂平面是由w、e構成的平面,即機械臂機體構成的平面.

圖2 臂平面、參考平面和臂型角示意圖Fig.2 The sketch map of arm plane, reference plane and arm-angle

(1)

臂型角φ為

(2)

2 逆運動學求解

2.1 基于臂型角的逆運動學求解

當機械臂的末端位姿和臂型角φ已知,機械臂腕點W的位置OW=pw=[pwxpwypwz]T可以通過末端位姿和正運動學求出

(3)

(4)

假設ci=cosθi,si=sinθi,式(3)和(4)對應項相等可得

c1[s2(d3+d5c4)+c2s3(a3-d5s4)]+
s1s3(a3-d5s4)=pwx

(5)

s1[s2(d3+d5c4)+c2s3(a3-d5s4)]-
c1s3(a3-d5s4)=pwy

(6)

(7)

式(5)～(7)中各式平方相加化簡得

(8)

因此,θ4的大小為

(9)

根據羅德里格斯公式,空間向量k圍繞單位向量p旋轉α的旋轉矩陣為

Rp,α=Ja+Jbcosα+Jcsinα

(10)

x3=Rn,π/2Rn,αw,y3=z3×x3

(11)

臂型角φ為0時肘關節的姿態矩陣

(12)

因此臂型角為φ的肘關節姿態矩陣為

(13)

肘關節姿態矩陣可以通過正運動學式求出

(14)

式(13)和(14)對應項相等,求出前3個關節的關節角

(15)

當求出前4個關節角度,通過姿態矩陣求出后3個關節角

(16)

(17)

式(16)和式(17)對應項相等求出θ5、θ6和θ7

(18)

對于給定的一組末端位姿和臂型角φ,可以通過式(9)、(15)和(18)求出各個關節的關節角.

當機械臂的位形使向量w和v共線時臂型角不存在,無法通過上述的計算過程進行逆運動學求解,此時可以根據肘部的空間位置約束進行逆運動學求解.

圖2中,假設E在基坐標系下的坐標位置為[xEyEzE]T,根據SRS機械臂的幾何結構特征有

(19)

假設肘部在基坐標系的y軸存在位置約束,即yE為常數.由式(20)求出xE和zE

(20)

式(20)中

(21)

由此求出肘部E的坐標,這樣可以計算肘部的姿態矩陣:z3軸垂直于臂平面,x3軸由向量e圍繞z3軸逆時針旋轉∠SEE′求得

(22)

求出肘關節的姿態矩陣后,各關節角度同樣可以根據式(14)～(18)求解.

2.2 臂型角規劃

通過式(2)計算出起始位置臂型角,根據任務要求設置目標位置臂型角的大小. 當在笛卡爾空間進行路徑規劃控制機械臂的運動時,采用直線插值方法規劃從初值位置到目標位置的臂型角的變化. 當在關節空間進行路徑規劃時,臂型角的變化和關節的規劃方式有關.

3 仿真與試驗

3.1 UG-Simulink聯合仿真

本文在UG-Simulink虛擬仿真環境中驗證包含肩關節奇異點路徑的逆運動解算過程.仿真中,機械臂末端初始位置和歐拉角設置為[-550 0 2 245]T、[90° 90° 0°]T,且末端姿態保持不變. 目標位置為[4 500 0 2 245]T.在笛卡爾空間中采用直線路徑規劃機械臂的運動軌跡,末端沿x軸勻速運動1 000 mm,時間為12 s. 機械臂位于初始位置時臂型角為0. 093 rad,設置目標位置的臂型角為0 rad,采用直線插值對臂型角進行規劃.

圖3為機械臂末端的位置變化曲線,在5.5 s時x=0,機械臂位于肩關節奇異位置.圖4為機械臂關節角度變化曲線,曲線變化光滑連續,符合機械臂運動要求.圖5為臂型角的變化曲線,曲線從初始0.093 rad線性減小至0 rad,曲線變化和規劃一致.仿真結果表明本文提出的逆運動求解方法可以使機械臂平穩通過包含肩關節奇異點的路徑.

圖3 仿真的末端位置變化Fig.3 Changes in the end position of simulation

圖4 仿真的關節位置變化曲線Fig.4 Changes in the joint position of simulation

圖5 仿真臂型角的變化Fig.5 Changes in the arm-angle of simulation

3.2 物理試驗

圖6為試驗系統簡圖,SRS機械臂安裝在氣浮臺上,機械臂重力由懸吊系統卸載,整個系統可以模擬空間微重力環境.OSE′EW為SRS機械臂,O-xyz為機械臂的基坐標系,z軸指向安裝平面的法線方向,y軸豎直向上.

圖6 試驗系統示意圖Fig.6 The Schematic diagram of experiment

試驗中末端初始位置和歐拉角為[-557 -75 2 270]T、[90° 90° 0°]T,目標位置和歐拉角為[243 -118 2 289]T、[90° 90° 0]T.試驗通過本文提出的逆運動學方法求解出機械臂運動至目標點的關節角,采用梯形速度插值方法規劃關節運動,運動時間共122 s,其中加速階段為10 s,減速階段30 s.機械臂位于初始位置的臂型角為0.095 rad,運動至目標點的臂型角設置為0 rad.

試驗的關節曲線變化如圖7所示,各關節曲線連續平滑,機械臂運動穩定.圖8為末端的位置變化曲線,在86 s接近肩關節奇異位形.圖9為臂型角的變化曲線,臂型角從初始0.095 rad減小至0 rad.圖10對比了兩種方法在豎直方向的空間位移,采用文獻[10]提出的關節角參數化方法時肘關節在豎直方向最大位移為170 mm,采用本文提出的求解方法時最大位移為60 mm.因此本文提出的逆運動學求解方法可以減小機械臂肘部在重力方向上的位移,更適合操作空間受限的試驗系統中.

圖7 試驗關節位置變化曲線Fig.7 Changes in the joint position of experiment

圖8 試驗的末端位置變化Fig.8 Changes in the end position of experiment

圖9 試驗臂型角變化曲線Fig.9 Changes in the arm-angle of experiment

圖10 不同逆運動求解方法下的y方向肘部位移Fig.10 The elbow positions of y direction under different inverse kinematics solution

4 結 論

針對SRS結構的機械臂,本文提出了一種基于臂型角的逆運動學求解方法. 根據目標位置構建臂型角,當臂型角非奇異時,利用SRS機械臂的幾何特征求出逆運動學解析解. 當臂型角奇異時,利用肘關節位置約束求出逆運動學解析解.

在UG-Simulink聯合仿真中,機械臂有效通過了笛卡爾空間內包含肩關節奇異點的直線路徑. 在物理試驗中,采用梯形速度插值方法規劃關節空間運動,完成了通過奇異點的空間抓捕和釋放操作. 對比本文方法和已有關節角參數化方法,本文方法在操作面外空間構型的位移量減小50%以上.

仿真和試驗結果表明提出的逆運動學算法能有效解決SRS機械臂肩關節存在奇異點時的多解問題,更適應于運動空間受限時的七自由度機械臂運動控制.