基于改進(jìn)人工勢場法雙向規(guī)劃的雙機(jī)械臂避碰路徑規(guī)劃

邱 博，單 梁，常 路，王志強(qiáng)，屈 藝

(南京理工大學(xué) 自動(dòng)化學(xué)院，南京 210094)

作為機(jī)器人的常見類型，機(jī)械臂被廣泛應(yīng)用于工業(yè)以及服務(wù)領(lǐng)域，類似人類手臂的形態(tài)使機(jī)械臂具備足夠的靈活性來完成復(fù)雜的任務(wù).盡管如此，單機(jī)械臂操作仍存在局限性，有些任務(wù)需要雙機(jī)械臂協(xié)同工作才能完成，例如零件的裝配以及大件物品的搬運(yùn).然而雙機(jī)械臂在運(yùn)動(dòng)過程中，往往存在工作空間的重疊，雙臂運(yùn)動(dòng)時(shí)，不僅要避開環(huán)境中的障礙，還要避免機(jī)械臂之間發(fā)生碰撞，因此有必要對(duì)雙機(jī)械臂進(jìn)行避碰路徑規(guī)劃.

國內(nèi)外學(xué)者對(duì)雙機(jī)械臂路徑規(guī)劃做了大量研究.文獻(xiàn)[1]將機(jī)械臂連桿間的最短距離作為避碰距離指標(biāo)對(duì)冗余雙臂機(jī)器人進(jìn)行協(xié)調(diào)避碰.文獻(xiàn)[2]從能量的角度規(guī)劃雙機(jī)械臂的路徑，在雙臂之間設(shè)置一個(gè)虛擬的彈簧，彈簧受到彈力會(huì)迫使雙臂互相遠(yuǎn)離.文獻(xiàn)[3]使用動(dòng)態(tài)神經(jīng)網(wǎng)絡(luò)算法規(guī)劃具有物理約束的雙機(jī)械臂避碰路徑.但是只考慮雙臂間的避碰并不能實(shí)現(xiàn)雙臂協(xié)同運(yùn)動(dòng)，還要同時(shí)考慮機(jī)械臂的路徑規(guī)劃.

機(jī)械臂的避障路徑規(guī)劃方法主要有兩種：自由空間法和人工勢場法[4-5].自由空間法需要預(yù)先得到全局的信息，算法開銷較大，如A*算法[6]以及快速搜索隨機(jī)樹[7]等.相較于自由空間法，人工勢場法不需要計(jì)算全局信息，其結(jié)構(gòu)簡單，算法效率高，更適合處理多關(guān)節(jié)機(jī)械臂的路徑規(guī)劃.近年來學(xué)者們一直在研究人工勢場法的改進(jìn)算法.對(duì)于人工勢場法易陷入局部極小點(diǎn)的問題，文獻(xiàn)[8]通過引入快速擴(kuò)展隨機(jī)算法跳出局部極小值點(diǎn).有學(xué)者則通過加入虛擬障礙[9]或者虛擬目標(biāo)點(diǎn)[10]使人工勢場算法跳出局部極小點(diǎn).但隨機(jī)算法和添加虛擬點(diǎn)的方式都增加了算法的計(jì)算量，并增大了路徑振蕩的可能.

針對(duì)這些問題，文中提出了一種基于改進(jìn)人工勢場法雙向規(guī)劃的避障算法，從改善振蕩問題、提升算法效率以及避免局部極小點(diǎn)3個(gè)方面對(duì)人工勢場法進(jìn)行改進(jìn)：引入了斥力變增益系數(shù)，采用了自適應(yīng)步長策略，提出了雙向規(guī)劃算法.將改進(jìn)后的算法應(yīng)用于主從模式下的雙機(jī)械臂路徑規(guī)劃，先針對(duì)環(huán)境障礙規(guī)劃主臂路徑，然后根據(jù)主臂每個(gè)時(shí)刻點(diǎn)的位姿得到主臂的動(dòng)態(tài)碰撞模型，將其作為障礙規(guī)劃從臂的路徑，最終實(shí)現(xiàn)雙機(jī)械臂避碰路徑規(guī)劃.

1 雙機(jī)械臂建模

