一種改進的人工勢場法路徑規(guī)劃算法

何世瓊，陳 雨

（四川大學電子信息學院，成都 610065）

0 引言

機械臂的路徑規(guī)劃是機械臂運動到指定位置執(zhí)行任務的前提，它的任務是找到一條從起始位置到目標位置的無碰撞運動軌跡。常見的路徑規(guī)劃算法有快速搜索隨機樹（RRT）、C空間搜索和人工勢場法等。其中，人工勢場法因為模型簡單、計算量小且實時性高等優(yōu)點而被廣泛使用。但是，作為一種典型的局部路徑規(guī)劃算法，傳統(tǒng)人工勢場法存在易陷入局部最小解的問題。

針對傳統(tǒng)人工勢場法的局部最小解問題，研究者從多個方面進行了改進。文獻［5-6］通過改進勢場函數(shù)模型來優(yōu)化傳統(tǒng)人工勢場法；文獻［7］將人工勢場法與RRT算法相結合，避免了人工勢場法陷入局部極小點的問題；文獻［8］將強化學習與人工勢場法結合，用于串聯(lián)機械臂的避障和路徑規(guī)劃；文獻［9］提出一種利用人工勢場法和蟻群算法相結合的路徑規(guī)劃方法。上述方法通過不同途徑解決了人工勢場法容易陷入局部最小解的問題，但是都不同程度地帶來了路徑不平滑，路徑離最優(yōu)較遠等問題。

本文針對傳統(tǒng)人工勢場法的缺陷，提出了一種改進的人工勢場路徑規(guī)劃算法，在解決局部最小解問題的同時，仍能得到較短的平滑路徑。算法先判斷路徑規(guī)劃是否陷入局部最小解，如果是，則啟動模擬退火策略進行隨機搜索，直到跳出局部最小點，得到起始位置到目標位置的有效路徑。

1 人工勢場法基本原理

人工勢場法是一種局部路徑規(guī)劃方法，它的基本思想是將機器人的運動空間看作一個虛擬的勢場，在這個勢場中，目標點產生引力引導機器人向其運動，障礙物產生斥力避免機器人與之碰撞，機器人在一個點所受的合力等于該點的引力和斥力之和，合力控制機器人向目標點運動。

人工勢場包括引力勢場和斥力勢場，假設勢場函數(shù)用表示，在點時的勢場合力用U表示，則有：

其中，()表示點的引力場函數(shù)，()表示斥力場函數(shù)。引力場函數(shù)通常定義為公式（2）：

式中：表示引力增益，(,)表示當前點到目標點之間的距離。

斥力場函數(shù)通常定義為公式（3）：

式中：表示斥力增益，()表示點距離最近障礙物的距離，表示障礙物斥力作用的距離閾值，超過這個閾值的障礙物對機器人不產生斥力。

在引力場和斥力場中，引力和斥力通常用兩種勢場的梯度來表示。假設點的勢場函數(shù)的值U為該點的能量，那么該點的梯度?U就能看作該點的力向量，引力場和斥力場中的虛擬力就可以表示為如下兩式：

結合勢場函數(shù)和勢場力的公式不難看出，勢場中某點處梯度的方向就是勢函數(shù)增長最快的方向，因此采用梯度下降法將人工勢場法應用于機器人的路徑規(guī)劃。所謂梯度下降法，就是讓機器人從起點沿著梯度的反方向運動，直到勢場的梯度為0。

人工勢場法對控制和傳感誤差有一定的魯棒性，但是這種方法也有明顯的缺點，當某點的引力和斥力的大小剛好相等、方向相反時，機器人會陷入局部最優(yōu)解，進而無法達到目標點。

2 基于模擬退火的改進人工勢場法

2.1 模擬退火法求全局最優(yōu)

模擬退火法的基本思想是從一個較高的初始溫度出發(fā)，伴隨溫度的不斷下降，結合一定的概率突跳特性在解空間中隨機尋找目標函數(shù)的全局最優(yōu)解，即概率性地跳出局部最優(yōu)解，并最終趨于全局最優(yōu)。以圖1為例，從A點出發(fā)，模擬退火算法在搜索到局部最優(yōu)解B點以后，隨機向右開始移動，以一定的概率沖過B和C之間的頂峰跳出局部最優(yōu)解。

圖1 模擬退火法跳出局部最優(yōu)示意圖

模擬退火算法更新解的機制依據(jù)Metropolis準則，Metropolis準則通常表示為：

