朱曉龍，頓向明

（上海交通大學(xué)機(jī)械與動(dòng)力工程學(xué)院，上海200240）

0 引言

目前，常用的機(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ì)不太適合。

1 機(jī)械臂模型及正運(yùn)動(dòng)學(xué)求解

以多關(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ù)。

2 幾何法運(yùn)動(dòng)學(xué)逆解

2.1 末端坐標(biāo)系位姿描述方法

對于五自由度機(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é)角空間解，且逆解一定存在。

2.2 幾何法運(yùn)動(dòng)學(xué)逆解

機(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ù)性找出合適的惟一解。

3 仿真實(shí)驗(yàn)

為了驗(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)較為平滑。

4 結(jié)束語

針對五自由度機(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.