以ABB公司生產(chǎn)的IRB 120型六軸機(jī)械臂為對(duì)象建立雙機(jī)械臂模型，對(duì)其進(jìn)行運(yùn)動(dòng)學(xué)分析，并以此來研究雙機(jī)械臂避碰路徑規(guī)劃.

1.1 機(jī)械臂運(yùn)動(dòng)學(xué)分析

采用D-H方法[11]建立機(jī)械臂的運(yùn)動(dòng)學(xué)模型.IRB120型機(jī)械臂尺寸參數(shù)及簡化模型如圖1、2.

圖1 ABB IRB 120型機(jī)械臂(單位:mm)

圖2 簡化模型

根據(jù)機(jī)械臂的簡化模型可以得到該機(jī)械臂的D-H參數(shù)，如表1.

表1 D-H參數(shù)

由機(jī)械臂D-H參數(shù)，可以通過坐標(biāo)變換得到相鄰兩個(gè)關(guān)節(jié)之間的轉(zhuǎn)換矩陣為：

(1)

由此，可進(jìn)一步得到機(jī)械臂末端與機(jī)械臂基座之間的轉(zhuǎn)換矩陣為：

(2)

綜上，可以通過機(jī)械臂的關(guān)節(jié)角度，得到機(jī)械臂末端的位姿，即正運(yùn)動(dòng)學(xué)；反之，也可以反解公式(2)通過末端位姿求得機(jī)械臂關(guān)節(jié)量，即逆運(yùn)動(dòng)學(xué).

1.2 雙機(jī)械臂位姿描述

文中兩臺(tái)機(jī)械臂分為主臂和從臂，兩機(jī)械臂具有相同的初始姿態(tài)，并關(guān)于X軸對(duì)稱，通過MATLAB中的Robotics System Toolbox建立圖3雙機(jī)械臂模型.

圖3 雙機(jī)械臂模型

從臂可以看作是主臂平移而來，從臂的基坐標(biāo)原點(diǎn)Xslaver滿足如下轉(zhuǎn)換公式：

Xslaver=Xmaster·Ttrans

(3)

式中：Xmaster為主臂的基坐標(biāo)原點(diǎn)；Ttrans為轉(zhuǎn)換矩陣.

2 基于改進(jìn)人工勢場法的雙向規(guī)劃算法

人工勢場法被廣泛應(yīng)用于移動(dòng)機(jī)器人的路徑規(guī)劃[12-14]，其基本思想就是將機(jī)器人視為質(zhì)點(diǎn)，在其工作空間中建立由引力場Uatt(q)和斥力場Urep(q)構(gòu)成的人工勢場，兩者產(chǎn)生的引力Fatt(q)和斥力Frep(q)的作用在機(jī)器人上趨近目標(biāo)點(diǎn).人工勢場法原理如圖4.

設(shè)機(jī)器人當(dāng)前位置為q，人工勢場施加在機(jī)器人上的合力為：

F(q)=Fatt(q)+Frep(q)=

-?Uatt(q)-?Urep(q)

(4)

與移動(dòng)機(jī)器人相比，人工勢場法規(guī)劃機(jī)械臂路徑時(shí)，不僅要保證機(jī)械臂末端到達(dá)目標(biāo)點(diǎn)，還要考慮機(jī)械臂各連桿與障礙物之間可能發(fā)生的碰撞.文中在機(jī)械臂的工作空間中建立了合適的人工勢場，并從振蕩問題、算法效率以及局部極小點(diǎn)等3個(gè)方面對(duì)人工勢場法進(jìn)行了改進(jìn).

圖4 人工勢場法原理

2.1 機(jī)械臂的引力場與斥力場

為了更好地將人工勢場法應(yīng)用于機(jī)械臂的路徑規(guī)劃，分別在關(guān)節(jié)空間以及笛卡爾空間中建立引力場和斥力場.

2.1.1 關(guān)節(jié)空間中建立引力場

引力場存在于機(jī)械臂與目標(biāo)位置之間，離目標(biāo)位置越遠(yuǎn)，引力越大，產(chǎn)生的引力牽引機(jī)械臂到達(dá)目標(biāo)位置.

