尹鳳儀，劉 芳，吳向陽

（北京特種機(jī)械研究所，北京 100143）

1 引 言

隨著無人車的高速發(fā)展，多無人車的應(yīng)用在軍事、農(nóng)業(yè)、工業(yè)等方面越來越廣泛，其中多無人車的編隊(duì)和路徑規(guī)劃也成為其中的重點(diǎn)。首先，領(lǐng)航跟隨法[1]作為常用的多無人機(jī)編隊(duì)控制方法，其實(shí)現(xiàn)方式較為簡單，能夠解決無人車之間的跟隨控制問題，具有很高的實(shí)際應(yīng)用價值，也是本文使用的編隊(duì)控制方法。

多無人車的路徑規(guī)劃也是國內(nèi)外的研究熱點(diǎn)，其規(guī)劃方向主要分為兩類：基于全局地圖信息的全局路徑規(guī)劃和基于局部地圖信息的局部路徑規(guī)劃[2]。常見的路徑規(guī)劃算法有A*算法、RRT算法、人工勢場法、群體智能算法及上述算法的改進(jìn)融合[3-8]。其中，人工勢場法[9]是一種簡單且實(shí)用性強(qiáng)的路徑規(guī)劃算法，其原理是將路環(huán)境空間作為虛擬力場空間，將目標(biāo)點(diǎn)作為引力點(diǎn)，障礙點(diǎn)作為斥力點(diǎn)，由目標(biāo)引力點(diǎn)和障礙斥力點(diǎn)的共同作用形成力場，規(guī)劃無人車行進(jìn)路線。人工勢場法也存在顯著的缺陷[10]：當(dāng)障礙物與目標(biāo)點(diǎn)位置較近或者障礙物在無人車和目標(biāo)點(diǎn)中間時，會出現(xiàn)目標(biāo)不可達(dá)的問題；當(dāng)出現(xiàn)引力與斥力合力為零的情況時，無人車會停擺不前或振蕩，無法逃出局部極小值的問題。針對上述問題，許多研究人員提出很多改進(jìn)方法。文獻(xiàn)[11]改寫了函數(shù)形式，選取適當(dāng)?shù)脑鲆鎱?shù)解決目標(biāo)不可達(dá)問題；文獻(xiàn)[12]提出在局部極小點(diǎn)設(shè)定虛擬最小區(qū)域以及改變斥力角度的方案；文獻(xiàn)[13]應(yīng)用模糊控制，在相應(yīng)時刻改變無人車的合力大小及方向，避免出現(xiàn)局部極小值振蕩問題；文獻(xiàn)[14]重新定義引力公式，提高機(jī)器人的避障和動態(tài)跟蹤靈活性；文獻(xiàn)[15]將蟻群算法引入人工勢場法，降低局部極小值的可能性，但算法過于復(fù)雜；文獻(xiàn)[16]在斥力場函數(shù)中加距離調(diào)節(jié)因子，使得在目標(biāo)點(diǎn)附近斥力為零；文獻(xiàn)[17]提出采用虛擬水流法解決局部極小值陷阱；文獻(xiàn)[18]采用“沿邊規(guī)劃”解決局部極小值問題；文獻(xiàn)[19]通過設(shè)置中間目標(biāo)點(diǎn)的方法，給機(jī)器人一個虛擬逃逸力逃出極小值情況；文獻(xiàn)[20]通過使用序列凸優(yōu)化方法，在保證輪式機(jī)器人最優(yōu)化規(guī)劃同時大大縮短計算時間；文獻(xiàn)[21]研究出基于模型預(yù)測控制的阿克曼轉(zhuǎn)向結(jié)構(gòu)車輛的運(yùn)動控制方法，證明了方法的有效性和魯棒性。

本文針對多無人車的編隊(duì)特性，將無人車分為領(lǐng)航車和跟隨車分別進(jìn)行人工勢場法的改進(jìn)。針對目標(biāo)不可達(dá)問題，通過動態(tài)改變障礙物的影響范圍解決目標(biāo)不可達(dá)問題；針對局部極小值問題，采用動態(tài)法向力設(shè)置虛擬力的方式逃脫振蕩情況。通過設(shè)置合適的對比試驗(yàn)，針對兩種問題比較傳統(tǒng)算法和改進(jìn)算法的實(shí)際規(guī)劃效果，在MATLAB 仿真環(huán)境中驗(yàn)證規(guī)劃算法的有效性。

2 傳統(tǒng)人工勢場法

人工勢場法主要思想是：將無人車的工作環(huán)境模擬為人工勢場，目標(biāo)點(diǎn)對無人車產(chǎn)生一個引力場，每個障礙物對無人車都產(chǎn)生一個斥力場，將引力場和所有斥力場合成一個勢能場。無人車在該勢場力的作用下進(jìn)行路徑規(guī)劃。

