基于APF算法的無人機動態(tài)航跡規(guī)劃研究及仿真*

白 園，劉 嬋，何健輝

（中國電子科技集團公司第三十研究所，四川 成都 610041）

0 引 言

目前，世界各國軍事實力不斷發(fā)展壯大，戰(zhàn)場環(huán)境越來越復雜，需要無人機可以在具有不確定信息的環(huán)境中躲避各種威脅[1]，提高戰(zhàn)場適應性和生存能力，順利完成作戰(zhàn)任務。航跡規(guī)劃是無人機任務規(guī)劃系統(tǒng)的核心部分。無人機航跡規(guī)劃的目標是在適當?shù)臅r間內(nèi)計算、選擇最優(yōu)或次優(yōu)的飛行航跡，使得執(zhí)行戰(zhàn)術(shù)任務的無人機能突防敵方的威脅環(huán)境，在敵方防空區(qū)域內(nèi)完成任務并保存自己，達到最佳的作戰(zhàn)效果[2]。航跡規(guī)劃按照航跡生成的實時性，分為靜態(tài)航跡規(guī)劃和動態(tài)航跡規(guī)劃[3]。靜態(tài)航跡規(guī)劃是無人機在起飛前根據(jù)戰(zhàn)略策略、相關(guān)的情報信息為無人機預先規(guī)劃的飛行路線。動態(tài)航路規(guī)劃是無人機在飛行中發(fā)現(xiàn)新的威脅或者任務目標改變時，實時動態(tài)規(guī)劃出新的飛行路線。因此，研究無人機如何在復雜的戰(zhàn)場環(huán)境中躲避各種突發(fā)威脅快速規(guī)劃出一條高效的航跡具有重要的意義。

相對于其他航跡規(guī)劃算法，人工勢場法（Artificial Potential Field，APF）不但規(guī)劃速度快且實時性好[4]，更適合用于無人機的動態(tài)航跡規(guī)劃。近年來，它一直是研究的熱點，但是其算法本身存在一些缺陷。本文針對這些缺陷提出了相應的解決辦法，并在改進人工勢場規(guī)劃算法的基礎上，進一步采取分層避障策略。根據(jù)實際情況，同時考慮變速和變向。變速時，分別計算加速和減速時速度的變化量，選擇變化量小的變速措施；變向時又根據(jù)UAV和障礙物的位置關(guān)系使用最小轉(zhuǎn)變角度。這大大節(jié)省了規(guī)劃時間，使無人機能快速躲避威脅，重新規(guī)劃航跡，從而順利到達目的地完成作戰(zhàn)任務。

1 傳統(tǒng)的人工勢場法及缺陷

人工勢場航跡規(guī)劃算法的基本思想是將無人機在作戰(zhàn)環(huán)境中的運動視為一種無人機在虛擬的人工受力場中的運動。作戰(zhàn)目標點對無人機產(chǎn)生引力，威脅源對無人機產(chǎn)生斥力，引力和斥力的合力最終決定無人機的運動方向，即在此合力的作用下控制無人機避開威脅而順利到達目標位置。

1.1 勢函數(shù)模型

1.1.1 引力勢函數(shù)

引力勢函數(shù)是無人機與目標點相對位置的函數(shù)。相對距離越大，目標點對無人機的吸引力越大，促使無人機向目標點移動。隨著距離的減小，吸引力隨之減小，當無人機到達目標點處，引力降為零。即引力大小與距離成正比。它的勢函數(shù)可以表示為[5]：

式中，katt表示引力場增益，p表示無人機的空間位置坐標，pgoal表示目標點的空間位置坐標。

引力是勢函數(shù)的負梯度，因此可以得到引力函數(shù)：

其中Fatt(p)是矢量，方向由無人機指向目標點。

1.1.2 斥力勢函數(shù)

與引力勢函數(shù)類似，斥力勢函數(shù)是無人機與威脅源相對位置的函數(shù)。不同的是，兩者成反比，即無人機離威脅源越近，受到的斥力越大，使其遠離威脅源以避免碰撞；距離越遠，斥力越小，當超過一定距離時，威脅源不對無人機產(chǎn)生排斥力。它的勢函數(shù)為：