在笛卡爾空間中規(guī)劃多自由度機(jī)械臂路徑時(shí)，需要不斷進(jìn)行逆運(yùn)動(dòng)學(xué)解算，常常導(dǎo)致奇異解的出現(xiàn).為了避免此問題，在機(jī)械臂的關(guān)節(jié)空間內(nèi)建立引力場Uatt(q)[15].

(5)

式中：q=(θ1,θ2,θ3,θ4,θ5,θ6)T為機(jī)械臂當(dāng)前位姿；qgoal為目標(biāo)位姿；ζ為引力增益.設(shè)置距離閾值d，防止產(chǎn)生引力過大導(dǎo)致避障失敗.

對(duì)引力場Uatt(q)求負(fù)梯度，可得引力：

(6)

2.1.2 笛卡爾空間中建立斥力場

斥力場存在于機(jī)械臂與障礙物之間，產(chǎn)生的斥力Frep(q)使機(jī)械臂遠(yuǎn)離障礙物，其大小與機(jī)械臂到障礙物最短的距離ρ(q)成反比.

在關(guān)節(jié)空間內(nèi)建立斥力場十分復(fù)雜，需要在發(fā)生碰撞的所有機(jī)械臂姿態(tài)Qobs中計(jì)算出最小的ρ(q).因此，在更容易計(jì)算ρ(q)的笛卡爾空間中建立機(jī)械臂的斥力場[15].

由于不能將機(jī)械臂看作質(zhì)點(diǎn)，在機(jī)械臂上定義幾個(gè)控制點(diǎn)作為斥力場中機(jī)械臂的受力點(diǎn).選擇機(jī)械臂的3、5和6關(guān)節(jié)所在坐標(biāo)原點(diǎn)O1、O2、O3作為控制點(diǎn)，如圖5.

圖5 機(jī)械臂控制點(diǎn)

設(shè)ai(q)為位姿q時(shí)控制點(diǎn)Oi的坐標(biāo)，對(duì)于控制點(diǎn)Oi有斥力勢能：

(7)

式中：ηi為控制點(diǎn)Oi斥力增益;ρ(ai(q))為Oi到障礙物的最短距離;ρ0為障礙物對(duì)Oi產(chǎn)生斥力的最大距離.

進(jìn)一步可得施加在Oi上的斥力為:

(8)

式中：?ρ(ai(q))=(ai(q)-Os)/‖ai(q)-Os‖，其中Os為障礙物上離控制點(diǎn)Oi最近的點(diǎn).

(9)

式中：Jv,i(q)為機(jī)械臂雅克比矩陣中線速度的部分.

2.2 引入斥力變增益系數(shù)

機(jī)械臂在斥力作用區(qū)的某些位置，會(huì)發(fā)生Fatt(q)>Frep(q)和Fatt(q)

對(duì)此，引入斥力變增益系數(shù)λ，使斥力增益在振蕩發(fā)生階段呈遞減趨勢，減小斥力增益與引力增益的比值，避免合力方向的突變，可以實(shí)現(xiàn)較為平滑的軌跡，具體如下：

ηi=λ·η0

(10)

式中：η0為初始斥力增；ρs為合力方向突變處到障礙物的最短距離；Δρ為λ作用范圍閾值；κ為(0,1]之間的常數(shù).

圖6為引入斥力變增益系數(shù)前后機(jī)械臂末端在Z軸上的運(yùn)動(dòng)，可以看出，引入斥力變增益系數(shù)后，機(jī)械臂在障礙物附近的振蕩情況明顯改善.

圖6 機(jī)械臂末端在Z軸上的運(yùn)動(dòng)

2.3 引入自適應(yīng)步長策略

為了進(jìn)一步提升算法的搜索效率，在路徑搜索的過程中引入自適應(yīng)步長策略.

機(jī)械臂下一步的位姿qnext通過梯度下降方向來確定，設(shè)機(jī)械臂當(dāng)前位姿為q，有：

(11)

式中：α為搜索步長；ò為最大允許誤差.

算法根據(jù)控制點(diǎn)與障礙物的距離自適應(yīng)修改步長α.當(dāng)障礙物較遠(yuǎn)時(shí)，采用大步長提高搜索效率；當(dāng)障礙物較近時(shí)，縮小步長，保障算法精度，具體如下：

