黃加紅，白 麗，范兼睿，張莉涓，雷 磊

（1.南京航空航天大學電子信息工程學院，南京 211106；2.北京航天自動控制研究所，北京 100854）

引 言

無人機航跡規(guī)劃［1］是無人機任務(wù)規(guī)劃中的一項重要任務(wù)，它是指無人機在運動學、動力學以及飛行任務(wù)的約束下，找到從起始位置到目標位置的較優(yōu)路徑。目前，航跡規(guī)劃方法大致分為兩類：（1）全局路徑規(guī)劃或稱為離線路徑規(guī)劃；（2）局部路徑規(guī)劃或稱為在線路徑規(guī)劃。全局路徑規(guī)劃方法通常根據(jù)已知的環(huán)境或過去對環(huán)境的感知信息生成一條優(yōu)化的路徑，但是這種方法無法應(yīng)對未知或突發(fā)威脅的情況。而局部路徑規(guī)劃算法不需要環(huán)境先驗信息，在面對突發(fā)威脅區(qū)域時通過機載傳感器提供的信息實現(xiàn)動態(tài)的航跡規(guī)劃與路線調(diào)整。其中，突發(fā)威脅區(qū)域指的是未提前獲知的對無人機生存產(chǎn)生威脅的地理區(qū)域，例如敵方雷達探測或火力覆蓋范圍。由于在突發(fā)威脅場景下，無人機無法得知全局威脅區(qū)域位置分布情況，導(dǎo)致全局路徑規(guī)劃算法無法工作，因此在此情況下只能采用局部路徑規(guī)劃。

在突發(fā)威脅環(huán)境下，無人機需要實時躲避突發(fā)威脅區(qū)域。目前，研究人員已經(jīng)提出了多種航跡規(guī)劃算法，然而文獻［2?6］采用的航跡規(guī)劃算法一方面搜索點冗余且時間復(fù)雜度高，無法滿足實時性要求；另一方面，由于在突發(fā)威脅環(huán)境下，要求規(guī)劃出的路徑長度短并且符合無人機飛行的性能指標，但上述文獻中的算法均不滿足無人機最小轉(zhuǎn)彎半徑或最大轉(zhuǎn)彎角度的限制。為了克服這些不足，一些學者提出了改進的算法。文獻［7?9］對A*算法的啟發(fā)式搜索函數(shù)進行了改進，一定程度上減少了搜索點的數(shù)量，并且對構(gòu)造出的路徑進行了平滑處理，滿足無人機的飛行需求，但是這種需要對整體路徑進行平滑處理的方法不符合實時躲避威脅區(qū)域的要求。文獻［10］通過雙層蟻群算法進行路徑尋優(yōu)，并對環(huán)境模型進行凸化處理，解決了算法陷入重復(fù)路徑點搜索的問題，但是時間復(fù)雜度仍然隨著環(huán)境的復(fù)雜程度而增加。文獻［11］將人工勢場法進行了優(yōu)化，該算法根據(jù)建立的勢能梯度下降算法尋找實際路徑，當出現(xiàn)局部最小值時構(gòu)造一個新的勢能，此時只有一個全局最小值與機器人的最終目標相匹配，從而克服了陷入局部最小值的問題，一定程度上縮短了路徑長度。但是該算法需要進行二次迭代，時間復(fù)雜度降低不明顯。

