基于改進(jìn)人工勢場法的多目標(biāo)點路徑規(guī)劃

唐瑞東，游向榮

（430065 湖北省 武漢市 武漢科技大學(xué) 汽車與交通工程學(xué)院）

0 引言

路徑規(guī)劃在智能小車自主導(dǎo)航技術(shù)[1]中是一種關(guān)鍵性技術(shù)，傳統(tǒng)的路徑規(guī)劃算法分為全局路徑規(guī)劃與局部路徑規(guī)劃。全局路徑規(guī)劃算法有BFS、Dijkstra 算法及A*算法[2]；局部路徑算法有人工勢場法（APF）、動態(tài)窗口法（DWA）。與其它算法比較，人工勢場法具有計算量小、避障響應(yīng)快、便于硬件實現(xiàn)等優(yōu)勢[3]。人工勢場法是一種虛擬力法[4]，其基本思想是將機器人在環(huán)境中的運動抽象成為一種人造引力場中的運動，目標(biāo)點對運動中的機器人產(chǎn)生“引力”作用，障礙物對機器人產(chǎn)生“斥力”作用，最后通過求合力來控制移動機器人的運動。應(yīng)用人工勢場法規(guī)劃的路徑一般比較平滑且安全，但是這種方法存在局部最優(yōu)點問題以及目標(biāo)點不可達(dá)問題，這是本文研究的核心內(nèi)容。

1 傳統(tǒng)的人工勢場法及改進(jìn)算法

1.1 傳統(tǒng)的人工勢能場算法及其缺陷

人工勢場法的原理是機器人在移動過程中障礙物形成虛擬的“斥力”勢場，目標(biāo)物體形成虛擬的“引力”勢場，在“引力”與“斥力”勢場的共同作用下，物體逐漸避開障礙物的高勢能場，到達(dá)目標(biāo)點低勢能場。如圖1 所示。

圖1 人工勢場法原理圖Fig.1 Schematic diagram of artificial potential field method

1.1.1 引力場

目標(biāo)點對移動機器人形成的引力勢能隨著機器人與目標(biāo)點之間距離的改變而變化，人工勢場中引力勢能與機器人與目標(biāo)點之間距離的平方成正比[5]，即距離越遠(yuǎn)，目標(biāo)點所產(chǎn)生的引力勢能越大。

引力勢場函數(shù)表示為

式中：Uatt（q）——目標(biāo)點對機器人產(chǎn)生的引力場；ε——大于0 的引力勢場增益函數(shù)；ρ（q,qgoal）——機器人當(dāng)前位置與目標(biāo)點之間的歐幾里得距離。

1.1.2 斥力場

在人工勢場中，機器人與障礙物之間的距離越遠(yuǎn)所受斥力越小，反之越大[6]。

斥力勢場函數(shù)表示為

式中：Urep（q）——障礙物對機器人當(dāng)前位置所形成的斥力勢場；η——斥力勢場增益函數(shù)；ρ（q,qobs）——障礙物與機器人當(dāng)前位置的歐幾里得距離；ρ0——障礙物的影響半徑，即斥力勢場的有效作用距離。當(dāng)ρ（q,qobs）>ρ0時，障礙物對機器人不再產(chǎn)生斥力作用。

機器人當(dāng)前所處位置周圍有n 個障礙物時，機器人當(dāng)前所處位置的勢能為

傳統(tǒng)的人工勢能法指機器人在當(dāng)前位置遍歷多個方向，最后尋找勢能最小的那個方向并移動一個步長，直至最后到達(dá)終點。其原理圖如圖2 所示。

圖2 多障礙物人工勢場法原理圖Fig.2 Schematic diagram of multi-obstacle artificial potential field method

1.1.3 傳統(tǒng)人工勢場法存在的缺陷

（1）局部勢能極小點問題。當(dāng)障礙物在移動機器人與目標(biāo)點之間零散排布時，存在局部勢能極小值點問題，導(dǎo)致移動機器人在兩點之間振蕩。

