超冗余全方位移動操作臂的逆運動學(xué)求解

徐慧娜，閆繼宏，于振中，陳志峰，趙 杰

(哈爾濱工業(yè)大學(xué)機器人研究所，哈爾濱150080，xuhuina511@yahoo.com.cn)

全方位移動操作臂與固定基座的操作臂相比，不但擴大了操作空間，而且大大增強了操作靈活性，提高了適應(yīng)復(fù)雜作業(yè)及多任務(wù)需求的能力.但是冗余度機器人的逆運動學(xué)求解也更加復(fù)雜，如何尋找一種能夠兼顧優(yōu)化、速度和精度的算法成為關(guān)鍵.

近年來國內(nèi)外對于冗余度機器人逆運動學(xué)求解方法已經(jīng)有了一定的研究［1］.傳統(tǒng)算法有梯度投影法［2］和最小范數(shù)解［3］等.Galicki等［4］研究了考慮控制反饋的逆運動學(xué)求解方法.美國CESAR機器人［5］的逆運動學(xué)用Jacobian矩陣分塊等方法，簡化了梯度投影法中求Jacobian矩陣的逆.JIA等［6］基于關(guān)節(jié)極限及位形優(yōu)化提出一種簡化的逆運動學(xué)控制方法.Ma等［7］對操作臂末端自由度進行合理分配，并引入兩個約束條件，極大地方便了逆運動學(xué)的求解.劉達等［8］針對不存在解析逆運動學(xué)解的機器人結(jié)構(gòu)，提出一種解析與數(shù)值相結(jié)合的求逆解算法.祖迪等［9］在梯度投影法的基礎(chǔ)上用固定關(guān)節(jié)法進行二次計算.葉平等［10］提出了一種冗余度機器人自運動變量選取的新方法.以上方法各有優(yōu)缺點，但都無法兼顧求逆解的優(yōu)化性、快速性和精確性.

本文針對所研制的9自由度超冗余全方位移動操作臂，提出了一種快速準確優(yōu)化的逆運動學(xué)求解方法.該方法根據(jù)機構(gòu)特點在位姿分離的基礎(chǔ)上，對自由度合理分組，分別基于關(guān)節(jié)等效和局部優(yōu)化的原則解除冗余，從而實現(xiàn)快速準確地求解.抓取目標物體的仿真實驗證實了該方法的有效性.

1 全方位移動操作臂運動學(xué)模型

如圖1所示，該移動操作臂由一個全方位移動平臺和一個固定在其上的輕型操作臂組成.其中，全方位移動平臺在平面內(nèi)具有3個自由度(x，y，φ);操作臂具有6個自由度θi(i=1，2，…，6).桿件參數(shù)及坐標系如圖2所示，{OW}和{OB}分別表示世界坐標系和固定在移動平臺上的坐標系，{Oi}(i=0，1，…，6)表示操作臂上的D－H坐標系.

圖1 全方位移動操作臂實物照片

圖2 全方位移動操作臂運動坐標系

根據(jù)圖2可知，全方位移動操作臂的正運動學(xué)模型可表示為

其中:

這里，c θi≡cos θi，s θi≡sin θi

2 全方位移動操作臂逆運動學(xué)分析

移動操作臂末端3個關(guān)節(jié)的軸線相交于一點(腕點W)，共同決定末端手爪的姿態(tài);其余6個關(guān)節(jié)決定腕點的位置.因此，只有在位置逆運動學(xué)的求解中才存在冗余自由度.下面將重點論述如何用一種簡單、優(yōu)化的方法來實現(xiàn)位置逆運動學(xué)的求解.

由圖2及式(1)可知，操作臂末端手爪在世界坐標系{OW}下的位姿矩陣可表示如下:

其中，(x，y，φ，θ1，θ2，θ3)決定腕點的位置，(θ4，θ5，θ6)決定手爪的姿態(tài).決定位置的6個變量可以分為兩組:

1)腕點W在xWoWyW平面內(nèi)的投影位置由全方位移動平臺的平動自由度x和y，轉(zhuǎn)動自由度φ以及操作臂的第一個回轉(zhuǎn)自由度θ1實現(xiàn).

2)腕點W沿zW方向的高度由操作臂的第2、第3個轉(zhuǎn)動自由度θ2，θ3來實現(xiàn).

對于以上兩組自由度變量，下文將采取兩種不同的方法分別處理.

2.1 關(guān)節(jié)等效法

