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

,

(浙江工業大學 信息工程學院，杭州 310023)

0 引言

路徑規劃[1]一直都是機器人領域中的重點難點，是機器人順利完成指定要求的前提條件，而避障路徑規劃[2]是指在給定障礙物以及起始點和目標點的環境中，找出一條能到達目標點并且能夠安全、無碰撞地到通過所有障礙。

常見的路徑規劃方法有基于局部信息的人工勢場法[3]等和基于全局信息的自由空間法[4]等兩大類，其中第一種是一種經典的最優規劃算法，它是把環境信息轉變為機械臂所受到的勢能，并在該合勢能的驅動下運動，由于其不需要全局信息，結構簡單，生成路徑平滑，規劃時間短等優點所以非常適合靜態環境下的實時路徑規劃，但由于并不是全局規劃算法，所以容易進入局部極小點。針對此問題，很多學者對算法進行了改進，比如修改勢能函數[5]消除局部極小點，加入虛擬目標點或者虛擬障礙物[6-7]來改變機械臂周圍環境從而跳出局部極小點，結合遺傳算法等其他智能算法退出局部極小點[8]，采用沿墻跟蹤算法[9]解決局部極小點問題等。

本文以NAO機器人為平臺，對運動學正解、碰撞檢測和路徑規劃等方面進行了研究，針對如何提高機械臂的有效空間，提出使用橢球包圍盒對障礙物進行建模；針對人工勢場法易陷入局部極小點的問題，則采用RRT[10-12]方法對其進行局部改進。

1 機械臂運動學建模與分析

1.1 機械臂運動學模型

本文基于NAO機器人平臺進行建模，其機械臂有5個自由度，具體結構如圖1所示，肩關節有兩個自由度，肘關節有兩個自由度，腕關節有一個自由度，因為腕關節不影響機械臂末端位置，因此角度固定為0。

圖1 NAO機械臂結構

通過圖1所示，運用D-H法[13]可以建立NAO兩個連桿的坐標系，如圖2所示。

圖2 NAO機械臂各連桿坐標系

根據圖2建立的NAO兩個連桿的坐標系和NAO機器人手臂的各項參數，可以得到NAO手臂的Modified D-H參數，如表1所示。

表1 NAO機械臂MDH參數

其中，αi代表扭角，為常量；ai代表連桿長度，為常量；di代表連桿偏置距離，因為NAO機械臂均為為轉動關節，因此為常量；θi代表關節角，因為NAO機械臂均為為轉動關節，所以θi均為變量。

1.2 機械臂運動學分析

在獲取MDH參數后，我們可以用關節角的變化來確定末端的位姿，這稱為機械臂的正運動學分析。在確定好基坐標系之后，我們通過相鄰兩連桿之間的變換矩陣通式(1)[13]來計算末端執行器相對于基坐標系的位姿。

(1)

將DH參數代入上式(1)，可得連桿之間的齊次變換矩陣：

其中：c1s1是cosθ1sinθ1的簡寫，Ci-1Si-1是cosαi-1sinαi-1的簡寫，將以上連桿齊次變換矩陣相乘可得末端相對于基座標的位姿，如式(2)：

0Te=0T11T22T33T44T5

(2)

2 碰撞檢測

碰撞檢測是避障路徑規劃的重要部分，想要機械臂在規定時間內找出一條無碰的可行路徑，需要對障礙物進行合理建模，之后進行碰撞檢測和分析。

2.1 障礙物建模

由于機械臂的工作環境不同，有的機械臂面對的是提前規劃好的規則障礙物，例如工業機械臂，然而更多的時候，機械臂運行環境中的障礙物都是不規則形狀，想要精確建模需要花費大量時間，因此我們一般采取包圍盒法[14]對障礙物進行近似建模。

包圍盒法建模有許多種類，例如球體，AABB，OBB，K-Dops等包圍盒[15]，但是不同的包圍盒的簡單性和緊密性是一對矛盾體[16]，由于機械臂是一個高度耦合的非線性系統，越大的工作空間越有利于控制，而且我們又希望對機械臂進行實時控制，復雜的包圍盒建立與碰撞檢測需要更多的時間，因此綜合考慮，我們采用橢球包圍盒來近似地對障礙物進行建模，此種方法建模難度雖然和常用的球體包圍盒相比增大了，但是緊密性較高，碰撞分析也不復雜。

