一種五自由度機械臂逆運動學求解的幾何法

朱曉龍，頓向明

（上海交通大學機械與動力工程學院，上海200240）

0 引言

目前，常用的機械臂逆運動學求解方法主要可以分為3種：代數法、迭代法［1－4］和幾何法［5－8］，其中代數法最為通用。代數法的求解思想是將運動學正向求解方程的兩側依次乘以各個變換矩陣的逆矩陣，從而分離出易于求解的關節角變量，進而進行求解。但對于五自由度機械臂，由于其自由度數不足6，不能以任意姿態逼近同一目標位置，進而造成在笛卡爾空間軌跡規劃所得到的軌跡節點目標位姿不存在運動學逆解，且逆解存在與否難以直觀判斷。因此，傳統的在笛卡爾空間進行軌跡規劃，并利用代數逆解法求取目標位姿對應的關節角度值的方法對于五自由度機械臂的運動算法設計不太適合。

1 機械臂模型及正運動學求解

以多關節單電機離散驅動的五自由度機械臂為研究對象，其結構簡圖如圖1所示。機械臂5個關節全部為旋轉關節，其中，3個為彎曲關節，2個為扭轉關節。機械臂驅動方式與機械結構較為特殊，但其運動學模型較為通用，具有一定的代表性。

圖1 機械臂結構

機械臂正運動學求解相對于逆運動學求解較為簡單，只需根據運動學模型，通過對各連桿坐標系變換矩陣進行運算就可以求得末端坐標系在基礎坐標系下的惟一位姿矩陣。為了方便后續幾何法運動學逆解的計算，這里各連桿坐標系的建立未完全按照D－H法［9］進行，具體坐標系建立情況如圖1所示。X0Y0Z0為基礎坐標系，X1Y1Z1，X2Y2Z2，X3Y3Z3，X4Y4Z4，X5Y5Z5分別為5個連桿坐標系，XhYhZh為方便末端軌跡規劃而建立的末端坐標系。圖1中，從基礎坐標系附近開始，5個連桿的長度分別為89.0mm，245.9mm，85.0mm，147.0mm，231.7 mm。圖1對應的機械臂姿態為其機構零位，各關節角度變量的取值范圍以及零位狀態下對應的關節角度初始值如表1所示。

表1 關節轉角變化范圍及當前值 （°）

根據機械臂的運動學模型以及相鄰連桿坐標系間的位姿關系，可以建立相鄰連桿坐標系間的變換矩陣如下：

Ci（i＝1，2，…，5）代表cosθi，Si（i＝1，2，…，5）代表sinθi。將各個坐標系位姿變換矩陣順序相乘，即可得到末端連桿坐標系的位姿矩陣：

n，o，a3個向量描述了機械臂末端連桿的姿態，向量描述了末端連桿的位置，這4個向量的各個元素是5個關節角變量的函數。

2 幾何法運動學逆解

2.1 末端坐標系位姿描述方法

對于五自由度機械臂而言，由于自由度小于6，其可達空間內的目標位置點不能以任意姿態逼近。因此，傳統的采用3個笛卡爾位置坐標和3個歐拉角坐標描述機械臂末端坐標系位姿的方法約束了過多的自由度，在末端軌跡規劃時如用這種描述方式進行變量插補，所得到的目標位姿矩陣無法保證其逆解的存在，同時這種描述方式也缺乏空間直觀性。為了解決這一問題，從機械臂自由度約束的角度出發，采用5個位姿變量描述末端連桿的位姿。由于這種方法的機械臂自由度數目和運動約束數目相同，保證了逆解的存在性。同時，這種描述方式較好的空間直觀性便于利用幾何法進行運動學反解，雖然未完全限制末端連桿的位姿，但沒有進行約束的那個自由度為繞末端連桿坐標系接近矢量［9］的旋轉運動，這種約束方式對很多應用場合下的機械臂適用，如噴涂機械臂，焊接機械臂等對末端執行器繞接近矢量方向的旋轉運動沒有嚴格要求的情況。

基于上述說明，對末端坐標系位姿進行描述時只能限制其5個方向的運動才能保證目標位姿有逆解。首先，以末端坐標系原點在基礎坐標系中的坐標作為機械臂的末端位置坐標，這樣限制了3個移動自由度。然后，拋棄傳統的以末端坐標系相對于基礎坐標系的歐拉角描述機械臂末端姿態的方式，采用末端連桿坐標系的接近矢量相對于基礎坐標系的3個方向余弦角中的任意兩個去描述機械臂末端姿態。3個方向余弦角變量相關，根據方向余弦角公式，其中任意一個方向余弦角可由另外兩個方向余弦角求出。因此，可以用任意兩個方向余弦角描述機械臂末端姿態。這樣，就實現了以5個變量描述機械臂末端的位姿。位姿變量示意圖如圖2所示，其中，α，β，γ分別為末端坐標系接近矢量相對于基礎坐標系X，Y，Z3個軸的余弦角，pn向量為末端執行器的位置坐標向量，xn，x′n為末端坐標系接近矢量。

圖2 位姿變量

基于上述約束自由度的思想，通過對軌跡節點上的末端坐標系原點相對于基礎坐標系的3個位置坐標（px，py，pz）以及末端坐標系接近矢量相對于基礎坐標系的3個方向余弦角（α，β，γ）中的任意兩個所組成的五維位姿變量進行插補規劃，然后利用幾何法進行逆運動學求解即可得到對應的關節角空間解，且逆解一定存在。

