基于改進人工勢場的無人機動態避障路徑規劃

叢玉華，趙宗豪，邢長達，王志勝

(1.南京理工大學紫金學院，南京 210023；2.南京航空航天大學自動化學院，南京 211106；3.南京航空航天大學深圳研究院， 廣東 深圳 518063)

1 引言

無人機作為一種新興的飛行器，相較于其他飛行器有著成本低、機動性強、速度快以及體積小等優點。隨著人工智能、微電子、控制理論、數據處理以及傳感器的發展，無人機逐步克服了之前技術上的一些阻礙[1-3]，在一些領域中開始投入實際應用，如軍用無人機、農業無人機、高空檢修無人機、道路勘探無人機、航拍無人機等，都體現了無人機在軍事和民用領域極大的應用潛能。然而，要使無人機能夠更好完成任務，需要好的路徑規劃算法適應變化的任務以及環境，使無人機能夠快速、自主地完成指定任務。關于無人機路徑規劃的技術已有許多相關研究以及理論成果，大部分無人機路徑規劃主要涉及兩類，全局規劃和局部規劃。全局規劃可以分為兩種類型：基于搜索的方法以及基于采樣的方法。基于搜索的方法包括：基于圖搜索的方法，如迪杰斯特拉算法(Dijkstra)，A*算法，混合A*算法等。基于采樣的方法包括：概率路圖法，RRT*算法，Informed RRT*算法等。局部規劃中經典的方法包括：人工勢場法(APF)，動態窗口法(DWA)[4-9]。其中，人工勢場法是一種經典的路徑規劃方法[10-13]。是由Khatib提出的一種虛擬力場法，考慮了無人機的運動學問題，路徑規劃實時性強，可動態規避障礙物，較好實現無人機路徑規劃。但易出現狹窄通道內軌跡振蕩問題以及目標點不可達問題，本文將針對這2個問題對人工勢場法提出改進，并進行仿真驗證。

2 基于勢場的路徑規劃方法

2.1 人工勢場法

使用人工勢場法引導無人機的過程，可將無人機視為一個在負梯度向量場中移動的“正電荷粒子”[10]。負梯度可以直觀地看作是作用在帶正電荷粒子無人機上的力，它被吸引到帶“負電荷”的目標上。與物理上的電荷不同的是，目標點的負電荷產生的梯度為負梯度，對無人機產生引力，使得無人機趨向目標點。障礙物具有“正電荷”，產生正梯度的場，對無人機產生斥力，迫使無人機遠離障礙物。引力和斥力的結合可以引導無人機在避開障礙物的同時從起始位置到達目標位置[14-16]。

1) 引力場分析。與物理上的引力場或電場隨距離增加而衰減的特點不同，引力場Uatt應當隨著無人機與目標點的距離而單調增加。通過測量無人機到目標點的距離，構建一個場函數，即：

U(x)=ζd(p，pgoal)

(1)

其中：p表示無人機當前位置，坐標為(x，y)；pgoal表示目標點位置，坐標為(xgoal，ygoal)，ζ是一個用于控制引力場對無人機作用強度的參數。d(p，pgoal)表示目標點與無人機當前位置之間的距離，通過該式得出引力場梯度為

(2)

梯度向量從目標點開始，以ζ量級向外遠離目標，目標點處未定義梯度向量。從目標點以外的任何一點出發，通過負梯度，可以找到一條通往目標的路徑。

在通過數學模型實現該方法時，由于引力梯度在原點處存在不連續，梯度下降可能存在“抖振”問題。因此建立一個連續可微的勢能函數，當無人機接近目標點時，引力梯度的強度就會減小。勢能函數是一個隨著無人機與目標點的距離二次增長的函數，以消除分母導致的梯度不連續問題，即：

(3)

通過平方后構建的引力場負梯度函數變為

(4)

從數學模型上不難看出，引力場是一個基于p的矢量，由目標點起始指向遠離目標點的方向，引力場的強度與無人機到目標點的距離間隔成比例關系。無人機離目標距離越遠，引力場作用的強度越強，無人機會快速接近目標；當無人機接近目標時，引力場效應變弱。引力場數學模型如圖1所示，圖1中Z軸表示勢場強度，平面上X和Y軸表示產生勢場的區域，引力場為式(5)，其中參數，(xgoal，ygoal)=(-5，5)，ζ=0.125。