式中，krepp表示斥力場增益，pobs表示威脅點的空間位置坐標，d0表示威脅源的影響半徑。

同樣地，對斥力勢函數(shù)求負梯度得斥力函數(shù)：

因此，在整個勢場中，無人機所受到的合力為：

正是這個合力Ftotal( p)決定了無人機的運動。

1.2 算法存在的缺陷

傳統(tǒng)人工勢場方法主要存在2個缺陷。

1.2.1 目標不可達問題

當目標點附近存在威脅源時，無人機接近目標點時，將會受到很大的斥力，而引力相對斥力來說很小，將會導致無人機受到的合力方向偏離目標點，使其無法到達目標點，即人工勢場法的目標不可達問題[6]。

1.2.2 局部極小值問題

在無人機運動過程中，某個時刻無人機受到的目標點的引力與威脅源產(chǎn)生的斥力大小相等、方向相反，即受到的合力為零。此時，無人機誤認為自己到達了目標點，就會使其停滯不前或者前后徘徊。這就是人工勢場法的局部最小值問題[7]。

2 改進的人工勢場法

為了解決目標不可達問題，在斥力勢函數(shù)中引入無人機和目標點之前的相對位置分量[8]。改進后的斥力勢函數(shù)如下：

其中，p-pgoal表示無人機和目標點之間的距離，是一個矢量，方向由無人機指向目標點?？梢钥闯?，引入此分量后，可以保證無人機在到達目標點時勢場值全局最小，即在到達目標點（p=pgoal）時斥力分量也為0，不會使無人機因受到威脅源強大的斥力而達不到目標點。

同樣，對勢函數(shù)求負梯度可以得到斥力表達式：

為了適應動態(tài)環(huán)境，在斥力勢場函數(shù)中引入相對速度項，即增加了無人機與威脅源之間的相對速度分量，其勢函數(shù)可以表示為：

其中，krepv是斥力增益；v是無人機的速度；vobs是障礙物的速度；V是兩者的相對速度，方向在無人機與障礙物的連線上，V＞0表示障礙物朝向無人機運動，V≤0表示障礙物遠離無人機運動；e是一個單位向量，方向由無人機指向障礙物。

對應的斥力分量為：

綜合前面引入的相對位置分量，改進后的斥力勢函數(shù)如下：

對勢函數(shù)求負梯度，即可得到改進后的斥力函數(shù)：

3 避障策略和流程

3.1 分層避障策略

在實際的作戰(zhàn)中，威脅源的分布范圍很廣，方式也不同，有靜態(tài)威脅也有突發(fā)的動態(tài)威脅。對于這些不同的威脅源，如果采取相同的避障措施，必然會降低無人機的靈活性和反應能力，延長航跡規(guī)劃時間而貽誤戰(zhàn)機。因此，本文提出根據(jù)不同的場景采取分層避障策略[9]。根據(jù)威脅源距離無人機的距離，將其分為三個層——航跡規(guī)劃層、常規(guī)避障層和緊急避障層，如圖1所示。無人機對進入其中某個區(qū)域的威脅源執(zhí)行相應操作。對航跡規(guī)劃層，采用人工勢場法規(guī)劃航跡，不采取避障動作直接駛向目的點；對進入常規(guī)避障層的威脅源，采用常規(guī)避障策略，即變速避障與繞行避障兩種，避障過程中仍使用人工勢場法進行航跡規(guī)劃；對進入緊急避障區(qū)采用緊急避障策略，如速度直接降為零等。

圖1 分層避障策略

3.2 避障方法和流程

常規(guī)避障層是本文研究的重點。如果是靜態(tài)威脅源，無人機只能通過改變運動方向來避免碰撞，需要對當前狀態(tài)進行避撞檢測，然后進行相應處理。對于動態(tài)威脅，UAV不僅可以通過改變方向進行避撞，還可以通過變速來避開動態(tài)威脅。無論是動態(tài)還是靜態(tài)，都需要進行碰撞檢測[10]。下面對本文所使用的方法進行簡要介紹。

