一種面向突發(fā)威脅的多因素Dubins航跡規(guī)劃算法

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

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

引 言

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

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

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

1 MFDA算法的路徑擴(kuò)展點(diǎn)選擇模型

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

1.1 Dubins路徑

Dubins路徑是在滿足曲率約束和規(guī)定的起始點(diǎn)和終點(diǎn)的切線方向的條件下，連接兩個(gè)二維平面（即X?Y平面）的最短路徑［12］，被廣泛應(yīng)用在智能機(jī)器人的路徑規(guī)劃上，并且Dubins路徑比直線路徑更接近無人機(jī)的實(shí)際飛行軌跡。假設(shè)無人機(jī)保持恒定的高度和速度，并且受到轉(zhuǎn)彎速度的限制，則Du?bins路徑是一種為無人機(jī)尋找從起始點(diǎn)到目標(biāo)點(diǎn)最短路徑的策略。Dubins路徑由3個(gè)路徑段組成，這些路徑段基于給定半徑的直線或圓弧。由兩條曲線段和一條直線段組成的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é)點(diǎn)表示為（xi，yi，ψi），其中xi表示無人機(jī)在X平面映射的橫坐標(biāo)，yi表示在Y平面映射的縱坐標(biāo)，ψi表示無人機(jī)的航向。通過對(duì)上述4種Dubins路徑的分析，可以發(fā)現(xiàn)，在航跡規(guī)劃過程中，除了考慮無人機(jī)的起始和目標(biāo)位置信息，在添加起始位置航向以及目標(biāo)位置航向的基礎(chǔ)上，當(dāng)無人機(jī)跨越威脅區(qū)域飛行時(shí)，Dubins路徑無疑是最短路徑［14?16］。

1.2 路徑擴(kuò)展點(diǎn)的選擇

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

圖2 擴(kuò)展點(diǎn)選擇Fig.2 Extension point selection

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

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

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

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

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

2.2 人工勢(shì)場(chǎng)法

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

圖3 人工勢(shì)場(chǎng)法基本原理Fig.3 Basic principles of artificial poten?tial field method

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

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

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

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

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

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

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

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

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

2.4 路徑擴(kuò)展點(diǎn)的代價(jià)計(jì)算與選取

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

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

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

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

步驟1計(jì)算圓Oi的圓心坐標(biāo)

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

步驟2生成直線航跡Qi Pn

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

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

3 仿真與分析

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

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

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

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

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

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

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

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

區(qū)域中共有6個(gè)威脅區(qū)域Othreat1~Othreat6，其位置、半徑和威脅等級(jí)表示為：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)。無人機(jī)起始位姿和最終位姿分別表示為：(2，2，0.167π)，(200，200，0.2π)。

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

3.3 算法性能比較與分析

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

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

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

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

圖10 3個(gè)場(chǎng)景下路徑擴(kuò)展點(diǎn)數(shù)量Fig.10 Number of path extension points under three scenarios

圖11 3個(gè)場(chǎng)景下路徑規(guī)劃時(shí)間Fig.11 Path planning time under three sce?narios

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

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

4 結(jié)束語(yǔ)

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

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