（2）目標(biāo)點不可達(dá)問題。當(dāng)目標(biāo)點周圍障礙物過多時，移動機器人無法越過較大的斥力勢場進(jìn)而到達(dá)不了目標(biāo)點。

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

本文的機器人是一款智能小車，采用八方位遍歷每個路徑點最小勢能的方法改進(jìn)，即：從智能小車當(dāng)前位姿出發(fā)，計算出正前方、東北方，西北方，正左方、正右方、東南方、西南方、正后方分別移動一個步長時各點的勢能，并選擇其中勢能最小點作為下一步位姿。

針對幾個障礙物零散排布在小車與目標(biāo)點之間導(dǎo)致出現(xiàn)局部勢能極小值點，從而使小車的行駛路徑在兩點之間反復(fù)振蕩的情況，引入模擬退火算法[7]，使其跳出振蕩點，從而脫離局部勢能極小值點[8]；針對目標(biāo)點周圍障礙物過多，導(dǎo)致目標(biāo)點不可達(dá)的問題，提出了一種基于碰撞預(yù)測及有效障礙物的算法來解決目標(biāo)點不可達(dá)的問題。

1.2.1 模擬退火算法解決局部勢能極小點

（1）算法流程

步驟1：記錄智能小車每一步的位姿，并判斷出下一步的位姿是否與上一步位姿為同一位置，若是，則轉(zhuǎn)入步驟2；若否，則轉(zhuǎn)入步驟3。

步驟2：啟用模擬退火算法，該算法分為3 步。第1 步，若當(dāng)前位姿的下一位姿與上一位姿處于同一位置，將當(dāng)前所處位姿紀(jì)錄為point，在point 附近選擇隨機點x（x 為以point 為圓心，1 個步長為半徑的圓上的點）作為初始點，并計算該點勢能xout；再在x 附近選擇隨機點y（y 是以x 為圓心,1個步長為半徑的圓上的點），若y 移動到point 上則令其勢能yout為無窮大；否則進(jìn)行第2 步。第2步，計算出小車當(dāng)前的勢能yout，并將其與上一步的位姿比較。計算兩者勢能之差Δ=xout-yout。若Δ>0，令x=y，f（x）=f（y）。若Δ<0,則執(zhí)行第3步。第3 步，理論概率為

當(dāng)P>rand（0，1）時，令xout=yout，x=y，執(zhí)行步驟3；否則，刪除該隨機點，重新執(zhí)行步驟1。

步驟3：判斷智能小車是否已經(jīng)擺脫震蕩點，即當(dāng)前位姿的下一位置與上一位置處于不同位姿，否則，返回步驟1。

（2）仿真實驗分析

為了驗證文中所提出的模擬退火算法的有效性，利用MATLAB 工具分別對傳統(tǒng)的人工勢場法與改進(jìn)的人工勢場法進(jìn)行仿真分析。

仿真參數(shù)設(shè)置為：引力勢場增益系數(shù)katt=40；斥力勢場增益系數(shù)krep=100；計算步長l=0.1；循環(huán)次數(shù)為200 次，障礙物的影響距離為2 m，小車的起始位姿為（0，0），目標(biāo)點的位置為（6，8）；傳統(tǒng)人工勢場法的仿真圖片如圖3 所示。

圖3 傳統(tǒng)人工勢場法小車路徑Fig.3 Vehicle path of traditional artificial potential field method

由圖3 可見，傳統(tǒng)的人工勢場法中當(dāng)智能小車與目標(biāo)點之間存在零散障礙物時，可能會出現(xiàn)局部勢場極小值點而產(chǎn)生振蕩的情況，導(dǎo)致小車無法逃脫該點，不能到達(dá)目標(biāo)點的問題。

改進(jìn)之后的人工勢場法中仿真參數(shù)設(shè)置：退火算法中控制參數(shù)初值T0=20，衰減因子?=0.99，步長為0.1，迭代次數(shù)為1 000 次。其余參數(shù)與傳統(tǒng)人工勢場法參數(shù)相同，改進(jìn)后的仿真圖片如圖4 所示。