從無人機的位置做兩條正切于障礙物的切線，如圖2所示。

圖2 碰撞檢測方案設計

定義切線與UO的夾角為α。無人機的速度Vr，動態(tài)威脅源的速度Vo，無人機和威脅源的相對速度Vro=Vr-Vo，Vro與UO的夾角定義為γ。如果是靜態(tài)威脅源，則相對速度即是無人機的速度Vr。從圖2可以看出，若γ＞α，則無人機將不會與威脅源發(fā)生碰撞；否則，將會碰撞，需要采取避障措施。避障流程如圖3所示，主要實現(xiàn)步驟為：

（1）計算相對速度、相關(guān)角度等參數(shù)；

（2）判斷UAV到障礙物的距離r是否小于D；

（3）判斷障礙物是動態(tài)還是靜態(tài)；

（4）判斷UAV到障礙物的距離在哪個層，若r＜L，進入緊急避障層，采取相應措施；否則，若L＜r＜S，處于常規(guī)避障層；

（5）判斷γ與α的大小，如果γ＞α，不避障，否則采取相應避障措施：若Vaindex和Vsindex均為假，說明變速不可行，無人機以最小速度運行并轉(zhuǎn)變方向避障；若Vaindex和Vsindex均為真，考慮加速避障；若Vaindex為真、Vsindex為假，加速避障；若Vaindex為假、Vsindex為真，減速避障；

（6）判斷障礙物是否遠離UAV，若是，恢復速度；否則，繼續(xù)加速或者減速避障。

當遇到障礙物需要轉(zhuǎn)變方向時，本文設計的具體轉(zhuǎn)變措施如下。

首先介紹所需要用到的角度（詳見圖2），UO與UX之間的角度為θ，UAV的速度與UX之間的夾角為θr，UO與UO1之間的角度為α，Vro與UX的夾角為θvro。

圖3 避障流程

令η=θ-θr，當η＞0時，代表障礙物在合力的左邊，如需避障，需要右轉(zhuǎn)角度Δγ；否則η＜0時，代表障礙物在合力的右邊，如需避障，需要左轉(zhuǎn)角度Δγ。

當η＞0時，如果θvro＞θ，則需右轉(zhuǎn)Δγ=α+γ；否則，右轉(zhuǎn) Δγ=α-γ；

當η＜0時，如果θvro＞θ，則需左轉(zhuǎn)Δγ=α-γ；否則，左轉(zhuǎn) Δγ=α+γ。

由于UAV自身的限制，轉(zhuǎn)角不能太大，因此Δγ須小于無人機本身的最大轉(zhuǎn)角。

4 動態(tài)航跡規(guī)劃仿真分析

4.1 航跡規(guī)劃流程

根據(jù)以上介紹內(nèi)容，基于人工勢場法的航跡規(guī)劃算法流程如圖4所示。

圖4 航跡規(guī)劃流程

（1）初始化各項參數(shù)；

（2）計算引力與斥力相應的所需角度；

（3）計算引力、斥力、合力大小及方向；

（4）避障算法，流程和具體實現(xiàn)參考3.2節(jié)；

（5）更新UAV和動態(tài)障礙物位置信息；

（6）判斷UAV是否到達目的地，若到達，則跳出。

4.2 環(huán)境建模

為了驗證設計的人工勢場算法進行動態(tài)航跡規(guī)劃的效果，在Matlab環(huán)境中進行仿真實驗。為了方便仿真分析，作出如下假設：

假設1：障礙物做相應的膨化處理后，無人機可以看做一個質(zhì)點，并認為其位置p和速度v是實時可測的；

假設2：目標G可以看作一個點，它的位置是已知的，且固定不動；

假設3：障礙物采用圓形包絡，障礙物的形狀、包絡、圓的位置和速度都是實時可測的；

假設4：無人機的飛行高度不變，因此無人機和障礙物可以看成二維分布；

假設5：在不避障情況下，無人機和障礙物均做勻速直線運動。

4.3 Matlab仿真分析