其中，表示新解的接受概率，表示當前的溫度，()表示新解對應的系統(tǒng)能量，()表示當前解對應的系統(tǒng)能量。從公式（6）中可以看出，當新解更優(yōu)時直接更新，當新解的能量高于當前解時，是否更新則與當前的溫度和能量有關。如圖1所示，初始狀態(tài)在A點，隨著迭代次數(shù)的增加更新到點局部最優(yōu)解，此時B點的能量低于A點。到達B點后，根據(jù)當前狀態(tài)、溫度和能量計算更新概率并跳出B點，達到全局最優(yōu)的C點后穩(wěn)定下來。

模擬退火法求全局最優(yōu)的算法流程如圖2所示，在任意溫度水平下，隨機擾動產生新解，并計算目標函數(shù)值的變化，決定是否接受新解。由于算法初始溫度比較高，所以使能量增大的新解在初始時也可能被接受，因而能跳出局部極小解，然后通過緩慢地降低溫度，最終可能收斂到全局最優(yōu)解。

圖2 模擬退火法求全局最優(yōu)流程圖

2.2 改進人工勢場法

改進人工勢場法將模擬退火法引入人工勢場法中，當路徑規(guī)劃出現(xiàn)局部最小解時，通過退火策略的隨機搜索跳出局部最小解，算法流程圖如圖3所示，詳細的實現(xiàn)過程如下：

圖3 改進人工勢場法算法流程

（1）設置機械臂末端的起點、終點以及地圖中障礙物的位置，設機械臂末端的當前點為，局部最小解的點為。

（2）判斷末端是否陷入局部最小解，如果陷入局部最小解，則啟用模擬退火策略，同時設置初始溫度=。

（3）在當前點附近產生一個隨機點，根據(jù)勢場函數(shù)公式計算當前點和隨機點的勢能，分別用U和U表示，設置Δ=U-U。

（5）判斷末端是否逃離局部最小解，如果U≤U，則認為機械臂末端逃離了局部最小解，否則，逐漸減小溫度，直到末端跳出局部最小解，通常()=(-1)，0.85≤≤1，表示步長。

（6）判斷機械臂末端是否達到目標點，達到目標點后結束，否則重新設置初始溫度進行搜索。

3 仿真實驗

3.1 二維路徑規(guī)劃仿真實驗

二維路徑仿真實驗中，二維避障環(huán)境設置為10×10的正方形，環(huán)境中的障礙物是半徑為0.5的圓，機械臂末端的起點為（0，0），目標點的位置為（10，10），兩種算法的迭代次數(shù)均為1000次，實驗次數(shù)為50次。傳統(tǒng)APF算法的參數(shù)設置如表1所示，改進APF算法中的其他參數(shù)設置見表2。其中表示引力增益系數(shù)，表示斥力增益系數(shù)，表示退火策略的初始溫度，T表示退火策略的最低溫度，表示退火策略的移動步長，表示路徑規(guī)劃時的搜索步長，表示障礙影響距離，當障礙和機械臂的距離大于這個距離時，斥力為0，即不再受該障礙的影響。

表1 傳統(tǒng)APF算法參數(shù)設置

表2 改進APF算法參數(shù)設置

圖4為傳統(tǒng)APF路徑規(guī)劃的仿真結果，實心圓點表示障礙物，網(wǎng)格表示自由運動區(qū)域，（0，0）點表示起點，（10，10）點表示終點，密集的折線表示可能陷入局部最小解時模擬退火策略的隨機搜索，點狀虛線表示從起點到終點的有效路徑。

圖4 傳統(tǒng)APF和改進APF算法二維路徑規(guī)劃仿真結果

其中，圖4（a）～（f）分別表示在少量障礙物、較多障礙物和大量障礙物的場景下，傳統(tǒng)APF和改進APF在同一地圖中成功規(guī)劃的仿真效果。圖4（a）、（b）、（c）表示在三種障礙物場景下，傳統(tǒng)APF都可以成功避障并完成從起點到目標點的路徑規(guī)劃，并且沒有陷入局部最小解。從圖4（a）可以看出，在少量障礙物環(huán)境中，傳統(tǒng)APF可以避開障礙物搜索到有效路徑達到目標點；從圖4（b）可以看出，在障礙物較多的環(huán)境中，傳統(tǒng)APF也可以成功避障達到目標點；在圖4（c）中，傳統(tǒng)APF避開了大量障礙物得到有效路徑。圖4（d）、（e）、（f）顯示了在三種不同的障礙物場景下，改進APF也能成功避開障礙搜索到有效路徑，從圖4（d）和（f）中可以看出，機器人在經過第一個障礙物和障礙物較多的位置時，啟動了退火策略產生了隨機擾動，最終成功規(guī)劃出了從起點到終點的有效路徑；圖4（e）中改進的APF規(guī)劃出了一條不同于傳統(tǒng)APF的路徑。

