賈正榮，王航宇，盧發(fā)興

海軍工程大學(xué) 兵器工程學(xué)院，武漢 430033

人工勢場法(Artificial Potential Field, APF)是一種用于路徑規(guī)劃的方法，具有形式簡單、實(shí)時(shí)性好的特點(diǎn)，能夠用于靜態(tài)路徑規(guī)劃與動態(tài)路徑規(guī)劃問題中[1-2]，并且可以并行求解以提高效率[3]。其基本原理是將終點(diǎn)作為引力源，障礙作為斥力源，生成一個(gè)空間內(nèi)的勢場，移動平臺沿著勢場方向運(yùn)動以規(guī)避障礙并到達(dá)終點(diǎn)[4-8]。

人工勢場法的一個(gè)主要問題是局部極小值問題：由于障礙形狀多樣，可能在空間中產(chǎn)生引力與斥力的平衡點(diǎn)，平臺到達(dá)這一平衡點(diǎn)后將無法逃逸而導(dǎo)致路徑規(guī)劃無解。

現(xiàn)有文獻(xiàn)為解決APF方法的勢場局部極小值問題，提出了各種改進(jìn)方法。文獻(xiàn)[9]通過引入目標(biāo)與機(jī)器人的相對距離，將原有斥力勢場函數(shù)乘以一個(gè)距離因子，使機(jī)器人在目標(biāo)位置處合力為零。針對多邊形障礙，引入一種邊緣探測方法，作多邊形的外接圓，從而將多邊形障礙轉(zhuǎn)換為點(diǎn)障礙。文獻(xiàn)[10]將作為勢場模型的Gaussian函數(shù)進(jìn)行了適當(dāng)變形，使其能更準(zhǔn)確地反映勢場環(huán)境。通過分析震蕩現(xiàn)象產(chǎn)生的原因，以及局部極小值點(diǎn)的特點(diǎn)，將粒子群算法引入到勢場的探測過程中，在此基礎(chǔ)上提出了等位線法用于逃逸局部極小值。文獻(xiàn)[11]提出了通過增加虛擬目標(biāo)點(diǎn)和原目標(biāo)點(diǎn)共同對機(jī)器人產(chǎn)生引力的方法來解決傳統(tǒng)人工勢場法中出現(xiàn)的局部極小點(diǎn)問題，然而得到的路徑結(jié)果具有一定的振蕩現(xiàn)象。文獻(xiàn)[12-14]利用模擬退火算法解決人工勢場法的局部極小點(diǎn)問題。文獻(xiàn)[15]針對傳統(tǒng)人工勢場法應(yīng)用于移動機(jī)器人路徑規(guī)劃存在的缺陷，建立了改進(jìn)的人工勢場模型；使用勢場強(qiáng)度代替力矢量進(jìn)行路徑規(guī)劃；在障礙物的斥力勢場中添加系數(shù)項(xiàng)，解決障礙物與目標(biāo)點(diǎn)過近導(dǎo)致的目標(biāo)不可達(dá)問題；考慮移動障礙物速度與機(jī)器人速度的影響，將速度信息引入到勢場函數(shù)中；引入“填平勢場”引導(dǎo)機(jī)器人走出局部極小點(diǎn)。文獻(xiàn)[16]提出添加附加控制力的方法，當(dāng)機(jī)器人所受的斥力與吸引力在一條直線上時(shí)，對機(jī)器人施加一個(gè)依賴于障礙物的控制力，使機(jī)器人盡快跳出局部極小點(diǎn)。文獻(xiàn)[17]通過在勢場局部極小值點(diǎn)附近增加虛擬障礙以改變附近的勢場分布。文獻(xiàn)[18]通過粒子群優(yōu)化算法解決勢場局部極小值問題。文獻(xiàn)[19]增加控制力的作用規(guī)避無解的情況。文獻(xiàn)[20]則通過改變障礙的形狀以提高路徑可解的概率。文獻(xiàn)[21]通過引入一種引力勢場選擇策略以規(guī)避勢場的局部極小值。文獻(xiàn)[22]通過引入優(yōu)先策略以更改APF算法解決路徑規(guī)劃無解的問題。文獻(xiàn)[23]通過引入引力源與斥力源之外的控制外力以優(yōu)化路徑規(guī)劃求解。文獻(xiàn)[24]通過引入虛擬路徑點(diǎn)產(chǎn)生外力以避免平臺進(jìn)入局部極小值。文獻(xiàn)[25]提出了基于改進(jìn)人工勢場的防碰撞控制方法，引入輔助斥力勢來提高碰撞規(guī)避的效果。文獻(xiàn)[26]為解決傳統(tǒng)人工勢場法無法避免的局部極小問題，將局部極小分為兩類，分別給出兩類極小值的判定定理及方法，仍然使用了一種增加的“填平勢場”用于改善局部極小值問題。文獻(xiàn)[27]研究全局靜態(tài)環(huán)境下移動機(jī)器人路徑規(guī)劃，提出一種改進(jìn)勢場蟻群算法，該方法針對離散(網(wǎng)格)地圖內(nèi)的路徑規(guī)劃。文獻(xiàn)[28]提出了一種基于滾動時(shí)域控制和快速粒子群優(yōu)化方法，將路徑規(guī)劃問題轉(zhuǎn)化為優(yōu)化問題，并將斥力場引入到代價(jià)函數(shù)中，提升路徑安全性。與之類似，文獻(xiàn)[29]也采用了一種預(yù)測控制的方法優(yōu)化路徑。