圖1 引力場數學模型示意圖Fig.1 Distribution map of gravitational field

(5)

2) 斥力場分析。斥力場Urep迫使無人機遠離障礙物。斥力的強度大小與無人機與障礙物之間的距離大小有關。與目標點產生的負梯度對無人機產生的引力場的特點相反，障礙物產生正梯度的斥力場，無人機離障礙物越近，斥力越強。因此，斥力通常以無人機到最近障礙物的距離D(x，y)為依據，斥力場Urep可表示為

(6)

從而得出斥力場梯度函數，

(7)

以上數學模型中的ρ為障礙物影響尺度因子，通過這個因子控制無人機忽略距離較遠的障礙物產生的斥力場。η是一個用于控制斥力場對無人機作用強度的參數。設(xobs，yobs)為障礙物的坐標。則得式(8)：

(8)

式(8)中，

(9)

設置3個障礙物的參數η均為η1=η2=η3=20，3個障礙物的坐標分別是(xobs1，yobs1)=(-2.05，1)，(xobs2，yobs2)=(4.05，-2)，(xobs3，yobs3)=(2，3)，對應的ρ分別為ρ1=3，ρ2=3，ρ3=1。斥力場數學模型如圖2所示，圖2中Z軸表示勢場強度，平面上X軸和Y軸表示產生勢場的區域。

圖2 斥力場分布示意圖Fig.2 Distribution map of repulsion field

3) 引力和斥力合場分析。通過上文獲得引力場與斥力場的數學模型后，將2個勢場疊加得到總的勢場函數。為了更易于理解，將勢場中的梯度向量類比于力向量，令Fatt=Uatt，令Frep=Urep，則無人機受到的合力為

Ft=Fatt+Frep

(10)

Fatt的參數為：ζ=0.125，(xgoal，ygoal)=(-5，-5)；Frep的參數為：參數η為η1=η2=η3=20；障礙物的坐標分別是(xobs1，yobs1)=(-2.05，1)，(xobs2，yobs2)=(4.05，-2)，(xobs3，yobs3)=(2，3)；對應的ρ分別為ρ1=3，ρ2=3，ρ3=1。圖3為引力場和斥力場合場Ft的分布圖。

對無人機所受的合力正交分解后，通過X軸的分力與Y軸的分力求出無人機的航向角，由式(11)可得航向角θ，式中n為區域內障礙物的數量。可見航向角由目標點的引力、障礙物的斥力合力決定，通過合理調整無人機的航向角，從而達到避開障礙物、抵達目標點的目的。

(11)

4) 人工勢場法路徑規劃步驟。梯度下降法是求解優化問題的一種常用方法。從初始狀態開始，朝與梯度相反方向移動，其算法偽代碼如下：

Algorithm APF

1:x(0)=xstart

2：y(0)=ystart

3：θ=θstart

4：i=0

5： while ▽U(x(i))≠0 and ▽U(y(i))≠0 do

6：θ=arctan(▽U(x(i))/▽U(y(i)))

7：x(i+1)=x(i)+γ*cos(θ)

8：y(i+1)=y(i)+γ*sin(θ)

9：i++

10： end while

進入下一個狀態，并重復這個過程，直到梯度為零，即抵達目標點。具體步驟為：

1) 確定引力場和斥力場的強度系數ζ和η，障礙物的影響尺度ρ，無人機運動的步長γ(設無人機勻速運動)，無人機起始位置(x，y)，無人機初始航向角θ。

2) 計算航向角以及下一步無人機的坐標及航向角，公式如下：

(12)

3) 無人機移動至(xk+1，yk+1)，設置k=k+1為當前位置。

4) 判斷無人機是否達到目標點，若抵達目標點則路徑規劃結束，若未到達目標點則返回步驟2)繼續執行。

2.2 人工勢場法仿真實驗與分析

2.2.1人工勢場法仿真驗證

人工勢場法應用在隨機生成障礙物地圖中進行路徑規劃，無人機起點坐標為(5，5)，終點坐標為(450，450)。障礙物半徑為25，單個障礙物面積為1 962.5，地圖面積為500×500。表1為具體實驗參數。

