改進人工勢場法在無人機路徑規(guī)劃中的應用研究

巫 茜，曾 青，羅金彪，鄺 茜，黃 浩

(重慶理工大學 計算機科學與工程學院， 重慶 400054)

隨著控制論、材料學、計算機與通信等相關(guān)學科技術(shù)的不斷進步，無人機的性能得以大幅提升，出現(xiàn)了大量形態(tài)各異但制造成本不斷降低的無人機，這使得無人機開始大規(guī)模普及，在軍事和民用領(lǐng)域發(fā)揮了巨大作用[1-3]。自主與智能控制是無人機未來的發(fā)展趨勢[4-5]，其中路徑規(guī)劃是提高無人機自主飛行水平和保障飛行安全的一項重要技術(shù)，已經(jīng)成為了無人機領(lǐng)域的研究熱點。

路徑規(guī)劃指的是在一定約束條件下，為無人機規(guī)劃從起點到目標點的最優(yōu)或可行航跡[6]。針對路徑規(guī)劃問題，國內(nèi)外學者開展了許多工作，提出了快速擴展隨機樹法、人工勢場法、A*算法、蟻群算法、粒子群算法、遺傳算法等眾多方法[7-8]。在現(xiàn)有方法中，人工勢場法具有實現(xiàn)簡單、實時性高、生成的路徑安全平滑等優(yōu)點，因此在實時在線路徑規(guī)劃問題上得到了廣泛應用。但該方法也存在目標不可達、易陷入局部極小值、局部震蕩等不足[9]。面對上述不足，學者們進行了深入研究，并提出了眾多改進方法。Sun等[10]通過改進斥力勢場函數(shù)和動態(tài)步長調(diào)節(jié)策略來解決目標不可達和局部震蕩問題，但僅在二維環(huán)境下進行簡單仿真，且沒有考慮局部極小值問題，不能滿足無人機在復雜三維環(huán)境下的飛行要求。馬婭婕等[11]將三維空間拆分為XOY和XOZ2個平面，對斥力勢場和引力勢場都進行了改進，以規(guī)避位于預設航跡上的障礙物并消除局部極小值帶來的影響，但計算量較大，實時性較低。付澤民等[12]和梁獻霞等[13]通過引入虛擬障礙物，解決了局部極小值問題，但文獻[12]中需要根據(jù)局部極小值附近的障礙物位置信息計算出虛擬障礙物的位置，存在一定局限性，且作者沒有給出選取障礙物的范圍以及計算方法，文獻[13]在局部極小值點處需要依次掃描60個扇區(qū)，當環(huán)境存在多個局部極小值且總是在排序靠后的扇形中找到虛擬障礙物時，該方法會帶來計算量過大的問題。唐嘉寧等[14]借鑒電勢場提出了模擬等勢線的概念，專門用于解決連續(xù)型障礙物帶來的易陷入局部極小值問題，但存在轉(zhuǎn)向角過大、路徑長度過長等問題。諶海云等[15]采用“沿目標方向90°移動”策略以跳出局部極小值，與文獻[14]類似，也會造成路徑中存在大角度轉(zhuǎn)向的問題，需要進一步平滑路徑后才能供無人機使用。

針對上述算法的缺點，本文在充分利用三維空間的基礎(chǔ)上，提出了一種改進人工勢場法，該算法主要用于規(guī)劃旋翼無人機和微小型固定翼無人機在低空復雜三維環(huán)境下的路徑。改進方法是：① 通過在斥力勢場函數(shù)中引入無人機與目標點的相對距離來解決目標不可達問題；② 提出三維目標分段法解決局部極小值和局部震蕩問題。在Matlab仿真軟件中進行了仿真實驗，仿真結(jié)果表明，提出的改進方法計算量小，能使無人機快速且連續(xù)擺脫多個局部極小值，在三維復雜環(huán)境下順利到達目標點。

1 傳統(tǒng)人工勢場法基本原理

人工勢場法的基本原理是構(gòu)建一個虛擬勢場，由目標點對無人機持續(xù)產(chǎn)生引力作用，障礙物在一定范圍內(nèi)對無人機產(chǎn)生斥力作用，根據(jù)平行四邊形法則產(chǎn)生二者的合力，無人機最終將在合力作用下到達目標點。

人工勢場的構(gòu)建依賴引力勢場函數(shù)和斥力勢場函數(shù)，分別如式(1)和式(2)所示。

(1)

(2)

式中：X、Xg、Xo分別代表無人機、目標點和障礙物所在位置，α為引力增益系數(shù)，β為斥力增益系數(shù)，ρ(X，Xg)為無人機與目標點之間的歐氏距離，ρ(X，Xo)為無人機與障礙物之間的歐氏距離，ρo為障礙物對無人機產(chǎn)生斥力作用的最遠距離。

