基于改進人工勢場法雙向規劃的雙機械臂避碰路徑規劃

邱 博，單 梁，常 路，王志強，屈 藝

(南京理工大學 自動化學院，南京 210094)

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

國內外學者對雙機械臂路徑規劃做了大量研究.文獻[1]將機械臂連桿間的最短距離作為避碰距離指標對冗余雙臂機器人進行協調避碰.文獻[2]從能量的角度規劃雙機械臂的路徑，在雙臂之間設置一個虛擬的彈簧，彈簧受到彈力會迫使雙臂互相遠離.文獻[3]使用動態神經網絡算法規劃具有物理約束的雙機械臂避碰路徑.但是只考慮雙臂間的避碰并不能實現雙臂協同運動，還要同時考慮機械臂的路徑規劃.

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

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

1 雙機械臂建模

以ABB公司生產的IRB 120型六軸機械臂為對象建立雙機械臂模型，對其進行運動學分析，并以此來研究雙機械臂避碰路徑規劃.

1.1 機械臂運動學分析

采用D-H方法[11]建立機械臂的運動學模型.IRB120型機械臂尺寸參數及簡化模型如圖1、2.

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

圖2 簡化模型

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

表1 D-H參數

由機械臂D-H參數，可以通過坐標變換得到相鄰兩個關節之間的轉換矩陣為：

(1)

由此，可進一步得到機械臂末端與機械臂基座之間的轉換矩陣為：

(2)

綜上，可以通過機械臂的關節角度，得到機械臂末端的位姿，即正運動學；反之，也可以反解公式(2)通過末端位姿求得機械臂關節量，即逆運動學.

1.2 雙機械臂位姿描述

文中兩臺機械臂分為主臂和從臂，兩機械臂具有相同的初始姿態，并關于X軸對稱，通過MATLAB中的Robotics System Toolbox建立圖3雙機械臂模型.

圖3 雙機械臂模型

從臂可以看作是主臂平移而來，從臂的基坐標原點Xslaver滿足如下轉換公式：

Xslaver=Xmaster·Ttrans

(3)

式中：Xmaster為主臂的基坐標原點；Ttrans為轉換矩陣.

2 基于改進人工勢場法的雙向規劃算法

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

設機器人當前位置為q，人工勢場施加在機器人上的合力為：

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

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

(4)

與移動機器人相比，人工勢場法規劃機械臂路徑時，不僅要保證機械臂末端到達目標點，還要考慮機械臂各連桿與障礙物之間可能發生的碰撞.文中在機械臂的工作空間中建立了合適的人工勢場，并從振蕩問題、算法效率以及局部極小點等3個方面對人工勢場法進行了改進.

圖4 人工勢場法原理

2.1 機械臂的引力場與斥力場

為了更好地將人工勢場法應用于機械臂的路徑規劃，分別在關節空間以及笛卡爾空間中建立引力場和斥力場.

2.1.1 關節空間中建立引力場

引力場存在于機械臂與目標位置之間，離目標位置越遠，引力越大，產生的引力牽引機械臂到達目標位置.

在笛卡爾空間中規劃多自由度機械臂路徑時，需要不斷進行逆運動學解算，常常導致奇異解的出現.為了避免此問題，在機械臂的關節空間內建立引力場Uatt(q)[15].

(5)

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

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

(6)

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

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

在關節空間內建立斥力場十分復雜，需要在發生碰撞的所有機械臂姿態Qobs中計算出最小的ρ(q).因此，在更容易計算ρ(q)的笛卡爾空間中建立機械臂的斥力場[15].

由于不能將機械臂看作質點，在機械臂上定義幾個控制點作為斥力場中機械臂的受力點.選擇機械臂的3、5和6關節所在坐標原點O1、O2、O3作為控制點，如圖5.

圖5 機械臂控制點

設ai(q)為位姿q時控制點Oi的坐標，對于控制點Oi有斥力勢能：

(7)

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

進一步可得施加在Oi上的斥力為:

(8)

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

(9)

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

2.2 引入斥力變增益系數

機械臂在斥力作用區的某些位置，會發生Fatt(q)>Frep(q)和Fatt(q)

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

ηi=λ·η0

(10)

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

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

圖6 機械臂末端在Z軸上的運動

2.3 引入自適應步長策略

為了進一步提升算法的搜索效率，在路徑搜索的過程中引入自適應步長策略.

機械臂下一步的位姿qnext通過梯度下降方向來確定，設機械臂當前位姿為q，有：

(11)

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

算法根據控制點與障礙物的距離自適應修改步長α.當障礙物較遠時，采用大步長提高搜索效率；當障礙物較近時，縮小步長，保障算法精度，具體如下：

(12)

式中：α0為初始步長；τi為控制點Oi的權重系數，τ3>τ2>τ1>0.

2.4 提出雙向規劃算法

針對人工勢場法規劃路徑時容易陷入局部極小點的問題，提出基于人工勢場法的雙向規劃算法.局部極小點具體表現為：當機械臂在進行路徑搜索時，在某個位置上機械臂受到的引力與斥力的合力恰好處于一個極小值點，此時任意關節角度的變化都將導致合力變大，機械臂將在此位置反復運動，機械臂陷入局部極小，算法將會失效.

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

雙向規劃的具體步驟如下：