綜上，面對突發(fā)威脅情況，上述傳統(tǒng)航跡規(guī)劃算法都存在一些局限性，不能應(yīng)用于真實場景中無人機的航跡規(guī)劃。因此，本文提出一種適合突發(fā)威脅場景的多因素Dubins算法（Multiple factors Dubins algorithm，MFDA）。該算法通過將無人機所處位置與突發(fā)威脅區(qū)域圓作Dubins曲線，并建立路徑擴展點評估函數(shù)進行選擇，不需要對威脅外的區(qū)域進行大量計算和重復(fù)搜索，大大地降低了路徑搜索點的冗余，同時也節(jié)省了路徑規(guī)劃時間。論文的主要創(chuàng)新點和貢獻如下：（1）突發(fā)威脅場景中，在滿足曲率約束和規(guī)定的起始點和終點的切線方向的條件下引入Dubins路徑規(guī)劃出適合無人機真實環(huán)境下的航跡曲線。（2）建立擴展點評估函數(shù)，引入路徑長度代價及威脅代價作為路徑評估因素，提高路徑搜索精度。（3）以Dubins路徑與威脅區(qū)域相切的點作為待選路徑擴展點，在待選擴展點集合中選擇最佳擴展點從而降低搜索點冗余性。

1 MFDA算法的路徑擴展點選擇模型

本文提出的MFDA算法是基于Dubins路徑實現(xiàn)的，因此本節(jié)對Dubins路徑的類型進行了介紹，并在此基礎(chǔ)上對路徑規(guī)劃過程中路徑擴展點的選擇模型進行了分析。

1.1 Dubins路徑

Dubins路徑是在滿足曲率約束和規(guī)定的起始點和終點的切線方向的條件下，連接兩個二維平面（即X?Y平面）的最短路徑［12］，被廣泛應(yīng)用在智能機器人的路徑規(guī)劃上，并且Dubins路徑比直線路徑更接近無人機的實際飛行軌跡。假設(shè)無人機保持恒定的高度和速度，并且受到轉(zhuǎn)彎速度的限制，則Du?bins路徑是一種為無人機尋找從起始點到目標點最短路徑的策略。Dubins路徑由3個路徑段組成，這些路徑段基于給定半徑的直線或圓弧。由兩條曲線段和一條直線段組成的Dubins路徑的4種不同類型如圖1所示。Dubins路徑的4種情況分別是［13］：左轉(zhuǎn)?直線?右轉(zhuǎn)（Left straight right，LSR）、左轉(zhuǎn)?直線?左轉(zhuǎn)（Left straight left，LSL）、右轉(zhuǎn)?直線?右轉(zhuǎn)（Right straight right，RSR）、右轉(zhuǎn)?直線?左轉(zhuǎn)（Right straight left，RSL）。

圖1 Dubins最短路徑Fig.1 The shortest path of Dubins

本文中節(jié)點表示為（xi，yi，ψi），其中xi表示無人機在X平面映射的橫坐標，yi表示在Y平面映射的縱坐標，ψi表示無人機的航向。通過對上述4種Dubins路徑的分析，可以發(fā)現(xiàn)，在航跡規(guī)劃過程中，除了考慮無人機的起始和目標位置信息，在添加起始位置航向以及目標位置航向的基礎(chǔ)上，當無人機跨越威脅區(qū)域飛行時，Dubins路徑無疑是最短路徑［14?16］。

1.2 路徑擴展點的選擇

本文提出的MFDA算法將直線航跡與威脅區(qū)域圓的切點作為算法的下一步擴展節(jié)點，如圖2所示。無人機的最小轉(zhuǎn)彎半徑為ρ，即無人機最小以轉(zhuǎn)彎半徑為ρ的圓調(diào)整航向。起始點位置為Pi，航向為ψi，目標點位置為Pf，航向為ψf。在無人機起始位置和目標位置之間存在一個半徑為rthreat的威脅區(qū)域。由前述分析可知，在最小轉(zhuǎn)彎半徑和航向的限制條件下，Dubins路徑是從起始位置到達目標位置的最短路徑。另一方面，將威脅區(qū)域表示為（xthreat，ythreat，rthreat，Gthreat），其中（xthreat，ythreat）代表威脅區(qū)域的中心坐標，rthreat代表威脅區(qū)域半徑，Gthreat代表威脅級別，威脅等級越大，表明對無人機可能造成的傷害程度越高，并且無人機距離威脅區(qū)域越近，遭遇威脅的可能性越大。從圖2可以看出，有兩條Dubins路徑可以繞過威脅區(qū)域到達目標點位置，分別是路徑Pi Pn1Pf和路徑Pi Pn2Pf，它們分別屬于LSR類型和RLS類型。因此將直線路徑與威脅區(qū)域圓的切點作為A*算法的下一步擴展節(jié)點，將節(jié)點Pn1和節(jié)點Pn2均加入OPEN（前向路徑曲線節(jié)點集合）中，通過評估函數(shù)分別計算兩個節(jié)點的代價值并進行比較，選擇出代價值最小的節(jié)點作為下一步的路徑擴展節(jié)點，并把此節(jié)點加入到CLOSE（后向完整路徑曲線節(jié)點集合）中。

