吳 挺，吳國魁，吳海彬

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

0 引 言

機器人運動學(xué)在機器人運動控制中扮演著重要角色。機器人特別是工業(yè)機器人主要用于完成一定的作業(yè)任務(wù)，在完成任務(wù)過程中不可避免地要進(jìn)行關(guān)節(jié)坐標(biāo)空間與笛卡爾等坐標(biāo)空間的相互轉(zhuǎn)換，不可避免地要進(jìn)行正運動學(xué)、逆運動學(xué)的求解以及插補算法等運動學(xué)方法的實現(xiàn)。機器人運動學(xué)的作用不言而喻。

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

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

1 工業(yè)機器人運動學(xué)逆解的求法

1.1 逆解求解過程

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

圖1 機器人D-H表示法參考坐標(biāo)

表1 機器人D-H參數(shù)表

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

由于執(zhí)行器形狀的不同，坐標(biāo)系7的姿態(tài)和位置也會不同。但對于一定執(zhí)行器，坐標(biāo)系6和坐標(biāo)系7的→關(guān)系是固定的。坐標(biāo)系7可由坐標(biāo)系6經(jīng)過平移變換，再依次繞自身坐標(biāo)系的 x，y，z軸旋轉(zhuǎn)3個角度φx，φy，φz得到，這些參數(shù)可以由執(zhí)行器的幾何結(jié)構(gòu)得到。故變換矩陣A7可表示為:

給定末端位姿矩陣T:

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

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

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

可得如下方程:

比較方程左右兩邊，由兩邊矩陣中元素(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 逆解優(yōu)化選取

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

在進(jìn)行軌跡跟蹤時，為了減小關(guān)節(jié)運動的時間，使各關(guān)節(jié)變化平穩(wěn)，應(yīng)使目標(biāo)關(guān)節(jié)角與當(dāng)前關(guān)節(jié)角盡可能接近?？梢圆捎孟率絹磉x取逆解:

式中:θigoal—第i關(guān)節(jié)角的目標(biāo)關(guān)節(jié)角，θinow—第i關(guān)節(jié)角的當(dāng)前角度。

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

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

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

圖2 逆解選取流程

2 插補算法

軌跡規(guī)劃在機器人運動控制中具有重要的作用，對機器人的作業(yè)有著重要的影響。本研究所述軌跡規(guī)劃是在笛卡爾坐標(biāo)系下進(jìn)行的，必須運用插補算法并反復(fù)求解逆運動學(xué)方程來計算關(guān)節(jié)角。插補算法在軌跡規(guī)劃中具有重要的地位，因此研究它具有重要的意義。在笛卡爾坐標(biāo)空間中描述機器人末端執(zhí)行器，包括位置描述和姿態(tài)描述，因此需要對位置進(jìn)行插補，同時也要對姿態(tài)進(jìn)行插補。

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

2.1 位置插補算法

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

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

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

其中:0≤i≤N。

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

圖3 圓弧插補示意圖

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

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

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

2.2 姿態(tài)插補算法

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

對無中間點約束的插補，設(shè)起點到終點的姿態(tài)轉(zhuǎn)換矩陣為T，并將T擴(kuò)充為一個齊次矩陣R。設(shè)R為:

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

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

于是姿態(tài)的插補便可轉(zhuǎn)化為轉(zhuǎn)角θ的插補，插補次數(shù)為N，則有:

插補點的姿態(tài)可以表示為:

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

將插補點的位置與姿態(tài)結(jié)合起來，便可得到插補點的位姿矩陣，再利用逆運動學(xué)方程便可解出各個關(guān)節(jié)角。

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

對NAB=NθAB/θ取整，可得AB弧的插補次數(shù)NAB，BC弧插補次數(shù)NBC=N－NAB。由此便可分別對AB弧和BC弧進(jìn)行姿態(tài)插補，方法同直線插補。

2.3 位置和姿態(tài)插補綜合

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

3 不同位姿表達(dá)方式之間的轉(zhuǎn)換

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

3.1 由旋轉(zhuǎn)參數(shù)轉(zhuǎn)化為位姿矩陣

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

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

3.2 由位姿矩陣轉(zhuǎn)化為旋轉(zhuǎn)參數(shù)

設(shè)機器人位姿矩陣為T:

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

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

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

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

4 用戶坐標(biāo)系的建立

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

為了建立這樣一個坐標(biāo)系，用戶需要給定3個適當(dāng)?shù)狞c，設(shè)為A，B，C，其中點A為用戶要建立的坐標(biāo)系原點，B為用戶建立的坐標(biāo)系x軸正向上一點，C為用戶建立坐標(biāo)系的xy平面中的一點。用戶坐標(biāo)系的建立如圖4所示。

圖4 用戶坐標(biāo)系的建立

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

5 實 驗

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

圖5 空間坐標(biāo)軌跡對比圖

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

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

圖6 各軸速度曲線對比圖

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

6 結(jié)束語

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

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

［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］李友虎，葉伯生，朱志紅.基于幾何法的機器人運動學(xué)逆解［J］.武漢船舶職業(yè)技術(shù)學(xué)院學(xué)報，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］劉永超，黃玉美，王效岳，等.基于遺傳算法的機器人運動學(xué)逆解［J］.機器人，1998，20(6):421-426.

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

［7］卓揚娃，白曉燦，陳永明.機器人的三種規(guī)則曲線插補算法［J］.裝備制造技術(shù)，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］殷際英，何廣平.關(guān)節(jié)型機器人［M］.北京:化學(xué)工業(yè)出版社，2003.