對式(1)和式(2)求負梯度，可以得到相應的引力函數(shù)和斥力函數(shù)，分別如式(3)和式(4)所示。

Fatt(X)=-▽Uatt(X)=αρ(X,Xg)

(3)

(4)

最終求得的合力如式(5)所示。

Ftotal(X)=Fatt(X)+Frep(X)

(5)

圖1直觀地展示了無人機在人工勢場法中的受力情況。障礙物1和障礙物2分別產(chǎn)生斥力Frep1和Frep2，障礙物對無人機的總斥力為Frep，而目標點對無人機的引力為Fatt，最終的合力為Ftotal。

圖1 人工勢場法中無人機受力分析示意圖

2 傳統(tǒng)人工勢場法缺陷分析

2.1 目標不可達問題

物體在目標點附近因斥力大于引力，無法到達目標點的情況被稱為目標不可達問題[16]。如圖2所示，當目標點與障礙物比較接近時，隨著無人機越來越靠近目標點，也越來越靠近障礙物，因此ρ(X，Xo)和ρ(X，Xg)遞減。由式(3)和式(4)可知，斥力Frep逐漸增大，而引力Fatt卻逐漸減小；越靠近目標點，斥力越大，以至于斥力大于引力，導致無人機無法到達目標點。

圖2 目標不可達問題示意圖

2.2 局部極小值與局部震蕩問題

傳統(tǒng)人工勢場法的另一個主要缺陷是存在局部極小值和局部震蕩問題[17-18]。原因在于人工勢場法中可能存在局部極小值點，當規(guī)劃過程中遇到這樣的點時，無人機受到的合力接近或等于零，造成規(guī)劃終止；而在遇到以U型障礙為代表的非凸障礙物時，斥力作用時而大于引力作用，時而小于引力作用，或者斥力作用一直占主要地位，但其方向頻繁大幅改變，從而導致規(guī)劃的路徑上產(chǎn)生局部震蕩現(xiàn)象。

典型的局部極小值問題如圖3所示。在圖3中，無人機、目標點以及障礙物在同一直線上，該直線上必有一處引力和斥力大小相等而方向相反的點，此時合力為零，形成局部極小值點，無人機在該點處將停滯不前。

圖3 典型局部極小值問題示意圖

圖4為典型的局部震蕩現(xiàn)象示意圖，兩障礙物組成狹窄通道，導致無人機在通過該通道時的前進方向頻繁大幅改變，因機動性能有限，無人機很有可能無法按該路徑飛行。

圖4 典型局部震蕩現(xiàn)象示意圖

3 傳統(tǒng)人工勢場法改進策略

3.1 改進斥力勢場函數(shù)

為解決目標不可達問題，可將無人機與目標點之間的相對距離引入斥力勢場函數(shù)[13,15]。改進后的斥力勢場函數(shù)如式(6)所示，式中n可取任意大于零的常數(shù)。

(6)

對式(6)所示的斥力勢場函數(shù)使用復合函數(shù)鏈式求導法則，可得斥力函數(shù)，如式(7)—(9)所示。

(7)

(8)

(9)

式(7)中的F1和F2分別如式(8)和式(9)所示，F(xiàn)1和F2中的偏微分都是單位向量，均指向無人機。

改進后的斥力將受無人機與目標點之間距離的影響，確保無人機在接近目標點時的斥力作用小于引力作用，使無人機能夠到達目標點。

3.2 三維目標分段法

考慮到無人機在復雜環(huán)境下可能遇到多個局部極小值點或局部震蕩問題，無法直接到達目標點，提出一種將路徑分段的方法，通過在合適的位置設定子目標點，以暫時取代目標點對無人機產(chǎn)生引力作用，保證無人機能到達子目標點，進而到達最終的目標點。

首先需要檢測無人機是否陷入局部極小值點或產(chǎn)生了局部震蕩現(xiàn)象。由于人工勢場法在每一輪迭代后，按照固定步長，在合力指向的方向下得到下一個路徑點，因此可以通過判斷第i(i≥ 2)次迭代時的位置和第i+1次迭代時的位置距離是否為零，第i-1次和第i+1次迭代時的位置距離是否小于移動步長來檢查無人機是否陷入局部極值或震蕩。當條件符合時，采用三維目標分段法。

借鑒二維平面下通過圓來尋找虛擬目標點的過程[19-20]，三維目標分段法中，需要先通過向量運算求出一個圓，然后取圓上一點作為子目標點。設空間中的局部極小值點或開始震蕩的點為(x1，y1，z1)，目標點為(x2，y2，z2)，連接上述兩點，可以得到一條線段，將其命名為L，設線段L上與局部極小值點一端接近的三等分點為(x3，y3，z3)，可以由式(10)求得該三等分點的位置。

(10)

