陳滿意，張 橋，，張 弓，梁濟民，侯至丞，楊文林，徐 征，王 建

(1.武漢理工大學 機電工程學院，湖北 武漢 430070；2.廣州中國科學院先進技術(shù)研究所，廣東 廣州 511458；3.中國科學院深圳先進技術(shù)研究院，廣東 深圳 518055)

1 問題的描述

如今，協(xié)作機器人發(fā)展迅速，在手機等3C行業(yè)發(fā)揮的作用越來越大，如圖1所示為多機器人協(xié)作進行手機配件裝盒。由于機器人布置密集，工作的時候存在很多干涉碰撞問題，因此協(xié)作機器人避障路徑規(guī)劃成為了一大關鍵突破點，眾多學者都對其進行了研究。避障路徑規(guī)劃是指在工作空間存在障礙物的情況下，機器人在有限的時間內(nèi)規(guī)劃出一條無碰撞的路徑[1-2]，由于工業(yè)機械臂的應用環(huán)境越來越復雜，避障路徑規(guī)劃的作用也越來越重要。

人工勢場法由Khatib[3]于1985年提出，其規(guī)劃的路徑質(zhì)量高，但是容易陷入局部極小而發(fā)生振蕩?？焖贁U展隨機樹(Rapidly-exploring Random Tree, RRT)算法由Lavalle[4]于1998年正式提出，其在多障礙物環(huán)境下搜索成功率很高，但是搜索隨機性大、效率低。很多學者對這兩種算法分別進行了改進，姬偉等[5]提出一種基于虛擬目標點的人工勢場法用于機械臂逃離局部極小值;Wang等[6]提出一種基于改進吸引力勢函數(shù)的人工勢場法,實現(xiàn)機械臂關節(jié)角速度的平滑過渡;Seyyed等[7]提出一種基于代價函數(shù)的人工勢場法，用于解決路徑搜索容易陷入局部極小值和目標不可達的問題;陳晉音等[8]提出改進的EB_RRT算法,解決障礙物附近路徑搜索迭代次數(shù)過高的問題;Wei等[9]提出一種S-RRT路徑規(guī)劃算法,提高路徑節(jié)點擴展效率;何兆楚等[10]提出人工勢場-RRT算法,解決路徑搜索容易陷入局部極小值的問題。

以上研究都是在機械臂的關節(jié)空間進行規(guī)劃，需要將障礙物轉(zhuǎn)換到關節(jié)空間，計算復雜。為此，本文在笛卡爾坐標空間進行機械臂避障路徑規(guī)劃，為了在每個步長都取得最優(yōu)的逆運動學關節(jié)角，保證前后步長對應關節(jié)角度值變化連續(xù)性，參考Craig[11]對機械臂運動學逆解的處理，提出最短行程逆解算法;參考Ketchel等[12]提出的包圍球和圓柱包絡法對障礙物和機械臂連桿進行近似模擬，檢測機械臂連桿和障礙物的碰撞,提出一種低振蕩人工勢場—自適應快速擴展隨機樹(Adaptive RRT, ARRT)混合算法，先采用低振蕩人工勢場法進行搜索，當遇到局部極小、碰撞等情況時，切換成ARRT算法進行搜索，兩種算法結(jié)合使用，直至到達目標點。低振蕩人工勢場法降低了路徑的振蕩程度，ARRT算法則在經(jīng)典RRT算法的基礎上加入自適應避障策略，提高搜索效率。另外，為提高路徑質(zhì)量，提出冗余路徑節(jié)點刪除策略，并使用四次貝塞爾曲線[13]對路徑進行擬合。

2 算法原理

2.1 碰撞檢測方法

本文研究對象為六自由度機械臂，碰撞檢測指的是障礙物與機械臂連桿的碰撞檢測，而實際工程應用中障礙物的外形一般不規(guī)則，很難通過精確的數(shù)學公式進行表達。因此，本文采用包圍球?qū)φ系K物進行近似模擬，采用圓柱包絡法對機械臂連桿進行簡化，如圖2所示。該方法雖然增加了障礙物和機器人本體的區(qū)域，但是大大簡化了計算量。

在三維空間內(nèi)計算障礙物中心點和每根連桿所在直線的距離dis，比較dis和R+r的大小，若dis≥R+r，則障礙物和連桿沒有發(fā)生碰撞;若障礙物與機器人各連桿均滿足無碰撞條件，則機器人當前狀態(tài)滿足無碰撞條件。