首先給出橢球的表達式(3)：

(3)

其中：(x0,y0,z0)為橢球的中心點，a,b,c為橢球的三個軸長，有了這6個參數，我們便可以建立障礙物的橢球包圍盒模型。為了獲取上述6個參數，我們將障礙物分別在xoy，yoz和xoz投影，從而獲取障礙物在x，y，z軸上的投影的最大值和最小值xmin,xmax,ymin,ymax,zmin,zmax，通過計算我們可以得出橢球中心點如下式(4)：

(4)

根據獲取的障礙物數據，我們可以任選一個點，例如點(xmax,ymax,zmax)，利用拉格朗日求極值方法獲取包圍障礙物的體積最小的橢球的剩余三個參數a,b,c。

首先我們列出計算橢球的體積公式(5)：

(5)

然后將我們任取的一點帶入橢球方程得式(6)：

(6)

此時，只需要求出參數a,b,c就可以計算出橢球的完整參數方程，這里通過拉格朗日求極值方法，我們可以寫出拉格朗日函數(7)：

L=V+k·F

(7)

對式(7)求出a,b,c三個參數的偏導數，并令其都為0，同時我們令式(6)也為0，聯立方程如下式(8)：

(8)

通過式(8)我們可以求出橢球的剩余3個參數a,b,c。至此，我們已經完全求出了包圍障礙物的最小的橢球參數方程。

2.2 碰撞檢測

進行碰撞檢測的時候，我們要將機械臂的半徑也加在障礙物模型中，使其簡化為一條線段，并通過DH參數法計算出機械臂連桿兩端的坐標，從而確定線段的參數方程(9)。

(9)

將上式帶入到橢球的方程(3)中可得式(10)：

(10)

我們對式(10)進行求解，可以求得k的值，通過對其值進行分析，可以判斷連桿和障礙物是否碰撞，具體情況如表2。

表2 碰撞檢測分析

3 改進人工勢場法

3.1 人工勢場法基本原理

人工勢場法是一種虛擬力法，在機械臂的工作環境中由目標點產生虛擬的引力勢場，由障礙物產生一個虛擬的斥力勢場，其中的引力勢場和斥力勢場函數可以由研究人員根據不同情況自己設定。機械臂在合勢能的驅動下，自動向合勢能的下降方向運動。傳統引力和斥力函數分別為式(11)和式(12)。

(11)

(12)

引力勢場的作用領域為全局，斥力勢場的作用領域為局部，其中ka為引力系數，kr為斥力系數，p為機械臂末端位置，pg為目標點位置，d為橢球到機械臂關節的距離，d0為斥力勢場的作用范圍。對于移動機器人來說，如果可以將其看作質點的話，某個障礙物只會對其產生一個排斥力，但是對于機械臂來說，每個關節都會受到一個障礙物的產生的斥力，本文實驗平臺為NAO機器人機械臂，其手臂為仿人形機械臂，所以會有肘部和腕部兩個關節受到斥力，因此斥力勢能為兩個關節受到的斥力勢能之和，如式(13)：

Urep=Urepw+Urepe

(13)

其中Urepw,Urepe分別是腕(wrist)部和肘部(elbow)關節受到的斥力勢能，則整個機械臂的合勢能為(14)：

U=Uatt+Urep

(14)

推斷出合勢能公式之后，我們搜尋一條合勢能下降最快的方向，驅動機械臂運動，圖3為其原理圖。

圖3 人工勢場法原理圖

由于人工勢場法是一種局部搜索方法，很容易就陷入局部極小點或者在某點附近振蕩，在使用的時候需要格外關注這種目標不可達的情況。針對這種缺陷，本文采用了添加虛擬目標點的思想，在全局尋找路徑的時候采用人工勢場法，當機械臂陷入到局部極小點的時候，轉而采用RRT方法，自動生成一個新的目標點，從而通過改變合勢場大小和方向，達到跳出局部極小點的目的。

3.2 RRT方法基本原理

RRT方法是LaValle教授于1998年提出的一種基于隨機采樣的運動規劃算法，基本思想用在機械臂上就是將其末端的初始位置作為根節點，對其工作空間進行隨機采樣，通過特定的擴展方式不停地增加葉子節點，直到到達目標點區域后停止，隨后按照一定的評價函數在生成的樹中確定一條從初始節點到目標點的路徑，這個評價函數可以是距離最近，步數最少或者時間最短等。