(12)

式中：α0為初始步長；τi為控制點(diǎn)Oi的權(quán)重系數(shù)，τ3>τ2>τ1>0.

2.4 提出雙向規(guī)劃算法

針對(duì)人工勢場法規(guī)劃路徑時(shí)容易陷入局部極小點(diǎn)的問題，提出基于人工勢場法的雙向規(guī)劃算法.局部極小點(diǎn)具體表現(xiàn)為：當(dāng)機(jī)械臂在進(jìn)行路徑搜索時(shí)，在某個(gè)位置上機(jī)械臂受到的引力與斥力的合力恰好處于一個(gè)極小值點(diǎn)，此時(shí)任意關(guān)節(jié)角度的變化都將導(dǎo)致合力變大，機(jī)械臂將在此位置反復(fù)運(yùn)動(dòng)，機(jī)械臂陷入局部極小，算法將會(huì)失效.

雙向規(guī)劃就是用人工勢場法從初始點(diǎn)和目標(biāo)點(diǎn)分別規(guī)劃出兩條反向的路徑，每次搜索兩段路徑的目標(biāo)點(diǎn)都會(huì)更新，機(jī)械臂受到的引力也會(huì)隨之變化，一定程度上避免了局部極小點(diǎn)的出現(xiàn).最后將這兩條路徑拼接成最終路徑.

雙向規(guī)劃的具體步驟如下：

步驟1：由機(jī)械臂末端在笛卡爾空間中的初始點(diǎn)以及目標(biāo)點(diǎn)pstart、pgoal得到機(jī)械臂在關(guān)節(jié)空間內(nèi)的初始值qstart以及目標(biāo)值qgoal.

步驟2：從qstart開始作正向規(guī)劃，初始目標(biāo)為qgoal；從qgoal作逆向規(guī)劃，初始目標(biāo)為qstart.

(13)

(14)

步驟4：重復(fù)步驟3，直到經(jīng)過j次搜索后，正向和逆向規(guī)劃得到結(jié)果滿足以下條件，算法結(jié)束.

(15)

然而，即便滿足式(15)，仍然有可能存在兩端路徑拼接處不平滑的情況，所以需要對(duì)雙向規(guī)劃的拼接處作平滑處理.

對(duì)單個(gè)關(guān)節(jié)，設(shè)有待處理數(shù)據(jù){(xk,φk) |k=1,2…,n}，xk為路徑時(shí)刻點(diǎn)，φk為關(guān)節(jié)角度，三次光滑樣條曲線函數(shù)f滿足以下最優(yōu)條件：

(16)

式中：μ≥0為曲線平滑系數(shù)，μ越大，生成的曲線越平滑.最終的平滑效果如圖7(以關(guān)節(jié)4、5為例).

圖7 拼接處平滑

3 雙臂避碰路徑規(guī)劃

對(duì)于工作空間存在重疊的雙機(jī)械臂，避碰路徑規(guī)劃十分必要，因此基于改進(jìn)人工勢場法的雙向規(guī)劃算法應(yīng)用于雙機(jī)械臂路徑規(guī)劃，雙臂采用主從運(yùn)動(dòng)模式，先針對(duì)環(huán)境障礙物規(guī)劃主臂路徑，然后將主臂作為動(dòng)態(tài)障礙規(guī)劃從臂的路徑.

3.1 雙機(jī)械臂主從運(yùn)動(dòng)模式

雙臂協(xié)作方式主要有3種：雙臂共同完成一個(gè)任務(wù)且機(jī)械臂末端無相對(duì)運(yùn)動(dòng)；雙臂共同完成一個(gè)任務(wù)且機(jī)械臂末端有相對(duì)運(yùn)動(dòng)；雙臂分別完成獨(dú)立的任務(wù).文中雙臂分別執(zhí)行不同的任務(wù)，有不同的起始點(diǎn)與目標(biāo)點(diǎn)，并且機(jī)械臂末端存在相對(duì)運(yùn)動(dòng).

為了簡化規(guī)劃算法，雙臂采用主從模式運(yùn)動(dòng)，將機(jī)械臂分為主臂和從臂，先對(duì)主臂進(jìn)行路徑規(guī)劃，然后將主臂視為動(dòng)態(tài)障礙規(guī)劃從臂路徑.

