機械臂逆運動學(xué)避障最優(yōu)求解算法

楊青，周建興*，葛亮

(1.西南石油大學(xué)電氣信息學(xué)院，成都 610500；2.西南石油大學(xué)機電工程學(xué)院，成都 610500)

近年來，機械臂技術(shù)快速發(fā)展，被廣泛用于制造、醫(yī)療、航天等各個領(lǐng)域。目前對機械臂的研究主要包括視覺標(biāo)定、軌跡規(guī)劃、運動控制等方面。實現(xiàn)機械臂的逆運動學(xué)求解是研究這些問題的前提，所以對機械臂逆運動學(xué)求解方法研究必不可少。因為機械臂的逆運動學(xué)求解存在非線性、多解性等特點，所以很難找到一種適用于不同結(jié)構(gòu)、不同工作環(huán)境的通用逆運動學(xué)求解方法。

逆運動學(xué)求解即是根據(jù)機械臂末端位姿矩陣求取關(guān)節(jié)角矩陣，目前，求解逆運動學(xué)的方法普遍采用數(shù)值解法[1]和解析解法[2]等。周東旭等[3]、陳禹含等[4]、T?rdal等[5]采用解析法對機械臂進行求解,求解速度快，根據(jù)機械臂模型和幾何結(jié)構(gòu)便可推導(dǎo)出相應(yīng)的逆運動學(xué)求解方程。但是解析法易受到結(jié)構(gòu)限制，只有滿足Pieper準(zhǔn)則的機械臂才可以獲得解析解[6]，不具有通用性。張栩曼等[7]、K?ker[8]、Yuan等[9]采用數(shù)值解法對逆運動學(xué)進行求解，因為數(shù)值解法具有一定的通用性，數(shù)值解法主要通過迭代完成，即求得機械臂的雅克比矩陣，但部分機械臂可能存在奇異問題，所以無法求逆運動學(xué)解，且求解結(jié)果的精度容易受到隨機初值的影響[10]。近年來，智能優(yōu)化算法得到了廣泛應(yīng)用，主要包括遺傳算法[11]，粒子群算法[12]和差分進化算法[13]等，這些算法也逐漸被用于求機械臂逆運動學(xué)解。林陽等[14]和馮晨旭等[15]利用智能優(yōu)化算法將逆運動學(xué)問題轉(zhuǎn)化為最小化位姿誤差的最優(yōu)求解問題，對機械臂構(gòu)型無特殊要求，可以彌補數(shù)值解法和解析解法的缺陷。

目前，前人研究方法多針對不存在障礙物的環(huán)境中機械臂逆運動學(xué)求解，而關(guān)于存在障礙物作業(yè)環(huán)境下的逆運動學(xué)求解方法研究較少。而實際工況下機械臂通常工作在障礙物環(huán)境中，如搬運、焊接、人機協(xié)作等。徐培等[16]通過解析法對逆運動學(xué)求解，再對其優(yōu)選，求得最少轉(zhuǎn)角的機械臂逆解優(yōu)選算法。同理，可通過數(shù)值解或解析解先求得逆運動學(xué)解，再對其進行優(yōu)選得到避障逆運動學(xué)解。但是該方法存在機械臂處于奇異情況下無法求解的情況，且通用性差。為此，針對在障礙物作業(yè)環(huán)境下的逆運動學(xué)求解問題，結(jié)合避障檢測算法，將機械臂避障逆運動學(xué)求解問題轉(zhuǎn)化為多元多目標(biāo)優(yōu)化問題，提出一種機械臂避障逆運動學(xué)最優(yōu)求解算法。