其中的擴展方式的基本方法是隨機選取一個工作空間中的點設為qtarget，然后計算樹中節點與qtarget歐式距離最近的點稱為qnearest，從qnearest開始向qtarget方向擴展L距離，設為qnew，判斷qnearest到qnew這段新生成的樹枝是否和障礙物碰撞，碰撞則舍棄，重新選取qtarget，否則加入到快速樹的節點中。

RRT方法可以對機械臂進行完整的避障路徑規劃，并且只要參數設置合理，并不會出現陷入局部極小點的現象，但是由于其采用隨機采樣生成快速隨機擴展樹，所以擴展會過于平均，整個算法的效率較低。而兩種方法相結合，既克服了人工勢場法容易陷入局部極小點或振蕩點的缺陷，又沒有加入RRT方法效率低下的缺點。

3.3 改進人工勢場法

整個算法的基本思想是在使用人工勢場法陷入極小點或振蕩點的時候，將此點設置為RRT樹的初始節點，開始進行有限次數的隨機擴展，然后計算出生成的隨機擴展樹中節點距障礙物歐氏距離最遠的一個葉子節點作為目標點再次使用人工勢場法進行接下來的路徑規劃。

下面是具體步驟：

1)使用第1，2節的方法分別建立機械臂和障礙物的模型。

2)設置目標點。

3)遍歷機械臂的相鄰關節角，分別為θi-λ,θi,θi+λ，其中θi為當前關節角，λ為步進步長。

4)根據第1節的正運動學分析，分別獲取腕關節和肘關節的位置。

5)使用第2節的方法對NAO機器人的機械臂進行碰撞檢測，如果碰撞了則舍棄并跳回到步驟3)；如若沒有，則分別求出腕關節和肘關節距障礙物的歐氏距離，以及腕關節到目標點的歐氏距離。

6)將步驟5)求得的距離信息帶入式(14)中求合勢能，記錄遍歷中的最小合勢能的機械臂末端位置。

7)將步驟6)求得的末端位置與前5次遍歷求得的機械臂末端位置隊列進行比較，如果出現在隊列中，證明進入了極值點或者目標點，此時進入步驟8)，否則進入步驟9)。

8)判斷末端執行器位置是否為最終目標點，如果是則算法結束，否則判斷是否是產生的虛擬目標點；如果是，我們將算法中的目標點替換為最終目標點，并跳回至步驟2，如果以上兩者都不滿足，則進入局部極小點處理流程，進入步驟10)。

9)機械臂運動到合勢能最小的位置。

10)將得到的局部極小點或振蕩點設為qinit，即為RRT樹的初始根節點。

11)設置循環次數N，迭代N次后停止算法。

12)結合機械臂的工作空間和障礙物位置并使用隨機函數產生空間隨機點，并設為qtarget。

13)求出RRT樹中距qtarget距離最近的節點qnear，給定歐式距離L，求出qnear與qtarget連線上距qnear節點長為L的點，設為qnew。

14)判斷qnear到qnew的空間線段是否和障礙物有碰撞，有碰撞則舍棄并跳回到步驟12，否則將qnew節點加到樹中。

15)判斷迭代是否完成，未完成則跳轉到步驟12)，完成則求出隨機樹節點中除根節點以外的距障礙物最遠的節點，將其設置為人工勢場法的虛擬目標點，即跳回到步驟2)。

其中qnew點擴展公式如式(15)：

(15)

圖4是算法的流程圖。

圖4 算法流程圖

4 仿真實驗

本文使用Matlab的數值仿真來模擬機械臂運動情況，進行兩組數值實驗，第一組是驗證使用橢球包圍盒的優越性，第二組是驗證能否成功跳出局部極小點或振蕩點。

第一組實驗具體參數設置如表3。

表3 實驗參數設置

機械臂初始關節角設定如下：

(θ1,θ2,θ3,θ4,θ5)=

(1.000°,0.500°,0.500°,-1.000°,0°)

此時機械臂成下垂狀態。

目標點設定如下：

(x,y,z)=(12 cm,8 cm,1 cm)

障礙物位置如下：

(xmin,xmax,ymin,ymax,zmin,zmax)=