表1 實驗參數Table 1 Experimental parameters

圖4為無人機路徑規劃結果，圖5為航向角變化曲線，曲線中兩個峰值處分別對應軌跡圖中無人機的兩次大角度轉向，最后的航向角振動對應軌跡圖中軌跡振蕩部分。

圖4 路徑規劃結果示意圖Fig.4 Result of path planning

圖5 航向角變化曲線Fig.5 Change curve of heading angle

從仿真效果中可以看出：人工勢場法可以較好實現路徑規劃的基本要求，但是無人機在避開障礙物時需要做大角度的航向角變化，并且在一些狹窄的障礙物間隙中運動軌跡發生了振蕩。

2.2.2人工勢場法存在的問題

1) 運動軌跡振蕩。人工勢場法軌跡振蕩問題，如圖6和圖7所示，圖6為無人機穿越狹窄通道時，軌跡出現不斷在2個障礙物方向上切換的現象，對應的圖7中航向角則表現為航向角在兩個方向上振蕩。

圖6 運動軌跡振蕩現象示意圖Fig.6 Oscillation of motion trajectory

圖7 振蕩區域航向角曲線Fig.7 Heading angle curve of oscillation area

圖6和圖7所體現的無人機運動軌跡振蕩問題雖然對于路徑規劃本身是否成功影響不大，但在實際情況中會增大對無人機硬件以及能源的消耗。

2) 目標點不可達。設定無人機的起點坐標(10，10)，終點坐標(450，495)，即當終點坐標位于邊緣障礙物附近時。路徑規劃結果如圖8所示，航向角變化如圖9所示。

圖8 目標點不可達現象示意圖Fig.8 Inaccessibility of target point

圖9 航向角變化曲線Fig.9 Change curve of heading angle

由圖8規劃路徑可見，當目標點距離邊界障礙物距離很近的時候，由于目標點和邊界障礙物對無人機產生的引力和斥力近似抵消，導致無人機在原地徘徊，無法抵達目標點，路徑規劃任務失敗。對應的圖9中航向角即為盤旋的走向。

3 人工勢場算法改進

根據以上提出的2種問題，通過增加衰減函數和距離因子來調整斥力場系數的方法來解決。

3.1 激活衰減函數防止軌跡振蕩

當無人機通過狹窄通道時，通過衰減函數來調整斥力系數，從而防止軌跡振蕩產生。具體步驟如下：

1) 狹窄通道的判斷。通過探測與障礙物間距離的方法來判斷狹窄通道，該方法以無人機為圓心，在無人機移動方向的一定扇區范圍設立半徑為R的扇形探測窗口，測量進入探測范圍的障礙物與無人機之間的距離。即測量無人機移動方向兩側±(β°,α°)扇區內是否在對稱方向上同時存在障礙物進入測量半徑R內，若存在則通過激光雷達等傳感器獲取障礙物與無人機之間的距離。同時測量無人機正前方是否有障礙物進入測量半徑R內，若不存在則認為當前無人機周圍的障礙物分布情況存在可以通過的狹窄通道。圖10為探測窗口示意圖。

圖10中虛線范圍為探測窗口，黑色質點表示無人機，兩側灰色區域為障礙物。

圖10 探測窗口示意圖Fig.10 Schematic diagram of detection window

2) 斥力場系數衰減。若存在狹窄通道則使用衰減函數減小該區域的斥力場系數，使無人機能夠通過狹窄通道并消除軌跡振蕩。同時設置最小的斥力場系數閾值，斥力系數的閾值取決于安全距離以防止撞擊障礙物，一般與無人機自身尺寸和飛行時速有關。根據該方法，斥力場方程變為

(13)

式(13)中：ρ1為障礙物與無人機之間的距離閾值；DLR(x，y)為無人機探測窗口范圍內左右兩側探測角度為[β°，α°]范圍內探測到的障礙物與無人機之間的最短距離，當兩側范圍內最短距離都小于閾值ρ1時，激活衰減函數f(η)；f(η)為設定的關于斥力場系數的衰減函數，根據設置的函數形式不同，該函數使斥力場系數以線性或者指數衰減，同時設定該系數存在一個最小值，以確保存在最小斥力防止無人機撞向障礙物。線性衰減函數為式(14)，指數衰減函數為式(15)。兩種衰減函數如圖11，指數衰減函數較快，更有利于迅速調整斥力場系數，因此選擇式(15)。

