朱曉龍,頓向明
(上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院,上海200240)
目前,常用的機(jī)械臂逆運(yùn)動(dòng)學(xué)求解方法主要可以分為3種:代數(shù)法、迭代法[1-4]和幾何法[5-8],其中代數(shù)法最為通用。代數(shù)法的求解思想是將運(yùn)動(dòng)學(xué)正向求解方程的兩側(cè)依次乘以各個(gè)變換矩陣的逆矩陣,從而分離出易于求解的關(guān)節(jié)角變量,進(jìn)而進(jìn)行求解。但對于五自由度機(jī)械臂,由于其自由度數(shù)不足6,不能以任意姿態(tài)逼近同一目標(biāo)位置,進(jìn)而造成在笛卡爾空間軌跡規(guī)劃所得到的軌跡節(jié)點(diǎn)目標(biāo)位姿不存在運(yùn)動(dòng)學(xué)逆解,且逆解存在與否難以直觀判斷。因此,傳統(tǒng)的在笛卡爾空間進(jìn)行軌跡規(guī)劃,并利用代數(shù)逆解法求取目標(biāo)位姿對應(yīng)的關(guān)節(jié)角度值的方法對于五自由度機(jī)械臂的運(yùn)動(dòng)算法設(shè)計(jì)不太適合。
以多關(guān)節(jié)單電機(jī)離散驅(qū)動(dòng)的五自由度機(jī)械臂為研究對象,其結(jié)構(gòu)簡圖如圖1所示。機(jī)械臂5個(gè)關(guān)節(jié)全部為旋轉(zhuǎn)關(guān)節(jié),其中,3個(gè)為彎曲關(guān)節(jié),2個(gè)為扭轉(zhuǎn)關(guān)節(jié)。機(jī)械臂驅(qū)動(dòng)方式與機(jī)械結(jié)構(gòu)較為特殊,但其運(yùn)動(dòng)學(xué)模型較為通用,具有一定的代表性。
圖1 機(jī)械臂結(jié)構(gòu)
機(jī)械臂正運(yùn)動(dòng)學(xué)求解相對于逆運(yùn)動(dòng)學(xué)求解較為簡單,只需根據(jù)運(yùn)動(dòng)學(xué)模型,通過對各連桿坐標(biāo)系變換矩陣進(jìn)行運(yùn)算就可以求得末端坐標(biāo)系在基礎(chǔ)坐標(biāo)系下的惟一位姿矩陣。為了方便后續(xù)幾何法運(yùn)動(dòng)學(xué)逆解的計(jì)算,這里各連桿坐標(biāo)系的建立未完全按照D-H法[9]進(jìn)行,具體坐標(biāo)系建立情況如圖1所示。X0Y0Z0為基礎(chǔ)坐標(biāo)系,X1Y1Z1,X2Y2Z2,X3Y3Z3,X4Y4Z4,X5Y5Z5分別為5個(gè)連桿坐標(biāo)系,XhYhZh為方便末端軌跡規(guī)劃而建立的末端坐標(biāo)系。圖1中,從基礎(chǔ)坐標(biāo)系附近開始,5個(gè)連桿的長度分別為89.0mm,245.9mm,85.0mm,147.0mm,231.7 mm。圖1對應(yīng)的機(jī)械臂姿態(tài)為其機(jī)構(gòu)零位,各關(guān)節(jié)角度變量的取值范圍以及零位狀態(tài)下對應(yīng)的關(guān)節(jié)角度初始值如表1所示。
表1 關(guān)節(jié)轉(zhuǎn)角變化范圍及當(dāng)前值 (°)
根據(jù)機(jī)械臂的運(yùn)動(dòng)學(xué)模型以及相鄰連桿坐標(biāo)系間的位姿關(guān)系,可以建立相鄰連桿坐標(biāo)系間的變換矩陣如下:
Ci(i=1,2,…,5)代表cosθi,Si(i=1,2,…,5)代表sinθi。將各個(gè)坐標(biāo)系位姿變換矩陣順序相乘,即可得到末端連桿坐標(biāo)系的位姿矩陣:
n,o,a3個(gè)向量描述了機(jī)械臂末端連桿的姿態(tài),向量描述了末端連桿的位置,這4個(gè)向量的各個(gè)元素是5個(gè)關(guān)節(jié)角變量的函數(shù)。
對于五自由度機(jī)械臂而言,由于自由度小于6,其可達(dá)空間內(nèi)的目標(biāo)位置點(diǎn)不能以任意姿態(tài)逼近。因此,傳統(tǒng)的采用3個(gè)笛卡爾位置坐標(biāo)和3個(gè)歐拉角坐標(biāo)描述機(jī)械臂末端坐標(biāo)系位姿的方法約束了過多的自由度,在末端軌跡規(guī)劃時(shí)如用這種描述方式進(jìn)行變量插補(bǔ),所得到的目標(biāo)位姿矩陣無法保證其逆解的存在,同時(shí)這種描述方式也缺乏空間直觀性。為了解決這一問題,從機(jī)械臂自由度約束的角度出發(fā),采用5個(gè)位姿變量描述末端連桿的位姿。由于這種方法的機(jī)械臂自由度數(shù)目和運(yùn)動(dòng)約束數(shù)目相同,保證了逆解的存在性。同時(shí),這種描述方式較好的空間直觀性便于利用幾何法進(jìn)行運(yùn)動(dòng)學(xué)反解,雖然未完全限制末端連桿的位姿,但沒有進(jìn)行約束的那個(gè)自由度為繞末端連桿坐標(biāo)系接近矢量[9]的旋轉(zhuǎn)運(yùn)動(dòng),這種約束方式對很多應(yīng)用場合下的機(jī)械臂適用,如噴涂機(jī)械臂,焊接機(jī)械臂等對末端執(zhí)行器繞接近矢量方向的旋轉(zhuǎn)運(yùn)動(dòng)沒有嚴(yán)格要求的情況。
基于上述說明,對末端坐標(biāo)系位姿進(jìn)行描述時(shí)只能限制其5個(gè)方向的運(yùn)動(dòng)才能保證目標(biāo)位姿有逆解。首先,以末端坐標(biāo)系原點(diǎn)在基礎(chǔ)坐標(biāo)系中的坐標(biāo)作為機(jī)械臂的末端位置坐標(biāo),這樣限制了3個(gè)移動(dòng)自由度。然后,拋棄傳統(tǒng)的以末端坐標(biāo)系相對于基礎(chǔ)坐標(biāo)系的歐拉角描述機(jī)械臂末端姿態(tài)的方式,采用末端連桿坐標(biāo)系的接近矢量相對于基礎(chǔ)坐標(biāo)系的3個(gè)方向余弦角中的任意兩個(gè)去描述機(jī)械臂末端姿態(tài)。3個(gè)方向余弦角變量相關(guān),根據(jù)方向余弦角公式,其中任意一個(gè)方向余弦角可由另外兩個(gè)方向余弦角求出。因此,可以用任意兩個(gè)方向余弦角描述機(jī)械臂末端姿態(tài)。這樣,就實(shí)現(xiàn)了以5個(gè)變量描述機(jī)械臂末端的位姿。位姿變量示意圖如圖2所示,其中,α,β,γ分別為末端坐標(biāo)系接近矢量相對于基礎(chǔ)坐標(biāo)系X,Y,Z3個(gè)軸的余弦角,pn向量為末端執(zhí)行器的位置坐標(biāo)向量,xn,x′n為末端坐標(biāo)系接近矢量。
圖2 位姿變量
基于上述約束自由度的思想,通過對軌跡節(jié)點(diǎn)上的末端坐標(biāo)系原點(diǎn)相對于基礎(chǔ)坐標(biāo)系的3個(gè)位置坐標(biāo)(px,py,pz)以及末端坐標(biāo)系接近矢量相對于基礎(chǔ)坐標(biāo)系的3個(gè)方向余弦角(α,β,γ)中的任意兩個(gè)所組成的五維位姿變量進(jìn)行插補(bǔ)規(guī)劃,然后利用幾何法進(jìn)行逆運(yùn)動(dòng)學(xué)求解即可得到對應(yīng)的關(guān)節(jié)角空間解,且逆解一定存在。
機(jī)械臂各連桿的空間位置示意圖如圖3所示,從圖中可以看出,無論5個(gè)關(guān)節(jié)角度變量取何值,連桿1、連桿2、連桿3、連桿4、連桿5的坐標(biāo)系原點(diǎn)始終在同一平面上,該平面的位置由且僅由θ1確定。
圖3 連桿空間位置
由于該機(jī)械臂幾何構(gòu)型的特殊性,如果已知了連桿5坐標(biāo)系的位置坐標(biāo)以及連桿5坐標(biāo)系接近矢量的方向即可方便地通過幾何求解的方法求解出各個(gè)關(guān)節(jié)角變量。已知條件為連桿5坐標(biāo)系全局位置坐標(biāo)以及連桿5坐標(biāo)系接近矢量x5(對應(yīng)圖1中X5Y5Z5坐標(biāo)系的X軸,后面相似向量對應(yīng)情況相同),具體求解流程簡述如下:
a.由連桿5坐標(biāo)系的位置坐標(biāo)直接求解得到θ1。
b.計(jì)算連桿5坐標(biāo)系在連桿1坐標(biāo)系中的位置坐標(biāo),由此坐標(biāo)以及連桿2、連桿3和連桿4的長度計(jì)算出θ2,θ3。
c.由θ1,θ2,θ3求出連桿4坐標(biāo)系軸向量z4,由z4及x5求出z5,進(jìn)而求出θ4。
d.利用連桿5坐標(biāo)系軸向量x5相對于連桿4坐標(biāo)系方向求解出θ5。
假設(shè)目標(biāo)末端連桿位姿矩陣為:
此時(shí),p5(px,py,pz),x5(nx,ny,nz),為已知量。由p5可直接求得:
或者
為了將后續(xù)計(jì)算轉(zhuǎn)化為平面計(jì)算,求出連桿5坐標(biāo)系在連桿1坐標(biāo)系中的位置坐標(biāo)p′5(p′x,p′y,p′z):
或者
至此,已求出θ1,θ2,θ3。由此,可求出連桿4坐標(biāo)系 向 量 z4(a4x,a4y,a4z)。a4x=C1S3-2;a4y=S1S3-2;a4z=C3-2。
當(dāng)z4與x5平行時(shí),可取θ4=θ5=0°。
當(dāng)z4與x5不平行時(shí),利用z5為z4與x5的公垂線求解出z5,進(jìn)而確定了連桿5坐標(biāo)系的完整位姿。由于z5的雙向性,z5(a5x,a5y,a5z)有2組解:
得到連桿5坐標(biāo)系的完整位姿后,通過連桿5坐標(biāo)系相對于連桿3坐標(biāo)系、連桿4坐標(biāo)系的位姿,可分別求出:
至此,5個(gè)關(guān)節(jié)角度變量完全求出,共有8組解,可以根據(jù)關(guān)節(jié)角變量取值范圍以及運(yùn)動(dòng)連續(xù)性找出合適的惟一解。
為了驗(yàn)證上述算法的可行性以及正確性,對一條指定的直線軌跡進(jìn)行運(yùn)動(dòng)追蹤,用Matlab編寫腳本程序進(jìn)行運(yùn)動(dòng)規(guī)劃以及運(yùn)動(dòng)學(xué)逆解,并通過三維圖像的形式將生成軌跡與實(shí)際期望軌跡進(jìn)行對比。
在該仿真實(shí)驗(yàn)中,以末端坐標(biāo)系XhYhZh為對象進(jìn)行運(yùn)動(dòng)規(guī)劃,軌跡起始點(diǎn)處末端坐標(biāo)系位置坐標(biāo)為 (354.0,173.0,403.0),Xh軸 方 向 角 為(29.27°,68.38°,71.25°),軌跡終止點(diǎn)處末端坐標(biāo)系位置坐標(biāo)為(343.0,286.0,453.0),Xh軸方向角為(34.46°,84.19°,56.17°)。由于每個(gè)目標(biāo)位姿可能對應(yīng)8組逆解。因此,需要尋求合理的惟一解,可以利用關(guān)節(jié)角度變量取值范圍以及運(yùn)動(dòng)連續(xù)性進(jìn)行最優(yōu)解的尋找。此處運(yùn)動(dòng)連續(xù)性判斷公式[10]是,Δ取最小時(shí)對應(yīng)的解即為最優(yōu)解:
l1,l2,l3,l4,l5為加權(quán)因子,由于靠近基座的關(guān)節(jié)變化對末端影響較大。因此,對應(yīng)的加權(quán)因子也取較大值,這里l1=l2=l3=1,l4=l5=0.5。
下面對位置坐標(biāo)以及方向角分別進(jìn)行線性插補(bǔ)。對于方向角的坐標(biāo),只對方向向量與X,Y軸的余弦角α,β進(jìn)行線性插補(bǔ),方向向量與Z軸的余弦角γ由α,β計(jì)算得出。插補(bǔ)后位置坐標(biāo)ph(pxh,pyh,pzh)和Xh軸方向角(αh,βh,γh)為:
T為運(yùn)動(dòng)周期,令T=10s,γh取跟前一次插補(bǔ)最接近的值。為了方便逆運(yùn)動(dòng)學(xué)求解的進(jìn)行,需求出關(guān)節(jié)5坐標(biāo)系對應(yīng)的位置坐標(biāo)p5(px,py,pz),以及連桿5方向向量x5(nx,ny,nz)。
位置追蹤結(jié)果如圖4所示,機(jī)械臂整體姿態(tài)如圖5所示,求解出的關(guān)節(jié)角度值如圖6所示。
圖4 位置追蹤結(jié)果
圖5 機(jī)械臂整體姿態(tài)
圖6 關(guān)節(jié)角度變化曲線
從仿真結(jié)果可以看出,仿真生成位置與期望位置吻合程度較好,證明了該算法的可行性與精確性,從理論角度來看沒有誤差。同時(shí),通過運(yùn)動(dòng)學(xué)逆解求出的關(guān)節(jié)空間解在關(guān)節(jié)變量可達(dá)空間內(nèi),且無突變,即關(guān)節(jié)運(yùn)動(dòng)較為平滑。
針對五自由度機(jī)械臂因自由度不足而導(dǎo)致的軌跡規(guī)劃所產(chǎn)生的目標(biāo)位姿不存在運(yùn)動(dòng)學(xué)逆解的問題,提出了一種在軌跡規(guī)劃階段新的末端位姿描述方法,并結(jié)合機(jī)械臂本身自由度少、結(jié)構(gòu)簡單的優(yōu)勢,采用幾何法進(jìn)行逆運(yùn)動(dòng)學(xué)求解,求解過程較為簡單,且不存在理論誤差。在實(shí)際實(shí)施過程中,由于該算法計(jì)算復(fù)雜度低。因此,利用計(jì)算機(jī)編程的方式實(shí)現(xiàn)該算法十分方便,實(shí)時(shí)性好。雖然該算法針對特定的機(jī)械臂構(gòu)型提出,但這種采用約束自由度的思想進(jìn)行末端位姿描述,并采用幾何法求解目標(biāo)末端位姿對應(yīng)的關(guān)節(jié)角空間解的思路,對于一般的五自由度機(jī)械臂有一定的通用性。
[1] Assal S F M,Watanabe K,Izumi K.Neural network learning from hint for the inverse kinematics Problem of redundant arm subject to joint limits[A].IEEE/RSJ International Conference on Intelligence Robots and System[C].Piscataway,NJ,USA:IEEE,2005.1477-1482.
[2] Nearchou A C.Solving the inverse kinematics problem of redundant robots operating in complex environments via a modified genetic algorithm[J].Mechanism and Machine Theory,1998,33(3):273-292.
[3] Oyama E,Agah A,MacDorman K F,et al.A modular neural network architecture for inverse kinematics model learning[J].Neurocomputing,2001,(38-40):797-805.
[4] Xia Y,Wang J.A dual neural network for kinematic control of redundant robot manipulators[A].IEEE Transactions on Systems,Man,and Cybernetics,Part B[C].2001,31(1):147-154.
[5] Mohamed H A F,Yahya S,Moghavvemi M,et al.A new inverse kinematics method for three dimensional redundant manipulators[A].ICCAS-SICE[C].Piscataway,NJ,USA:IEEE,2009.1557-1562.
[6] Song D H,Jung S.Geometrical analysis of inverse kinematics solutions and fuzzy control of humanoid robot arm under kinematics constrains[A].IEEE International Conferrence on Mechatronics and Automation[C].Piscataway,NJ,USA:IEEE,2007.1178-1183.
[7] Yahya S,Mohamed H A F,Moghavvemi M,et al.A new geometrical inverse kinematics method for planar hyper redundant manipulator[A].Innovative Technologies in Intelligent Systems and Industrial Applications[C].Piscataway,NJ,USA:IEEE,2009.20-22.
[8] Li S,Wang Y Q,Chen Q W,et al.A new geometrical method for the inverse kinematics of the hyper-redundant manipulators[A].IEEE International Conferrence on Robotics and Biomimetics[C].Piscataway,NJ,USA:IEEE,2006.1356-1359.
[9] 熊有倫.機(jī)器人學(xué),2版[M].北京:機(jī)械工業(yè)出版社,1993.
[10] 陳 鵬,劉 璐,余 飛,等.一種仿人機(jī)械臂的運(yùn)動(dòng)學(xué)逆解的幾何求解方法[J].機(jī)器人,2012,34(2):211-216.