可見，一般通過3種方法解決APF方法的勢場局部極小值問題，一是在勢場局部極小值附近引入選擇策略，這種方法在障礙數(shù)量較小時(shí)易于實(shí)現(xiàn)，但是當(dāng)障礙數(shù)量增多后，策略將變得較為復(fù)雜，同時(shí)，當(dāng)平臺位于多個(gè)障礙之間且并未陷入局部極小值時(shí)，路徑可能產(chǎn)生較為明顯的振蕩；二是采用智能算法尋找逃出勢場局部極小值的路徑，這種方法需要大量的運(yùn)算，掩蓋了APF實(shí)時(shí)性好的特點(diǎn)；三是增加引力或斥力源以改變勢場分布情況，但是如何增加引力或斥力源對于是否可解有很大的影響，若引力或斥力源增加失當(dāng)，則可能造成無解或路徑振蕩。

另外，現(xiàn)有方法還采用了預(yù)測控制的思路[28-29]，在路徑規(guī)劃的每個(gè)時(shí)間步長上展開一個(gè)預(yù)測窗口，并設(shè)立若干優(yōu)化指標(biāo)，滾動地優(yōu)化路徑以取得更好的效果。但是如果能夠避免傳統(tǒng)APF方法局部極小值導(dǎo)致無解的問題，在理論上將會進(jìn)一步提升預(yù)測控制路徑規(guī)劃的效率。

除此之外，現(xiàn)有方法在路徑規(guī)劃過程中，為了避免凹障礙的不利影響，一般都對障礙采用了凸化處理[9]，但是當(dāng)障礙數(shù)量較多、彼此相交情況復(fù)雜時(shí)，需要一種更為詳盡、全面的方法進(jìn)行障礙的凸化處理。

綜上，為了提高APF方法的實(shí)用性，需要解決的問題主要有兩點(diǎn)，一是在復(fù)雜障礙環(huán)境內(nèi)應(yīng)用APF方法的問題，二是因勢場局部極小值導(dǎo)致無解的問題。

1 問題描述

移動平臺需要在障礙空間內(nèi)運(yùn)動至指定的目標(biāo)位置，移動平臺的運(yùn)動受到轉(zhuǎn)彎半徑的約束，其轉(zhuǎn)彎半徑不能小于最小轉(zhuǎn)彎半徑rmin。

實(shí)際環(huán)境中出現(xiàn)的大部分障礙都能夠通過多邊形障礙、圓形障礙以及它們的組合表示。多邊形障礙包括凸多邊形與凹多邊形，可以描述環(huán)境空間中的禁飛區(qū)、一般實(shí)體障礙等。圓形障礙不僅可以表示禁飛區(qū)、實(shí)體障礙，而且可以描述威脅區(qū)域或敵方火力作用區(qū)域。

在使用APF進(jìn)行路徑規(guī)劃過程中，由于障礙存在凹陷，當(dāng)平臺進(jìn)入這些凹陷區(qū)域后，斥力與引力的平衡會導(dǎo)致平臺進(jìn)入局部最小值點(diǎn)，從而無解。

