七軸機(jī)械手最小能量直線軌跡算法優(yōu)化*

梁赫西，黃曉東，周　密

（1.湖北師范學(xué)院　教育信息與技術(shù)學(xué)院，湖北　黃石 435002）2.武漢大學(xué)　電子信息學(xué)院，湖北　武漢　430074）

七軸機(jī)械手最小能量直線軌跡算法優(yōu)化*

梁赫西1，黃曉東1，周密2

（1.湖北師范學(xué)院教育信息與技術(shù)學(xué)院，湖北黃石 435002）
2.武漢大學(xué)電子信息學(xué)院，湖北武漢430074）

對比分析了偽逆法和最優(yōu)能量算法的原理，給出了最優(yōu)能量算法的優(yōu)化方案。笛卡爾空間體系中采用齊次矩陣和四元數(shù)法獲得位置和姿態(tài)的插值；利用優(yōu)化后的最優(yōu)能量算法求出逆解，得到與笛卡爾空間中對應(yīng)的在關(guān)節(jié)空間中的關(guān)節(jié)值。文中給出了實際仿真實例，該實例表明本文給出的優(yōu)化方案在滿足正常作業(yè)情況下，相對于偽逆法，方案中四元數(shù)算法在姿態(tài)插值運算上更快并且能量優(yōu)化效果更好，七軸機(jī)械手最小能量消耗節(jié)約10％～20％左右。

七軸機(jī)械手；能量；四元數(shù)

0　引言

近年來，冗余度機(jī)械手末端執(zhí)行器廣泛應(yīng)用于裝配、噴涂、微操作等工業(yè)生產(chǎn)領(lǐng)域。其主要沿直線軌跡進(jìn)行作業(yè)，研究機(jī)械臂直線軌跡能量消耗對其實際應(yīng)用至關(guān)重要。目前在冗余機(jī)械手優(yōu)化方面取得了許多有價值的研究成果，主要集中在逆運動學(xué)優(yōu)化和直線軌跡規(guī)劃兩大方面。參考文獻(xiàn)［1］給出了一個雅可比矩陣的建立實例及其奇異性分析；參考文獻(xiàn)［2］對逆運動學(xué)進(jìn)行了實例分析；參考文獻(xiàn)［3］和參考文獻(xiàn)［4］提出了冗余機(jī)械手的逆運動優(yōu)化的基于拓展的雅可比矩陣的算法；參考文獻(xiàn)［5］中提出了一個求解等式最優(yōu)化的不使用罰函數(shù)或濾子技術(shù)的信賴逐步二次規(guī)劃方法及其全局收斂理論；參考文獻(xiàn)［6］中Liu和Yuan等人優(yōu)化了該理論，并提出了在線搜索逐步二次規(guī)劃方法及其全局和局部收斂性理論。

在直線軌跡的規(guī)劃方面，參考文獻(xiàn)［7-9］提出了利用Hermite插值和條樣、曲線插值等方法來進(jìn)行直線軌跡規(guī)劃；參考文獻(xiàn)［10］和參考文獻(xiàn)［11］則將四元數(shù)法引入到機(jī)械手軌跡規(guī)劃的姿態(tài)規(guī)劃中。但其優(yōu)化效果均不理想。本文對逐步二次規(guī)劃方法與四元數(shù)方法進(jìn)一步優(yōu)化對七軸機(jī)械臂的直線軌跡進(jìn)行研究。通過仿真實例表明，相對于偽逆法，方案中四元數(shù)算法在姿態(tài)插值運算上速度更快并且能量優(yōu)化效果更好，七軸機(jī)械手最小能量消耗節(jié)約10％～20％左右。

1　逆運動學(xué)的優(yōu)化

1.1雅可比矩陣

逆運動學(xué)的優(yōu)化主要利用關(guān)聯(lián)機(jī)械臂末端的速度和關(guān)節(jié)速度的雅可比矩陣J為主要的研究工具來進(jìn)行，其在機(jī)械手運動學(xué)中具有重要的作用。令七軸機(jī)械手的正運動學(xué)方程為：

式（1）表示操作空間x和關(guān)節(jié)空間q之間的函數(shù)關(guān)系，通常用D-H方法來表示。同時對式（1）兩邊對時間t求導(dǎo)，即可得出笛卡爾空間的速度x˙與關(guān)節(jié)空間的關(guān)節(jié)速度之間的關(guān)系：

