何世瓊，陳 雨

（四川大學(xué)電子信息學(xué)院，成都 610065）

0 引言

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

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

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

1 人工勢場法基本原理

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

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

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

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

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

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

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

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

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

2 基于模擬退火的改進(jìn)人工勢場法

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

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

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

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

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

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

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

2.2 改進(jìn)人工勢場法

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

圖3 改進(jìn)人工勢場法算法流程

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

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

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

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

（6）判斷機械臂末端是否達(dá)到目標(biāo)點，達(dá)到目標(biāo)點后結(jié)束，否則重新設(shè)置初始溫度進(jìn)行搜索。

3 仿真實驗

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

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

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

表2 改進(jìn)APF算法參數(shù)設(shè)置

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

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

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

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

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

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

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

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

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

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

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

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

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

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

4 結(jié)語

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

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

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