(3 cm,7 cm,4 cm,8 cm,0 cm,10 cm)

圖5展示了采用球體包圍盒時機械臂的運行情況。

圖5 使用球體包圍盒時機 圖6 使用橢球包圍盒時機械臂運行情況展示 械臂運行情況展示

由圖5可以看出使用球體包圍盒時目標點在障礙物模型中，此時會因為算法中針對這種情況的報錯而直接停止。圖6是相同的障礙物設置情況下使用橢球包圍盒進行障礙物建模時的機械臂運動情況。

如圖所示，機械臂準確地到達了目標點，誤差在10-4數量級，而且僅步進了166步，滿足了機械臂控制的精確性和實時性，如果對控制精度要求不高，對實時性的要求比較高的情況下，可以增加步進步長，這樣可以大幅提高規劃速度，但是要犧牲一定的精確度。

由此可見相同障礙物的情況下，使用橢球包圍盒所占用的體積遠遠小于球體包圍盒，此時機械臂可以到達目標點，而使用球體包圍盒卻不能到達目標點。

第二組實驗是算法進入極小點的情況，實驗參數設置如表3。

機械臂初始關節角設定如下：

(θ1,θ2,θ3,θ4,θ5)=(0.5°,-90°,0°,-45°,0°)

目標點設定如下：

(x,y,z)=(14.46 cm,5 cm,3.96 cm)

障礙物設定如下：

(xmin,xmax,ymin,ymax,zmin,zmax)=

(14 cm,15 cm,1.5 cm,2.5 cm,3 cm,5 cm)

圖7是機械臂此時的狀態和障礙物還有目標點的位置。

圖7 機械臂陷入局部極小點

進入局部極小點之后直接跳轉到RRT算法，快速計算出一個虛擬目標點，具體情況如圖8所示。

圖8 RRT算法產生虛擬目標點

在由RRT算法產生虛擬目標點之后，我們將其設置為虛擬目標點，本次實驗中獲取的虛擬目標點如下：

(x,y,z)=(12.89 cm,0.44 cm,2.04 cm)

圖9 機械臂末端運行軌跡

由圖9可知，機械臂的末端從局部極小點開始向尋找到的虛擬目標點運動，運動到虛擬目標點之后將最終的目標點設定為算法中的目標點之后，機械臂繼續向著最終目標點運動，已經跳出了局部極小點，圖10是機械臂整體運行情況的展示。

如圖10所示，機械臂從局部極小點經由RRT產生的虛擬目標點最終到達真正的目標點，從局部極小點到虛擬目標點僅需要34步，從虛擬目標點到真正目標點僅需46步，誤差同樣保持在10-4數量級，整個過程快速精確，可以滿足機械臂的實時控制。

為了驗證本文方法的適用范圍，針對正常情況和陷入極值點情況分別做了10組實驗，實驗參數設置均能使機械臂在工作空間內與障礙物不碰撞的情況下到達目標點，正常情況下的10次實驗均規劃成功，步進步數隨著障礙物和目標點距離初始點的遠近不等，最少步進步數為132步，最多步進步數為268步；進入局部極小點情況下10次實驗均規劃成功，其中有1次實驗進行了2次RRT算法尋找虛擬目標點的過程，沒有實驗進行3次及以上RRT算法過程，步進步數隨著障礙物和目標點距離初始點的遠近不等，最少步進步數為80步，最多步進步數為197步，其中從局部極小點到虛擬目標點最少步進步數為34，最多步進步數僅為91。

5 結束語

本文針對工作空間狹小的機械臂提出了使用橢球包圍盒建模方法，使機械臂能在有限的工作空間內盡量正常工作；針對人工勢場法這種算法容易陷入局部極小點或振蕩點的缺陷，提出了采用RRT算法來動態產生虛擬目標點的方法，通過此點來改變機械臂的周圍環境來跳出局部極小點的目的，相對于通過幾何方法算出虛擬目標點而言，采用動態虛擬目標點，不需要進行虛擬目標點的計算，整個過程快速準確，隨著障礙物的變動，不需要重新計算虛擬目標點，算法的適應性更強，更加適合機械臂的實時控制。本文下一步的研究方向則是針對多障礙物和移動障礙物進行實時避障路徑規劃，進一步提高本方法的適用范圍，同時提高規劃速度。