機械臂避障逆運動學(xué)最優(yōu)求解算法應(yīng)該滿足以下條件：機械臂全局避障而非末端一點，求解精度高，各個關(guān)節(jié)運動量小。現(xiàn)將約束條件設(shè)置為各關(guān)節(jié)最大值與最小值限位，算法主要包括：①避障檢測算法：通過簡化模型，將機械臂與障礙物實體間的碰撞檢測轉(zhuǎn)化為包絡(luò)面之間的碰撞檢測問題；②逆運動學(xué)求解算法：逆運動學(xué)解是正運動學(xué)的反解，將求解結(jié)果的正向運動學(xué)結(jié)果與目標(biāo)位置矩陣與姿態(tài)矩陣比較，計算出其姿態(tài)誤差，根據(jù)避障檢測結(jié)果、姿態(tài)誤差、最短行程原則，通過加權(quán)系數(shù)法建立目標(biāo)函數(shù)，將目標(biāo)求解問題轉(zhuǎn)化為多目標(biāo)優(yōu)化問題；③權(quán)重自適應(yīng)算法：通過差分進化算法對目標(biāo)函數(shù)進行優(yōu)化，為了提高求解速度和求解精度，設(shè)計一種權(quán)重系數(shù)自適應(yīng)調(diào)整方法，在優(yōu)化過程中自適應(yīng)調(diào)整權(quán)重系數(shù)。

1 機械臂模型的建立

選擇較為典型的六自由度串聯(lián)機械臂UR5進行研究分析。UR5機械臂的結(jié)構(gòu)參數(shù)如圖1所示，連桿坐標(biāo)系如圖2所示。該機械臂具有6個自由度，6個關(guān)節(jié)均為轉(zhuǎn)動關(guān)節(jié)，其中第2～4關(guān)節(jié)相互平行。

圖1 機械臂結(jié)構(gòu)Fig.1 Structure of mechanical arm

由于機械臂的逆運動學(xué)求解是正向運動學(xué)求解的逆過程，因此要分析機械臂逆運動學(xué)解，首先要建立機械臂正向運動學(xué)模型，為分析機械臂正向運動學(xué)模型中從基坐標(biāo)系到末端執(zhí)行器的位姿變化關(guān)系，采用D-H(Denavit-Hartenberg)法對機械臂建立正運動學(xué)模型，D-H參數(shù)如表1所示。

表1 D-H參數(shù)Table 1 D-H parameters

Xi、Yi、Zi分別為機械臂各個關(guān)節(jié)坐標(biāo)的坐標(biāo)軸；ai、di分別為第i連桿的連桿長度和連桿偏矩;i=1,2,…,6圖2 機械臂坐標(biāo)Fig.2 Coordinate of manipulator

(1)

式(1)中：RX、RZ分別為X、Z方向旋轉(zhuǎn)算子；DX、DZ分別為X、Z方向平移算子；αi-1為連桿轉(zhuǎn)角。

(2)

式(2)中：c和s分別為函數(shù)cos和sin函數(shù)；ai-1為連桿長度。

(3)

關(guān)節(jié)坐標(biāo)系上的點在基坐標(biāo)系中的位置可表示為

(4)

式(4)中：0P為關(guān)節(jié)坐標(biāo)系上的點在基坐標(biāo)系中的位置；nP為關(guān)節(jié)坐標(biāo)系上的點在第n關(guān)節(jié)坐標(biāo)系中的位置。

2 避障檢測算法

避障檢測算法即碰撞檢測算法，其目的是判斷機械臂連桿與障礙物之間是否發(fā)生碰撞，碰撞檢測的對象是空間中機械臂連桿和障礙物。由于機械臂的結(jié)構(gòu)復(fù)雜和障礙物的形狀的不確定性，很難用準(zhǔn)確的數(shù)學(xué)模型描述，因此，可以通過幾何模型對機械臂連桿和障礙物進行描述，最常用的方法是包圍盒技術(shù)[17]。包圍盒技術(shù)的基本思想是用體積稍大且特性簡單的幾何體(包圍盒)來近似代替復(fù)雜的幾何對象。

研究對象是六自由度串聯(lián)型機械臂，由于機械臂第一個連桿在運動過程中機械臂第一個連桿只會轉(zhuǎn)動，位置不會變化，所以不用考慮碰撞檢測。對第2～6連桿機械臂和障礙物進行碰撞檢測，采用圓柱體和球面對機械臂連桿進行包絡(luò)，將第2、3關(guān)節(jié)用圓柱體進行包絡(luò)，因后3個連桿長度較短，為簡化計算量，將后3個關(guān)節(jié)用球面進行包絡(luò)，模型簡化圖如圖3所示。采用球面對障礙物進行包絡(luò)。當(dāng)機械臂包絡(luò)面與障礙物包絡(luò)面之間不發(fā)生碰撞，機械臂和障礙物實體之間就一定不會發(fā)生碰撞。