圖4 退火算法小車路徑Fig.4 Vehicle path with annealing algorithm

從圖4 可以看出，加入模擬退火算法的人工勢場法克服了局部勢能極小值點的問題，成功規(guī)避了障礙物到達(dá)目標(biāo)點。

1.2.2 碰撞預(yù)測算法判斷障礙物失效解決目標(biāo)點不可達(dá)問題

（1）行駛路徑上障礙物過多

當(dāng)智能小車與目標(biāo)點之間存在密集的U 型或L型障礙物時，小車無法越過高斥力勢能場進(jìn)而到達(dá)目標(biāo)點。此時這些障礙物會形成一個局部勢能極小區(qū)域[9]而非局部勢能極小值點，導(dǎo)致退火算法失去效果，如圖5 所示。

圖5 加入退火算法的人工勢場法Fig.5 Artificial potential field method with annealing algorithm

解決方法：實際工況下，陷入單個局部勢能極小值點的情況較少，所以退火算法的作用次數(shù)一般較少，當(dāng)退火算法使用次數(shù)>3 時，可能就陷入了局部勢能最小區(qū)域，即障礙物密集區(qū)域，把退火算法使用次數(shù)counter=3 時作為一個臨界點，設(shè)置一個計數(shù)器counter，令其初始值為0，每使用一次退火算法其值加1，當(dāng)退火算法的使用次數(shù)>3 時，便執(zhí)行碰撞預(yù)測算法[10]。

碰撞預(yù)測算法：設(shè)置一個預(yù)測距離[11]。當(dāng)多個障礙物與小車距離小于該預(yù)測距離時，令小車以當(dāng)前位置為圓心，選擇一個半徑和偏轉(zhuǎn)角度，設(shè)置一個虛擬目標(biāo)點逃脫該陷阱障礙物。算法流程：

第1 步：起點處或到達(dá)虛擬目標(biāo)點后counter置為0，使用一次退火算法則counter+1，counter>3時執(zhí)行第2 步，否則，執(zhí)行第5 步。

第2 步：設(shè)置一個安全預(yù)測距離sfrep。計算各障礙物距小車的距離，并記錄距離小于預(yù)測距離的障礙物，本文中將該類障礙物稱為“陷阱障礙物”，由于最少4 個障礙物便可形成一個L 型障礙物，所以當(dāng)陷阱障礙物個數(shù)≥4 時，執(zhí)行第3 步。否則，執(zhí)行第5 步。

第3 步：在陷阱障礙物中，以智能小車當(dāng)前位置為圓心，小車到距離最遠(yuǎn)的陷阱障礙物的距離為半徑，找到小車最右側(cè)的陷阱障礙物，并以此障礙物為基準(zhǔn)再向右偏轉(zhuǎn)9°,設(shè)置虛擬目標(biāo)點，若該虛擬目標(biāo)點與障礙物重合，則再偏轉(zhuǎn)3°設(shè)置障礙物，以此類推。

第4 步：智能小車向虛擬目標(biāo)點移動。

第5 步：遍歷8 個方向?qū)ふ覄菽茏畹忘c，作為下一位姿。執(zhí)行第1 步。

仿真實驗分析：為了驗證本文提出的碰撞預(yù)測算法的可實施性，利用MATLAB 工具在U 型障礙物下分別對加入退火算法的人工勢場法以及在此基礎(chǔ)上再加入碰撞預(yù)測算法的人工勢場法進(jìn)行比較。

加入退火算法改進(jìn)方法的仿真參數(shù)設(shè)置：引力勢場增益系數(shù)katt=40；斥力勢場增益系數(shù)krep=100；計算步長l=0.1；循環(huán)次數(shù)為200 次，障礙物的影響距離為2 m，小車的起始位置為（0，0），目標(biāo)點的位置為（10，10）；退火算法中控制參數(shù)初值T0=20；衰減因子?=0.99，步長為0.1，迭代次數(shù)為1 000 次。在U 型障礙物下，只加入退火算法的人工勢場法的仿真結(jié)果如圖5 所示。