凹障礙的出現(xiàn)主要有2種情況，分別為凹多邊形障礙與相交的凸障礙產(chǎn)生的凹區(qū)域，如圖1和圖2所示。因此，可以將凹障礙凸化以避免APF方法無解的問題。另外，在凸多邊形障礙環(huán)境中，傳統(tǒng)APF方法仍然存在局部極小值的問題，比如當(dāng)凸多邊形的邊界與平臺-目標(biāo)位置連線垂直時(shí)，合勢場也有可能為0，如圖3所示。因而還需要對APF斥力勢場進(jìn)行一定的改進(jìn)，使斥力勢場沿著多邊形障礙繞行，如圖4所示，從而避免圖3中的情況。

圖2 相交凸障礙產(chǎn)生的凹區(qū)域

圖3 合勢場為0的情況

圖4 環(huán)繞多邊形障礙的勢場方向

2 環(huán)流APF方法

2.1 平臺運(yùn)動模型

設(shè)平臺狀態(tài)向量為Xp=[xp,yp,vp,βp,ωp]T，xp、yp為平面內(nèi)的位置坐標(biāo)，vp為速率，βp為航向，ωp為角速度，在離散形式下，平臺的運(yùn)動可以描述為

(1)

式中：Δt為時(shí)間步長；ap為加速度；φp為角加速度，通過調(diào)整平臺航向來控制平臺的路徑。

由于本文僅涉及單一平臺在靜態(tài)障礙環(huán)境中的運(yùn)動，不涉及坐標(biāo)的變換，因此可以以平面內(nèi)任意一點(diǎn)作為坐標(biāo)原點(diǎn)建立慣性坐標(biāo)系，全文中的坐標(biāo)均在同一個(gè)坐標(biāo)系下描述，后文中將不再強(qiáng)調(diào)坐標(biāo)系。

2.2 APF方法路徑規(guī)劃

APF方法可以實(shí)時(shí)生成規(guī)避障礙的路徑。在APF方法中，終點(diǎn)產(chǎn)生引力，障礙產(chǎn)生斥力，合力的梯度即勢場決定了平臺的運(yùn)動方向。

設(shè)終點(diǎn)位置為Xg=[xg,yg]T，平臺當(dāng)前位置為Xs=[xp,yp]T，障礙為Ωi，障礙的數(shù)量為nob，則APF方法構(gòu)建斥力Ur為

(2)

引力Ua為

Ua=fa(Xs,Xg)

(3)

則合力為

U=Ur+Ua

(4)

梯度場為

(5)

(6)

|ωp|≤ωmax

(7)

從而實(shí)際的角速度應(yīng)當(dāng)為

(8)

式中：sign()為符號函數(shù)，即

(9)

2.3 引力與斥力構(gòu)建

在本文的分析中，傳統(tǒng)APF方法與基于環(huán)流斥力勢場的改進(jìn)APF方法僅梯度構(gòu)建形式不同，引力與斥力的函數(shù)相同。

對于平臺位置Xs，終點(diǎn)位置Xg，凸障礙Ωi以及其與Xs的最近點(diǎn)Xd,i，引力Ua為

(10)

式中：Ua,max為一常量，為使Ua在Xs的主要活動范圍內(nèi)取值為[0,1]之間，可以取

Ua,max=kmax|Xg-Xs,0|

(11)

式中：Xs,0為平臺Xs的初始位置；kmax為大于1的常量。

斥力Ur為平臺Xs與每個(gè)障礙Ωi的斥力和:

(12)

式中：Ur,i為平臺Xs與障礙Ωi的斥力,表達(dá)式為

(13)

式中：dob,i為平臺Xs與障礙Ωi的距離；ρ為斥力作用距離；當(dāng)Xs與Ωi的距離小于ρ時(shí)開始產(chǎn)生斥力，斥力作用距離應(yīng)當(dāng)取值為一定倍數(shù)的平臺最小轉(zhuǎn)彎半徑以使平臺能夠規(guī)避障礙。通過以上方式構(gòu)建的勢場能夠在Xg不在斥力作用范圍內(nèi)時(shí)，使Xg成為全局合力的最小值點(diǎn)。

對于引力Ua，顯然有

Ua(Xg)=0

(14)

對于斥力，顯然有

(15)

從而，當(dāng)Ur(Xg)=0，即Xg不在斥力作用范圍內(nèi)時(shí)，有

U(Xg)=Ua(Xg)

(16)

即終點(diǎn)Xg是全局最小值點(diǎn)。

2.4 凸多邊形與圓形障礙的斥力描述