在簡化模型的基礎(chǔ)上，將機械臂與障礙物兩個于是將物體間的碰撞轉(zhuǎn)化為連桿直線段與球心點的距離判斷[18]、球心與球心之間的距離判斷。具體碰撞檢測方法為：第2、3關(guān)節(jié)與障礙物的碰撞檢測分為兩個類型，示意圖如圖4所示。

類型一當(dāng)連桿直線與障礙物不在一條直線上時，首先計算障礙物距離機械臂每一桿向量的垂直距離，關(guān)節(jié)角確定后，可通過正向運動學(xué)計算出每一連桿末端和起始端在空間坐標(biāo)系的位置坐標(biāo)，計算出第i桿的末端坐標(biāo)減和第i桿起始點坐標(biāo)即可得到第i桿在空間中的直線向量，計算障礙物球面包絡(luò)面中心點與第i桿的直線向量的距離即可得到si，的具體計算步驟如下。

步驟1計算圖3中對應(yīng)的A、B、C、D點的空間坐標(biāo)，結(jié)合D-H參數(shù)，通過齊次坐標(biāo)變換的方法，可以建立各連桿處的齊次變換矩陣為

(5)

A～E分別為機械臂各個關(guān)節(jié)末端在空間中的坐標(biāo)點圖3 模型簡化圖Fig.3 Model simplification diagram

Li為關(guān)節(jié)中心軸線;Si為球形包圍盒到圓柱體包圍盒的距離;Ci為兩球形包圍盒之間的距離；R1為障礙物球形包圍盒半徑；R2為連桿球形包圍盒半徑；R3為障礙物球形包圍盒半徑圖4 障礙物與機械臂位置關(guān)系示意圖Fig.4 Schematic diagram of position relationship between obstacles and manipulator

式(5)中：T1～T6為第1～6連桿末端的齊次變換矩陣。

則A、B、C、D、E點的空間坐標(biāo)為

(6)

式(6)中：PA為點A在關(guān)節(jié)一坐標(biāo)系的位置坐標(biāo)，PC為點C在關(guān)節(jié)二坐標(biāo)系的位置坐標(biāo)。

則直線向量L1和L2可分別表示為

L1=B-A=[xB-xA,yB-yA,zB-zA]

(7)

L2=D-C=[xD-xC,yD-yC,zD-zC]

(8)

通過空間中點到直線向量的距離公式計算si，si為球心到圓柱體軸線的距離，通過比較si與R1+R的大小，其中R為圓柱的底面半徑，若si均大于R1+R，障礙物和機械臂不會發(fā)生碰撞，否則會發(fā)生碰撞。

類型二障礙物包絡(luò)面在連桿線段的延長線上時，若si>R3+R，則障礙物與機械臂不會發(fā)生碰撞，當(dāng)siR2+R3時，則障礙物與機械臂不會發(fā)生碰撞，反之則會發(fā)生碰撞，ci為兩個球形包圍盒之間的距離。

步驟2后3個關(guān)節(jié)與障礙物的碰撞檢測：后3個關(guān)節(jié)用球面包絡(luò)面包圍。由圖1可以看出，選取圓心為E點，球面半徑選取d4即可將后3個關(guān)節(jié)完全包絡(luò)。已經(jīng)計算出E點坐標(biāo)，d4由表1得到，將后3個關(guān)節(jié)與障礙物的碰撞檢測方法為兩個球形包絡(luò)面之間的距離。si即為球面中心之間的距離，當(dāng)si大于兩球面的半徑之和時,則不會發(fā)生碰撞，反之則會發(fā)生碰撞。

3 逆運動學(xué)求解算法

逆運動學(xué)是正運動學(xué)的反解，即當(dāng)給定其末端位姿數(shù)據(jù)后求其取關(guān)節(jié)角矩陣的過程。逆運動學(xué)存在多解問題,對于六自由度UR5機械臂而言,最多可能求得8組逆解,而這8組逆解都能達到該位姿點,其中可能存在位姿使得機械臂與障礙物發(fā)生碰撞。為了得到全局避障、行程最短的最優(yōu)逆運動學(xué)解，需進一步根據(jù)結(jié)構(gòu)及任務(wù)需求等限制求得最優(yōu)解。