對于七軸機(jī)械手來說，x∈R6×1，J∈R6×7，q∈R7×1。其中，J是關(guān)聯(lián)笛卡爾空間速度與關(guān)節(jié)空間速度的雅可比矩陣。顯然，式（2）中關(guān)節(jié)速度的反解并不是唯一的。

1.2偽逆法求雅可比反解

在實際情況下，式（2）一般是相容的，即：rank（J|x）=rank（J），于是式（2）中的關(guān)節(jié)速度的通解由雅可比矩陣的偽逆和一組任意的7維向量給出：

其極小范數(shù)解即令‖q‖2最小的解為：

其中J+∈R7×6稱為J的偽逆也稱為加號逆，E∈R7×7，Z是任意7維向量，其中‖·‖2為Euclid范數(shù)，在實際用偽逆法求解逆運動解時，常選取q˙ls為式（2）的反解。

當(dāng)機(jī)械手臂處于非奇異位置時，此時J為行滿秩矩陣J+=JT（JJT）-1；當(dāng)機(jī)械手處在奇異位置時，rank（J）＜6，此時可用最大秩分解法將J分解為Q、R兩個行滿秩或列滿秩矩陣再分別求Q和R的偽逆。這時，J的偽逆可表示為J+=Q+R+，但是在實際運用中上述兩種算法運算量都比較大，一種時間復(fù)雜度更小、應(yīng)用更為廣泛的算法是對J作奇異值分解，則：

實際上，為了獲得唯一解常選取qls作為唯一的反解，即有q=qls=VD+UT，盡管得到了逆運動學(xué)的解，但在以某些運動學(xué)或動力學(xué)指標(biāo)來衡量時，偽逆法并不是最優(yōu)的。

1.3在某些指標(biāo)下的最優(yōu)雅可比反解

解雅可比矩陣的反解其最終目的是通過反解來使機(jī)械手末端在笛卡爾空間內(nèi)沿著指定的軌跡移動，由式（1）可知：

在實際的數(shù)值算法中△x和△q都代表一定的數(shù)值，△t是其運行時間，則對其優(yōu)化問題可以轉(zhuǎn)化為線性方程組下的最值問題。

現(xiàn)設(shè)xs和xn分別為開始時刻ts和當(dāng)前時刻tn的機(jī)械臂末端6個自由度的值，qs和qn分別為開始時刻ts和當(dāng)前時刻tn的7個關(guān)節(jié)空間角度值。并在此時有式（1）成立，則此時可以根據(jù)式（7）來進(jìn)行優(yōu)化。

一般情形下，優(yōu)化指標(biāo)函數(shù)為關(guān)節(jié)角度的二次項形式為：（qn-qs）TH（qn-qs）+c（qn-qs），其中H是正定或半正定矩陣，一般為diag（h1，h2，h3，…，h7），為7×7階的對角陣，c∈R1×7。在一條軌跡確定的情況下，式（7）中△x是已知的，則式（7）是含有6個方程、7個未知數(shù)的線性方程組。因為在tn時的優(yōu)化指標(biāo)函數(shù)是最優(yōu)的，那么經(jīng)過△t時刻時的優(yōu)化目標(biāo)函數(shù)是：

由于qn-qs在tn+△t時刻是已知的，所以對P（q）的常數(shù)部分不予考慮，因此P（q）與下面的優(yōu)化函數(shù)Ps（△q）等價：

由于關(guān)節(jié)空間的角度值qn+△q具有一定的限制，qn+△q∈［θmin，θmax］，因此有以下限制條件：

其中ai，bi∈｛α∈R|0≤α≤2π｝，且ai≤bi。

現(xiàn)在的目標(biāo)就是在式（7）和式（10）成立的條件下求出式（9）的最小值。由參考文獻(xiàn)［6］可知其充分必要條件（即Kuhn-Tucker條件）為：

這就把問題轉(zhuǎn)化為一組線性不等方程組，其中μi，λi，νi都為未知數(shù)，此時要做的就是解出這個方程的一組△q求出最優(yōu)解。參考文獻(xiàn)［6］中所提出的解線性不等式組的旋轉(zhuǎn)算法可以很好地求解式（11）。

1.4逆運動解

上面利用雅可比矩陣為研究對象提出了兩種求解逆運動學(xué)反解的算法，但是實際應(yīng)用中，利用求其積分來得到運動學(xué)的逆解是有一定誤差的。在實際的逆運動學(xué)中是通過反饋法來實現(xiàn)其運動學(xué)校正的。由期望的任務(wù)空間的軌跡路徑xd和實際的任務(wù)空間的軌跡路徑xr偏差來不斷迭代糾正，直到最后的解在誤差范圍內(nèi)。

