6R工業機器人運動學算法的改進*

吳 挺，吳國魁，吳海彬

(福州大學機械工程及自動化學院，福建福州350108)

0 引 言

機器人運動學在機器人運動控制中扮演著重要角色。機器人特別是工業機器人主要用于完成一定的作業任務，在完成任務過程中不可避免地要進行關節坐標空間與笛卡爾等坐標空間的相互轉換，不可避免地要進行正運動學、逆運動學的求解以及插補算法等運動學方法的實現。機器人運動學的作用不言而喻。

機器人運動學涵蓋面較廣，本研究只研究其中較為重要的運動學逆解及其優化選取、插補算法及一些常用坐標系的建立方式。目前，運動學逆解的方法主要有數值迭代法［1］、幾何法［2-3］，神經網絡法［4］及遺傳算法［5］等方法。機器人運動學逆解算法復雜，數值迭代法依賴于初始點，收斂速度較慢。幾何法依賴于機器人的機構模型。遺傳算法涉及大量個體的計算，難以滿足控制系統實時性的要求。在對逆解的選取時有學者提出一種基于“最短行程”的方法［6］，但該方法忽略了機械手當前狀態，且需要求出機器人的所有解，實時性不高。在插補算法方面也有不少學者提出一些插補算法［7］，但一般只介紹位置插補的實現，對于姿態插補卻沒有給出明確的算法。

本研究采用分離變量法求取逆解，提出一種基于機器人幾何結構及解的特點的逆解優化方法，該方法無需求出機器人的所有逆解便可選擇出較好的逆解，可節約機時，提高實時性，在插補算法中同時綜合位置與姿態插補，以滿足軌跡規劃中對位置和姿態的要求。

1 工業機器人運動學逆解的求法

1.1 逆解求解過程

本研究研究的6自由度工業機器人如圖1所示，根據D-H法［8］對機器人建立坐標系。為了計算方便，這里在機器人末端三軸交匯處建立了坐標系z6－x6，并在執行器末端建立了與z6－x6固連的坐標系z7－x7。因此，坐標系7并非按照D-H法建立。機器人DH法關節連桿參數表，如表1所示。

圖1 機器人D-H表示法參考坐標

表1 機器人D-H參數表

依照D-H表示法原理，可得出相鄰坐標系間的變化矩陣，記為:A1、A2、A3、A4、A5、A6。6 個變換矩陣可按下式計算:

由于執行器形狀的不同，坐標系7的姿態和位置也會不同。但對于一定執行器，坐標系6和坐標系7的→關系是固定的。坐標系7可由坐標系6經過平移變換，再依次繞自身坐標系的 x，y，z軸旋轉3個角度φx，φy，φz得到，這些參數可以由執行器的幾何結構得到。故變換矩陣A7可表示為:

給定末端位姿矩陣T:

解方程T=A1A2A3A4A5A6A7的過程即為求逆解過程。注意到 A7為常量矩陣，可在方程 T=A1A2A3A4A5A6A7兩邊同時右乘A7的逆矩陣，得到:

式(3)中的參數均由TA7－1計算得來，這里為了書寫方便，將其各用一個參數替代。對A7處理后，相當于將圖1中執行器末端坐標系原點等效到了機器人末端三軸交匯處。于是逆解的過程也轉化為求T1=A1A2A3A4A5A6的過程，不必再考慮坐標系z7－x7。

易知手腕3個轉動關節軸線交于一點，滿足Pieper給出的具有封閉解的條件［9］，故可用如下方法求逆解。由于運動方程中有許多角度的耦合，研究者不能從矩陣中提取足夠的元素來求解單個的正弦和余弦來計算角度。為使角度解耦，可以用矩陣T1左乘矩陣An－1，進行變量分離，再對比左右矩陣求得相應角度。

可得如下方程:

比較方程左右兩邊，由兩邊矩陣中元素(3，4)相等得到:

考慮式(4)左右兩邊矩陣中元素(1，4)和(2，4)相等，并計算化簡可得:

考慮式A2－1A1－1T1=A3A4A5A6左、右兩邊矩陣元素(1，4)和(2，4)相等，可得:

或者:

考慮式 A4－1A3－1A2－1A1－1T1=A5A6左右兩邊矩陣中元素(3，1)和(3，2)相等可得:

1.2 逆解優化選取

從以上求取逆解的過程可以看出，對應于某一位置，可以有8組不同的封閉解。但實際的機器人運動控制只能選擇其中一組解，并且關節角轉角是有范圍限制的，同時，在軌跡運動時，如何使各關節變化平緩，減少機器人工作過程中角度突變，也對解的選擇提出了要求。

在進行軌跡跟蹤時，為了減小關節運動的時間，使各關節變化平穩，應使目標關節角與當前關節角盡可能接近。可以采用下式來選取逆解:

式中:θigoal—第i關節角的目標關節角，θinow—第i關節角的當前角度。

選取關節角度時，所要的關節角應滿足使K值最小。但該法需要算出機器人的所有封閉解，從中選擇出使K值為最小的解，這樣會浪費機時去求無用解，且算法較為復雜，效率不高。

可以從機器人結構及解的特性來簡化算法。在運動中為使機器人有較好的軌跡跟蹤精度，則必然要求每次轉過的關節角較小。從上文求出的θ1來看，θ1對應的兩個解相差180°，從中可以得到一個與當前θ1較接近的解，另一個解可以直接舍棄。選定了θ1后，θ3只有兩個解且θ3的兩個解相差的角度依賴于θ3的大小，θ2完全依賴于θ3和θ1，因此可以求出這兩組解，采用下式來選取使J最小的解:

θ4的兩個解也相差180°，選擇與當前角度相差較小的解，否則從圖1中工業機器人的結構可以看出后面的兩個關節角將產生突變，這是不允許的。θ5和θ6完全取決于前面的解。至此，已選出一組較好的解。該法可以避免去解不符合要求的解，提高效率，簡化算法。具體流程如圖2所示。

圖2 逆解選取流程

2 插補算法

軌跡規劃在機器人運動控制中具有重要的作用，對機器人的作業有著重要的影響。本研究所述軌跡規劃是在笛卡爾坐標系下進行的，必須運用插補算法并反復求解逆運動學方程來計算關節角。插補算法在軌跡規劃中具有重要的地位，因此研究它具有重要的意義。在笛卡爾坐標空間中描述機器人末端執行器，包括位置描述和姿態描述，因此需要對位置進行插補，同時也要對姿態進行插補。

總體原則是:在進行插補時，將起點和終點間的變換分解為一個平移(位置插補)和一個繞空間中某根軸k的旋轉(姿態插補)。平移實現末端執行器坐標系原點的移動，旋轉實現末端執行器姿態的改變，兩變換同時進行。

2.1 位置插補算法

空間曲線的位置插補可以近似地等效為由直線段和圓弧段所組成，對應的位置插補就是直線插補和圓弧插補。

對直線插補，設直線的起點坐標為A(XA，YA，ZA)，終點坐標為B(XB，YB，ZB)，選定插補次數N，則有:

于是直線的插補點(xi，yi，zi)可以表示為:

其中:0≤i≤N。

對圓弧插補，設該圓弧為由空間中任意不共線的3點A、B、C所組成的，示意圖如圖3所示。

圖3 圓弧插補示意圖

易得線段AB的中垂面、線段BC的中垂面如下:

以及點ABC所在平面的方程如下:

聯立以上3個方程，解方程便可得到圓弧的圓心坐標(x0，y0，z0)，進而可得到圓弧半徑R和圓心角θ。給定插補次數N，則可得步距角δ=θ/N。為求插補點坐標可按圖3所示過Pi點做切線PiP'i+1，進而得到圓弧上的點Pi+1，該遞推公式已有學者推出［6］。

2.2 姿態插補算法

根據曲線特點可將姿態插補分為有中間點約束的姿態插補(如圓弧插補)和無中間點約束的姿態插補(如直線插補)。

對無中間點約束的插補，設起點到終點的姿態轉換矩陣為T，并將T擴充為一個齊次矩陣R。設R為:

則可將該旋轉變換(矩陣R)轉化為繞空間中某個k軸旋轉θ角得到，得到的k軸單位向量(u1，u2，u3)和轉角 θ可表示如下［10］:

此外，可以得到繞k軸轉過某個角度θ1得到的轉化矩陣T(θ1):