一般的APF方法假設(shè)障礙為點(diǎn)障礙，從而斥力Ur可以直接表示為障礙點(diǎn)Xob,i與平臺當(dāng)前位置Xs的函數(shù)，當(dāng)障礙為凸多邊形與圓形時(shí)，與點(diǎn)障礙類似，可以求解Xs到凸多邊形或圓形的最近點(diǎn)作為障礙點(diǎn)計(jì)算斥力。

1) 凸多邊形障礙最近距離點(diǎn)

設(shè)凸多邊形障礙Ωop,i由有序頂點(diǎn)集合構(gòu)成，記為{Xop,ij=[xop,ij,yop,ij]T}，i為障礙序號。求解平臺Xs與凸多邊形障礙Ωop,i的距離，即求解Xs與Ωop,i中每條邊距離的最小值。

以邊Xop,ij到Xop,ij+1為例，設(shè)該邊上的點(diǎn)Xτ,ij通過參數(shù)τ描述，即

Xτ,ij=Xop,ij+τ(Xop,ij+1-Xop,ij)

(17)

式中：參數(shù)τ∈[0,1]，從而Xs與Xτ,ij的距離為|Xs-Xτ,ij|，距離極值對應(yīng)參數(shù)τ*為

(18)

dop,ij=

(19)

對應(yīng)邊Xop,ij到Xop,ij+1上的點(diǎn)Xdp,ij為

Xdp,ij=

(20)

對于凸多邊形障礙Ωop,i的所有邊，依次求解dop,ij與Xdp,ij，取dop,ij的最小值作為Xs與Ωop,i的距離，記為dop,ij，對應(yīng)Ωop,i邊上的點(diǎn)記為Xdp,ij，之后即可直接采用點(diǎn)障礙的斥力模型進(jìn)行斥力求解。

2) 圓形障礙最近距離點(diǎn)

圓形障礙記為Ωoc,i，圓心為Xoc,i，半徑為roc,i，從而平臺Xs與圓形障礙的最近距離doc,i為

doc,i=|Xs-Xoc,i|-roc,i

(21)

對應(yīng)Ωoc,i上的位置為Xdc,i

(22)

在后文中，當(dāng)不嚴(yán)格區(qū)分凸多邊形障礙與圓形障礙時(shí)，Xs與障礙的最近距離與障礙上的對應(yīng)點(diǎn)統(tǒng)一記為dob,i與Xd,i。

2.5 環(huán)流APF勢場

環(huán)流APF方法的引力勢場記為Fc,a，斥力勢場記為Fc,r，合勢場記為Fc。環(huán)流APF方法的引力勢場與傳統(tǒng)方法相同，區(qū)別在于斥力勢場。

(23)

環(huán)流斥力勢場應(yīng)當(dāng)與平臺原有的運(yùn)動方向盡量一致，記平臺航向方向單位向量為Vp=[cosβp,sinβp]T，則Fc,r,i為

Fc,r,i=

(24)

圖5 環(huán)流斥力勢場方向的選取

另外，規(guī)定當(dāng)Vp與-Ur,i方向相同時(shí)(朝向障礙邊界且與障礙邊界垂直)，選取Ur,i,+作為環(huán)流斥力勢場的方向；當(dāng)Vp與-Ur,i方向相反時(shí)(遠(yuǎn)離障礙邊界且與障礙邊界垂直)，選取Ur,i,-作為環(huán)流斥力勢場的方向，如圖6所示。

圖6 環(huán)流斥力勢場方向的選取(特殊情況)

(25)

從而環(huán)流斥力勢場和為Fc,r:

(26)

合勢場為

Fc=Fc,a+Fc,r

(27)

由于環(huán)流斥力勢場的方向與移動平臺航向相關(guān)，因此在理論上能夠克服傳統(tǒng)APF方法的局部極小值問題，具體證明見附錄A。

3 障礙凸化

障礙凸化針對環(huán)境中出現(xiàn)的所有凹障礙以及障礙相交產(chǎn)生的凸區(qū)域。凸化過程分為2步：首先，進(jìn)行障礙初始凸化，將環(huán)境中的所有凹多邊形障礙凸化成為凸多邊形障礙，從而使環(huán)境中只存在凸多邊形障礙與圓形障礙；然后，進(jìn)行相交障礙集合迭代凸化，判斷環(huán)境中的障礙是否相交，將相交的障礙進(jìn)行組合、凸化，消除相交障礙產(chǎn)生的凸區(qū)域。由于組合、凸化后的障礙會產(chǎn)生新的相交情況，因此還需要對于新得到的障礙再次進(jìn)行相交判斷與組合、凸化，即障礙集合迭代凸化。

