摘  要： 對比分析了偽逆法和最優能量算法的原理，給出了最優能量算法的優化方案。笛卡爾空間體系中采用齊次矩陣和四元數法獲得位置和姿態的插值；利用優化后的最優能量算法求出逆解，得到與笛卡爾空間中對應的在關節空間中的關節值。文中給出了實際仿真實例，該實例表明本文給出的優化方案在滿足正常作業情況下，相對于偽逆法，方案中四元數算法在姿態插值運算上更快并且能量優化效果更好，七軸機械手最小能量消耗節約10%~20%左右。

　　關鍵詞： 七軸機械手；能量；四元數

0 引言

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

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

1 逆運動學的優化

　　1.1 雅可比矩陣

　　逆運動學的優化主要利用關聯機械臂末端的速度和關節速度的雅可比矩陣J為主要的研究工具來進行，其在機械手運動學中具有重要的作用。令七軸機械手的正運動學方程為：

　　x=x（q）（1）

　　式（1）表示操作空間x和關節空間q之間的函數關系，通常用D-H方法來表示。同時對式（1）兩邊對時間t求導，即可得出笛卡爾空間的速度x與關節空間的關節速度q之間的關系：

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

　　1.2 偽逆法求雅可比反解

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

　　其極小范數解即令‖q‖2最小的解為：

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

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

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

　　1.3 在某些指標下的最優雅可比反解

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

　　在實際的數值算法中x和q都代表一定的數值，t是其運行時間，則對其優化問題可以轉化為線性方程組下的最值問題。

　　現設xs和xn分別為開始時刻ts和當前時刻tn的機械臂末端6個自由度的值，qs和qn分別為開始時刻ts和當前時刻tn的7個關節空間角度值。并在此時有式（1）成立，則此時可以根據式（7）來進行優化。

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

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

　　由于關節空間的角度值qn+?駐q具有一定的限制，qn+q∈[θmin，θmax]，因此有以下限制條件：

　　其中ai，bi∈{∈R|0≤≤2π}，且ai≤bi。

　　現在的目標就是在式（7）和式（10）成立的條件下求出式（9）的最小值。由參考文獻[6]可知其充分必要條件（即Kuhn-Tucker條件）為：

　　這就把問題轉化為一組線性不等方程組，其中i，λi，νi都為未知數，此時要做的就是解出這個方程的一組q求出最優解。參考文獻[6]中所提出的解線性不等式組的旋轉算法可以很好地求解式（11）。

　　1.4 逆運動解

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

2 直線軌跡規劃

　　2.1 模型的描述

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

　　RPs=RTuUPs（12）

　　到

　　RPd=RTuUPd（13）

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

　　RPd=RTsD（14）

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

　　D=RPS-1RPd=UPS-1UPd（15）

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

　　其中，n，o，a表示機械手的姿態向量，p表示機械手的位置向量，即從Ps到Pd時轉過的角度或經過的空間距離。在進行規劃時，主要對這些參量進行插值，而產生一系列的運動軌跡數據。

　　2.2 直線軌跡位置規劃

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

　　2.3 直線軌跡位姿規劃

　　2.3.1 姿態矩陣與四元數

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

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

　　由參考文獻[10]可知四元數的定義為：

　　q=[w，v]=[w，（x，y，z）]（19）

　　其中v∈R3×1，如果q為單位四元數，則相應的姿態變換矩陣為：

　　由式（19）和式（20）得到四元數與姿態矩陣的關系，可以看出四元數不僅節省空間而且具有更高的運算效率。

　　2.3.2 姿態軌跡規劃

　　機械手末端的執行機構的位姿為pc，那么機械手末端執行機構繞向量r轉θ角度之后的機械手執行機構的位姿為：

　　pr=Rpc（22）

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

　　求出從Ps到Pd旋轉的角度值?茁后，又由于對姿態旋轉的插值就是對旋轉量的插值，定義姿態插值函數Ip（n），結合2.2節中Np表達式，其中，n∈{x∈N*|0≤x≤Np}，Ip（n）∈[0，1]。則可得位姿插值四元數：

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

　　pp（n）=RI（n）pc（24）

　　當n=Np時，上式與式（22）等價，借助于式（19）與式（20），可獲得每一個插值姿態四元數對應的姿態矩陣：

　　上式為其姿態插值矩陣，聯合式（17）可得在每一個插值點處的位姿坐標參數為：

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

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

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

　　采用以上參數在MATLAB上進行實驗，采用本文第2節所述直線軌跡規劃方法獲得插值點，之后利用本文1.2節和1.3節所提出的不同的求逆解的方法來求出逆解。利用直線軌跡的起點和終點，并確定機械手的運行周期和軌跡運行總時間，由此確定了直線軌跡上的插值點總數和每一個插值點位姿坐標，從而確定了整條軌跡，圖1給出了所規劃的直線軌跡。

　　規劃了直線的軌跡之后，利用偽逆法、最小能量法、參考文獻[3]和參考文獻[4]提出的逆運動學優化方法來對上述逐步優化函數求積分，即可得到能耗函數表達式：

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

　　上圖中處于最上面的曲線是未優化的能耗曲線，即采用偽逆法來求逆解的能量損耗。處于最下面的是采用能量優化之后的能耗曲線。從圖中可以看出，最小能量法比偽逆法要節省能量10%~20%左右，并且比參考文獻[3]和參考文獻[4]的優化方法更節省能量。

4 結束語

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

參考文獻

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

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

　　[3] 廖柏林，梁平元，楊喜.基于偽逆的冗余機械臂運動控制的優化方案[J].信息與控制，2013，42（5）：646-652.

　　[4] 陽方平，李洪誼，王越超，等.一種求解冗余機械臂逆運動學的優化方法[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 generation based on-spline curves for parking manoeuvres[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.