在退火算法的基礎(chǔ)上再加入碰撞預(yù)測算法的人工勢場法的仿真參數(shù)為：安全預(yù)測距離為sfrep=1.5，陷阱障礙物最大個數(shù)danobs=4，其余參數(shù)配置跟加入退火算法的人工勢場法一致，仿真結(jié)果如圖6 所示。可見，加入退火算法的人工勢場法在小車到達(dá)目標(biāo)點之前有較多障礙物時，小車會繞一定區(qū)域振蕩，即退火算法只能夠應(yīng)對局部勢能最小值點問題，難以解決局部勢能最小區(qū)域問題；加入碰撞預(yù)測算法后智能小車則創(chuàng)建了一個虛擬目標(biāo)點，即圖6 中三角區(qū)域，有效規(guī)避了U 型障礙物，具有較好的可實施性。

圖6 在退火算法基礎(chǔ)上加入碰撞預(yù)測算法的人工勢場法Fig.6 Artificial potential field method with collision prediction algorithm added to annealing algorithm

（2）目標(biāo)點周圍障礙物過多

當(dāng)障礙物不在小車到達(dá)目標(biāo)點的路徑上，而在目標(biāo)點周圍時，此時當(dāng)小車接近目標(biāo)點時會有較大的斥力勢場，進(jìn)而導(dǎo)致目標(biāo)點不可達(dá)。

解決方法：計算小車當(dāng)前位置與目標(biāo)點間的距離dit 以及與障礙物間的距離dio，當(dāng)dio>dit 時，則判斷該障礙物失效，令其對小車產(chǎn)生的勢能為0。

進(jìn)行仿真以驗證其有效性。傳統(tǒng)仿真結(jié)果如圖7 所示，加入失效障礙物后的仿真結(jié)果如圖8 所示。圖7、圖8 起點為（1，1），目標(biāo)點為（4.5，4.5）。結(jié)果表明，該方法有效解決了障礙物在目標(biāo)點周圍所形成勢場過大導(dǎo)致目標(biāo)點不可達(dá)的問題。

圖7 傳統(tǒng)的人工勢場法目標(biāo)點周圍存在障礙物情況Fig.7 Obstacles existing around target point of traditional artificial potential field method

圖8 加入失效障礙物后的人工勢場法Fig.8 Artificial potential field method with failed obstacles

綜合以上2 種方法，有效解決了人工勢場法中因目標(biāo)點周圍障礙物以及路徑上陷阱障礙物過多而導(dǎo)致的目標(biāo)點不可達(dá)問題。

2 智能小車多目標(biāo)點路徑規(guī)劃

2.1 智能小車移動模型的建立

實際工作中的移動機器人，例如物流機器人[12]往往途經(jīng)多個需要停留作業(yè)的目標(biāo)點，而不是單一目標(biāo)點，因此本文根據(jù)上述改進(jìn)的人工勢場法提出一種多目標(biāo)點的路徑規(guī)劃方法。

在智能小車的移動過程中，設(shè)定一個初始點以及多個目標(biāo)點，以目標(biāo)點橫坐標(biāo)為依據(jù)，每次從小到大選擇一個有效目標(biāo)點，且每次設(shè)置一個目標(biāo)點后，其余目標(biāo)點對小車所形成的引力勢能都為0。當(dāng)一個目標(biāo)點到達(dá)后，此目標(biāo)點作為下一目標(biāo)點的起始點，以每次的起始點為圓心，以起始點到目標(biāo)點的距離為半徑繪制一個圓形區(qū)域，在區(qū)域內(nèi)的障礙物則被認(rèn)定為本次移動過程中的有效障礙物，如圖9 所示，以此來簡化移動過程中對障礙物的計算，同時還可以減少目標(biāo)點周圍的障礙物個數(shù)，消除部分造成目標(biāo)點不可達(dá)的因素。