于是姿態的插補便可轉化為轉角θ的插補，插補次數為N，則有:

插補點的姿態可以表示為:

式中:0≤i≤N，TA—直線初始點的姿態。

將插補點的位置與姿態結合起來，便可得到插補點的位姿矩陣，再利用逆運動學方程便可解出各個關節角。

對有中間點約束的姿態插補，如圖3中的圓弧插補，考慮中間點B的姿態，可按下述方法進行。由下式可得AB弧所對圓心角θAB:

對NAB=NθAB/θ取整，可得AB弧的插補次數NAB，BC弧插補次數NBC=N－NAB。由此便可分別對AB弧和BC弧進行姿態插補，方法同直線插補。

2.3 位置和姿態插補綜合

軌跡插補需要有位置和姿態信息才能進行，需要將插補點的位置與姿態結合起來，得到插補點的位姿矩陣，再用逆運動學方程便可解出各個關節角。由上文可以得到插補點的位置Pi(xi，yi，zi)T，及姿態矩陣Ti，由此便可得到插補點的位姿矩陣Ri:

3 不同位姿表達方式之間的轉換

工業機器人的運動學控制，位姿狀態的表述通常采用位姿矩陣，但是位姿矩陣中姿態參數較多且不相互獨立，在做速度或力矩控制時，往往由于雅克比矩陣無法從位姿矩陣直接通過計算得到而帶來麻煩。而如果采用相互獨立的6參數表示機器人末端執行器的位姿狀態，并建立該6參數分別與6個關節角之間的函數關系式，則可以較方便地得到雅克比矩陣。這6個參數分別是:dx，dy，dz表示執行器坐標系原點坐標，3個角度 φ1，φ2，φ3分別表示繞參考坐標系的 x，y，z軸轉過的角度。它可以較為形象地表達機器人的位姿。

3.1 由旋轉參數轉化為位姿矩陣

記Rot(d，θ)表示為繞d軸旋轉θ角得到的位姿矩陣。則按順序依次繞參考坐標系x，y，z軸旋轉后所得的的矩陣R可按下式計算:

3個位置參數可以看成，旋轉后再相對于參考坐標系平移(dx，dy，dz)，于是最終的位姿可按下式計算:

3.2 由位姿矩陣轉化為旋轉參數

設機器人位姿矩陣為T:

則有T=R，比較矩陣兩邊元素，可得:

式中:T(a，b)—矩陣T中第a行第b列元素。

此外，矩陣T可由機器人正運動學得到，設6個關節角分別為 θ1，θ2，θ3，θ4，θ5，θ6，則矩陣 T 中元素均可表示為 θ1，θ2，θ3，θ4，θ5，θ6的函數。將其代入式(25)中，可得:

對式(26)求微分，則可直接得到雅克比矩陣，并可用于機器人的速度控制。

4 用戶坐標系的建立

對用戶來說，工業機器人參考坐標系的建法未必很明確和實用，參考坐標系往往不方便于空間定位，且隨著環境改變，描述操作對象的坐標系也應隨之而變。為了能夠方便地操作機器人，更好地實現工作空間中的定位，更好地規劃機械手的運動，有必要找到一個能夠建立坐標系并實現其與機器人參考坐標系相互轉化的方法。

為了建立這樣一個坐標系，用戶需要給定3個適當的點，設為A，B，C，其中點A為用戶要建立的坐標系原點，B為用戶建立的坐標系x軸正向上一點，C為用戶建立坐標系的xy平面中的一點。用戶坐標系的建立如圖4所示。

圖4 用戶坐標系的建立

得到用戶坐標系后，就可以比較方便地確定空間中的點在該坐標系下的坐標。為了讓機器人識別這些點，需要研究用戶坐標系與機器人參考坐標系的相互轉化。設用戶坐標系為U，機器人參考坐標系為S，用符號STU表示坐標系U相對于坐標系S的轉化矩陣。已知用戶坐標系下的點UP，可由式轉化為機器人參考坐標系下的點的坐標。同理由UP=可將機器人參考坐標系下的點轉化為用戶坐標系下。若已知用戶坐標系下的位姿矩陣UTR，可由式得到其在機器人參考坐標系下的位姿矩陣。由式可將機器人參考系下的位姿轉化為用戶坐標系下的位姿。

