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

　　關(guān)鍵詞： 七軸機(jī)械手；能量；四元數(shù)

0 引言

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

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

1 逆運(yùn)動(dòng)學(xué)的優(yōu)化

　　1.1 雅可比矩陣

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

　　x=x（q）（1）

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

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

　　1.2 偽逆法求雅可比反解

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

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

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

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

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

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

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

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

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

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

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

　　由于關(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條件）為：

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

　　1.4 逆運(yùn)動(dòng)解

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

2 直線軌跡規(guī)劃

　　2.1 模型的描述

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

　　RPs=RTuUPs（12）

　　到

　　RPd=RTuUPd（13）

　　則從Ps到Pd的運(yùn)動(dòng)可表示為：

　　RPd=RTsD（14）

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

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

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

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

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

　　從Ps到Pd所經(jīng)過(guò)的空間距離可用式（16）中的p表示，其中Ps和Pd在機(jī)械手坐標(biāo)系的位置參數(shù)為ps={xs，ys，zs}，pd={xd，yd，zd}。如果采用S型加速曲線，確定實(shí)現(xiàn)軌跡的總時(shí)間為ta，插補(bǔ)中每?jī)牲c(diǎn)之間的插補(bǔ)周期為T，那么總的插值點(diǎn)數(shù)為Np=ta/T」+1，并設(shè)插值函數(shù)Il（n），其定義根據(jù)不同的情形而定，一般可取n/Np，其中，n∈{x∈N*|0≤x≤Np}，Il（n）∈[0，1]。于是得出插值點(diǎn)在機(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向量所指示的方向移動(dòng)的同時(shí)進(jìn)行姿態(tài)的旋轉(zhuǎn)，式中?駐n，?駐o，?駐a包含了旋轉(zhuǎn)中的角度信息，可以從式（18）解析出。建立圍繞?駐p的旋轉(zhuǎn)矩陣，然后再根據(jù)獲得的角度信息進(jìn)行旋轉(zhuǎn)角度插值，最后獲得插值中間點(diǎn)的姿態(tài)矩陣。但是，這種方法每次插值需要進(jìn)行大量的矩陣運(yùn)算，速度比較慢，可用四元數(shù)法對(duì)姿態(tài)矩陣和旋轉(zhuǎn)進(jìn)行解析，然后再進(jìn)行插值。下面將論述這種方法的原理。

　　由參考文獻(xiàn)[10]可知四元數(shù)的定義為：

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

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

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

　　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)的位姿為：

　　pr=Rpc（22）

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

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

　　中間每一個(gè)插值點(diǎn)的位姿可表示為：

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

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

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

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

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

　　本文以七軸機(jī)械手為研究對(duì)象，在三維空間中它具有一個(gè)冗余度。本文給出的一般逐步優(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）為例，來(lái)求出以上述能量函數(shù)為目標(biāo)函數(shù)的最優(yōu)軌跡規(guī)劃，其D-H參數(shù)表如表1所示[1]。

　　采用以上參數(shù)在MATLAB上進(jìn)行實(shí)驗(yàn)，采用本文第2節(jié)所述直線軌跡規(guī)劃方法獲得插值點(diǎn)，之后利用本文1.2節(jié)和1.3節(jié)所提出的不同的求逆解的方法來(lái)求出逆解。利用直線軌跡的起點(diǎn)和終點(diǎn)，并確定機(jī)械手的運(yùn)行周期和軌跡運(yùn)行總時(shí)間，由此確定了直線軌跡上的插值點(diǎn)總數(shù)和每一個(gè)插值點(diǎn)位姿坐標(biāo)，從而確定了整條軌跡，圖1給出了所規(guī)劃的直線軌跡。

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

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

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

4 結(jié)束語(yǔ)

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

參考文獻(xiàn)

　　[1] 李誠(chéng)，謝志江，倪衛(wèi)，等.六自由度裝校機(jī)器人雅可比矩陣的建立及奇異性分析[J]. 中國(guó)機(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ī)械臂運(yùn)動(dòng)控制的優(yōu)化方案[J].信息與控制，2013，42（5）：646-652.

　　[4] 陽(yáng)方平，李洪誼，王越超，等.一種求解冗余機(jī)械臂逆運(yùn)動(dòng)學(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 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.