圖9 智能小車移動模型Fig.9 Smart car moving model

圖9 中，以從左往右第1 個三角形為起始點，其余2 個為目標(biāo)點，小圓形則為障礙物。3 個圓弧形所包圍的范圍分別代表3 次移動過程中有效障礙物的范圍。從圖9 可以看出：從起點到第1 個目標(biāo)點的過程中，該方法減少了移動過程中對小車不起作用的障礙物的計算；從第1 個目標(biāo)點到第2 個目標(biāo)點的過程中，該方法排除了目標(biāo)點周圍部分障礙物的影響，將改進(jìn)的人工勢場法運用到該模型上進(jìn)行多目標(biāo)點路徑規(guī)劃。規(guī)劃策略流程如圖10 所示。

圖10 路徑規(guī)劃流程圖Fig.10 Path planning flowchart

2.2 改進(jìn)方法在小車移動模型上的仿真

為了驗證改進(jìn)的人工勢場法在所建立的智能小車移動模型上是否適用，對其進(jìn)行仿真分析，仿真參數(shù)設(shè)置為：引力勢場增益系數(shù)katt=40；斥力勢場增益系數(shù)krep=100；計算步長l=0.1；循環(huán)次數(shù)為200 次，障礙物的影響距離為2 m，小車的起始位姿為（0，0），目標(biāo)點的位置分別為（3，5），（6，8），（7，7），（10，10）；退火算法中控制參數(shù)初值T0=20，衰減因子?=0.99，步長為0.1，迭代次數(shù)為1 000 次。安全預(yù)測距離sfrep=1.5，陷阱障礙物最大個數(shù)danobs=4。在此基礎(chǔ)上進(jìn)一步完善障礙物失效條件，即dio>dit 時，障礙物對小車所產(chǎn)生的人工勢能為0。仿真結(jié)果如圖11 所示。

圖11 基于改進(jìn)人工勢場法的多目標(biāo)點路徑規(guī)劃Fig.11 Multi-objective point path planning based on improved artificial potential field method

仿真結(jié)果表明，在智能小車多目標(biāo)點移動模型上，改進(jìn)的人工勢場法可以有效到達(dá)各目標(biāo)點,在由坐標(biāo)點（7，7）到（10，10）的過程中，通過產(chǎn)生虛擬目標(biāo)點有效地逃出陷阱障礙物所組成的U形區(qū)；由坐標(biāo)點（6，8）到（7，7）的過程成功越過了中間的障礙物；順利地克服了目標(biāo)點不可達(dá)的問題，且全程沒有出現(xiàn)因局部勢能極小值點而導(dǎo)致小車在兩點間反復(fù)振蕩的現(xiàn)象，說明該算法在多目標(biāo)點模型上具有良好的適用性。

3 結(jié)論

傳統(tǒng)的人工勢場法存在局部勢能極小值問題和目標(biāo)點不可達(dá)問題，本文通過對傳統(tǒng)的人工勢場法進(jìn)行改進(jìn)解決了以下問題：

（1）引入了模擬退火算法（控制參數(shù)初值T0=20，衰減因子?設(shè)置為0.99）解決了局部勢能極小值問題；（2）引入碰撞預(yù)測算法（安全預(yù)測距離sfrep=1.5，陷阱障礙物最大個數(shù)danobs=4）解決了行駛路徑上障礙物過多目標(biāo)點不可達(dá)問題；（3）比較小車與障礙物的距離與小車與目標(biāo)點間的距離，進(jìn)一步完善了障礙物失效的條件，解決了目標(biāo)點周圍因障礙物過多不可達(dá)的問題。

將改進(jìn)后的人工勢場法引入到所建立的智能小車多目標(biāo)點移動模型上，最后得到了理想的仿真結(jié)果。