主臂路徑的規(guī)劃比較簡單，與常規(guī)的單機(jī)械臂避障路徑規(guī)劃相同，只需要考慮避開環(huán)境中的障礙物，使用提出的基于改進(jìn)人工勢場法的雙向規(guī)劃算法對(duì)主臂進(jìn)行避障規(guī)劃，具體流程如圖8.

圖8 主從運(yùn)動(dòng)規(guī)劃流程

3.2 基于主臂動(dòng)態(tài)碰撞模型的從臂路徑規(guī)劃

規(guī)劃從臂路徑時(shí)情況要復(fù)雜一些，不僅要考慮環(huán)境障礙物，還要將主臂作為一個(gè)運(yùn)動(dòng)的障礙物進(jìn)行避碰.不同于靜態(tài)障礙物，需要對(duì)主臂建立比較準(zhǔn)確的動(dòng)態(tài)碰撞模型.

由于主臂路徑已知，相當(dāng)于主臂每個(gè)時(shí)刻的運(yùn)動(dòng)姿態(tài)都已經(jīng)得到，可以據(jù)此建立主臂的動(dòng)態(tài)碰撞模型. 通常都是用膠囊體幾何結(jié)構(gòu)來簡化機(jī)械臂模型，即用球來代替機(jī)械臂關(guān)節(jié)，圓柱代替連桿.通過此方式建立的碰撞模型雖然精度較高，但是若將機(jī)械臂視為障礙，那么某些位姿下建立的碰撞模型可能是非凸的 (nonconvex)[17]，從而導(dǎo)致人工勢場法失效.

為了建立有效的動(dòng)態(tài)模型，取各關(guān)節(jié)坐標(biāo)原點(diǎn)處橫截面最小外接圓Oi(i=1,2…,6)構(gòu)成點(diǎn)集υ，然后以υ創(chuàng)建凸包作為主臂的碰撞模型.這樣在保證算法有效性的同時(shí)也減少了計(jì)算開銷，圖9為[0.75,1,-1,0,1.57,0]T姿態(tài)下的主臂碰撞模型.

圖9 主臂碰撞模型

在規(guī)劃從臂路徑時(shí)，按照雙向規(guī)劃從路徑兩端到中間的策略，依次更新主臂碰撞模型，并將其均勻的分布在從臂的規(guī)劃過程中.算法在最后收斂階段步長較小，搜索次數(shù)較多，此時(shí)保持主臂模型為中間狀態(tài)，直至從臂路徑規(guī)劃完成.

4 算法仿真

基于MATLAB軟件構(gòu)建仿真環(huán)境，通過仿真驗(yàn)證改進(jìn)人工勢場法雙向規(guī)劃算法的有效性以及雙臂避碰路徑規(guī)劃的可實(shí)現(xiàn)性.

4.1 主臂路徑規(guī)劃結(jié)果及算法驗(yàn)證

首先規(guī)劃主臂路徑，并與傳統(tǒng)人工勢場法對(duì)比驗(yàn)證提出算法的有效性.

設(shè)主臂基坐標(biāo)為[0.350, 0, 0]，從臂基坐標(biāo)為[-0.350, 0, 0].障礙物為兩個(gè)半徑0.1 m的球體，球心坐標(biāo)分別為[-0.100, 0.200, 0.750]和[-0.1, -0.03, 0.95].

主臂初始關(guān)節(jié)量為[0,0,0,0,0,0]T，末端坐標(biāo)為[0.350, 0.380, 0.620].設(shè)主臂末端目標(biāo)點(diǎn)為[-0.100, -0.200, 0.750]，初始搜索步長α0=0.05，距離閾值d=0.4，引力增益系數(shù)ζ=1 000，斥力產(chǎn)生最小距離ρ0=0.4，斥力初始增益η0=1 000.

首先采用傳統(tǒng)(未改進(jìn))人工勢場法進(jìn)行路徑規(guī)劃，經(jīng)過43次左右的搜索后，主臂會(huì)在點(diǎn)[0.033, 0.100, 0.783]附近陷入局部極小點(diǎn)，無法完成路徑規(guī)劃，已規(guī)劃的部分也存在障礙物附近振蕩的問題.具體結(jié)果如圖10、11.