3.1 障礙初始凸化

對于凹多邊形障礙Ωopc,i，頂點(diǎn)為{Xopc,ij=[xopc,ij,yopc,ij]T}，通過如下步驟可以得到一個(gè)由Ωopc,i的頂點(diǎn)構(gòu)成的凸多邊形。

算法1多邊形凸化

(28)

(29)

4) 如果

(30)

則完成求解，得到的所有頂點(diǎn)(不包括第k+1個(gè)頂點(diǎn))構(gòu)成一個(gè)凸多邊形，若不然，則跳轉(zhuǎn)至步驟2)。

3.2 障礙相交判斷

1) 多邊形相交判斷

多邊形相交即判斷兩個(gè)多邊形的邊是否相交，以多邊形ia與多邊形ib的兩條邊Xop,iaja到Xop,iaja+1與Xop,ibjb到Xop,ibjb+1為例，若線性方程

(31)

的解τia與τib滿足：

(32)

式中：

(33)

則說明交點(diǎn)位于兩條邊上，從而兩多邊形相交。分別對兩個(gè)多邊形的邊進(jìn)行相互判斷，若均不相交，還需要判斷兩多邊形是否為包含關(guān)系，即相互判斷多邊形ia中的一點(diǎn)(任取一頂點(diǎn)即可)是否為多邊形ib的內(nèi)點(diǎn)，內(nèi)點(diǎn)判斷可以直接采用內(nèi)角和判斷方法，若ib相鄰頂點(diǎn)與ia中一點(diǎn)的夾角和為2π，則該點(diǎn)是內(nèi)點(diǎn)，兩多邊形為包含關(guān)系，相交。

2) 多邊形與圓形相交判斷

多邊形與圓形相交判斷即判斷多邊形的每條邊與圓心的距離是否小于等于圓的半徑，圓心與多邊形邊的距離計(jì)算模型可以使用式。

3) 圓形相交判斷

圓形相交判斷即判斷兩圓心的距離是否小于等于兩圓的半徑和。

3.3 相交障礙凸化

彼此相交的障礙構(gòu)成一個(gè)相交障礙的集合，在這個(gè)集合中，對于任意一個(gè)障礙，均存在另一個(gè)障礙與該障礙相交，而并非要求集合內(nèi)的任意兩個(gè)障礙均相交。如圖7所示，圖7中C-A與C-B相交，C-B與C-C相交，C-A與C-C不相交，但是C-A，C-B，C-C構(gòu)成一個(gè)相交障礙的集合，這是因?yàn)槿糁玲槍-A與C-B進(jìn)行相交障礙凸化，則凸化后的圖形仍然會與C-C產(chǎn)生一個(gè)凹區(qū)域。

圖7 相交障礙集合

與圖7中的情況類似，首先將所有障礙分成若干個(gè)相交障礙集合Ψγ={Ωi}，其中γ為序號，對于集合Ψγ中的任意一個(gè)障礙Ωi，均存在另一個(gè)障礙Ωi*∈Ψγ與Ωi相交。若某一個(gè)障礙不與任何其他障礙相交，則無需凸化，因而不屬于Ψγ。

對于Ψγ，需要求解其中任意兩個(gè)障礙的特征點(diǎn)(這兩個(gè)障礙可能不相交)，以構(gòu)成一個(gè)能夠包含Ψγ中所有障礙的凸多邊形。

不同障礙間的特征點(diǎn)與凸化結(jié)果如圖8～圖10所示，特征點(diǎn)為藍(lán)色“×”，凸多邊形由這些特征點(diǎn)通過算法1得到。

1) 多邊形的特征點(diǎn)

多邊形相交的特征點(diǎn)為兩個(gè)多邊形所有頂點(diǎn)的并集，如圖8所示。

2) 多邊形與圓形的特征點(diǎn)

多邊形與圓形的特征點(diǎn)為多邊形所有頂點(diǎn)，以及所有多邊形頂點(diǎn)(不包括圓形內(nèi)部的頂點(diǎn))與圓形的切點(diǎn)構(gòu)成的并集，如圖9所示，圓形外點(diǎn)與圓的切點(diǎn)求解不再贅述。

3) 圓形的特征點(diǎn)

圓形的特征點(diǎn)為兩個(gè)圓的公切點(diǎn)(公切線不相交)，如圖10所示，公切線求解方法在此不再贅述。