如圖1 所示，在無人車二維空間運(yùn)行環(huán)境中存在一個目標(biāo)點(diǎn)和一個障礙物，其中無人車位置q、目標(biāo)點(diǎn)位置qa、障礙物位置qr坐標(biāo)分別為［x,y］、［xa,ya］、［xr,yr］。在虛擬勢能場中，當(dāng)無人車與障礙物的距離越小時，障礙物產(chǎn)生對無人車的斥力越大；而當(dāng)無人車和目標(biāo)的距離越大時，目標(biāo)產(chǎn)生對無人車的引力越大，因此引力場與斥力場勢能設(shè)計如下，引力場勢能函數(shù)Ua表示為

圖1 虛擬人工勢場法Fig. 1 Virtual artificial potential field method

式中，d(q,qa)表示無人車和目標(biāo)點(diǎn)的歐式距離，e˙ (q,qa)是一個從無人車指向目標(biāo)點(diǎn)方向的單位矢量。ka為正實(shí)數(shù)，表示引力系數(shù)。

則引力場產(chǎn)生的引力ka可表示為

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

式中，d（q,qr）表示無人車和障礙物的距離，e˙ (q,qr)是一個從障礙物指向無人車方向的單位矢量，ρ0為正實(shí)數(shù)，表示避障半徑，即當(dāng)無人車處在避障半徑為ρ0的區(qū)域內(nèi)才需要進(jìn)行避障。k r為正實(shí)數(shù)，表示斥力系數(shù)。

則斥力場產(chǎn)生的斥力kr可表示為

由圖1 所示，假設(shè)運(yùn)動過程中有n個障礙物，有無人車所受到的合力為引力與斥力之和，因此無人車所受合力為

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

3.1 目標(biāo)不可達(dá)問題

當(dāng)目標(biāo)點(diǎn)附近存在障礙物，小車在行駛到目標(biāo)點(diǎn)附近時，受到引力越來越小，而障礙物的斥力越來越大，將會是小車處于停滯狀態(tài)，從而無法達(dá)到目標(biāo)點(diǎn)，如圖2 所示。通過改變目標(biāo)點(diǎn)附近原障礙物斥力場的影響范圍，使目標(biāo)點(diǎn)不再受到障礙物斥力作用，從而使無人車可以到達(dá)目標(biāo)點(diǎn)。并且在遠(yuǎn)離目標(biāo)點(diǎn)位置保持斥力作用，避免撞到障礙物，因此引入無人車到目標(biāo)點(diǎn)的距離影響因子d n(q,qa)，對斥力場函數(shù)改進(jìn)

圖2 目標(biāo)不可達(dá)示意圖Fig. 2 Target unreachable diagram

3.2 局部極小值問題

針對局部極小值問題，本文進(jìn)行實(shí)時局部路徑規(guī)劃設(shè)計，當(dāng)無人車、障礙物和目標(biāo)點(diǎn)處于同一直線時（圖3），隨著引力的減少和斥力的增加，會出現(xiàn)合力平衡的臨界狀態(tài)，此時無人車處于停滯振蕩狀態(tài)。在實(shí)際情況中，一般會出現(xiàn)接近三點(diǎn)共線狀態(tài)，此時需要規(guī)劃方案以避免陷入極小值陷阱。

圖3 局部極小值示意圖Fig. 3 Local minima diagram

判斷形成局部極小值的條件為，斥力和引力的方向夾角θ∈ （ π -β,π+β）。其中，β為引力和斥力的夾角閾值。為了避免這種情況，施加動態(tài)法向力幫助無人機(jī)逃脫局部極小值。由于多無人車的特性，分別對領(lǐng)航車和跟隨車施加不同類型的法向力。對于領(lǐng)航車，法向力方向?yàn)轭I(lǐng)航車前一步運(yùn)動方向的垂直方向；對于跟隨車，法向力方向?yàn)轭I(lǐng)航車與跟隨車相對矢量距離的垂直方向。

為防止無人車受力突變導(dǎo)致不平滑曲線，對法向力進(jìn)行定義

式中，k⊥為法向力系數(shù)，法向力大小隨著無人車接近障礙物而增大，防止其陷入局部極小值。當(dāng)無人車受到的引力與斥力的夾角偏離θ的范圍時，取消法向力作用。

3.3 算法流程

由上述可得，改進(jìn)人工勢場法的算法流程如圖4 所示。

圖4 改進(jìn)勢場法算法流程Fig. 4 Improved artificial potential field method algorithm flowchart

4 仿真結(jié)果分析

4.1 多無人車編隊(duì)

使用MATLAB 進(jìn)行仿真實(shí)驗(yàn)，對于多無人車的編隊(duì)設(shè)置，實(shí)驗(yàn)選擇三角形型的領(lǐng)航-跟隨編隊(duì)，其通信拓?fù)浣Y(jié)構(gòu)如圖5 所示。

圖5 領(lǐng)航-跟隨編隊(duì)結(jié)構(gòu)Fig. 5 Formation structure of leader-follower method

4.2 目標(biāo)不可達(dá)問題

