一種改進人工勢場法的機械臂路徑規劃方法

趙光明，馬明月，曾祥凱

（公安部道路交通安全研究中心，北京100068）

1 引言

路徑規劃技術在機器人領域中是一項尤為重要的技術，多自由度機械臂的避障路徑規劃技術更是其中的難點。人工勢場法由美國科學家Khatib于二十世紀八十年代所提出來的一種虛擬力法[1-2]。相比于其他算法，人工勢場法算法優點在于算法實現簡單，算法的計算量較小，所規劃處的路徑相對較為平坦，但是也存在障礙物附近目標不可達以及在復雜障礙物環境中易陷入局部最小值無法運動等缺陷。

為了解決這些問題，國內外學者對其進行了改進：文獻[3]通過對斥力場模型進行改進優化，通過設置虛擬目標點解決移動機器人在搜索路徑研究過程中可能出現的局部最小值問題；文獻[4]提出了一種機械臂逆運動學數值解求解方法，并結合改進勢場人工勢場法實現機械臂的避障路徑規劃；文獻[5]通過對勢場函數進行改進，同時增加虛擬障礙物以避免機械臂進入局部最小值處；文獻[6]提出一種改進勢場的人工勢場法，當路徑搜索陷入局部最小值時，引入虛擬排斥力對局部最小值位置進行逃逸。

文獻[7]提出采用遺傳算法與人工勢場算法融合以此來對人工勢場法進行改進，搜索出一條無碰撞的路徑；文獻[8]通過改進斥力場的人工勢場與模糊算法相結合，利用模糊決策系統調整機器人運動，使得規劃出的路徑平滑；文獻[9]通過改進人工勢場法中的勢場模型，去除斥力場，設置動態引力場，在機器人運動過程中不斷對引力場進行修正，最后對生成的路徑進行平滑；文獻[10]通過改進人工勢場法中的勢力場模型，引入指數因子，采用量子遺傳算法對規劃出的路徑進行優化平滑。文獻[11]提出在排斥勢場函數中加入距離修正因子，解決了目標不可達的問題，同時采用正則六邊形導向法來改進局部最小值問題，并且提出動態環境下運動目標的相對速度檢測與躲避方法，結果表明該算法能夠及時地避開動態障礙物。文獻[12]對人工勢場法的函數模型進行修整，通過在機器人路徑搜索的過程中引入對周圍環境的辨識，引入虛擬目標點對局部最小值點進行逃逸。文獻[13]將人工勢場法與A*算法結合，利用人工勢場法在A*算法生成的初始節點之間進行路徑搜索，對降低路徑的振蕩性起到了一定的作用。文獻[14-16]通過使用RRT算法自動生成虛擬目標點解決了局部最小值問題。文獻[17]提出一種結合機器人自身狀態以及環境信息的“預測”人工勢場法，解決“凹型”障礙物的局部極小值問題，提高算法對環境適應性的同時降低了路徑長度。文獻[17]等提出一種振蕩性較小的人工勢場函數模型，在相應的關鍵節點通過路徑擬合的方法來進行路徑搜索。文獻[18]提出將人工勢場與模擬退火算法結合，克服了人工勢場法在路徑搜索時易陷入局部最小值無法繼續路徑搜索的缺陷。文獻[19]在改進的勢函數基礎上，提出通過扇區劃分的方法，在移動機器人陷入局部極小點周圍適當范圍內增加虛擬障礙物，使其和原障礙物及目標點共同作用下對移動機器人產生作用力，以能夠解決傳統人工勢場法中出現的局部極小點情況。文獻[20]采用螢火蟲算法并引入NUBRS 曲線來光滑處理局部路徑，縮短總路徑長度。

在現有研究基礎上，針對人工勢場法應用在機械臂路徑規劃領域存在的問題，提出一種改進的人工勢場法。通過改進勢場模型，提出一種自適應大步長模擬退火-人工勢場算法，結合冗余節點刪除策略與拐點消除算法對生成路徑進行平滑處理，得到平滑無碰撞的軌跡。

2 傳統人工勢場法

傳統人工勢場法中的目標點引力場函數模型如下：