得到集合Ψγ內(nèi)任意兩障礙的特征點(diǎn)后，構(gòu)成一個(gè)所有特征點(diǎn)的集合，采用算法1凸化成為一個(gè)凸多邊形，該凸多邊形與集合Ψγ內(nèi)所有圓構(gòu)成覆蓋集合Ψγ內(nèi)所有障礙的凸圖形，并作為相交障礙凸化結(jié)果。

圖8 多邊形的特征點(diǎn)

圖9 多邊形與圓形的特征點(diǎn)

圖10 圓形的特征點(diǎn)

3.4 相交障礙集合迭代凸化

經(jīng)過相交障礙凸化，得到的結(jié)果可以分為3部分：① 所有不與其他障礙相交的障礙；② 相交障礙集合構(gòu)成的凸多邊形；③ 相交障礙集合內(nèi)的圓。

由于產(chǎn)生的新凸多邊形可能與已有障礙相交而產(chǎn)生凹區(qū)域，因此還需要對結(jié)果進(jìn)行障礙相交判斷與相交障礙凸化，即障礙集合的迭代凸化，當(dāng)前后兩次的結(jié)果一致時(shí)，迭代結(jié)束，得到的所有障礙作為凸化的障礙用于路徑規(guī)劃。

4 仿真分析

為驗(yàn)證方法有效性，首先在復(fù)雜障礙場景下與現(xiàn)有方法進(jìn)行路徑規(guī)劃求解對比；之后在不同數(shù)量障礙的場景下分析方法的運(yùn)算耗時(shí)。

4.1 復(fù)雜障礙場景下的求解對比

分別采用不同的APF方法求解不同場景下的路徑規(guī)劃。方法包括傳統(tǒng)APF方法，環(huán)流APF方法(不使用障礙凸化)，環(huán)流APF方法(使用障礙凸化)。在仿真分析過程中，將所有長度去量綱化處理，設(shè)定平臺的最小轉(zhuǎn)彎半徑為3，平臺速度為3/s，計(jì)算時(shí)間步長為0.3 s，斥力作用距離為9。兩種場景設(shè)定為

場景A21個(gè)障礙，其中，4個(gè)圓形障礙，3個(gè)凹多邊形障礙，14個(gè)凸多邊形障礙。

場景B53個(gè)障礙，其中，15個(gè)圓形障礙，4個(gè)凹多邊形障礙，34個(gè)凸多邊形障礙。

不同APF方法得到的路徑規(guī)劃結(jié)果如圖11和圖12所示，兩種場景下的障礙凸化結(jié)果如圖13和圖14所示。

圖11 場景A路徑規(guī)劃結(jié)果

圖12 場景B路徑規(guī)劃結(jié)果

圖13 障礙凸化-場景A

圖14 障礙凸化-場景B

根據(jù)結(jié)果可得：

在兩種場景下，由于在路徑初始階段設(shè)置了一個(gè)凹多邊形的障礙，且凹陷部分正對平臺航向，傳統(tǒng)APF陷入了勢場的局部最小值而無解，雖然已經(jīng)對合勢場為0時(shí)的情況進(jìn)行預(yù)先設(shè)置了規(guī)則，即當(dāng)合勢場為0時(shí)進(jìn)行90°的轉(zhuǎn)向，但是這種簡單的策略并沒有改善傳統(tǒng)APF的效果。

相比之下，環(huán)流APF方法在場景A中雖然有解，但是路徑的一部分在凹多邊形障礙中出現(xiàn)了繞行，而在場景B中環(huán)流APF方法無解。雖然環(huán)流APF方法通過改變傳統(tǒng)斥力勢場的方向使平臺能夠貼著障礙邊界運(yùn)動，但是當(dāng)平臺進(jìn)入凹多邊形凹陷部分時(shí)，可能一直在凹多邊形凹陷部分內(nèi)繞行甚至進(jìn)入障礙(見圖15)而依然無解?？梢姡绻粚φ系K進(jìn)行凸化，環(huán)流APF能否有解將取決于障礙凹陷的程度，環(huán)流APF能夠逃離較淺的凹多邊形，但是卻有極大的可能陷入較深的凹多邊形內(nèi)。另外，注意到圖15中平臺進(jìn)入了障礙，這是由于凹區(qū)域與平臺的距離極值點(diǎn)不唯一，距離計(jì)算錯(cuò)誤地選擇了不應(yīng)該產(chǎn)生斥力的邊。

相應(yīng)地，在對障礙進(jìn)行凸化后，環(huán)流APF能夠給出較為合理的路徑規(guī)劃結(jié)果。