為了驗(yàn)證改進(jìn)算法的有效性，使用原算法和改進(jìn)算法進(jìn)行仿真對比。首先針對目標(biāo)不可達(dá)問題，對傳統(tǒng)人工勢場法進(jìn)行仿真?？梢园l(fā)現(xiàn)，當(dāng)目標(biāo)點(diǎn)附近有障礙物時，存在目標(biāo)不可達(dá)情況。這是由于障礙物的斥力場遠(yuǎn)大于目標(biāo)點(diǎn)的引力場作用，導(dǎo)致車隊(duì)出現(xiàn)停滯，無法抵達(dá)目標(biāo)點(diǎn)。在第4.1 節(jié)的編隊(duì)設(shè)置下，對目標(biāo)不可達(dá)問題的仿真如圖6 所示。

圖6 目標(biāo)不可達(dá)問題對比仿真Fig. 6 Comparative simulation of target unreachable problem

由圖6 可知，傳統(tǒng)人工勢場法下無人車無法到達(dá)目標(biāo)點(diǎn)，改進(jìn)算法通過加入無人車到目標(biāo)點(diǎn)的距離影響因子，增加目標(biāo)點(diǎn)對于無人車的引力場影響，使其可以在誤差允許情況下到達(dá)終點(diǎn)。

4.3 局部極小值問題

針對局部極小值問題，考慮目標(biāo)、障礙物、無人車三點(diǎn)共線的情況時，無人車會出現(xiàn)停滯現(xiàn)象，無法繼續(xù)前進(jìn)，對傳統(tǒng)人工勢場法算法和改進(jìn)勢場法算法進(jìn)行仿真，可以得到無人車局部極小值的逃離情況如圖7 所示。

圖7 局部極小值問題對比仿真Fig. 7 Comparative simulation of local minima problem

由圖7 分析可得，傳統(tǒng)算法由于局部極小值影響，導(dǎo)致路線出現(xiàn)多次停滯振蕩，甚至跟隨車2 出現(xiàn)無法前進(jìn)和恢復(fù)隊(duì)形的情況。改進(jìn)后的算法通過調(diào)節(jié)動態(tài)法向力，讓無人車逃離局部極小值情況，減少路線的振蕩，引導(dǎo)多無人車編隊(duì)平滑順利到達(dá)目標(biāo)點(diǎn)。

4.4 改進(jìn)算法整體規(guī)劃

整體實(shí)驗(yàn)使用MATLAB 進(jìn)行仿真對比，實(shí)驗(yàn)分別對傳統(tǒng)勢場法和改進(jìn)勢場法進(jìn)行仿真對比，算法主要參數(shù)設(shè)定如表1 所示。

表1 路徑規(guī)劃參數(shù)設(shè)置Table 1 Path planning parameter settings

傳統(tǒng)算法與改進(jìn)算法的仿真對比如圖 8所示。

由圖8 可得，傳統(tǒng)算法由于人工勢場法的缺陷，會導(dǎo)致無人車陷入局部極小值而停滯振蕩，多無人車編隊(duì)無法恢復(fù)，并且由于目標(biāo)點(diǎn)附近障礙物影響，無法到達(dá)目標(biāo)點(diǎn)的情況。改進(jìn)算法通過引入動態(tài)法向力，幫助逃脫極小值陷阱；同時通過引入距離影響因子，從而在多個障礙物的影響下規(guī)劃出多無人車各車的平滑曲線，保證多無人車編隊(duì)隊(duì)形并順利到達(dá)目標(biāo)點(diǎn)。

圖8 整體路徑規(guī)劃對比仿真Fig. 8 Overall path planning comparative simulation

5 結(jié) 論

多無人車編隊(duì)路徑規(guī)劃通常采用人工勢場法，但是傳統(tǒng)人工勢場法存在目標(biāo)不可達(dá)和局部極小值問題，為解決其缺陷，本文主要做了兩個方面的改進(jìn)：首先，通過增加距離影響因子改變目標(biāo)點(diǎn)附近障礙物的斥力場函數(shù)，使目標(biāo)點(diǎn)對無人車的引力場作用增大，從而使無人車到達(dá)目標(biāo)點(diǎn)，并且在和傳統(tǒng)算法的對比仿真中，可以看出改進(jìn)算法可以引導(dǎo)無人車到達(dá)目標(biāo)點(diǎn)；其次，通過增加動態(tài)法向力作為虛擬逃逸力，使無人車逃離局部極小值情況，對領(lǐng)航車和跟隨車采取不同的法向力方向。通過對比仿真可以看出，無人車逃離局部極小值點(diǎn)，并恢復(fù)隊(duì)形到達(dá)目標(biāo)點(diǎn)。MATLAB 的仿真實(shí)驗(yàn)充分證明了改進(jìn)算法的有效性和可行性，但是關(guān)于更復(fù)雜路況下多無人車編隊(duì)的避障規(guī)劃能力仍有待進(jìn)一步研究。