圖11 線性衰減和指數衰減曲線Fig.11 Linear and exponential decay curves

f(η)=η/DLR(x，y)

(14)

f(η)=η/eDLR(x，y)

(15)

3.2 加入距離因子防止目標不可達

隨著無人機與目標之間距離減小，距離因子變小，使得斥力場系數變小，從而使障礙物產生的斥力場減小，最終無人機可以靠近并達到目標點。B.H.Krogh.A曾提出在斥力場計算公式的基礎上加入無人機與目標點相對距離的平方項[17]，如式(16) 所示：

(xgoal-x)2+(ygoal-y)2

(16)

這種方法解決了目標不可達的問題，但由于直接采用距離作為系數，導致斥力場被放大，且無人機與目標點距離越大，斥力場越強。為此采用式(17) 的方式確保斥力場不會被放大并且在無人機與目標點距離較遠時以較為緩和的趨勢衰減。

(17)

式(17)中：dist(p，pgoal)作為影響斥力系數的依據如式(18)所示，當無人機與目標距離很近時則減小斥力值。式(18)中m為指數可調節衰減速度。

dist(p，pgoal)=((xgoal-x)2+(ygoal-y)2)m

(18)

3.3 改進后的人工勢場算法

引入距離因子和衰減函數后，人工勢場算法改為

(19)

改進后人工勢場算法的步驟為，當無人機前方不存在狹窄通道時，按照傳統人工勢場法執行。當前方出現狹窄通道時執行如下中的功能函數使斥力場系數開始衰減。算法偽代碼如下：

Algorithm Window

1： DIST_R=DETECT_POL(β，α)

2： DIST_L=DETECT_POL(-β，-α)

3：η=η0

4： dist_head=DETECT_POL (0)

5： for each dist_r in DIST_R

6： for each dist_l in DIST_L

7： if dist_r< threshold and dist_l < threshold and dist_head =Randη>ηmin

8：η=f(η)

9： elseη=η0

3.4 仿真驗證

仿真地圖障礙物半徑為25，單個障礙物面積為1 962.5，地圖面積為500×500。仿真前設置扇形探測窗口的大小、引力和斥力系數、衰減函數、距離因子等，表2為具體的實驗參數。

表2 實驗參數

1) 軌跡振蕩。無人機起點坐標為(5，5)，終點坐標為(450，470)，路徑規劃和航向角變化仿真結果如圖12和圖13所示。圖12為改進前后路徑規劃對比，圖13為改進前后無人機航向角變化曲線對比。可看出路徑在穿越狹窄區時航跡變得平滑，航向角振蕩消除。

圖12 改進前后路徑規劃示意圖Fig.12 Comparison of path planning before and after improvement

圖13 改進前后航向角變化曲線Fig.13 Change curve of heading angle before and after improvement

2) 目標不可達。無人機的起點坐標(10，10)，終點坐標(450，495)。仿真結果如圖14所示，可以看出改進后目標不可達問題已解決。

圖14 改進前后目標點抵達效果示意圖Fig.14 Target point arrival effect before and after improvement

通過仿真分析可見改進后算法很好地解決了在狹窄通道內軌跡振蕩問題和障礙物附近目標不可達問題。

4 結論

1) 通過設置探測窗口判斷是否存在狹窄通道，如存在則激活衰減函數，進而調整斥力場系數，消除了狹窄通道處軌跡振蕩現象。

2) 通過設置無人機與目標點距離因子來調整斥力場系數，當靠近目標點時，減小斥力系數從而減小斥力，解決了目標與障礙物距離很近時目標點不可達問題。

3) 通過理論分析和仿真驗證，表明改進后方法可以更有效地進行路徑規劃，但人工勢場法為局部方法易陷入局部最優，后續將在此基礎上研究解決局部最優問題，并進行三維空間上路徑規劃的研究。