目標(biāo)函數(shù)包含以下三部分。

第一部分為了保證算法求解的逆運動學(xué)解的精度，即所求得的逆運動學(xué)解通過正向運動學(xué)計算得到的目標(biāo)位姿與期望位姿誤差最小，目標(biāo)函數(shù)首先考慮目標(biāo)位姿與期望位姿之間的誤差。

將機械臂在笛卡爾空間坐標(biāo)系下的初始位姿矩陣Tt簡化為

(9)

式(9)中：Rt、Pt分別為目標(biāo)姿態(tài)矩陣和目標(biāo)位置矩陣。

所求得的位姿矩陣Ta簡化為

(10)

位置誤差Ep可用所求得的機械臂末端執(zhí)行器的位置和目標(biāo)位置之間的誤差平方和表示，表達式為

Ep=(Pt-Pa)2

(11)

式(11)中：Pa為位置矩陣。

姿態(tài)誤差ER可用所求得的機械臂末端執(zhí)行器的姿態(tài)矩陣各元素和目標(biāo)姿態(tài)矩陣各元素之間的誤差平方和表示，表達式為

ER=(Rt-Ra)2

(12)

式(12)中：Ra為姿態(tài)矩陣。

由于位置誤差和姿態(tài)誤差數(shù)量級存在差異，位姿誤差可表示為

fE=λ1EP+λ2Ea

=λ1(Pt-Pa)2+λ2(Rt-Ra)2

(13)

式(13)中：λ1、λ2分別為位置誤差、姿態(tài)誤差函數(shù)的權(quán)重系數(shù)。

第二部分為了保證機械臂全局避障，可通過碰撞檢測方法實現(xiàn)，標(biāo)記碰撞檢測結(jié)果為fCO，若機械臂與障礙物發(fā)生碰撞，則fCO取值為1,反之取值為0，fCO可表示為

(14)

第三部分在不考慮關(guān)節(jié)轉(zhuǎn)角范圍約束的條件下，機械臂通過逆運動學(xué)求解可以求出8組逆解，這8組不同的解到達終點所需的各關(guān)節(jié)轉(zhuǎn)角不同[19]。為了保證行程最短、減少運行時間和能耗，于是引入最短行程算法[20]，即在保證避障和求解精度的前提下，實現(xiàn)求解目標(biāo)關(guān)節(jié)角度與當(dāng)前位置的關(guān)節(jié)角度之間的變化量盡可能小，又因為機械臂越靠近基坐標(biāo)的連桿承受重量越重，消耗能量大，所以求最短行程時需要對每個關(guān)節(jié)角度進行加權(quán)，用fq表示加權(quán)運動量，即第三部分目標(biāo)函數(shù)為

(15)

式(15)中：Δθi為每個關(guān)節(jié)角度值的變化量；ηi為每個關(guān)節(jié)角變化量的加權(quán)系數(shù)。

最后，將三部分加權(quán)求和得到最終的目標(biāo)函數(shù)f可表示為

f=min|λEfE+λCOfCO+λqfq|

(16)

式(16)中：λCO、λE、λq分別為目標(biāo)函數(shù)的各項權(quán)重系數(shù)。

要求首先保證機械臂全局避障，所以應(yīng)該保證：λCO?λE，其次要保證求解高精度，最后考慮其行程最短，故應(yīng)該滿足：λE>λq。

4 權(quán)重自適應(yīng)算法

采用差分進化算法對目標(biāo)函數(shù)f進行最小值優(yōu)化求解,差分進化算法詳細原理可參考文獻[16]。在求解的目標(biāo)位姿與初始位姿不同時，在優(yōu)化后期機械臂的運動量fq始終大于0，當(dāng)權(quán)重系數(shù)λq固定時，λqfq始終大于零，最終趨于穩(wěn)定狀態(tài)。因為目標(biāo)函數(shù)中包含了λqfq，當(dāng)其大于零時會影響位姿誤差函數(shù)fE的收斂速度，所以當(dāng)權(quán)重系數(shù)λq固定時存在收斂速度慢且求解精度不高的缺點。