表3列出了傳統(tǒng)APF算法和改進APF算法的時間性能數(shù)據(jù)，實驗數(shù)據(jù)取算法成功的情況下的平均值。從表3可以看出，相較于傳統(tǒng)APF算法，改進APF算法由于增加了模擬退火策略，需要更長的路徑搜索時間。此外，隨著環(huán)境中障礙物數(shù)量的增加，APF算法需要更多的迭代次數(shù)搜索路徑，所以運行時間更長。但是在路徑長度方面，改進APF算法優(yōu)于傳統(tǒng)APF算法。

表3 傳統(tǒng)APF（Tra_APF）和改進APF（Imp_APF）二維仿真結果

圖5給出了不同數(shù)量的障礙物環(huán)境下傳統(tǒng)APF算法和改進APF算法的部分路徑規(guī)劃仿真結果。圖5（a）、（b）、（c）中，由于傳統(tǒng)APF算法的缺陷，機器人都在障礙物附近陷入了局部最小解，無法繼續(xù)搜索路徑并運動到目標點，路徑規(guī)劃失敗。圖5（d）、（e）、（f）分別表示了在少量障礙物、較多障礙物和大量障礙物場景下APF算法對陷入局部最小解問題的優(yōu)化，可以看出，當機器人陷入局部最小解時，觸發(fā)了算法中的模擬退火策略，在局部最小值附近產生了隨機搜索，最終跳出了局部最小解，成功規(guī)劃出一條有效路徑。

圖5 改進APF算法對傳統(tǒng)APF的局部最小解問題的優(yōu)化仿真結果

3.2 三維路徑規(guī)劃仿真實驗

在三維仿真實驗中，路徑規(guī)劃環(huán)境為10×10×10的立方體空間，機械臂末端的起點設置為［0，0，0］，目標點設置為［10，10，10］，兩種算法的參數(shù)設置與二維仿真實驗相同。

圖6為三維仿真實驗結果，球體表示使被包圍球包絡的障礙物，實線表示從起點到終點的路徑。圖6（a）為傳統(tǒng)APF路徑規(guī)劃結果，可以看出當兩個障礙物距離較遠時傳統(tǒng)APF算法可以成功規(guī)劃出一條從起點到終點的有效路徑；圖6（b）表示了傳統(tǒng)APF陷入了局部最小解時機器人無法繼續(xù)向目標點移動，從而導致路徑規(guī)劃失??；圖6（c）表示了改進APF算法成功跳出局部最小解，并運動到了目標位置。

圖6 傳統(tǒng)APF和改進APF三維路徑規(guī)劃仿真結果

表4列出了在三維仿真環(huán)境中兩種APF算法的性能數(shù)據(jù)仿真結果。在運行時間方面，改進APF算法的運行時間相比傳統(tǒng)APF算法平均延長了0.2 s。在路徑長度方面，兩種算法得到的路徑隨著環(huán)境中障礙物數(shù)量的增加而增大，并且改進APF算法得到的路徑長度要小于傳統(tǒng)APF算法。

表4 傳統(tǒng)APF（Tra_APF）和改進APF（Imp_APF）三維仿真結果

二維和三維仿真實驗結果都驗證了改進APF算法的有效性。在機器人沒有陷入局部最小解的情況下，改進APF算法的運行時間比傳統(tǒng)APF算法的時間平均延長了0.2s，規(guī)劃出的路徑長度比傳統(tǒng)APF算法更短；當路徑規(guī)劃陷入了局部最小解時，傳統(tǒng)APF算法路徑規(guī)劃失敗，改進APF算法可以成功跳出局部最小解并運動到目標位置，并且得到更短的路徑。

4 結語

針對傳統(tǒng)人工勢場法的缺陷，本文提出了改進算法并通過實驗驗證了其有效性。主要研究結論有如下兩點：

（1）基于模擬退火的改進人工勢場路徑規(guī)劃方法能有效地解決傳統(tǒng)人工勢場法的缺陷，使路徑規(guī)劃成功逃離局部最小解，得到一條從起始位置到目標位置的有效路徑。

（2）相比傳統(tǒng)人工勢場法，改進人工勢場法獲得的路徑更短，但運行時間平均增加了0.2s。