4.2 方法計(jì)算耗時(shí)分析

基于障礙凸化的改進(jìn)APF方法主要分為兩個(gè)過程，一是障礙凸化，二是路徑規(guī)劃。障礙凸化需要一次計(jì)算完成，而路徑規(guī)劃可以在每個(gè)時(shí)間步長內(nèi)單獨(dú)計(jì)算。

在不同障礙數(shù)量條件下分析方法的計(jì)算耗時(shí)，目標(biāo)位置與平臺初始狀態(tài)保持一致，改變障礙數(shù)量，不同場景的障礙數(shù)量如表1所示。

表1 不同場景的障礙數(shù)量

在不同障礙數(shù)量的場景下進(jìn)行路徑規(guī)劃，得到的路徑規(guī)劃結(jié)果如圖15所示。

圖15 不同障礙數(shù)量場景下的路徑規(guī)劃結(jié)果

不同障礙數(shù)量場景下的路徑規(guī)劃單步計(jì)算耗時(shí)如圖16所示，平均單步計(jì)算耗時(shí)、平均單步耗時(shí)/障礙數(shù)量、障礙凸化耗時(shí)如表2所示。

圖16 單步計(jì)算耗時(shí)

表2 計(jì)算耗時(shí)

根據(jù)表2結(jié)果可得:障礙凸化耗時(shí)與障礙數(shù)量、障礙復(fù)雜程度有直接的相關(guān)性，并且隨著障礙數(shù)量與障礙復(fù)雜程度的提升，障礙凸化耗時(shí)的增加并不是線性的。在場景C-3與C-4中，由于存在相交障礙，因此障礙凸化耗時(shí)相比C-1與C-2顯著增加，這是由于障礙集合迭代凸化過程耗時(shí)較長，但是總體耗時(shí)仍然在可以接受的程度之內(nèi)(C-4場景82個(gè)障礙，耗時(shí)1 786.111 ms)。另外，雖然C-1中的障礙數(shù)量少于C-2，但是障礙凸化耗時(shí)與平均單步耗時(shí)長于C-2，這是由于C-1中的多邊形數(shù)量多于C-2，多邊形的初始凸化、相交判斷、斥力計(jì)算相比圓形的耗時(shí)更長。

在單步耗時(shí)方面，所有場景的單步耗時(shí)均在ms級，并且平均單步耗時(shí)與障礙數(shù)量的比值趨同，對于每一個(gè)障礙，計(jì)算耗時(shí)約為0.02～0.03 ms?？梢砸源藶楦鶕?jù)設(shè)定計(jì)算的時(shí)間步長，使在每個(gè)時(shí)間步長內(nèi)均可以保證完成路徑規(guī)劃求解。

計(jì)算環(huán)境為i7-7700T@2.90 GHz，8 GB RAM，Windows 7 x64，MATLAB 2018a。

5 結(jié) 論

在可行性方面，改進(jìn)方法相比傳統(tǒng)APF方法或不采用障礙凸化的環(huán)流APF方法，能夠明顯提高復(fù)雜障礙環(huán)境下的APF路徑規(guī)劃求解可行性。

在計(jì)算耗時(shí)方面，方法的計(jì)算耗時(shí)主要分為障礙凸化耗時(shí)與路徑規(guī)劃單步計(jì)算耗時(shí)2部分。障礙凸化耗時(shí)主要與障礙數(shù)量、障礙復(fù)雜程度相關(guān)，仿真環(huán)境下，在障礙數(shù)量82，相交障礙數(shù)量為42時(shí)，障礙凸化耗時(shí)約為1 786.111 ms；單步計(jì)算耗時(shí)與障礙數(shù)量密切相關(guān)，并且呈近似線性關(guān)系，每個(gè)障礙所需的單步計(jì)算耗時(shí)約為0.02～0.03 ms，可以以此為基準(zhǔn)設(shè)定時(shí)間步長，以保證每個(gè)時(shí)間步長均能夠完成路徑規(guī)劃計(jì)算，進(jìn)而支持在線的APF方法路徑規(guī)劃。

附錄A

為便于理論分析，引入理想移動平臺的概念，理想移動平臺總能沿著勢場方向運(yùn)動，即最大角速度與角加速度趨于∞。環(huán)流APF方法能夠保證理想移動平臺航路規(guī)劃有解，即理想移動平臺采用環(huán)流APF方法，總能抵達(dá)終點(diǎn)。