2.2 最短行程解算法

本次路徑規(guī)劃研究在笛卡爾空間完成，根據(jù)機械臂末端位姿采用逆運動學求解最多可求得八組解，為了在每個步長都取得最優(yōu)的逆運動學關節(jié)角，保證前后步長對應關節(jié)角度值變化連續(xù)性，提出了最短行程逆解算法，因此兩個步長之間每個關節(jié)的角度變化需盡可能小，即實現(xiàn)“最短行程”。機械臂空間搜索每移動一個步長的最短行程解即使得每一個關節(jié)角度變化最小，求出關節(jié)角變量的L1范數(shù)最小值即可，又因為機械臂6個連桿中靠近末端的連桿主要決定機械臂末端執(zhí)行器的姿態(tài)，遠離末端的連桿主要決定機械臂末端執(zhí)行器的位置，所以求最短行程解時需要加權(quán)，滿足式(1)的各關節(jié)角度值即為最短行程解：

(1)

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

2.3 低振蕩人工勢場法

人工勢場法是局部路徑規(guī)劃方法，目標點對機械臂產(chǎn)生引力作用，障礙物對機械臂產(chǎn)生排斥力作用，引力和斥力的合力方向決定機器人運動方向，如圖3所示。

(2)

(3)

其中：Uatt為引力勢函數(shù);ka為引力系數(shù)，Probot為機器人當前位置;Pg為目標點位置;Urep表示斥力勢函數(shù);kr為斥力系數(shù);l為機器人與障礙物之間的距離;l0為斥力場作用范圍。對于如何選擇其中的常量值，文獻[14]進行了詳細的論述。

機器人的勢能總和

U=Uatt+Urep。

(4)

如圖4所示為經(jīng)典人工勢場法和低振蕩人工勢場法的仿真效果對比。在障礙物、步長等其他條件均相同的情況下，圖中粗線代表低振蕩人工勢場法規(guī)劃的路徑，其振蕩幅度相比于經(jīng)典人工勢場法路徑明顯降低。

2.4 ARRT算法

經(jīng)典RRT算法的原理如圖5所示，機器人每一次路徑搜索都需要尋找一個隨機點xrand，在已經(jīng)存在的路徑節(jié)點中選取距離xrand最近的點xnear并以步長step朝著隨機點xrand的方向進行擴展，以此方式搜索路徑直至到達終點。

(x1-x0)(x-x0)+(y1-y0)(y-y0)+

(z1-z0)(z-z0)=0。

(5)

則通過式(5)可以計算

zE=(x1-x0)(x0+cosθ-x0)+

(y1-y0)(y+sinθ-y0)/z1-z0+z0。

(6)

x2=x0+

y2=y0+

z2=z0+

(7)

如表1所示為經(jīng)典RRT算法和ARRT算法經(jīng)過MATLAB仿真分析之后的迭代次數(shù)對比，在障礙物、步長等其他條件均相同的情況下仿真分析20次，ARRT算法的平均迭代次數(shù)相比于經(jīng)典的RRT算法的平均迭代次數(shù)從143.8次降低到了89.8次，有效地提高了障礙物附近機器人避障路徑搜索的效率。

表1 經(jīng)典RRT和ARRT平均迭代次數(shù)對比

3 低振蕩人工勢場-ARRT混合算法

3.1 人工勢場法和RRT算法的結(jié)合

利用一種算法的優(yōu)點來彌補另外一種算法的缺點是學術(shù)研究的重要思路[15-16]，本文提出低振蕩人工勢場—ARRT混合算法，機器人先采用低振蕩人工勢場法進行搜索，當遇到局部極小、碰撞等情況時，切換成ARRT算法進行搜索，兩種方法結(jié)合使用，直至到達目標點。判斷碰撞的方法已經(jīng)在前文敘述，判斷機器人由于局部極小值點等引起的振蕩則采用以下策略。

當前節(jié)點為qend，其前一個節(jié)點為qend-1，qnew為當前確定的新的路徑節(jié)點。由圖7中夾角θ可以看出，qend-1、qend、qnew這3個節(jié)點已經(jīng)有出現(xiàn)振蕩的趨勢，因此判斷夾角θ的大小即可判斷人工勢場法是否出現(xiàn)振蕩，在平面內(nèi)和三維空間都適用。設qend坐標為A(x0,y0,z0)，qend-1坐標為B(x1,y1,z1)，qnew坐標為C(x2,y2,z2)，則由余弦定理：