以三等分點(x3，y3，z3)為圓心，k倍步長為半徑r，在空間中可得無數(shù)個圓。選擇與線段L垂直的圓作為產(chǎn)生子目標點的圓，這樣做可以確定唯一一個圓，大大減少了搜索空間，而且能確保產(chǎn)生的子目標點位于局部極小值點和目標點之間，符合航跡長度盡量短的原則。將與線段L垂直的圓所在的平面命名為A，將平面A的法向量命名為n，n可由目標點和式(10)所求得的三等分點得到，設n=(n1，n2，n3)；取平面A中的任意向量，將其命名為m，設m=(m1，m2，m3)，由于n是法向量，因此m與n正交。根據(jù)向量正交相乘結(jié)果為零的原則，可以由n求得m，設n的一個坐標點為零，其余2個坐標點互換位置，符號相反即可，例如m=(-n2，n1，0)，但n12與n22之和不能為零，否則由于z坐標為零，m將是一個零向量。

(11)

在式(11)中輸入一個角度值θ，可以求得圓上一點，擬將該點作為子目標點，但還需要判斷該點與障礙物之間的關(guān)系。求該點與障礙物之間的距離d，若該距離大于障礙物碰撞半徑，則該點成為子目標點，否則增大圓的半徑，另求一個點作為備選的子目標點，直到找到符合條件的子目標點。圖5展示了三維目標分段法中的子目標點與其他物體的位置關(guān)系。

圖5 三維目標分段法中的子目標點示意圖

4 路徑規(guī)劃仿真及結(jié)果分析

4.1 改進斥力勢場函數(shù)路徑規(guī)劃仿真

為了驗證改進斥力勢場函數(shù)的有效性，使用Matlab仿真軟件進行仿真。設定2組仿真實驗，其中無人機的起點均為(0，0，0),目標點分別為(2，2，2.5)、(2.8，2.8，2.6),靠近目標點的障礙物分別在(2，2，2)和(3，3，3)處，另設一個障礙物分別在(1.5，1.2，2.7)和(1.5，1.2，1.7)處，由此構(gòu)成目標點與障礙物距離較近的實驗條件。借鑒文獻[16]的規(guī)劃任務參數(shù)，本文的參數(shù)設置如表1所示。

表1 人工勢場法參數(shù)表

仿真結(jié)果如圖6所示，紅色路徑表示傳統(tǒng)人工勢場法的規(guī)劃結(jié)果，藍色路徑表示改進斥力勢場函數(shù)后的規(guī)劃結(jié)果。可以看出，傳統(tǒng)人工勢場法的路徑在目標點附近反復徘徊，無法到達；而改進人工勢場中，當無人機靠近目標點時，目標點附近的障礙物對無人機的斥力減小，引力可以一直大于斥力，最終使無人機順利到達目標點。

圖6 斥力勢場改進前后的仿真結(jié)果示意圖

4.2 三維目標分段法路徑規(guī)劃仿真

為驗證三維目標分段法的有效性，使用Matlab進行2項仿真實驗，每項實驗均設2組。

首先驗證對于典型局部極小值問題的改進效果。第1組實驗將無人機的起點設置為(0，0，0)，目標點設置為(6，6，7)，障礙物1個，其位置為 (6，6，4)；第2組實驗將無人機的起點設置為(6，0，3)，目標點設置為(6，6，3)，障礙物1個，其位置為(6，4.5，3)。算法的通用參數(shù)如表1所示，三維目標分段法參數(shù)k初始值為2，θ取0～360之間的任意實數(shù)。第1組實驗的仿真結(jié)果如圖7和圖8所示；第2組實驗的仿真結(jié)果如圖9和圖10所示。圖7和圖9中，由于起點、障礙物、目標點3點共線，導致傳統(tǒng)人工勢場法陷入局部極值，并在該極值點處達到受力平衡，使規(guī)劃提前結(jié)束。而在使用三維目標分段法后的結(jié)果如圖8和圖10所示，產(chǎn)生子目標點使受力平衡被打破，令無人機能夠擺脫局部極小值點的束縛，在到達子目標后，接著到達目標點。由此可知，三維目標分段法可以有效解決三維空間中的局部極小值問題。

圖7 傳統(tǒng)人工勢場法規(guī)劃結(jié)果Ⅰ

圖8 三維目標分段法規(guī)劃結(jié)果Ⅰ

圖9 傳統(tǒng)人工勢場法規(guī)劃結(jié)果Ⅱ

圖10 三維目標分段法規(guī)劃結(jié)果Ⅱ