值得注意的是，限定障礙為凸障礙的目的是使對于每個(gè)障礙而言，平臺與障礙之間的距離最小值點(diǎn)有且只有一個(gè)。而在凹障礙中，當(dāng)平臺處與障礙凹陷部分時(shí)，可能會出現(xiàn)多個(gè)平臺與障礙的距離最小值點(diǎn)。從而無法定義障礙對于平臺的斥力。

定理A1在凸障礙環(huán)境中，環(huán)流APF方法的勢場不存在局部極小值

證明：

采用反證法，設(shè)凸障礙環(huán)境中，環(huán)流APF方法的勢場存在局部極小值點(diǎn)，即對于Xmin≠Xg，有

Fc(Xmin)≡0=Fc,r+Fc,a

(A1)

此時(shí)，引力勢場Fc,a與斥力勢場Fc,r的分布如圖A1所示。

圖A1 合勢場為0的情況

設(shè)Vp(X)是指向Xmin處的單位向量。由于Xmin是局部極小值，因此存在Xmin的去心鄰域，該去心鄰域內(nèi)的所有位置勢場均指向Xmin。因而也存在位置微元δX，Vp(Xmin+δX)與Vp(Xmin-δX)均指向Xmin，由于Vp是單位向量，因此有

Vp(Xmin+δX)=-Vp(Xmin-δX)

(A2)

注意到環(huán)流APF方法斥力勢場的方向定義，當(dāng)平臺分別處于Xmin+δX與Xmin-δX位置并且航向指向Xmin時(shí)，平臺航向相反，環(huán)流斥力勢場的方向也相反，即

(A3)

如圖A2所示。

圖A2 斥力勢場相反的情況

Fig.A2 Situation that repulsive potential fields are opposite

因此，平臺分別以航向Vp(Xmin+δX)與Vp(Xmin-δX)經(jīng)過Xmin時(shí)，有

Fc,r[Xmin|Vp(Xmin+δX)]=

-Fc,r[Xmin|Vp(Xmin-δX)]

(A4)

從而平臺分別以航向Vp(Xmin+δX)與Vp(Xmin-δX)經(jīng)過Xmin時(shí)，若存在

Fc[Xmin|Vp(Xmin+δX)]=0

(A5)

則必有

Fc[Xmin|Vp(Xmin-δX)]=0=

-Fc,r[Xmin|Vp(Xmin+δX)]+Fc,a=

2Fc,a≠0

(A6)

其中，由于Xmin不是終點(diǎn)，因而Fc,a≠0，與假設(shè)矛盾。因此Xmin不是局部極小值點(diǎn)。

另外，當(dāng)理想移動平臺以航向Vp(Xmin+δX)經(jīng)過Xmin時(shí)，若有合勢場

Fc[Xmin|Vp(Xmin+δX)]=0

(A7)

且Fc(Xmin+δX)指向Xmin，移動平臺將會被“拉回”Xmin并且航向會被反轉(zhuǎn)，當(dāng)移動平臺以航向Vp(Xmin-δX)再次經(jīng)過Xmin時(shí)，則有

Fc[Xmin|Vp(Xmin-δX)]=2Fc,a(Xmin)

(A8)

從而斥力勢場Fc,r[Xmin|Vp(Xmin-δX)]與引力勢場Fc,a(Xmin)相同，合力勢場將引導(dǎo)移動平臺離開Xmin位置并且不再返回。但是對于終點(diǎn)位置Xg，由于Fc,a(Xg)=0，從不同方向進(jìn)入Xg的合勢場是可以相等且為0的。

根據(jù)以上分析，由于環(huán)流APF方法的斥力勢場構(gòu)建方式與移動平臺航向相關(guān)，對于同一個(gè)位置，若移動平臺的航向相反，則斥力方向也相反，因而環(huán)流APF方法對應(yīng)合勢場為0的位置，并不會表現(xiàn)為一個(gè)傳統(tǒng)的局部極小值點(diǎn)使移動平臺陷入該點(diǎn)，當(dāng)移動平臺再次從相反的方向經(jīng)過該位置時(shí)，該位置的勢場將不再為0而是與引力方向相同使移動平臺向終點(diǎn)位置移動。

定理A1僅說明對于理想移動平臺，環(huán)流APF方法能夠保證有解，對于實(shí)際的移動平臺，其角速度與角加速度是受限的，因此實(shí)際中仍然可能出現(xiàn)無解的情況。