(8)

具體閾值取θ≤π/6時，判定人工勢場法可能出現(xiàn)振蕩，切換為ARRT算法進行搜索，逃離之后再切換回低振蕩人工勢場法進行搜索。

算法偽代碼如下：

low-oscillation artificial potential field-ARRT hybrid

1.T←init(qinit)；

2.qnew←Low-Oscillation_Artificial_Potential(x);

3.Collosion_detection(qnew) or Oscillation_detection(qnew,qend,qend-1) then

4.qnew←?

5.qnew←ARRT(x);

6.If Collosion_detection(qnew)

7.Return advanced;

8.Else

9.If qnew=qendthen

10.Return Reached;

11.Else

12.Extend(T,qnew)

13.Return Low-Oscillation_Artificial_Potential(x);

14.Else

15.If qnew=qendthen

16.Return Reached;

17.Else

18.Extend(T,qnew);

算法流程圖如圖8所示。使用最短行程解算法來保證機械臂在笛卡爾空間規(guī)劃的關節(jié)角度變化連續(xù)性，規(guī)劃過程在笛卡爾空間進行，不需要將障礙物轉(zhuǎn)化到關節(jié)空間，可節(jié)省很大的計算量。

2.2 路徑優(yōu)化

2.2.1 冗余路徑節(jié)點刪除策略

由于RRT算法的隨機性，Sudhakara等[17]提出RRT*算法進行節(jié)點路徑的優(yōu)化。但是RRT*的算法所使用的路徑優(yōu)化策略只針對于單獨使用RRT算法的情況，因此本文提出一種冗余路徑節(jié)點刪除策略，其步驟描述如下：

(1)低振蕩人工勢場法切換到ARRT算法。

(2)定義Tree數(shù)組，Tree相當于一個節(jié)點容器用來收集低振蕩人工勢場—ARRT混合算法的節(jié)點。定義RRTtree數(shù)組，該數(shù)組用來收集ARRT算法的節(jié)點，確定本次ARRT擴展的新節(jié)點qnew。

(3)新增節(jié)點qnew到Tree數(shù)組，qnew的前一個節(jié)點，也就是當前節(jié)點為qend，將qend和qnew到RRTtree數(shù)組。

(4)按照節(jié)點添加的時間先后順序，在RRTtree數(shù)組中從第一個節(jié)點開始計算和qnew的距離distance，已知步長d，假設滿足如下條件：

distance

(9)

則定義該節(jié)點為qinter，并直接連接qnew和qinter，若沒有節(jié)點和qnew的距離小于一個步長，則直接轉(zhuǎn)步驟(6)。

(5)刪除Tree數(shù)組和RRTtree數(shù)組中qinter和qnew之間的所有節(jié)點。

(6)切換回人工勢場法。刪除振蕩中間點，即當前節(jié)點qend的前一個節(jié)點qend-1。

如圖9所示，由節(jié)點2、3、4等可判斷路徑出現(xiàn)振蕩，因此刪除節(jié)點3，并連接節(jié)點2和節(jié)點4。又因節(jié)點4和12之間無碰撞，且距離接近一個步長，連接節(jié)點4和12并刪除它們之間的節(jié)點5、6、7、8、9、10、11。

如圖10所示為在低振蕩人工勢場-ARRT混合算法的基礎上使用冗余路徑節(jié)點刪除策略與否的仿真效果對比。在障礙物、步長等其他條件均相同的情況下，使用冗余路徑節(jié)點刪除策略，路徑明顯得到優(yōu)化。

2.2.2 四次貝塞爾曲線擬合

為了使得路徑在節(jié)點轉(zhuǎn)折點處變得光滑，使用四次貝塞爾曲線對路徑進行擬合，包含n+1個頂點的n次貝塞爾曲線方程如式(5)：

(10)

式中：Pi為控制多邊形的頂點;Bi,n(t)為伯恩斯坦基函數(shù)。