式中：Uatt—機器人當前點處的引力勢場；q=[x y z]—機器人當前位置坐標；qg=[xg yg zg]—空間中機器人的目的坐標位置；Katt—所述引力勢場的增益系數；‖q-qg‖—機器人當前位置與機器人目的坐標的距離。

對應的引力為：

障礙物點處斥力場函數模型如下：

式中：Ureq—障礙物在機器人點出的斥力勢場；Kreq—所述斥力勢場的增益系數；ρ—機器人當前點q與障礙物點qo的兩者之間的直線距離；qo=[xo yo zo]—障礙物在空間中的坐標位置；ρ0—障礙物點的影響距離。

對應的斥力為：

機器人所受的合勢場為：

機器人所受的合力為：

式中：N—障礙物的數目。

典型障礙物環境中算法仿真示意圖，如圖1所示。

圖1 典型障礙物算法仿真示意圖Fig.1 Simulation Diagram of Typical Obstacle Algorithm

3 改進人工勢場法

3.1 改進勢場函數模型

改進引力場函數模型為：

式中：ds—引力勢場距離閾值。

對應的引力為：

改進后的斥力場函數模型如下：

式中：n—大于0的實數。

與之對應的斥力為：

其中，

定義Freq1的方向由障礙物指向機器人，Freq2的方向由機器人指向目標點位置。此時機器人受到的合外力為：

3.2 局部最小值判定

這里采用的局部最小值的判定準則如下，若滿足以下兩個條件之一，均斷定機器人路徑搜索均陷入局部最小值：

條件1：若機器人在路徑搜索的節點處按照式中計算出來該節點出的合外力F(q)為0時，此時機器人處于局部最小值狀態，即：

條件2：記錄搜索步為第t步與第t，步時機器人在路徑搜索的節點處的位置坐標，即qt=[xt yt zt]、qt，=[xt，yt，zt，]。若在第t，步處節點的位置坐標qt，與第t步節點的位置坐標qt的距離小于給定的路徑搜索步長αλ0，α∈( 0，1 )時（這里中α取值為0.9），此時判定機器人處于局部最小值狀態，即：

3.3 局部最小值逃逸算法

由于多自由度機械臂在笛卡爾空間中的路徑規劃不僅需要考慮機械臂末端在三維空間中的路徑規劃，還需要考慮機械臂奇異解以及目標構型是否適用的問題。通過設定搜索半徑對周圍的環境進行探測，計算當前位置、目標位置與障礙物環境的位置關系，判斷是否能夠從當前位置與目標點之間構建一條“跨越”障礙物的路徑，若可以，則跳出局部最小值；反之，采用模擬退火算法進行路徑搜索。為能夠盡快逃離局部最小值，一方面通過對局部最小值處的空間進行劃分，使模擬退火算法能夠向障礙物空間較少的方向進行搜索；另一方面，使用大步長進行路徑搜索，使其能夠盡快逃離局部最小值位置。為避免在路徑搜索過程中出現機械臂奇異解問題，結合機械臂的工作空間分析，在機械臂的奇異空間處添加虛擬障礙物，以此避免路徑搜索取該點為路徑節點。局部最小值逃逸算法的具體步驟如下：

（1）記錄當前局部最小的位置，初始化算法中如下設計參數：初始工作溫度為T0、退火速率為α、機器人探測半徑R、截止溫度為Ts=0.05T0；

（2）判斷局部最小值位置處與目標中間是否存在障礙物，若不存在，將目標點作為路徑搜索的下一節點，結束局部最小值逃逸；反之，進入（3）；

（3）以局部最小值為原點建立笛卡爾坐標系，Z坐標軸的正方向由原點指向目標點，X坐標軸方向與基座標系相同，Y坐標軸由右手定則確定。將空間四等分，計算機器人探測半徑中各個象限中障礙物的數目分別為n1、n2、n3和n4，如圖2所示。

圖2 機器人搜索空間劃分示意圖Fig.2 Robot Search Space Division

（4）隨機產生新解：根據（3）在計算障礙物最少的象限中，在XOY平面中隨機角度范圍內給移動機器人隨機角度θx、θz，機器人以式計算隨機點的坐標位置qrand=[xrand yrand zrand]，并在陷入局部最小值的位置處放置虛擬障礙物。