第2項仿真實驗用于驗證三維目標分段法對典型路徑震蕩問題的改進效果。第1組仿真實驗中，將無人機的起點設置為(1，3.5，-0.2)，目標點設置為(5，3.8，0.2)，2個障礙物的位置分別為(3，3，0)和(3，4.2，0)；第2組仿真實驗中，將無人機的起點設置為(2，3.5，0)，目標點設置為(5，3.5，0)，6個障礙物的位置分別為(3，3，0)，(3.8，2.7，0)，(4.6，3.2，0)，(3.8，4.1，0)，(4.6，4.5，0)，(3，4.2，0)。算法的參數(shù)和第1項仿真實驗一致。

仿真結(jié)果如圖11—14所示。圖11中的路徑在方框處產(chǎn)生了劇烈震蕩，因此該路徑對無人機而言不可用，而圖13中傳統(tǒng)人工勢場法無法規(guī)劃出完整路徑。圖12和圖14中，改進后的算法在判定產(chǎn)生局部震蕩后，使用三維目標分段法產(chǎn)生子目標點，在子目標點的引導下，局部震蕩現(xiàn)象消失，規(guī)劃出的路徑安全且平滑，可供無人機使用。因此，三維目標分段法可以有效解決三維空間中的局部震蕩問題。

圖11 傳統(tǒng)人工勢場法路徑局部震蕩Ⅰ

圖12 三維目標分段法解決局部震蕩問題Ⅰ

4.3 復雜環(huán)境路徑規(guī)劃仿真

4.1節(jié)和4.2節(jié)的仿真實驗分別驗證了在斥力勢場函數(shù)中引入相對距離因子和三維目標分段法用于改進傳統(tǒng)人工勢場法的有效性，但實驗環(huán)境相對比較簡單，為了進一步驗證本文提出改進方法的可行性，在Matlab仿真軟件中進行復雜三維環(huán)境下的路徑規(guī)劃仿真。

圖13 傳統(tǒng)人工勢場法路徑局部震蕩Ⅱ

圖14 三維目標分段法解決局部震蕩問題Ⅱ

本節(jié)同樣設2組仿真實驗，第1組實驗中，無人機起點坐標設為(5，5，2)，目標點坐標設為(5，5，12)，障礙物共設6個，其坐標分別設為(5，5，3.5)，(5，5，11.5)，(5.7，4.5，6.3)，(5.7，5.4，6.3)，(4.5，5，9)，(5.8，5.8，5)。第2組實驗中，無人機起點坐標設為(0，0，0)，目標點坐標設為(9，9，9)，障礙物共設12個，從而組成含有多個局部極小值點、路徑震蕩以及目標不可達問題的三維空間，其坐標分別設為(1，1，1)，(1.2，1.8，2)，(1.8，1，2)，(2.7，3.6，3.2)，(3.3，4，3.7)，(4，4，4)，(4.3，3.2，3.6)，(5，5，4)，(7，7，5)，(7，7，7)，(9，9，9.5)，(9，9，8)。算法使用的參數(shù)和4.2節(jié)一致。

實驗結(jié)果如圖15—17所示，圖15由(a)和(b)2個部分組成，皆為復雜環(huán)境Ⅰ中三維視角下的路徑規(guī)劃結(jié)果；而圖16和圖17分別為復雜環(huán)境Ⅱ中XOZ平面和YOZ平面下的規(guī)劃結(jié)果，為了便于描述，將目標點記為Goal，按子目標點出現(xiàn)的先后順序，分別記為SubG1、SubG2、SubG3。從起點出發(fā)前往Goal的過程中，無人機陷入局部極小值點或局部震蕩，此時三維目標分段法生成子目標點SubG1; 在前往SubG1的途中，無人機再次被困，此時以SubG1作為目標點生成子目標點SubG2；當無人機到達SubG2之后，將 Goal恢復為目標點，因此無人機沒有必要再前往SubG1；再次前往Goal的途中，無人機又一次陷入局部極小值點或局部震蕩，此時生成子目標點SubG3，在無人機到達SubG3之后，恢復Goal作為目標點的地位；隨后，無人機最終到達離障礙物距離很近的Goal，規(guī)劃結(jié)束。

圖15 復雜環(huán)境Ⅰ下三維目標分段法規(guī)劃結(jié)果

通過上述實驗可以看出，本文提出的改進方法，尤其是三維目標分段法，簡潔易行，通過向量運算可以快速找到子目標點，具備在復雜環(huán)境中為無人機尋找可行路徑的能力。

5 結(jié)論

分析了傳統(tǒng)人工勢場法的原理及其存在的主要缺陷，提出了解決方案。通過引入無人機與目標點之間的相對距離因子改造斥力勢場函數(shù)，解決目標不可達問題；將虛擬目標點法拓展至三維空間，提出了三維目標分段法，解決了傳統(tǒng)人工勢場法的局部極小值和局部震蕩問題。在Matlab仿真平臺上開展了一系列仿真實驗，證實了該方法的可行性和有效性，表明本文提出的改進方法可以適用于復雜三維環(huán)境下的無人機路徑規(guī)劃。