全方位移動平臺的轉(zhuǎn)動自由度φ與操作臂的第一個回轉(zhuǎn)自由度θ1的旋轉(zhuǎn)軸方向相同，兩關(guān)節(jié)具有等效性.在實際控制時，可以固定其中一個關(guān)節(jié)不動.臂的初始位置如圖2所示，為了保持操作臂始終位于車的正前方，以擁有較大的操作空間，可設(shè)定θ1=0.根據(jù)目標物體的位置坐標(px，py，pz)，不妨可以預(yù)先設(shè)定:

其中(x0，y0，0)為移動平臺的當(dāng)前位置.于是根據(jù)關(guān)節(jié)等效的原理簡化了2個自由度，決定位置的自由度數(shù)由6個減為4個.

2.2 局部優(yōu)化法

操作臂的第2，3個轉(zhuǎn)動自由度θ2和θ3的旋轉(zhuǎn)軸方向相同，共同決定腕點W在zW方向的高度.但是，這兩個關(guān)節(jié)在增強操作臂的靈活性方面具有很重要的作用，故采用優(yōu)化的方法處理.首先根據(jù)目標物體的位姿矩陣求出末端手爪達到該位姿時腕點的高度HW，然后建立關(guān)于θ2，θ3的優(yōu)化模型.

2.2.1 求取腕點高度

3個關(guān)節(jié)旋轉(zhuǎn)軸相交于一點的腕部，相當(dāng)于一個球鉸.在根據(jù)目標物體的位姿矩陣求取腕點的高度時，可以先將前面的關(guān)節(jié)與腕部3個關(guān)節(jié)相分離，然后再將其與腕部球鉸連接上即可，不會影響末端手爪的位姿.具體求解方法如下:

在腕點W上建立基準坐標系{OW＇}，{OW＇}的原點與腕點W重合，各坐標軸與世界坐標系{OW}平行，如圖3所示.

圖3 腕部坐標系示意圖

末端手爪在腕點基準坐標系{OW＇}下的位姿矩陣可表示為

其中:

(為了計算方便，可假設(shè){O3}移至腕點)，4T5，5T6見式(1)，代入式(3)可求得

式中，ci≡cos θi;si≡sin θi.

如前文所述，末端手爪在世界坐標系{OW}下的位姿矩陣為

由于{OW'}與{OW}各坐標軸平行，所以二者的姿態(tài)矩陣相等，即R'3×3=R3×3.據(jù)此可求出θ'4，θ'5和θ'6，從而求得末端手爪在腕點基準坐標系{OW'}下的位置坐標(p'x，p'y，p'z).

由圖3可知，腕點W在zW方向的高度:

需要注意的是，θ'4，θ'5和 θ'6表示坐標系{O3}固定，手爪達到目標姿態(tài)時腕部3個關(guān)節(jié)需要運動的角度.但是實際上{O3}是隨著前面幾個關(guān)節(jié)變化的，所以應(yīng)該在求出θ2和θ3后再代入式(1)求解θ4、θ5和θ6.二者之所以數(shù)值不同，只是由于相對的基準坐標系不同，實際腕部3個關(guān)節(jié)的位姿并沒變.所以，由此求出的HW是可靠的.

2.2.2 建立優(yōu)化模型

根據(jù)圖2所示，HW只與θ2和θ3兩個變量有關(guān)，三者滿足以下關(guān)系式:

式中，d0，d1，a2，d4為系統(tǒng)結(jié)構(gòu)參數(shù)，是已知量.

為了降低系統(tǒng)能耗，并使操作臂在工作過程中保持最好的柔順性，以關(guān)節(jié)角運動幅度最小為優(yōu)化指標.建立的目標函數(shù)為

式中:θic為關(guān)節(jié)i運動范圍的中值;θim為關(guān)節(jié)i運動范圍的一半.在該全方位移動操作臂系統(tǒng)中，θ2和θ3的運動范圍均為(－π/2，π/2).這樣，操作臂第2，3個關(guān)節(jié)θ2和θ3的優(yōu)化求解便轉(zhuǎn)化為有兩個未知數(shù)和一個約束條件的二元函數(shù)的條件極值問題.相比Dubby對CESEAR七自由度機器人逆運動學(xué)提出的梯度投影算法(計算量為141個乘法，102個加法)，顯然大大減小了計算量，保證了實時性的要求.

經(jīng)過等效關(guān)節(jié)和局部優(yōu)化求解之后，該系統(tǒng)的9個自由度便只剩下5個自由度是未知的.具體求解過程同普通的非冗余機器人逆解求解方法，在此不再贅述.以上所求的逆解均為精確的解析解，滿足了準確性的要求.

3 仿真實驗

設(shè)目標矩陣為

假設(shè)初始時刻移動平臺位于世界坐標系原點，抓取過程中無障礙，采用點到點(PTP，pointto-point)的軌跡規(guī)劃方法，時間為5 s.注意到:如果是固定基座的操作臂，末端手爪無法到達目標點.所以全方位移動平臺極大地擴展了操作臂的工作空間.

目標點的位置坐標(5 000，5 000，1 000)，由式(2)可得φ=－π/4.根據(jù)上述逆運動學(xué)求解方法求得其他各個關(guān)節(jié)變量，采用基于關(guān)節(jié)變量描述的插值算法得到各關(guān)節(jié)的MATLAB仿真曲線如圖4所示.

圖4 各關(guān)節(jié)的位移曲線

其中，θ2，θ3，θ5的運動范圍為(－π/2，π/2)，θ4，θ6的運動范圍為(－π，π)，由圖4可知各關(guān)節(jié)均未超出其運動范圍，且曲線連續(xù)平滑，可以實現(xiàn)平穩(wěn)抓取.

末端手爪軌跡的三維視圖及分別在xW，yW，zW方向的視圖如圖5所示.

由圖5可以看出利用該逆解算法可以實現(xiàn)末端手爪對目標點的準確抓取.

在運算時間方面，在Pentium(R)4 2.4GHz處理器上采用Windows XP操作系統(tǒng)對該逆運動學(xué)求解方法進行了時間測試.通過多次計算求均值的方法估算每次求解耗時6 ms左右，控制周期約為20 ms，完全可以滿足一般機器人系統(tǒng)的實時性要求.

圖5 末端手爪的位移曲線

4 結(jié)論

1)針對一類9自由度超冗余全方位移動操作臂，在位姿分離的基礎(chǔ)上，分別利用關(guān)節(jié)等效和局部優(yōu)化的方法處理不同的冗余自由度.