式中：λ′—局部最小值逃逸步長，取λ′=3λ。

（5）根據式（5）計算隨機點的合勢能Urand、前一個點的合勢能Ubef以及勢能變化量ΔU=Urand-Ubef；

（6）采用Metropolis準則，判斷是否接受當前隨機解，若ΔU＜0，則接受當前隨機解，否則產生一個隨機數r，且當滿足式（15）時，則接受當前隨機解，進行下一步；否則，重復（4）；

（7）按照退火策略T=αT更新參數，判斷是否跳出局部最小值，若已逃逸，退出局部最小值逃逸算法。否則，重復（2）至（6），若T＜Ts，仍未逃逸局部最小值，將角度搜索范圍擴大到整個坐標系進行搜索，重復（1）到（6）直至逃逸出局部最小值點，若T ＜Ts且無法逃離局部最小值，輸出路徑搜索無解。

算法流程圖。如圖3所示。

圖3 局部最小值逃逸算法流程圖Fig.3 Flow Chart of Local Minimum Escape Algorithm

相同仿真參數，如表1所示。利用提出的局部最小值逃逸策略重新進行仿真，仿真示意圖，如圖4所示。

圖4 典型局部障礙物仿真結果圖Fig.4 Simulation Results of Typical Local Obstacles

表1 算法參數表Tab.1 Algorithm Parameters Table

從圖4中可以看出，改進后的人工勢場法能夠解決傳統人工勢場法的缺陷，在四種典型障礙物環境中均能完成路徑搜索任務。但是存在鋸齒形的震蕩點，在實際工程應用中無法適用，因此需要對生成的軌跡進行平滑處理。

3.4 路徑冗余節點刪除策略以及平滑處理

由上文可知，雖然改進人工勢場法能夠解決傳統人工勢場法的缺陷，但是生成的路徑并不平滑，存在“鋸齒”狀的冗余節點，導致生成的路徑無法應用于實際。因此，為了提高生成路徑的質量，這里提出一種路徑優化平滑算法，消除改進后算法生成的路徑節點出現的震蕩現象。算法的具體步驟如下：

（1）將路徑搜索到的路徑節點放入數組，Qi(xi，yi，zi)，i∈[ 1，j]，新建篩選后的路徑節點數組為LQ；其中：J—路徑搜索得到的路徑節點的數目；

（2）初始化障礙物位置坐標數組為Ci，i∈[0，N]，其中，N為障物的數目。設置路徑平滑算法的起始點為Qm，其中：m=1。設置下一判斷路徑節點為Qn，其中：n=m+2。判斷線段QmQn與各個障礙物Ci是否存在碰撞，若無碰撞現象，n=n+1；否則，將Qm放入篩選后的路徑節點數組LQ中，并且m=m+1；

（3）重復上述（2），直至m=J；經過路徑冗余刪除策略處理后的路徑節點示意圖，如圖5所示。其中黑色的路徑節點為改進人工勢場法規劃出的路徑，綠色軌跡為經過路徑冗余刪除策略處理后的軌跡。

圖5 路徑冗余節點刪除策略示意圖Fig.5 Schematic Diagram of Path Redundant Node Deletion Strategy

從圖5可以看出，通過設置相應的安全距離參數，經過冗余節點刪除策略處理后，路徑雖然得到了優化，但是其中仍然存在拐點，因此，需要通過步驟四對經步驟三得到的路徑進行拐角平滑處理，處理后的示意圖，如圖6所示。其中，一種顏色軌跡為經過路徑平滑策略處理后的軌跡。

圖6 路徑平滑算法示意圖Fig.6 Schematic Diagram of Path Smoothing Algorithm

（4）將獲得折線路徑的拐角左右兩條折線上取得中點以及靠近拐點的四等分點，采用四次貝塞爾樣條插值原理進行平滑處理，當對n個頂點進行p次B樣條插值時，其曲線方程式為：

式中：P(i)(i=0，1，2...n)—控制點的坐標；Ni，p(t)(i=0，1，2...n)—p次B樣條基函數；i—序號；t=[t0，t1，...，tn+p+1]—樣條函數的節點矢量。其中，

算法流程圖，如圖7所示。