圖2 擴展點選擇Fig.2 Extension point selection

2 MFDA路徑規(guī)劃算法

2.1 傳統(tǒng)A*算法

A*算法是最著名的路徑規(guī)劃算法之一，該算法采用啟發(fā)式搜索和基于最短路徑的搜索相結(jié)合的方法［17］。A*算法被定義為最佳優(yōu)先搜索算法，因為將空間進行單元格劃分后，每個單元格的代價都是由估價函數(shù)f（n）來計算的。

式中：函數(shù)g（n）定義為從已知開始節(jié)點到任意節(jié)點n的一條最優(yōu)路徑的代價，函數(shù)h（n）定義為從節(jié)點n到目標節(jié)點的整個目標節(jié)點集合內(nèi)的最小代價路徑的代價。節(jié)點n到目標節(jié)點的隨機路徑為h（n），即節(jié)點n到目標節(jié)點的最佳路徑。

A*算法在進行節(jié)點擴展的過程中，存在上、下、左、右、左上、右上、左下、右下共8個方向的網(wǎng)格點可以作為下一步的待選路徑擴展點，會循環(huán)地判斷待選節(jié)點是否在OPEN表或者CLOSE表中。如果節(jié)點不在OPEN表中，將其加入OPEN表并通過式（1）計算每個節(jié)點的代價值，通過比較當前節(jié)點周圍節(jié)點的代價值，從中選擇代價值最小的節(jié)點作為下一步的擴展節(jié)點，并將其加入到CLOSE表中，循環(huán)上述步驟，直到找到目標節(jié)點，形成一條從起始點到目標點的最優(yōu)路徑。由此可以看出，在從起始點到目標點的路徑規(guī)劃過程中，每一步都需要計算8個方向的代價值，因此傳統(tǒng)的A*算法存在搜索點冗余的缺陷，并且較多的計算次數(shù)會大大增加路徑規(guī)劃時間。

2.2 人工勢場法

人工勢場法在1986年由Khatib提出，主要思想是通過建立目標點的引力場以及障礙物的斥力場，在兩個勢力場合力的作用下沿著勢場函數(shù)下降的方向移動，由此形成一條避開障礙物的最優(yōu)路徑。由圖3可以看出，由目標產(chǎn)生的引力場方向指向目標位置，由障礙物產(chǎn)生的斥力場，方向遠離障礙物。

圖3 人工勢場法基本原理Fig.3 Basic principles of artificial poten?tial field method

在人工勢場法中合力即為勢能函數(shù)的梯度，聯(lián)合合力和無人機的運動狀態(tài)方程即可得到其飛行軌跡，由于人工勢場法避免了搜索最優(yōu)解或非線性參數(shù)優(yōu)化的過程，因此路徑規(guī)劃的速度相比于A*算法更快。但是人工勢場法也存在明顯的缺陷，即局部極值問題。當人工勢場的合力指向局部極值點時，無人機的飛行軌跡將會陷入局部震蕩，而無其他外力將飛機拉出局部極點，因此會導(dǎo)致路徑長度極度增加，更糟糕的情況會直接導(dǎo)致航跡規(guī)劃失敗。