為了降低路徑的波動性，但又需要盡量保證擬合的曲線和原軌跡曲線的一致性，選用四次貝塞爾曲線進行擬合，四次貝塞爾曲線最多只能擬合5個點，因此需要使用多段貝塞爾曲線進行拼接，為了保證幾何連續(xù)性，在起點xinit與節(jié)點1中間取過渡點1-2，之后每隔3個節(jié)點取1個過渡點，每2個過渡點和它們之間的3個節(jié)點共5個點擬合一條四次貝塞爾曲線，以此類推，如圖11所示。

2.3 技術(shù)路線流程圖

已知起點、終點和步長，采用低振蕩人工勢場-ARRT算法進行搜索，每搜索一個步長，則采用最短行程逆解算法計算機械臂的最優(yōu)關節(jié)角，再進行碰撞檢測，規(guī)劃出一條無碰撞路徑之后，采用冗余路徑節(jié)點刪除策略和貝塞爾曲線擬合提高路徑質(zhì)量，如圖12所示為技術(shù)路線流程圖。

3 仿真分析

為驗證算法的有效性，采用丹麥優(yōu)傲公司的UR5機器人模型作為研究對象，使用MATLAB進行仿真分析。在空間中設置起點和終點，在有限的時間若能規(guī)劃出一條從起點到終點的無碰撞路徑，則避障路徑規(guī)劃成功。該機器人為六自由度機械臂，如圖13所示。

如表2所示為機器人的運動控制參數(shù)，其中起點和終點根據(jù)機器人工作空間范圍進行選擇，搜索步長和斥力場作用范圍根據(jù)機器人和障礙物外形尺寸進行設定。

表2 機械臂運動控制參數(shù)表

仿真分析1算法對環(huán)境復雜程度的適應性實驗。

經(jīng)過4次分析對比，每次相比前一次多增加一個障礙物，模擬環(huán)境復雜程度。由圖14可以看出，隨著障礙物數(shù)量增加，低振蕩人工勢場-ARRT混合算法仍然能夠指引機械臂成功避障。

仿真分析2算法搜索效率驗證。

在障礙物、步長等其他條件均相同的情況下，比較低振蕩人工勢場—ARRT混合算法與經(jīng)典人工勢場法、經(jīng)典RRT算法的路徑搜索成功率和搜索時間，實驗環(huán)境為MATLAB 2016a，計算機處理器為Intel core i3，每組仿真分析均進行20次，結(jié)果取平均值，如表3所示。經(jīng)典人工勢場法雖然在障礙物比較少的情況下路徑搜索時間短，但是障礙物數(shù)量一旦增加，路徑搜索成功率低；經(jīng)典RRT算法搜索時間長，而且隨著障礙物數(shù)量的增加，其路徑搜索成功率降低到60%；低振蕩人工勢場—ARRT混合算法隨著障礙物數(shù)量增加，能保證路徑搜索成功，滿足避障要求，相比于經(jīng)典RRT算法，在多障礙物環(huán)境下的平均路徑搜索時間從26.1 s下降到3.6 s，避障路徑規(guī)劃效果得到顯著改善。

表3 低振蕩人工勢場-RRT混合算法路徑搜索成功率和搜索時間與經(jīng)典算法的比較

4 結(jié)束語

本文通過分析機械臂在工業(yè)生產(chǎn)以及協(xié)調(diào)作業(yè)方面的需求，對機械臂在工作空間中避障的能力進行了研究。在機械臂的正逆運動學的基礎上提出了最短行程逆解算法，在經(jīng)典人工勢場法和快速擴展隨機樹法(RRT)的基礎上提出了低振蕩人工勢場法和自適應快速擴展隨機樹法(ARRT)，并將兩種算法結(jié)合成低振蕩人工勢場—ARRT混合算法，完成了對算法在多障礙物環(huán)境下的適應能力的提升，提高了算法的搜索成功率和搜索效率。通過仿真分析驗證了算法的效果，實現(xiàn)了在多障礙物環(huán)境下避障路徑搜索，滿足避障要求，相比于經(jīng)典RRT算法，算法在多障礙物環(huán)境下的平均路徑搜索時間從26.1 s下降到3.6 s，避障路徑規(guī)劃效果得到顯著改善。

本文針對機械臂的避障研究基于靜態(tài)障礙物環(huán)境，下一步將針對動態(tài)障礙物環(huán)境對機械臂的避障算法進行研究，加快算法實時性，在滿足機械臂動力學參數(shù)的情況下優(yōu)化路徑質(zhì)量。