2　直線軌跡規(guī)劃

2.1模型的描述

機(jī)械臂在完成作業(yè)時，夾手的位姿可用一系列Pi點來描述，所以進(jìn)行直線軌跡規(guī)劃的首要問題是解決直線中相鄰的兩個點的軌跡規(guī)劃問題。設(shè)Ps為直線軌跡的路徑起點，Pd為直線軌跡的路徑終點，U為全局坐標(biāo)系，R為機(jī)器手底座坐標(biāo)系。一般從起點Ps到達(dá)終點Pd的運動可表示為從

則從Ps到Pd的運動可表示為：

綜合式（12）～式（14）得：

其中，D包含從Ps到Pd的距離和角度等信息，可表示為：

其中，△n，△o，△a表示機(jī)械手的姿態(tài)向量，△p表示機(jī)械手的位置向量，即從Ps到Pd時轉(zhuǎn)過的角度或經(jīng)過的空間距離。在進(jìn)行規(guī)劃時，主要對這些參量進(jìn)行插值，而產(chǎn)生一系列的運動軌跡數(shù)據(jù)。

2.2直線軌跡位置規(guī)劃

從Ps到Pd所經(jīng)過的空間距離可用式（16）中的△p表示，其中Ps和Pd在機(jī)械手坐標(biāo)系的位置參數(shù)為ps=｛xs，ys，zs｝，pd=｛xd，yd，zd｝。如果采用S型加速曲線，確定實現(xiàn)軌跡的總時間為ta，插補中每兩點之間的插補周期為T，那么總的插值點數(shù)為Np=ta/T」+1，并設(shè)插值函數(shù)Il（n），其定義根據(jù)不同的情形而定，一般可取n/Np，其中，n∈｛x∈N*|0≤x≤Np｝，Il（n）∈［0，1］。于是得出插值點在機(jī)械手坐標(biāo)系的位置坐標(biāo)為：

2.3直線軌跡位姿規(guī)劃

2.3.1姿態(tài)矩陣與四元數(shù)

從Ps到Pd的姿態(tài)變換可用式（16）中的△n，△o，△a表示為：

從Ps到Pd的軌跡不僅包含位置的變換還伴隨著姿態(tài)的變換，可以看出機(jī)械手的末端沿著△p向量所指示的方向移動的同時進(jìn)行姿態(tài)的旋轉(zhuǎn)，式中△n，△o，△a包含了旋轉(zhuǎn)中的角度信息，可以從式（18）解析出。建立圍繞△p的旋轉(zhuǎn)矩陣，然后再根據(jù)獲得的角度信息進(jìn)行旋轉(zhuǎn)角度插值，最后獲得插值中間點的姿態(tài)矩陣。但是，這種方法每次插值需要進(jìn)行大量的矩陣運算，速度比較慢，可用四元數(shù)法對姿態(tài)矩陣和旋轉(zhuǎn)進(jìn)行解析，然后再進(jìn)行插值。下面將論述這種方法的原理。

［10］可知四元數(shù)的定義為：

其中v∈R3×1，如果q為單位四元數(shù)，則相應(yīng)的姿態(tài)變換矩陣為：

由式（19）和式（20）得到四元數(shù)與姿態(tài)矩陣的關(guān)系，可以看出四元數(shù)不僅節(jié)省空間而且具有更高的運算效率。

2.3.2姿態(tài)軌跡規(guī)劃

機(jī)械手末端的執(zhí)行機(jī)構(gòu)的位姿為pc，那么機(jī)械手末端執(zhí)行機(jī)構(gòu)繞向量r轉(zhuǎn)θ角度之后的機(jī)械手執(zhí)行機(jī)構(gòu)的位姿為：

如果令R=［cosθ，rsinθ］，則上式可表示為：

因為式（18）中的Tpos表示從開始位姿Ps旋轉(zhuǎn)到位姿 Pd的位姿變換矩陣，可以求出Tpos對應(yīng)的四元數(shù)ppos，將其表示為R的形式，由于已知圍繞旋轉(zhuǎn)的向量△p即R中的r，那么就可以求出從Ps到Pd旋轉(zhuǎn)的角度值β。