2.3 MFDA算法路徑規(guī)劃流程

本文提出一種多因素Dubins路徑算法，重新建立基于路徑長度代價及威脅代價作為路徑評估因素的評估函數(shù)，通過1.2節(jié)路徑擴展點的選取可以看出，在當前路徑擴展點選擇下一步路徑擴展點的過程中，根據(jù)Dubins曲線的特性，待選路徑擴展點中最多只會存在兩個選擇，并且Dubins路徑只作與威脅區(qū)域相切的曲線，那么在從起始點到目標點的整個路徑規(guī)劃過程中路徑擴展點的數(shù)量會大幅度降低，另外Dubins曲線是在滿足曲率約束和規(guī)定的起始點和終點的切線方向的條件下，連接兩個二維平面（即X?Y平面）的最短路徑，那么最終通過MFDA算法得到的規(guī)劃路徑長度也會減小。MFDA算法路徑規(guī)劃過程如圖4所示。

圖4 MFDA算法路徑規(guī)劃過程Fig.4 Path planning process of MFDA

算法路徑規(guī)劃過程步驟如下：

（1）首先將起始點當作當前路徑擴展點，生成到目標點的路徑。如果在此方向上不存在威脅區(qū)域，則將目標點Pf加入到CLOSE表中，構(gòu)建成一條從起始點到目標點的路徑曲線，算法運行結(jié)束。如果在此方向上存在威脅區(qū)域，則生成當成當前路徑擴展點到威脅區(qū)域的多條Dubins曲線。

（2）通過MFDA算法估價函數(shù)計算并選出代價值最小的點作為下一步的路徑擴展點，將此路徑擴展點加入到CLOSE表中。

（3）將步驟（2）中選取的路徑擴展點作為當前的位置，然后重新執(zhí)行步驟（1）的操作。之后重復(fù)上述過程，不斷產(chǎn)生新的路徑擴展點，直到到達目的點，構(gòu)建出完整路徑。

由上述MFDA算法路徑規(guī)劃過程可知，路徑搜索的過程也就是通過計算不同Dubins路徑的代價值從而選取路徑擴展點的過程，因此下節(jié)著重分析步驟（2）中路徑擴展點的代價計算和選取。

2.4 路徑擴展點的代價計算與選取

MFDA算法同時考慮路徑代價和威脅代價對無人機軌跡的影響，估價函數(shù)建立如下

式 中：g（n）=w1·Distance+w2·Hazard，w1表示距離代價的影響因子，w2表示威脅代價的影響因子，w1+w2=1。Distance距離由兩端弧長和一段直線長度構(gòu)成，由圖5所示，分別計算LSR和RLS兩種類型的Dubins路徑距離。

圖5 LSR路徑類型Fig.5 Path type of LSR

起始節(jié)點（xi，yi，ψi），求解圓Oi和圓Othreat的切線Qi Pn的坐標。

步驟1計算圓Oi的圓心坐標

威脅區(qū)域Othreat的圓心坐標已知。

步驟2生成直線航跡Qi Pn

通過上述代價值的計算與比較，選擇代價值較小的節(jié)點作為MADA算法的下一步路徑擴展點。

因此，根據(jù)2.3節(jié)和2.4節(jié)的MFDA算法的路徑擴展點選取過程，通過不斷的循環(huán)計算，可以規(guī)劃出一條在突發(fā)威脅場景下從起始點到目標點的無人機飛行航跡。

3 仿真與分析

根據(jù)前述分析，對本文設(shè)計的航跡規(guī)劃算法進行了仿真驗證，并與傳統(tǒng)的A*算法、人工勢場法和雙層蟻群算法進行仿真性能比較。

3.1 少量突發(fā)威脅區(qū)域路徑規(guī)劃

圖6、7分別代表無人機起始點和目標點連線上存在一個或兩個障礙物時，4種無人機航跡規(guī)劃算法的仿真曲線。