2)提出了一種根據(jù)手爪姿態(tài)求取腕點位置的方法，從而把關(guān)于兩個變量的優(yōu)化問題轉(zhuǎn)化為二元函數(shù)的條件極值問題，提高了優(yōu)化計算的速度.

3)仿真實驗證明該方法能夠在兼顧優(yōu)化的前提下快速地求取解析逆解，對冗余度移動操作臂的逆運動學(xué)求解具有一定的通用性.

［1］劉迎春，余躍慶，姜春福.冗余度機器人研究動向［J］.機械設(shè)計與研究，2003，19(1):23－27.

［2］LIEGEOIS A.Automatic supervisory control of the configuration and behavior of multibody mechanisms［J］.IEEE Trans，System，Man，Cybemetics，1977，7(12): 868－871.

［3］CHAN T F，DUBEY R V.A weighted least-norm solution based scheme for avoiding joint limits for redundant manipulator［C］//Proceedings of IEEE International Conference on Robotics and Automation.Atlanta，USA:［s n.］，1993:395－402.

［4］GALICKI M.Control-based solution to inverse kinematics for mobile manipulators using penalty functions［J］.Journal of Intelligent and Robotic Systems，2005，42 (3):213－238.

［5］DUBEY R V.Real-time implementation of an optimization scheme for seven-degree-of-freedom redundant manipulators［J］.IEEE Tr on Robotics and Automation，1991，7(5):579－588.

［6］JIA Qing-xuan，ZHAN Qiang，SUN Han-xu，et al.Implementation and kinematic control of a hyper-redundant mobile manipulator System［J］.Chines He Journal of Aeronautics，2006，19(1):83－88.

［7］MA Bojun，F(xiàn)ANG Yongchun，ZHANG Xuebo.Inverse kinematics analysis for a mobile manipulator with redundant DOFs［C］//Proceedings of the 26th Chinese Control Conference.Zhangjiajie，China:［s n.］，2007:118－122.

［8］劉 達，王田苗.一種解析與數(shù)值相結(jié)合的機器人逆解算法［J］.北京航空航天大學(xué)學(xué)報，2007，33(6): 727－730.

［9］祖 迪，吳鎮(zhèn)煒，談大龍.一種冗余機器人逆運動學(xué)求解的有效方法［J］.機械工程學(xué)報，2005，41(6):71－75.

［10］葉 平，孫漢旭，張秋豪.基于自運動控制的冗余機器人運動學(xué)優(yōu)化［J］.機械工程學(xué)報，2004，40(12): 128－132.