步驟1：由機械臂末端在笛卡爾空間中的初始點以及目標點pstart、pgoal得到機械臂在關節空間內的初始值qstart以及目標值qgoal.

步驟2：從qstart開始作正向規劃，初始目標為qgoal；從qgoal作逆向規劃，初始目標為qstart.

(13)

(14)

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

(15)

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

對單個關節，設有待處理數據{(xk,φk) |k=1,2…,n}，xk為路徑時刻點，φk為關節角度，三次光滑樣條曲線函數f滿足以下最優條件：

(16)

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

圖7 拼接處平滑

3 雙臂避碰路徑規劃

對于工作空間存在重疊的雙機械臂，避碰路徑規劃十分必要，因此基于改進人工勢場法的雙向規劃算法應用于雙機械臂路徑規劃，雙臂采用主從運動模式，先針對環境障礙物規劃主臂路徑，然后將主臂作為動態障礙規劃從臂的路徑.

3.1 雙機械臂主從運動模式

雙臂協作方式主要有3種：雙臂共同完成一個任務且機械臂末端無相對運動；雙臂共同完成一個任務且機械臂末端有相對運動；雙臂分別完成獨立的任務.文中雙臂分別執行不同的任務，有不同的起始點與目標點，并且機械臂末端存在相對運動.

為了簡化規劃算法，雙臂采用主從模式運動，將機械臂分為主臂和從臂，先對主臂進行路徑規劃，然后將主臂視為動態障礙規劃從臂路徑.

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

圖8 主從運動規劃流程

3.2 基于主臂動態碰撞模型的從臂路徑規劃

規劃從臂路徑時情況要復雜一些，不僅要考慮環境障礙物，還要將主臂作為一個運動的障礙物進行避碰.不同于靜態障礙物，需要對主臂建立比較準確的動態碰撞模型.

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

為了建立有效的動態模型，取各關節坐標原點處橫截面最小外接圓Oi(i=1,2…,6)構成點集υ，然后以υ創建凸包作為主臂的碰撞模型.這樣在保證算法有效性的同時也減少了計算開銷，圖9為[0.75,1,-1,0,1.57,0]T姿態下的主臂碰撞模型.

圖9 主臂碰撞模型

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

4 算法仿真

基于MATLAB軟件構建仿真環境，通過仿真驗證改進人工勢場法雙向規劃算法的有效性以及雙臂避碰路徑規劃的可實現性.

4.1 主臂路徑規劃結果及算法驗證

首先規劃主臂路徑，并與傳統人工勢場法對比驗證提出算法的有效性.

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

主臂初始關節量為[0,0,0,0,0,0]T，末端坐標為[0.350, 0.380, 0.620].設主臂末端目標點為[-0.100, -0.200, 0.750]，初始搜索步長α0=0.05，距離閾值d=0.4，引力增益系數ζ=1 000，斥力產生最小距離ρ0=0.4，斥力初始增益η0=1 000.

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

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

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

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

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

主臂在關節空間以及笛卡爾空間內的軌跡如圖13、14.

圖13 主臂關節角

圖14 主臂末端位置

為了進一步驗證所提算法，在[-0.100, -0.200, 0.750]附近取了15個不同的點作為目標點，分別使用傳統人工勢場法和文中算法對主臂進行路徑規劃，最終得到實驗結果如表2.

表2 規劃結果對比

綜上，相較于傳統單向的人工勢場法路徑規劃，基于改進人工勢場法的雙向規劃算法夠避免局部極小值以及振蕩現象的發生，以較高的成功率實現避障路徑規劃.生成的路徑無論在關節空間內還是在任務空間內都更加平滑，機械臂在運動過程中不會出現奇異解.

4.2 從臂路徑規劃結果

根據規劃出的主臂路徑得到主臂的動態碰撞模型，將其作為障礙規劃從臂的路徑.

設從臂末端初始點為[-0.350, 0.380, 0.620]，目標點為[0.100, -0.400, 0.300]，使用雙向規劃算法對從臂進行路徑規劃，正向和逆向規劃搜索次數各為39次，分別收斂在點[0.158, 0.124, 0.150]和[0.169, 0.112, 0.149]，從臂在關節空間以及笛卡爾空間內的軌跡如圖15、16.

圖15 從臂關節角

圖16 從臂末端位置

同時執行主臂和從臂路徑，雙臂避碰路徑仿真結果如圖17.

圖17 雙臂避碰路徑

仿真結果顯示，雙機械臂在運動過程中沒有發生碰撞，基于改進人工勢場法的雙向規劃算法可以實現雙機械臂的避碰路徑規劃.

5 結論

文中對工作空間存在重疊的雙機械臂避碰路徑規劃問題進行了研究，首先提出了基于改進人工勢場法的雙向規劃算法，從振蕩問題、算法效率以及局部極小點等3個方面對傳統人工勢場法進行了改進.雙機械臂在主從運動模式下，先使用改進算法規劃主臂路徑，然后根據主臂路徑得到主臂的動態碰撞模型，將其作為障礙規劃從臂路徑.仿真結果顯示，提出的規劃算法能夠實現雙機械臂的避碰路徑規劃，并且有著較高的路徑質量.后續將進一步提升算法在未知動態環境下的規劃能力.