圖10 主臂路徑(傳統(tǒng)人工勢場法)

圖11 主臂末端位置(傳統(tǒng)人工勢場法)

而使用雙向規(guī)劃可以成功規(guī)劃主臂避障路徑，正向和逆向規(guī)劃搜索次數(shù)各為31次，分別收斂在點(diǎn)[0.031,0.173,0.815]和[0.038,0.162,0.826]，仿真結(jié)果如圖12.

圖12 主臂路徑(雙向規(guī)劃)

主臂在關(guān)節(jié)空間以及笛卡爾空間內(nèi)的軌跡如圖13、14.

圖13 主臂關(guān)節(jié)角

圖14 主臂末端位置

為了進(jìn)一步驗(yàn)證所提算法，在[-0.100, -0.200, 0.750]附近取了15個(gè)不同的點(diǎn)作為目標(biāo)點(diǎn)，分別使用傳統(tǒng)人工勢場法和文中算法對(duì)主臂進(jìn)行路徑規(guī)劃，最終得到實(shí)驗(yàn)結(jié)果如表2.

表2 規(guī)劃結(jié)果對(duì)比

綜上，相較于傳統(tǒng)單向的人工勢場法路徑規(guī)劃，基于改進(jìn)人工勢場法的雙向規(guī)劃算法夠避免局部極小值以及振蕩現(xiàn)象的發(fā)生，以較高的成功率實(shí)現(xiàn)避障路徑規(guī)劃.生成的路徑無論在關(guān)節(jié)空間內(nèi)還是在任務(wù)空間內(nèi)都更加平滑，機(jī)械臂在運(yùn)動(dòng)過程中不會(huì)出現(xiàn)奇異解.

4.2 從臂路徑規(guī)劃結(jié)果

根據(jù)規(guī)劃出的主臂路徑得到主臂的動(dòng)態(tài)碰撞模型，將其作為障礙規(guī)劃從臂的路徑.

設(shè)從臂末端初始點(diǎn)為[-0.350, 0.380, 0.620]，目標(biāo)點(diǎn)為[0.100, -0.400, 0.300]，使用雙向規(guī)劃算法對(duì)從臂進(jìn)行路徑規(guī)劃，正向和逆向規(guī)劃搜索次數(shù)各為39次，分別收斂在點(diǎn)[0.158, 0.124, 0.150]和[0.169, 0.112, 0.149]，從臂在關(guān)節(jié)空間以及笛卡爾空間內(nèi)的軌跡如圖15、16.

圖15 從臂關(guān)節(jié)角

圖16 從臂末端位置

同時(shí)執(zhí)行主臂和從臂路徑，雙臂避碰路徑仿真結(jié)果如圖17.

圖17 雙臂避碰路徑

仿真結(jié)果顯示，雙機(jī)械臂在運(yùn)動(dòng)過程中沒有發(fā)生碰撞，基于改進(jìn)人工勢場法的雙向規(guī)劃算法可以實(shí)現(xiàn)雙機(jī)械臂的避碰路徑規(guī)劃.

5 結(jié)論

文中對(duì)工作空間存在重疊的雙機(jī)械臂避碰路徑規(guī)劃問題進(jìn)行了研究，首先提出了基于改進(jìn)人工勢場法的雙向規(guī)劃算法，從振蕩問題、算法效率以及局部極小點(diǎn)等3個(gè)方面對(duì)傳統(tǒng)人工勢場法進(jìn)行了改進(jìn).雙機(jī)械臂在主從運(yùn)動(dòng)模式下，先使用改進(jìn)算法規(guī)劃主臂路徑，然后根據(jù)主臂路徑得到主臂的動(dòng)態(tài)碰撞模型，將其作為障礙規(guī)劃從臂路徑.仿真結(jié)果顯示，提出的規(guī)劃算法能夠?qū)崿F(xiàn)雙機(jī)械臂的避碰路徑規(guī)劃，并且有著較高的路徑質(zhì)量.后續(xù)將進(jìn)一步提升算法在未知?jiǎng)討B(tài)環(huán)境下的規(guī)劃能力.