2.2 幾何法運動學逆解

機械臂各連桿的空間位置示意圖如圖3所示，從圖中可以看出，無論5個關節角度變量取何值，連桿1、連桿2、連桿3、連桿4、連桿5的坐標系原點始終在同一平面上，該平面的位置由且僅由θ1確定。

圖3 連桿空間位置

由于該機械臂幾何構型的特殊性，如果已知了連桿5坐標系的位置坐標以及連桿5坐標系接近矢量的方向即可方便地通過幾何求解的方法求解出各個關節角變量。已知條件為連桿5坐標系全局位置坐標以及連桿5坐標系接近矢量x5（對應圖1中X5Y5Z5坐標系的X軸，后面相似向量對應情況相同），具體求解流程簡述如下：

a.由連桿5坐標系的位置坐標直接求解得到θ1。

b.計算連桿5坐標系在連桿1坐標系中的位置坐標，由此坐標以及連桿2、連桿3和連桿4的長度計算出θ2，θ3。

c.由θ1，θ2，θ3求出連桿4坐標系軸向量z4，由z4及x5求出z5，進而求出θ4。

d.利用連桿5坐標系軸向量x5相對于連桿4坐標系方向求解出θ5。

假設目標末端連桿位姿矩陣為：

此時，p5（px，py，pz），x5（nx，ny，nz），為已知量。由p5可直接求得：

或者

為了將后續計算轉化為平面計算，求出連桿5坐標系在連桿1坐標系中的位置坐標p′5（p′x，p′y，p′z）：

或者

至此，已求出θ1，θ2，θ3。由此，可求出連桿4坐標系 向 量 z4（a4x，a4y，a4z）。a4x＝C1S3－2；a4y＝S1S3－2；a4z＝C3－2。

當z4與x5平行時，可取θ4＝θ5＝0°。

當z4與x5不平行時，利用z5為z4與x5的公垂線求解出z5，進而確定了連桿5坐標系的完整位姿。由于z5的雙向性，z5（a5x，a5y，a5z）有2組解：

得到連桿5坐標系的完整位姿后，通過連桿5坐標系相對于連桿3坐標系、連桿4坐標系的位姿，可分別求出：

至此，5個關節角度變量完全求出，共有8組解，可以根據關節角變量取值范圍以及運動連續性找出合適的惟一解。

3 仿真實驗

為了驗證上述算法的可行性以及正確性，對一條指定的直線軌跡進行運動追蹤，用Matlab編寫腳本程序進行運動規劃以及運動學逆解，并通過三維圖像的形式將生成軌跡與實際期望軌跡進行對比。

在該仿真實驗中，以末端坐標系XhYhZh為對象進行運動規劃，軌跡起始點處末端坐標系位置坐標為 （354.0，173.0，403.0），Xh軸 方 向 角 為（29.27°，68.38°，71.25°），軌跡終止點處末端坐標系位置坐標為（343.0，286.0，453.0），Xh軸方向角為（34.46°，84.19°，56.17°）。由于每個目標位姿可能對應8組逆解。因此，需要尋求合理的惟一解，可以利用關節角度變量取值范圍以及運動連續性進行最優解的尋找。此處運動連續性判斷公式［10］是，Δ取最小時對應的解即為最優解：

l1，l2，l3，l4，l5為加權因子，由于靠近基座的關節變化對末端影響較大。因此，對應的加權因子也取較大值，這里l1＝l2＝l3＝1，l4＝l5＝0.5。

下面對位置坐標以及方向角分別進行線性插補。對于方向角的坐標，只對方向向量與X，Y軸的余弦角α，β進行線性插補，方向向量與Z軸的余弦角γ由α，β計算得出。插補后位置坐標ph（pxh，pyh，pzh）和Xh軸方向角（αh，βh，γh）為：

T為運動周期，令T＝10s，γh取跟前一次插補最接近的值。為了方便逆運動學求解的進行，需求出關節5坐標系對應的位置坐標p5（px，py，pz），以及連桿5方向向量x5（nx，ny，nz）。

位置追蹤結果如圖4所示，機械臂整體姿態如圖5所示，求解出的關節角度值如圖6所示。

圖4 位置追蹤結果

圖5 機械臂整體姿態

圖6 關節角度變化曲線

從仿真結果可以看出，仿真生成位置與期望位置吻合程度較好，證明了該算法的可行性與精確性，從理論角度來看沒有誤差。同時，通過運動學逆解求出的關節空間解在關節變量可達空間內，且無突變，即關節運動較為平滑。

4 結束語

針對五自由度機械臂因自由度不足而導致的軌跡規劃所產生的目標位姿不存在運動學逆解的問題，提出了一種在軌跡規劃階段新的末端位姿描述方法，并結合機械臂本身自由度少、結構簡單的優勢，采用幾何法進行逆運動學求解，求解過程較為簡單，且不存在理論誤差。在實際實施過程中，由于該算法計算復雜度低。因此，利用計算機編程的方式實現該算法十分方便，實時性好。雖然該算法針對特定的機械臂構型提出，但這種采用約束自由度的思想進行末端位姿描述，并采用幾何法求解目標末端位姿對應的關節角空間解的思路，對于一般的五自由度機械臂有一定的通用性。

［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］ 熊有倫.機器人學，2版［M］.北京：機械工業出版社，1993.

［10］ 陳 鵬，劉 璐，余 飛，等.一種仿人機械臂的運動學逆解的幾何求解方法［J］.機器人，2012，34（2）：211－216.