5 實 驗

實驗借助一臺工作半徑為650 mm的垂直6關節工業機械手，將以上算法編成動態鏈接庫，通過VC++程序調用動態鏈接庫來控制機械手的運動。在機械手做直線插補過程中，采集空間位置坐標和6個關節速度，并與原出廠程序控制的機械手的位置和速度進行對比。采集得到的位置坐標如圖5所示。本研究算法得出的軌跡如圖5(a)所示，機械手的出廠程序得出的軌跡如圖5(b)所示。

圖5 空間坐標軌跡對比圖

從圖5可以看出，圖5(a)的直線跟蹤能力比圖5(b)強。由此可得，改進后的逆運動學和插補算法是可行的，并且比原算法具有更強的直線軌跡跟蹤能力。

機械手在做直線插補時前4個關節的速度曲線圖如圖6所示，采用本研究提出的方法得到的曲線如圖6(a)所示，采用原出廠程序得到的曲線如圖6(b)所示。

圖6 各軸速度曲線對比圖

圖6(a)、6(b)所對應的笛卡爾空間軌跡是完全一樣的。從圖6(b)可以看到，在一段較長時間內，第四軸的速度要比其他三軸速度大很多，因此，第四軸每個插補點所轉過的角度比其他軸都大很多，有可能產生突變，導致插補的精度下降，同時插補時間也明顯增加(約為圖6(a)的2倍)。圖6(a)中各軸速度較為均勻，不會出現某一軸速度長時間大于其他軸，由此可知各軸插補角度較為均勻，有效地減少了角度突變的可能。由于各插補角度較為均勻，可實現較小步距角的插補，有利于提高精度。

6 結束語

本研究針對6自由度工業機器人，采用分離變量法求其運動學逆解，并根據逆解的特點和機器人的幾何結構從逆解中選取最優逆解，將曲線插補中的姿態插補轉化為對繞空間中的某根軸旋轉的角度的插補，并將以上逆解的改進算法運用到插補算法中。實驗結果表明，機器人曲線插補更為精確，有效地避免了各關節角度突變。筆者給出了6參數位姿表示法與位姿矩陣之間的變換算法，由此可以得到雅克比矩陣，用于速度或力矩控制。用戶坐標系的建立，使機器人運動空間的定位更加符合和方便用戶需要，極大方便了用戶的使用。

改進后的算法也存在一些缺點，當機械手的第四軸的旋轉范圍小于(－180°，+180°)且機械手插補過程中經過奇異點時，容易出現旋轉角度超出范圍，這些缺點有待于進一步研究解決。

［1］MANOCHA D，CANNY J F.Efficient inverse kinematics for general 6R manipulator［J］.IEEE Transactions on Robotics and Automation，1994，10(5):648-687.

［2］李友虎，葉伯生，朱志紅.基于幾何法的機器人運動學逆解［J］.武漢船舶職業技術學院學報，2002(1):9-11.

［3］NEPPALLI S，CSENCSITS M A，JONEB B A.A geometrical approach to Inverse Kinematics for Continuum manipulators［C］//Intelligent Robot and systems，2008:3565-6570.

［4］KARLIK B，AYDIN S.An improved approach to the solution of inverse kinematics problems for robot manipulators［J］.Engineering Applications of Artificial Intelligence:The International Journal of Intelligent Real-Time Automation，2000，13(2):159-164.

［5］劉永超，黃玉美，王效岳，等.基于遺傳算法的機器人運動學逆解［J］.機器人，1998，20(6):421-426.

［6］王 偉，謝明紅，周國義.6-DOF工業機器人逆解優化及其工作空間的研究［J］.機械與電子，2011(1):57-60.

［7］卓揚娃，白曉燦，陳永明.機器人的三種規則曲線插補算法［J］.裝備制造技術，2009(11):27-29.

［8］NIKU S B.Introduction to Robotics:Analysis，Systems，Applications［M］.Beijing:Publishing House of Electronics Industry，2004.

［9］PIEPER D L.The kinematics of manipulators under computer control［D］.Stanford:Stanford University cdlege of Engineer，1968:151-157.

［10］殷際英，何廣平.關節型機器人［M］.北京:化學工業出版社，2003.