求出從Ps到Pd旋轉(zhuǎn)的角度值β后，又由于對姿態(tài)旋轉(zhuǎn)的插值就是對旋轉(zhuǎn)量的插值，定義姿態(tài)插值函數(shù)Ip（n），結(jié)合2.2節(jié)中Np表達(dá)式，其中，n∈｛x∈N*|0≤x≤Np｝，Ip（n）∈［0，1］。則可得位姿插值四元數(shù)：

中間每一個插值點的位姿可表示為：

當(dāng)n=Np時，上式與式（22）等價，借助于式（19）與式（20），可獲得每一個插值姿態(tài)四元數(shù)對應(yīng)的姿態(tài)矩陣：

上式為其姿態(tài)插值矩陣，聯(lián)合式（17）可得在每一個插值點處的位姿坐標(biāo)參數(shù)為：

其中，n=1，2，3，……，Np。對于上述插值點之間的插補運動，應(yīng)根據(jù)實際操作的精度和機(jī)械手自身的精度來決定采用三次或五次條樣連續(xù)插補或者更高階次的插補。這樣可以保證機(jī)械手直線規(guī)劃時運動的精度，即運動時偏差比較小。

3　直線最小能量軌跡規(guī)劃仿真及分析

本文以七軸機(jī)械手為研究對象，在三維空間中它具有一個冗余度。本文給出的一般逐步優(yōu)化函數(shù)的形式為P（t）=xTHx+cx，其中x代表機(jī)械手的角速度，本文以H= diag（1，2，3，4，5，6，7），c=（1，1，1，1，1，1，1）為例，來求出以上述能量函數(shù)為目標(biāo)函數(shù)的最優(yōu)軌跡規(guī)劃，其D-H參數(shù)表如表1所示［1］。

采用以上參數(shù)在MATLAB上進(jìn)行實驗，采用本文第2節(jié)所述直線軌跡規(guī)劃方法獲得插值點，之后利用本文

表1　七軸機(jī)械手D-H參數(shù)表

其中，Link為機(jī)械手關(guān)節(jié)與關(guān)節(jié)之間的連接；alpha表示前一個關(guān)節(jié)軸繞到當(dāng)前關(guān)節(jié)軸的角度，逆時針為正；A為當(dāng)前關(guān)節(jié)軸到下一個關(guān)節(jié)軸的垂直距離；如果以關(guān)節(jié)軸為z軸，用x，y，z來確定關(guān)節(jié)的位置參數(shù)，則theta表示當(dāng)前關(guān)節(jié)軸的x坐標(biāo)需要繞行的角度，使得當(dāng)前關(guān)節(jié)的x，z軸和下一個關(guān)節(jié)的x，z軸處在同一平面上；D為當(dāng)前關(guān)節(jié)軸與上一個關(guān)節(jié)軸的公垂線和當(dāng)前關(guān)節(jié)軸與下一個關(guān)節(jié)軸的公垂線分別和當(dāng)前關(guān)節(jié)軸的交點之間的距離；Joint表示當(dāng)前關(guān)節(jié)軸的類型，R表示旋轉(zhuǎn)關(guān)節(jié)；Offset Angle表示各個關(guān)節(jié)角度的當(dāng)前偏移量。

1.2節(jié)和1.3節(jié)所提出的不同的求逆解的方法來求出逆解。利用直線軌跡的起點和終點，并確定機(jī)械手的運行周期和軌跡運行總時間，由此確定了直線軌跡上的插值點總數(shù)和每一個插值點位姿坐標(biāo)，從而確定了整條軌跡，圖1給出了所規(guī)劃的直線軌跡。

圖1　空間直線插值軌跡

規(guī)劃了直線的軌跡之后，利用偽逆法、最小能量法、參考文獻(xiàn)［3］和參考文獻(xiàn)［4］提出的逆運動學(xué)優(yōu)化方法來對上述逐步優(yōu)化函數(shù)求積分，即可得到能耗函數(shù)表達(dá)式：

求出上述4種方法的耗能并作比較，圖2給出了2.5s內(nèi)這4種方法的耗能曲線。

圖2　機(jī)械臂能量曲線

上圖中處于最上面的曲線是未優(yōu)化的能耗曲線，即采用偽逆法來求逆解的能量損耗。處于最下面的是采用能量優(yōu)化之后的能耗曲線。從圖中可以看出，最小能量法比偽逆法要節(jié)省能量10％～20％左右，并且比參考文獻(xiàn)［3］和參考文獻(xiàn)［4］的優(yōu)化方法更節(jié)省能量。

