機(jī)械臂逆運(yùn)動(dòng)學(xué)避障最優(yōu)求解算法

楊青，周建興*，葛亮

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

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

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

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

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

1 機(jī)械臂模型的建立

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

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

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

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

Xi、Yi、Zi分別為機(jī)械臂各個(gè)關(guān)節(jié)坐標(biāo)的坐標(biāo)軸；ai、di分別為第i連桿的連桿長度和連桿偏矩;i=1,2,…,6圖2 機(jī)械臂坐標(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)系上的點(diǎn)在基坐標(biāo)系中的位置可表示為

(4)

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

2 避障檢測算法

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

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

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

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

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

(5)

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

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

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

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

(6)

式(6)中：PA為點(diǎn)A在關(guān)節(jié)一坐標(biāo)系的位置坐標(biāo)，PC為點(diǎn)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)

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

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

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

3 逆運(yùn)動(dòng)學(xué)求解算法

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

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

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

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

(9)

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

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

(10)

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

Ep=(Pt-Pa)2

(11)

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

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

ER=(Rt-Ra)2

(12)

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

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

fE=λ1EP+λ2Ea

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

(13)

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

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

(14)

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

(15)

式(15)中：Δθi為每個(gè)關(guān)節(jié)角度值的變化量；ηi為每個(gè)關(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ù)的各項(xiàng)權(quán)重系數(shù)。

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

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

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

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

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

(17)

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

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

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

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

6 仿真實(shí)驗(yàn)驗(yàn)證

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

通過解析法求解得到的8組逆解如表2所示。利用本文算法對(duì)上述末端位姿隨機(jī)求取逆運(yùn)動(dòng)學(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é)果的平均值代入正向運(yùn)動(dòng)學(xué)方程中，得到位姿矩陣，在三維空間中的各連桿的姿態(tài)與障礙物的位置如圖7所示。

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

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

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

表3 解析解Table 3 Parameter configuration

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

當(dāng)關(guān)節(jié)角θ5=0 rad時(shí)，關(guān)節(jié)4與關(guān)節(jié)6是共線的，此時(shí)UR5機(jī)械臂處于腕部奇異位置。令初始關(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]，對(duì)應(yīng)的位姿矩陣分別為

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

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

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

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

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

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

7 結(jié)論

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

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

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

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