為解決這一問題，設(shè)計了一種自適應(yīng)權(quán)重系數(shù)方法，實現(xiàn)對逆運動學(xué)快速收斂且高精度地求解。具體方法：在優(yōu)化初期選取較大的初始權(quán)重系數(shù)λq，使種群盡快被“吸引”到滿足最短行程原則且具有避障功能的峰值附近；隨著種群進化代數(shù)增加，從1/v倍最大進化代數(shù)為分界線處逐漸減小權(quán)重系數(shù)λq，直到迭代結(jié)束時λq=0，從而逐漸減小λqfq對fE的影響；迭代結(jié)束時λqfq=0，fE逐漸趨于0，最終求解到更精確的運動學(xué)逆解，v為大于1的可調(diào)參數(shù)，使得0≤G/v≤G。

所以，設(shè)計自適應(yīng)權(quán)重λq在優(yōu)化過程中滿足：

(17)

式(17)中：λqini為迭代初期權(quán)值；j為當(dāng)前迭代步數(shù)；G為最大迭代步數(shù)。

5 避障逆運動學(xué)最優(yōu)求解算法流程

采用差分進化算法對機械臂求避障最優(yōu)運動學(xué)逆解，其流程圖如圖5所示，首先對機械臂、障礙物進行建模和簡化表示。同時初始化相關(guān)參數(shù)：輸入機械臂的目標(biāo)位姿矩陣、初始關(guān)節(jié)角、障礙物的位置和大小、由差分進化算法產(chǎn)生初始種群K。其次計算碰撞檢測結(jié)果fCO、目標(biāo)求解角度與當(dāng)前角度的加權(quán)變化量fq、位姿誤差fE，通過差分進化算法選擇、交叉、變異得到子代，在迭代過程中不斷調(diào)整權(quán)值，多次迭代后達到終止條件。最后求解出唯一最優(yōu)逆運動學(xué)解。

圖5 算法流程圖Fig.5 Algorithm flow chart

6 仿真實驗驗證

通過MATLAB Robotics Toolbox(version9.1)工具箱對算法進行仿真實驗驗證。所采用的差分進化算法參數(shù)配置如下：變量維度：6，變量定義域：[-π,-π,-π,-π,-π,-π]～[π,π,π,π,π,π]，種群規(guī)模：20，初始變異率：0.3，交叉算子：0.8，終止條件：10-5，最大迭代次數(shù)：300。

通過解析法求解得到的8組逆解如表2所示。利用本文算法對上述末端位姿隨機求取逆運動學(xué)解10次，目標(biāo)函數(shù)中的權(quán)重系數(shù)設(shè)置為

10次求解結(jié)果如表3所示。解析法的求解結(jié)果在三維空間中的連桿位姿與障礙物的位置如圖6所示(此處展示第1、2組解析解在空間中的位置)。

圖6 解析解姿態(tài)圖Fig.6 Analytical solution attitude diagram

將本文算法10次求解結(jié)果的平均值代入正向運動學(xué)方程中，得到位姿矩陣，在三維空間中的各連桿的姿態(tài)與障礙物的位置如圖7所示。

圖7 最優(yōu)解姿態(tài)圖Fig.7 Analytical solution attitude diagram

由圖6可以直觀地看出：末端姿態(tài)相同的情況下，求取的逆運動學(xué)解對應(yīng)的機械臂姿態(tài)存在與障礙物發(fā)生碰撞的情況。由表3可以看出，機械臂在同一末端位姿情況下，由解析法求出的8組解中，存在4組解對應(yīng)的機械臂連桿與障礙物發(fā)生碰撞。解析法的求解結(jié)果無法保證機械臂連桿能避開障礙物。本文方法的10次求解結(jié)果對應(yīng)的機械臂姿態(tài)均未與障礙物發(fā)生碰撞。將表3和表4的數(shù)據(jù)對比不難發(fā)現(xiàn)，表3中第一組解析解的關(guān)節(jié)運動量最小，但該逆運動學(xué)解所對應(yīng)的機械臂關(guān)節(jié)在空間中會與障礙物發(fā)生碰撞。本文算法所求得的逆運動學(xué)解對應(yīng)于解析解的第2組解，對應(yīng)的機械臂關(guān)節(jié)在空間中不會與障礙物發(fā)生碰撞，且在不發(fā)生碰撞的前提下，機械臂的關(guān)節(jié)的運動量最小。