圖6區(qū)域中只存在一個威脅區(qū)域，其位置、半徑和威脅等級表示為Othreat=(100，100，15，2)。起始位姿和最終位姿分別表示為：(2，2，0.167π)，(200，200，0.2π)。

圖6 單威脅區(qū)域場景下航跡規(guī)劃結(jié)果Fig.6 Track planning results under single threat area scenario

在此場景下，MFDA算法所得路徑由2段Dubins曲線組成，長度為270.914 6 m，并且路徑光滑。在相同條件下，由A*算法、人工勢場法、雙層蟻群算法計算出路徑長度分別為295.386 9、390.261 1、294.871 2 m，均大于本文提出的MFDA算法規(guī)劃的路徑長度，并且路徑不光滑，在實際應(yīng)用中不能直接使用。

圖7區(qū)域中存在兩個威脅區(qū)域，其位置、半徑和威脅等級分別表示為：Othreat1=(65，65，15，2)，Othreat2=(130，130，15，1)，起始位姿和最終位姿不變。在此場景下，MFDA算法規(guī)劃出的路徑由3段Dubins曲線組成，長度為271.377 0 m。相同條件下，A*算法、人工勢場法以及雙層蟻群算法計算出路徑長度分別為296.558 4、462.562 3、295.678 6 m，同樣大于MFDA算法規(guī)劃的路徑長度。

3.2 大量突發(fā)威脅區(qū)域路徑規(guī)劃

區(qū)域中共有6個威脅區(qū)域Othreat1~Othreat6，其位置、半徑和威脅等級表示為：Othreat1=(70，80，15，2)，Othreat2=(90，130，15，2)，Othreat3=(110，180，15，2)，Othreat4=(120，110，15，1)，Othreat5=(140，150，15，2)，Othreat6=(190，140，15，4)。無人機起始位姿和最終位姿分別表示為：(2，2，0.167π)，(200，200，0.2π)。

圖8中MFDA算法、A*算法、人工勢場法、雙層蟻群算法規(guī)劃出的路徑長度分別為：272.121 2，298.244 7，481.427 8，296.360 1 m，因此MFDA算法具有明顯的優(yōu)勢。并且本文提出的MFDA算法計算出的路徑曲線滿足了無人機航行路徑平滑性要求，并且曲線與直線相切，滿足路徑連續(xù)性要求。但是A*算法在此情況下所規(guī)劃出的路徑存在局部路徑轉(zhuǎn)角突變的情況，由于無人機存在自身轉(zhuǎn)彎半徑和轉(zhuǎn)彎最小角度的限制，因此無法滿足實際飛行的需求。同樣，人工勢場法在此場景下會存在局部極小值點，導(dǎo)致持續(xù)震蕩，極大地增加路徑長度［18］。

3.3 算法性能比較與分析

分別對上述不同場景下航跡規(guī)劃算法的性能指標進行統(tǒng)計并分析。圖9~12分別代表了單威脅區(qū)域場景、雙威脅區(qū)域場景以及多威脅區(qū)域場景3種場景下不同航跡規(guī)劃算法的路徑規(guī)劃長度、路徑擴展點數(shù)量、路徑規(guī)劃時間和路徑航向變化4個性能指標。

從圖9中可以看出，在路徑規(guī)劃長度方面，MFDA算法計算出路徑最短，A*算法稍差，人工勢場法所規(guī)劃的路徑長度則遠大于另外兩種算法計算出的路徑長度。在3種場景下，MFDA算法路徑規(guī)劃平均長度為271.47 m，A*算法路徑規(guī)劃平均長度為296.73 m，人工勢場法路徑規(guī)劃平均長度為444.750 4 m，MFDA算法相比于A*算法路徑長度平均減少了8.4%，相比于人工勢場法平均減少了39%，雙層蟻群算法路徑規(guī)劃平均長度為295.636 6 m，相比于人工勢場法平均減少了8.2%。因此MFDA算法規(guī)劃的路徑可以保證無人機以較短的距離從起始點到達目的點。