4　結(jié)束語

本文介紹了機(jī)械手運動學(xué)模型中的雅可比矩陣，然后說明了求解逆運動學(xué)問題的偽逆法，隨后針對偽逆法的耗能過高等問題提出了基于逐步二次項能量最優(yōu)化的優(yōu)化算法。對于直線軌跡的規(guī)劃問題，文章先根據(jù)運動模型導(dǎo)出直線軌跡的變換表達(dá)式，然后利用四元數(shù)法運算高效等優(yōu)點進(jìn)行直線軌跡插值，最后將兩者綜合進(jìn)行綜合仿真。從仿真結(jié)果來看，本文介紹的方法具有一定的推廣價值。

參考文獻(xiàn)

［1］李誠，謝志江，倪衛(wèi)，等.六自由度裝校機(jī)器人雅可比矩陣的建立及奇異性分析 ［J］.中國機(jī)械工程，2012，23（10）：1165-1174.

［2］Xu De，CALDERON C A A，GAN J Q，et al.An analysis of the inverse kinematics for a 5-DOF manipulator［J］.International Journal of Automation and Computing，2005，2（2）：114-124.

［3］廖柏林，梁平元，楊喜.基于偽逆的冗余機(jī)械臂運動控制的優(yōu)化方案［J］.信息與控制，2013，42（5）：646-652.

［4］陽方平，李洪誼，王越超，等.一種求解冗余機(jī)械臂逆運動學(xué)的優(yōu)化方法［J］.機(jī)器人，2012，34（1）：17-22.

［5］GOULD N I M，TOINT PH L.Nonlinear programming without a penalty function or filter［J］.Mathematical Programming，2010，122（5）：155-196.

［6］LIU X W，YUAN U.A sequential quadratic programming method without a penalty function or a filter for nonlinear equality constrained optimization［J］.SIAM Journal on Optimization，2011，21（2）：545-571.

［7］Su Benyue，Zou Liping.Manipulator trajectory planning based on the algebraic-trigonometric hermite blended interpolation spline［J］.Procedia Engineering，2012，29（1）：2093-2097.

［8］PETRINEC K，KOVACIC Z.Trajectory planning algori-thm based on the conti-nuity of jerk［C］.Mediterranean Conference on Control and Automation，2007，Greece Athens：IEEE Press，2007：1-5.

［9］BRAVO F，CUESTA F.Continuous curvature path generationbasedon-splinecurvesforparkingmanoeuvres［J］.Robot Auton Syst，2008，56（4）：360-372.

［10］SMOEMAKE K.Animating rotation with quaternion curves［J］.Computer Graphic-s，1985，19（3）：245－254．

［11］OZDEMIR M.The roots of split quaternion［J］.Applied Mathematics Letters，2009，22（2）：258-263.

The optimizing of seven-axis manipulator minimum energy linear trajectory algorithm

Liang Hexi1，Huang Xiaodong1，Zhou Mi2

（1.College of Education Information and Technical，Hubei Normal University，Huangshi 435002，China；
2.College of Electronic Information，Wuhan University，Wuhan 430074，China）

After compared the principle of pseudo-inverse method with the energy optimal algorithm，a optimization scheme of the energy optimal algorithm is given.the Cartesian space system adopts homogeneous matrix and quaternion method to get the interpolation of position and posture.After getting inverse solution by using optimized energy optimal algorithm，the joint values in the joint space corresponding with Cartesian space can be got.In the paper，we have given the practical simulation examples which indicates that our prioritization scheme′s quaternion method have better operating speed and effect of energy optimization than pseudo method in the attitude interpolation operation when it satisfies the case of normal operation.The seven axis manipulator′s minimum energy consumption can save energy about 10％to 20％.

seven-axis manipulator；energy；quaternion

TP241

A

1674-7720（2015）15-0001-04

梁赫西，黃曉東，周密.七軸機(jī)械手最小能量直線軌跡算法優(yōu)化［J］.微型機(jī)與應(yīng)用，2015，34（15）：1-4.

2015-04-04）

梁赫西（1982-），男，博士生，講師，主要研究方向：數(shù)字系統(tǒng)設(shè)計。

黃曉東（1993-），男，本科在讀，主要研究方向：自動控制。

周密（1990-），女，碩士研究生，主要研究方向：人工智能。

2012-2013年國家大學(xué)生創(chuàng)新訓(xùn)練項目（201210513022）