何世瓊，陳 雨

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

0 引言

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

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

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

1 人工勢(shì)場(chǎng)法基本原理

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

2.2 改進(jìn)人工勢(shì)場(chǎng)法

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

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

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

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

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

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

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

3 仿真實(shí)驗(yàn)

3.1 二維路徑規(guī)劃仿真實(shí)驗(yàn)

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

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

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

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

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

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

表3列出了傳統(tǒng)APF算法和改進(jìn)APF算法的時(shí)間性能數(shù)據(jù)，實(shí)驗(yàn)數(shù)據(jù)取算法成功的情況下的平均值。從表3可以看出，相較于傳統(tǒng)APF算法，改進(jìn)APF算法由于增加了模擬退火策略，需要更長(zhǎng)的路徑搜索時(shí)間。此外，隨著環(huán)境中障礙物數(shù)量的增加，APF算法需要更多的迭代次數(shù)搜索路徑，所以運(yùn)行時(shí)間更長(zhǎng)。但是在路徑長(zhǎng)度方面，改進(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算法的缺陷，機(jī)器人都在障礙物附近陷入了局部最小解，無(wú)法繼續(xù)搜索路徑并運(yùn)動(dòng)到目標(biāo)點(diǎn)，路徑規(guī)劃失敗。圖5（d）、（e）、（f）分別表示了在少量障礙物、較多障礙物和大量障礙物場(chǎng)景下APF算法對(duì)陷入局部最小解問(wèn)題的優(yōu)化，可以看出，當(dāng)機(jī)器人陷入局部最小解時(shí)，觸發(fā)了算法中的模擬退火策略，在局部最小值附近產(chǎn)生了隨機(jī)搜索，最終跳出了局部最小解，成功規(guī)劃出一條有效路徑。

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

3.2 三維路徑規(guī)劃仿真實(shí)驗(yàn)

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

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

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

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

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

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

4 結(jié)語(yǔ)

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

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

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