圖7 路徑優化算法流程圖Fig.7 Flow Chart of Path Optimization Algorithm

4 仿真與實驗驗證

4.1 典型障礙物條件下路徑優化算法的驗證

為對提出的路徑冗余節點刪除策略以及平滑算法進行驗證，同樣選擇人工勢場法的典型障礙情況下，在改進后算法生成路徑節點的基礎上利用冗余節點刪除策略以及路徑平滑算法進行優化，與傳統算法相比，路徑平滑后的仿真結果示意圖，如圖8所示。

圖8 路徑平滑效果圖Fig.8 Path Smoothing Rendering

4.2 復雜障礙物情況算法仿真對比

分別采用文獻[18-19]中所提出的算法以及本文提出的算法在復雜障礙物環境中進行仿真分析，在得到機器人的路徑軌跡，并采用本文提出的路徑優化策略將得到的路徑節點分別進行路徑優化處理，所得到的優化前后的路徑仿真示意圖，如圖9（b）、圖9（c）所示，這里提出的算法所得到的優化前后的路徑仿真示意圖，如圖9（d）所示。從圖8（a）可以看出，機器人在當前障礙物環境中無法完成路徑軌跡規劃。相同參數下、同樣的環境中利用文獻以及本文的算法進行仿真，從圖9（b）、圖9（c）、圖9（d）中可以看出，三種算法均能使得機器人克服傳統人工勢場法的缺點，均能在障礙空間環境內從設置的起點S與終點G之間搜索出一條無碰撞的路徑。

圖9 復雜環境中算法對比圖Fig.9 Comparison of Algorithms in Complex Environment

為了避免算法的隨機性，同樣的環境中進行30次仿真，得到四種算法的路徑平滑前后的平均路徑長度，如表2所示。

表2 算法仿真對比路徑優化前后長度Tab.2 Algorithm Simulation Comparing the Length Before and After Path Optimization

結合圖與表可以看出，相比之下，這里提出的改進算法生成的路徑更短，能夠更快地逃出局部最小值點，但是在經過路徑優化策略處理前，三種算法所得到的路徑并不平滑且均存在振蕩點。

在經過這里提出的路徑優化策略進行處理后，三種算法均能消除無碰撞搜索路徑的振蕩特性，都能得到一條平滑無碰撞的軌跡，表明了算法的有效性。

4.3 機械臂避障實驗

以AUBO-I5機械臂為研究對象，利用這里提出的機械臂避障策略進行避障實驗驗證，設置機械臂初始末端關節坐標為（-0.058，0.085，0.046），設置機械臂目標位置為（-0.817，0.101，0.107，保持算法各項參數與單障礙物環境機械臂避障仿真一致，將障礙物信息輸入到上位機完成環境信息構建，點擊“路徑規劃啟動按鈕”，只針對機械臂末端TCP點在笛卡爾空間坐標系中的位置進行路徑規劃，機械臂在路徑搜索過程中與障礙物的示意圖，如圖10所示。

圖10 機械臂避障路徑規劃圖Fig.10 Obstacle Avoidance Path Planning Diagram of Manipulator

從圖10中可以看出，機械臂能夠避開障礙物安全地到達指定的終點。為進一步分析，利用機械臂提供的接口函數獲取機械臂在運動過程中各個關節的角度，如圖11所示。

圖11 實驗環境機械臂關節角的變化Fig.11 Variation of Joint Angle of Manipulator in Experimental Environment

從圖11中可以看出，機械臂各個關節均在電機的角度約束范圍以內，并且生成的角度平滑，結合前文的基于Matlab的避障仿真實驗，進一步驗證了本文提出的機械臂避障路徑規劃策略的有效性。

5 總結

在分析傳統人工勢法固有缺陷基礎上，通過在引力場中添加引力安全閾值，在斥力場中添加路徑搜索當前點與目標點的歐式距離，引入自適應大步長的模擬退火算法對局部最小值進行逃逸。為消除搜索出的路徑出現的震蕩點以及冗余節點，提出一種冗余節點刪除策略與拐點消除算法。并在典型障礙物環境、隨機障礙物環境以及復雜障礙物環境對算法進行了仿真驗證，仿真結果表明路徑搜索算法的有效性。