根據(jù)前面的航跡規(guī)劃流程和環(huán)境建模，對算法進行仿真。仿真中主要的參數(shù)設置如下：起始點設置為坐標（0，0），目標點設置為（100，100）和（150，150）兩種。其中，點表示無人機的運動軌跡，其運動步長設置1.5，即運動速度設置為1.5；用圓表示靜態(tài)和動態(tài)障礙物，其半徑和速度大小根據(jù)不同的場景設置不同的值；動態(tài)引力勢場模型中，katt取數(shù)值100，斥力勢場模型中krepp取數(shù)值100，krepv取數(shù)值50。krepp取值大小代表相對位置項在斥力場中所占的比重，krepv取值大小代表相對速度項在斥力場中所占的比重。

4.3.1 避障算法仿真

首先對避障算法的性能進行仿真，如圖5所示。

其中，圖5（a）是在遇到動態(tài)障礙物時只采取改變方向的措施進行避障，圖5（b）表示在轉(zhuǎn)變方向的同時進行變速。其中，圓圈表示半徑為2的動態(tài)障礙物的直線運動軌跡，點表示無人機在避障情況下的航跡，縱橫坐標均表示坐標點。從仿真結(jié)果可以明顯看出，設計的變速加變向的避障策略能更快規(guī)劃出了合理的航跡。

圖5 避障算法性能仿真

此外，對多種應用場景進行了仿真。

4.3.2 多動態(tài)障礙物仿真

在單動態(tài)障礙物的基礎上增加一個動態(tài)障礙物，兩個障礙物的大小、速度均不同。圖6為加速通過第一個障礙物后又減速通過第二個障礙物的仿真圖。其中圓圈表示動態(tài)障礙物的運動軌跡，點表示無人機在避障情況下的航跡，縱橫坐標均表示坐標點。從仿真結(jié)果可以看出，根據(jù)不同的威脅，無人機可以實時采取相應的避障策略，動態(tài)規(guī)劃相應的航跡，且加速或減速躲避威脅后又恢復到原來的速度飛行，保證了無人機飛行的穩(wěn)定性。

4.3.3 復雜場景仿真

場景中包括動態(tài)障礙物和靜態(tài)障礙物兩種。設置6個半徑不同的靜態(tài)障礙物和2個運動速度、半徑、軌跡不同的動態(tài)障礙物。

首先，無人機穿過2個靜態(tài)障礙物，然后遇到動態(tài)障礙物并加速通過，順利躲避動態(tài)障礙物；躲避動態(tài)障礙物后又遇到只有狹窄通道的2個靜態(tài)障礙物，這種特殊位置下極易形成局部極小值。經(jīng)過多次調(diào)整角度，順利躲避2個靜態(tài)障礙物，驗證了設計的算法解決了局部最小值問題。然后，遇到第二個動態(tài)障礙物，減速等待；最后，在目標點的附近設置了一個半徑很大的靜態(tài)障礙物，也能完成路徑規(guī)劃，順利到達目標點，驗證了設計的算法解決了目標不可達問題，仿真結(jié)果如圖7所示。其中，圓圈表示動態(tài)障礙物的運動軌跡，點表示無人機在避障情況下的航跡，縱橫坐標均表示坐標點。

圖6 多動態(tài)障礙物下仿真

圖7 復雜場景下仿真

綜合以上仿真結(jié)果，證明設計的算法可以很好地解決傳統(tǒng)人工勢場算法存在的缺陷，并可以在復雜的戰(zhàn)場環(huán)境中順利躲避各種威脅，動態(tài)規(guī)劃一條合理的航跡，實現(xiàn)了無人機動態(tài)路徑規(guī)劃和避障。

5 結(jié) 語

本文在改進斥力場勢函數(shù)的基礎上，采取分層避障策略，并在實際避障時根據(jù)實際應用場景智能選擇最合適的避障方式，不僅解決了人工勢場方法的局部最小和目標不可達問題，而且減少了規(guī)劃航跡的冗余，加快了無人機向目標點移動的速度，節(jié)省了作戰(zhàn)時間，提高了效率。最后，通過仿真驗證了設計的合理性和有效性，對實際作戰(zhàn)具有重要的理論指導意義。