圖9 3個場景下路徑規(guī)劃長度Fig.9 Path planning length under three scenarios

從圖10路徑擴展點指標上可以看出，本文提出的MFDA算法在路徑規(guī)劃過程中得到的路徑擴展點數(shù)量遠小于其他兩種算法。在3種場景下，MFDA算法平均路徑擴展點數(shù)量為6個，A*算法平均路徑擴展點數(shù)量為222個，人工勢場法平均路徑擴展點數(shù)量為384個，雙層蟻群算法平均路徑擴展點數(shù)量為144個。無人機在實際飛行過程中，通過路徑擴展點確定下一步飛行到達的位置，并且從前一個路徑擴展點到下一個路徑擴展點，由于無人機自身物理條件以及實際環(huán)境的限制，無人機需要頻繁地調(diào)整飛行姿態(tài)及速度，因此在整個路徑規(guī)劃過程中，路徑擴展點越少越好?？梢钥闯霰疚奶岢龅腗FDA算法相比于另外3種航跡規(guī)劃算法，具有明顯較少的路徑擴展點。圖11表示3種場景下路徑規(guī)劃時間，可以看出MFDA算法規(guī)劃出一條從起始點到目標點的完整路徑所需要的時間明顯少于A*算法和人工勢場法。在3種場景下，MFDA算法平均路徑規(guī)劃時間相比于A*算法減少了90%，相比于人工勢場法減少了55%，相比于雙層蟻群算法減少了82%。因此，MFDA算法在路徑規(guī)劃過程中具有較優(yōu)的實時性。

圖10 3個場景下路徑擴展點數(shù)量Fig.10 Number of path extension points under three scenarios

圖11 3個場景下路徑規(guī)劃時間Fig.11 Path planning time under three sce?narios

圖12表示4種航跡規(guī)劃算法在場景3的情況下，無人機在繞過第一個威脅區(qū)域的飛行過程中航向變化（與Ox正半軸的夾角）。從圖12中可以看出，在規(guī)避威脅區(qū)域的整個過程中，MFDA算法規(guī)劃出的路徑航向并沒有出現(xiàn)極值即突變情況，在距離起點105 m附近，MFDA算法規(guī)劃處的路徑通過逐步過渡的情況改變航向，符合無人機實際飛行過程中航向角變化。但是人工勢場法、A*算法、雙層蟻群算法規(guī)劃的路徑均存在航向突變的情況，不符合無人機實際飛行時的航向變化，無法應(yīng)用在真實飛行環(huán)境中。

圖12 路徑航向變化Fig.12 Change of course heading

4 結(jié)束語

針對無人機飛行過程中遭遇突發(fā)威脅區(qū)域的場景，提出了一種多因素Dubins算法，該算法在Du?bins路徑的基礎(chǔ)上建立路徑擴展點評估函數(shù)，通過引入路徑長度評估因子和威脅評估因子對路徑擴展點進行選擇，從而可以有效地降低路徑搜索點的數(shù)量。同時結(jié)合啟發(fā)式搜索的思想，對可能出現(xiàn)的路徑長度代價和威脅代價進行評估，達到了縮短路徑長度的目的。并且增加無人機起始點方向和到達目的點方向限制條件，構(gòu)建了適用于真實環(huán)境中的無人機飛行航跡。仿真表明，MFDA算法在突發(fā)威脅區(qū)域場景下，可以規(guī)劃出較短的路徑，同時相比于傳統(tǒng)的航跡規(guī)劃算法具有較少的路徑搜索點，并且得到的路徑符合無人機實際飛行時的航向變化。

本文僅考慮了單架無人機的航跡規(guī)劃，在下一步的研究中將研究在無人機集群情況下的航跡安全和威脅區(qū)域規(guī)避等基本問題。