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

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

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

0 引 言

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

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

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

1 運動學建模

1.1 正運動學建模

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

表1 SRS機械臂DH參數(shù)表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構(gòu)成的平面,向量v根據(jù)目標位置進行設(shè)置.臂平面是由w、e構(gòu)成的平面,即機械臂機體構(gòu)成的平面.

圖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)

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

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)

根據(jù)羅德里格斯公式,空間向量k圍繞單位向量p旋轉(zhuǎn)α的旋轉(zhuǎn)矩陣為

Rp,α=Ja+Jbcosα+Jcsinα

(10)

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

(11)

臂型角φ為0時肘關(guān)節(jié)的姿態(tài)矩陣

(12)

因此臂型角為φ的肘關(guān)節(jié)姿態(tài)矩陣為

(13)

肘關(guān)節(jié)姿態(tài)矩陣可以通過正運動學式求出

(14)

式(13)和(14)對應(yīng)項相等,求出前3個關(guān)節(jié)的關(guān)節(jié)角

(15)

當求出前4個關(guān)節(jié)角度,通過姿態(tài)矩陣求出后3個關(guān)節(jié)角

(16)

(17)

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

(18)

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

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

圖2中,假設(shè)E在基坐標系下的坐標位置為[xEyEzE]T,根據(jù)SRS機械臂的幾何結(jié)構(gòu)特征有

(19)

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

(20)

式(20)中

(21)

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

(22)

求出肘關(guān)節(jié)的姿態(tài)矩陣后,各關(guān)節(jié)角度同樣可以根據(jù)式(14)～(18)求解.

2.2 臂型角規(guī)劃

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

3 仿真與試驗

3.1 UG-Simulink聯(lián)合仿真

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

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

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

圖4 仿真的關(guān)節(jié)位置變化曲線Fig.4 Changes in the joint position of simulation

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

3.2 物理試驗

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

圖6 試驗系統(tǒng)示意圖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.試驗通過本文提出的逆運動學方法求解出機械臂運動至目標點的關(guān)節(jié)角,采用梯形速度插值方法規(guī)劃關(guān)節(jié)運動,運動時間共122 s,其中加速階段為10 s,減速階段30 s.機械臂位于初始位置的臂型角為0.095 rad,運動至目標點的臂型角設(shè)置為0 rad.

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

圖7 試驗關(guān)節(jié)位置變化曲線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 結(jié) 論

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

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

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