表3 最優(yōu)解Table 3 Optimal solution

表3 解析解Table 3 Parameter configuration

其次，當(dāng)機械臂處于奇異位置時，存在解析法無法求解的情況。此時將本文算法的避障逆運動學(xué)最優(yōu)求解算法與文獻[21]中不具有避障功能的最優(yōu)求解算法(基于差分進化算法的逆運動學(xué)求解方法)仿真實驗對比：

當(dāng)關(guān)節(jié)角θ5=0 rad時，關(guān)節(jié)4與關(guān)節(jié)6是共線的，此時UR5機械臂處于腕部奇異位置。令初始關(guān)節(jié)角K0=[-1.943 2 rad，-1.761 9 rad，2.589 1 rad，-2.398 0 rad，1.570 8 rad，-2.769 2 rad]，目標(biāo)位姿Ka=[-1.943 2 rad，-1.761 9 rad，2.589 1 rad，-2.398 0 rad，0，-2.769 2 rad]，對應(yīng)的位姿矩陣分別為

目標(biāo)函數(shù)中的權(quán)重系數(shù)設(shè)置與前文相同。分別利用本文算法與文獻[21]中的基于差分進化的機械臂逆運動學(xué)求解方法(傳統(tǒng)方法)對UR5機械臂進行10次求解，10組逆運動學(xué)解對應(yīng)的連桿姿態(tài)的避障性能分析結(jié)果如表4所示。

由表4可知，在機械臂處于奇異位置時，傳統(tǒng)的啟發(fā)式求解方法求出的結(jié)果具有隨機性，不具有避障性能。本文算法求出的逆運動學(xué)解具有良好的避障性能、運動量小且能保證較好的求解精度。

表4 奇異情況下的避障性能分析Table 4 Analysis of obstacle avoidance performance under singular conditions

最后，為了驗證權(quán)重自適應(yīng)算法的優(yōu)越性,在障礙物位置、差分進化算法參數(shù)、位姿矩陣等相同實驗參數(shù)條件下，將固定權(quán)重系數(shù)方法與自適應(yīng)權(quán)重系數(shù)方法對比。得到優(yōu)化過程中的關(guān)節(jié)角均方誤差收斂曲線如圖8所示，初始誤差不同是由于種群的隨機初始化。

由關(guān)節(jié)角均方誤差收斂曲線(圖8)可知，當(dāng)權(quán)重固定時，由于收到fq的影響，在收斂后期關(guān)節(jié)角均方誤差E會上下波動，最后趨于穩(wěn)定；采用自適應(yīng)權(quán)重系數(shù)時，優(yōu)化過程中關(guān)節(jié)角均方誤差收斂更快，且誤差更小。

圖8 誤差收斂曲線Fig.8 Error convergence curve

7 結(jié)論

以六自由度機械臂UR5為研究對象，針對機械臂在障礙物環(huán)境下的逆運動學(xué)求解問題，提出了一種結(jié)合碰撞檢測算法、最短行程算法、差分進化算法的機械臂逆運動學(xué)最優(yōu)求解新方法。利用MATLAB對算法進行仿真實驗驗證，通過分析算法原理和實驗結(jié)果，得出以下結(jié)論。

(1)本文方法能夠使機械臂在避開障礙物的同時求得準(zhǔn)確的逆運動學(xué)解。

(2)與傳統(tǒng)算法相比，所提出的機械臂避障最優(yōu)求解算法不僅能求解出具有避障功能、行程最短的的逆運動學(xué)最優(yōu)解，同時具有收斂速度快、求解精度高、通用性強的優(yōu)越性。

(3)理論上，本文算法可被擴展到任意自由度的串聯(lián)機械臂的逆運動學(xué)求解中，且在無障礙物環(huán)境和復(fù)雜的多障礙物環(huán)境中均適用。下一步，將進行該算法的實際實驗研究，針對實際機械臂系統(tǒng)進行軌跡規(guī)劃的應(yīng)用研究。