基于APF-RRT算法的無人機(jī)航跡規(guī)劃

陳 俠，劉奎武，毛海亮

(沈陽(yáng)航空航天大學(xué)，沈陽(yáng) 110000)

0 引言

隨著社會(huì)的進(jìn)步和科學(xué)技術(shù)的發(fā)展，無人機(jī)的應(yīng)用領(lǐng)域越來越廣泛，如農(nóng)業(yè)、工業(yè)和軍事等領(lǐng)域[1-3]。航跡規(guī)劃是無人機(jī)控制系統(tǒng)不可缺少的部分，一直是各國(guó)學(xué)者廣泛關(guān)注的問題。隨著飛行任務(wù)要求的不斷提高，無人機(jī)飛行環(huán)境信息變得越來越復(fù)雜，如何高效、快速地規(guī)劃出可行路徑一直是無人機(jī)技術(shù)的重要研究課題。

近年來，各國(guó)學(xué)者提出了許多航跡規(guī)劃方法，例如人工勢(shì)場(chǎng)法[4]、A*算法[5]和Dijkstra[6]等傳統(tǒng)航跡規(guī)劃算法，以及遺傳算法[7]、粒子群算法[8]等智能優(yōu)化算法。然而，當(dāng)戰(zhàn)場(chǎng)環(huán)境變得復(fù)雜，無人機(jī)任務(wù)多樣性提高，原有算法已經(jīng)很難滿足無人機(jī)航跡規(guī)劃要求。于是，有關(guān)學(xué)者開始對(duì)算法進(jìn)行改進(jìn)來滿足無人機(jī)飛行要求。GIRIJA等[9]提出了一種在多障礙物環(huán)境下，基于粒子群和人工勢(shì)場(chǎng)法結(jié)合的混合航跡規(guī)劃；GURUJI等[10]提出了一種改進(jìn)高效A*算法，縮短了路徑處理時(shí)間及路徑長(zhǎng)度；程澤新等[11]給出了一種差異進(jìn)化變異策略來增加算法多樣性，并將遺傳算法與模擬退火算法相結(jié)合，擺脫了局部最優(yōu)。然而上述算法仍然比較耗時(shí)，很難滿足空戰(zhàn)的實(shí)際需求。因此，如何快速獲得較優(yōu)的無人機(jī)航跡規(guī)劃仍為研究熱點(diǎn)。

快速隨機(jī)搜索樹(RRT)算法是由LAVALLE[12]提出的一種快速搜索算法，它避免了準(zhǔn)確的環(huán)境模型，并可以在較短的時(shí)間內(nèi)找到一條路徑。但是該算法找不到最優(yōu)路徑，同時(shí)全局采樣導(dǎo)致收斂緩慢，存在大量冗余點(diǎn)，不能滿足無人機(jī)飛行要求，不少學(xué)者針對(duì)這些問題進(jìn)行了改進(jìn)。成浩浩等[13]利用概率引導(dǎo)隨機(jī)樹的生長(zhǎng)方向和動(dòng)態(tài)步長(zhǎng)搜索未知空間，通過雙層平滑優(yōu)化策略規(guī)劃路徑，獲得無人機(jī)飛行的可行路徑；JANJO等[14]將RRT算法生成的路徑點(diǎn)序列集輸入四次樣條曲線，獲得了一條平滑的動(dòng)態(tài)可行路徑;尹高揚(yáng)等[15]改進(jìn)了原始RRT算法最近鄰點(diǎn)的選取策略，但是仍然采取全局隨機(jī)采樣策略，在一定程度上增加了算法的搜索時(shí)間;劉成菊等[16]在目標(biāo)偏置RRT算法的基礎(chǔ)上加入了人工勢(shì)場(chǎng)法的引力思想，給出了機(jī)器人的路徑規(guī)劃算法，該算法雖提高了搜索效率，但并未考慮障礙物的斥力作用。雖然上述文獻(xiàn)對(duì)RRT算法進(jìn)行了改進(jìn)，取得了一些研究成果，但仍然需要提高算法效率，并未解決原算法的路徑隨機(jī)性問題。

針對(duì)已有算法的改進(jìn)與不足，本文提出一種將改進(jìn)RRT算法、人工勢(shì)場(chǎng)法及遺傳算法相結(jié)合(APF-RRT)的無人機(jī)航跡規(guī)劃方法。首先，利用目標(biāo)偏置來改變隨機(jī)采樣點(diǎn)的生成方式，在原有全局采樣的基礎(chǔ)上，目標(biāo)點(diǎn)有一定概率作為采樣點(diǎn)，提高了搜索效率。然后,引入人工勢(shì)場(chǎng)法來優(yōu)化新節(jié)點(diǎn)的生成方式，目標(biāo)點(diǎn)作為最近鄰點(diǎn)的引力，障礙物作為最近鄰點(diǎn)的斥力，將二者的合力方向作為新節(jié)點(diǎn)的生成方向。最后，規(guī)劃出基于改進(jìn)RRT算法的航跡點(diǎn)，并進(jìn)一步將該航跡點(diǎn)序列作為遺傳算法的初始種群，建立適應(yīng)度函數(shù)，通過選擇、交叉和變異操作獲得種群的較優(yōu)無人機(jī)飛行航跡，解決了路徑隨機(jī)性問題，且改進(jìn)后算法生成的路徑長(zhǎng)度更短，消耗時(shí)間更少。

1 改進(jìn)RRT算法

1.1 基本RRT算法

RRT算法是在空間中隨機(jī)采樣構(gòu)造搜索樹[12]，搜索樹從飛行起點(diǎn)擴(kuò)展到目標(biāo)點(diǎn)，在節(jié)點(diǎn)擴(kuò)展過程中應(yīng)滿足無人機(jī)的性能約束。圖1為算法的節(jié)點(diǎn)擴(kuò)展示意圖。

圖1中，qstart和qgoal分別為飛行起點(diǎn)和目標(biāo)點(diǎn)，qrand代表空間中的隨機(jī)采樣點(diǎn)，qnear為搜索樹中的最近鄰點(diǎn)，qnew是擴(kuò)展后生成的新節(jié)點(diǎn)，L為無人機(jī)的飛行步長(zhǎng)。

圖1 算法節(jié)點(diǎn)擴(kuò)展示意圖Fig.1 Node extension diagram

RRT算法流程如圖2所示。

圖2 RRT算法流程圖Fig.2 Flow chart of RRT algorithm

首先進(jìn)行初始化設(shè)置，qstart作為擴(kuò)展樹的第一個(gè)最近鄰點(diǎn)qnear，M是最大迭代次數(shù)，然后在空間隨機(jī)選取一點(diǎn)作為qrand，根據(jù)qrand與搜索樹集合中的點(diǎn)的歐氏距離選取最近鄰點(diǎn)qnear，以qnear為起點(diǎn)朝qrand方向擴(kuò)展步長(zhǎng)L獲得新節(jié)點(diǎn)qnew，如果qnear與qnew的連線途經(jīng)障礙或qnew處于障礙中，則需要重新選擇qnear，反之將qnew儲(chǔ)存在搜索樹集合中。當(dāng)qnew到達(dá)目標(biāo)點(diǎn)或達(dá)到最大迭代次數(shù)M時(shí)，算法終止。

1.2 RRT算法改進(jìn)

本文主要從兩個(gè)方面對(duì)RRT算法進(jìn)行改進(jìn)，即隨機(jī)采樣點(diǎn)qrand的選取和新節(jié)點(diǎn)qnew的生成。在原始RRT算法中，qrand由搜索空間隨機(jī)采樣獲得，本文對(duì)qrand的選取做了改進(jìn)，引入目標(biāo)偏置思想，改進(jìn)后的qrand選取為

(1)

式中：rand()為0～1間的隨機(jī)數(shù)；δ為設(shè)定的目標(biāo)偏置閾值。當(dāng)rand()值大于設(shè)定閾值時(shí)，在采樣空間中選取隨機(jī)點(diǎn)作為qrand，反之將qgoal作為隨機(jī)采樣點(diǎn)。

在原始RRT算法的基礎(chǔ)上，通過加入人工勢(shì)場(chǎng)法優(yōu)化qnew的生成方式，加快算法的收斂速度。KHATIB[17]首次提出了人工勢(shì)場(chǎng)法，其中包括引力場(chǎng)Uatt(q)和斥力場(chǎng)Urep(q)。假設(shè)p是無人機(jī)的一個(gè)航跡點(diǎn)，則引力勢(shì)場(chǎng)函數(shù)、斥力勢(shì)場(chǎng)函數(shù)和合力勢(shì)場(chǎng)函數(shù)分別表示為

(2)

(3)

Utotal=∑Urep+∑Uatt

(4)

式中：ka為引力場(chǎng)增益常數(shù)；kr為斥力場(chǎng)增益常數(shù)；ρ0為航跡點(diǎn)p距離威脅范圍的最大安全距離；ρg(p)和ρ(p)分別表示點(diǎn)p與目標(biāo)點(diǎn)、點(diǎn)p與最近威脅中心的歐氏距離。引力和斥力的大小分別是引力場(chǎng)函數(shù)和斥力場(chǎng)函數(shù)的負(fù)梯度。根據(jù)式(2)～(4)可知

Fatt(p)=kaρg(p)

(5)

(6)

Ftotal=∑Frep+∑Fatt。

(7)

通過結(jié)合人工勢(shì)場(chǎng)法，目標(biāo)點(diǎn)qgoal和隨機(jī)采樣點(diǎn)qrand對(duì)qnear產(chǎn)生引力，障礙物對(duì)qnear產(chǎn)生斥力，三者的合力決定了qnew的生成方向。qnear的受力情況如圖3所示。

圖3 qnear受力分析示意圖Fig.3 Force analytical graph of qnear

圖3中，F(xiàn)att1和Fatt2分別為qrand和qgoal對(duì)qnear的引力，F(xiàn)rep為障礙物對(duì)qnear的斥力，根據(jù)平行四邊形法則，紅色實(shí)線為Fatt1與Frep的合力方向，藍(lán)色實(shí)線由紅色實(shí)線與Fatt2矢量求和獲得，是qnear所受合力方向，即qnew的擴(kuò)展方向。

然而，當(dāng)無人機(jī)距離障礙物比較近時(shí)，其所受的斥力會(huì)變得很大。當(dāng)斥力大于引力時(shí)，無人機(jī)將不能到達(dá)目標(biāo)點(diǎn)。因此，本文改進(jìn)了勢(shì)場(chǎng)函數(shù)，改進(jìn)后的勢(shì)場(chǎng)函數(shù)為

(8)

(9)

式中：p，pgoal，pobs分別代表qnear，qgoal和障礙物的位置;n為一個(gè)正整數(shù);ρ(p,pobs)為qnear距其最近的障礙物之間的歐氏距離;ρg(p,pgoal)為qnear和qgoal之間的距離;當(dāng)無人機(jī)靠近障礙物時(shí)，障礙物產(chǎn)生的斥力會(huì)隨著ρg(p,pgoal)的減小而變小，這樣避免了斥力大于引力的現(xiàn)象。改進(jìn)后的斥力表達(dá)式為

Frep(p)=-▽Urep(p)=Frep1nOR+Frep2nRG

(10)

(11)

(12)

式中,單位向量nOR=▽?duì)?p,pobs)和nRG=-▽?duì)?p,pgoal)分別為qnear和qgoal的方向矢量，以及qnear和障礙物之間的方向矢量。當(dāng)無人機(jī)接近目標(biāo)點(diǎn)時(shí)，與其最近的障礙物之間的斥力就會(huì)變得很小，可以保證無人機(jī)能夠到達(dá)目標(biāo)點(diǎn)。在求解qnew時(shí)，首先，需要分別計(jì)算qrand和qgoal對(duì)qnear的引力，然后計(jì)算障礙物對(duì)qnear的斥力，之后將合力Ftotal分解為x軸和y軸兩個(gè)方向的分力，分別用Fx，F(xiàn)y表示。假設(shè)qnear的坐標(biāo)為(xc，yc)，擴(kuò)展步長(zhǎng)由qnear在x，y軸上受到的合力的分量決定。選擇每個(gè)坐標(biāo)軸上的合力分量最大值的絕對(duì)值作為Fmax，則無人機(jī)的擴(kuò)展步長(zhǎng)可以確定為

(13)

式中:l為無人機(jī)的擴(kuò)展步長(zhǎng)，lx和ly分別為無人機(jī)在x，y軸上的前進(jìn)分量；k是比例系數(shù)，由合力分量最大值與l確定。這樣就可以得到qnew的坐標(biāo)，令qnew的坐標(biāo)為(xnew，ynew)，則qnew的坐標(biāo)方程為

(14)

2 基于遺傳算法的航跡優(yōu)化

遺傳算法是一種通過模擬自然演化過程來尋找最優(yōu)解的方法，通常從需要求解的問題中找到多種可能的解決方案，形成一組解集。然后，將該解集視為一個(gè)種群，模擬生物進(jìn)化中的自然選擇和遺傳機(jī)制。通過適應(yīng)度函數(shù)評(píng)價(jià)每個(gè)個(gè)體，經(jīng)過選擇、交叉、變異等操作，最終獲得問題的最優(yōu)解。本文將改進(jìn)后的RRT算法生成的一組航跡點(diǎn)作為初始種群。

適應(yīng)度函數(shù)直接影響收斂速度，是確定群體中個(gè)體優(yōu)劣程度的指標(biāo)。本文適應(yīng)度函數(shù)由路徑長(zhǎng)度函數(shù)、路徑平滑函數(shù)和路徑安全函數(shù)組成。

L(p)是路徑總長(zhǎng)度，(xi，yi)是第i個(gè)航跡點(diǎn)的坐標(biāo)，m是航跡點(diǎn)總數(shù)，路徑長(zhǎng)度函數(shù)定義為

(15)

(16)

R(p)代表航跡的安全度，Di是第i個(gè)航跡點(diǎn)與距離它最近的障礙的距離。路徑安全函數(shù)定義為

(17)

適應(yīng)度函數(shù)f(p)可以表示為

(18)

式中，α，β和γ分別代表著路徑長(zhǎng)度、路徑平滑度和路徑安全性的權(quán)重因子。

平滑度如圖4所示。

圖4 平滑度示意圖Fig.4 Schematic diagram of smoothness

通過適應(yīng)度函數(shù)分別評(píng)估每個(gè)個(gè)體。選取個(gè)體中適應(yīng)度函數(shù)值最大的個(gè)體作為最佳個(gè)體,交叉概率設(shè)置為0.6,突變概率設(shè)置為0.01。

3 仿真分析

本文使用Matlab來驗(yàn)證該改進(jìn)算法的有效性。將本文算法分別與原始RRT算法和只加入目標(biāo)引力RRT(tRRT)算法[16]進(jìn)行比較。分別在簡(jiǎn)單環(huán)境和復(fù)雜環(huán)境下進(jìn)行算法驗(yàn)證，障礙物分布信息分別如表1、表2所示。

表1 簡(jiǎn)單環(huán)境下障礙物信息Table 1 Obstacle information in simple environment

表2 復(fù)雜環(huán)境下障礙物信息Table 2 Obstacle information in complex environment

仿真地圖設(shè)置為100 km×100 km的二維空間。起點(diǎn)坐標(biāo)設(shè)置為(1 km，1 km)，目標(biāo)點(diǎn)設(shè)置為(90 km，90 km)。簡(jiǎn)單環(huán)境中的仿真結(jié)果如圖5所示，其中，藍(lán)色線條表示隨機(jī)樹，紅色線條為算法規(guī)劃出的無人機(jī)可行航跡，紅色圓圈代表著障礙物，無人機(jī)不能在該區(qū)域通過。

圖5 簡(jiǎn)單環(huán)境中的仿真Fig.5 Simulation in simple environment

在圖5(a)中，由于原始RRT算法隨機(jī)采樣，地圖中存在大量冗余節(jié)點(diǎn)。圖5(b)是tRRT算法的仿真結(jié)果，與原始RRT算法相比，隨機(jī)樹的生長(zhǎng)方向更具有指向性，加快了原始RRT算法的搜索速度。雖然tRRT算法提高了搜索效率，但是，由于缺少障礙物的斥力，該算法在障礙物周圍出現(xiàn)冗余點(diǎn)。圖5(c)為本文算法，障礙物作用的加入，使得本文算法與tRRT算法相比，搜索效率更高，無人機(jī)能更快到達(dá)目標(biāo)點(diǎn)。

在障礙物多的復(fù)雜環(huán)境下，仿真結(jié)果如圖6所示。

隨著環(huán)境變得越來越復(fù)雜，3種算法的樹節(jié)點(diǎn)的數(shù)量就會(huì)增加，這表明迭代的次數(shù)就會(huì)增加。圖6中，藍(lán)色線條表示隨機(jī)樹，紅色線條是算法規(guī)劃出的可行航跡。通過兩種環(huán)境下與其他兩種算法的對(duì)比仿真，本文算法極大地提高了搜索效率，生成的節(jié)點(diǎn)數(shù)量和迭代次數(shù)均有顯著減少。

為了驗(yàn)證算法的穩(wěn)定性，本文對(duì)每種算法在復(fù)雜環(huán)境中進(jìn)行30次實(shí)驗(yàn)，記錄算法的運(yùn)行時(shí)間、生成路徑的長(zhǎng)度和迭代次數(shù)。實(shí)驗(yàn)均值及對(duì)比結(jié)果分別如表3和圖7所示。

表3 30次實(shí)驗(yàn)平均值Table 3 Mean value of 30 tests

圖7 3種算法數(shù)據(jù)比較Fig.7 Data comparison of three algorithms

通過分析表3和圖7數(shù)據(jù)，與tRRT算法相比，本文算法生成的平均航跡長(zhǎng)度減少了12.27%，平均運(yùn)行時(shí)間減少了87.68%，平均迭代次數(shù)減少了79.64%。這表明本文算法能夠顯著減少隨機(jī)點(diǎn)數(shù)和迭代次數(shù)，提高算法收斂速度，同時(shí)證明了本文算法的穩(wěn)定性。

在圖6(c)中，雖然改進(jìn)算法生成了一個(gè)可行的航跡，但在航跡中仍然存在許多冗余點(diǎn)，路徑的曲率也不能滿足無人機(jī)的飛行條件。本文將改進(jìn)算法生成的航跡作為遺傳算法輸入種群，經(jīng)過選擇、交叉、變異等方式獲得了一條較優(yōu)航跡，將航跡長(zhǎng)度與改進(jìn)算法的平均航跡長(zhǎng)度進(jìn)行比較。為了驗(yàn)證優(yōu)化算法有效性，在圖6環(huán)境中，新增一種環(huán)境對(duì)優(yōu)化算法進(jìn)行驗(yàn)證，圖6環(huán)境稱為環(huán)境1，新增環(huán)境稱為環(huán)境2。仿真結(jié)果及優(yōu)化前后航跡長(zhǎng)度對(duì)比分別如圖8和表4所示。

圖8 仿真結(jié)果Fig.8 Simulation results

表4 優(yōu)化前后航跡長(zhǎng)度值對(duì)比Table 4 Comparison of path length before and after optimization

為了滿足無人機(jī)的飛行要求，使用三次樣條法對(duì)生成的航跡進(jìn)行平滑處理，綠色線條即為平滑后的無人機(jī)飛行軌跡。本文將改進(jìn)RRT算法生成的航跡點(diǎn)序列集合作為輸入種群，由圖8(b),8(d)可知，環(huán)境1中，遺傳算法經(jīng)過6次迭代完成了優(yōu)化，環(huán)境2中，13次迭代完成了優(yōu)化，縮短了遺傳算法的運(yùn)行時(shí)間。由表4數(shù)據(jù)可知，環(huán)境1中，優(yōu)化后的航跡長(zhǎng)度較之前減少了7.34%，環(huán)境2中較之前減少了2.49%，獲得了較優(yōu)航跡。

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

本文提出了一種改進(jìn)RRT算法，首先通過目標(biāo)偏置引導(dǎo)隨機(jī)采樣點(diǎn)的生成，然后結(jié)合人工勢(shì)場(chǎng)法，改進(jìn)了新的節(jié)點(diǎn)生成方式。改進(jìn)算法解決了原始RRT算法隨機(jī)性大、收斂速度慢等問題，極大地減少了算法冗余點(diǎn)的數(shù)量，提高了算法的搜索效率。最后，利用遺傳算法對(duì)本文算法生成的航跡進(jìn)一步優(yōu)化，以獲得基于種群的較優(yōu)航跡，同時(shí)使用三次樣條法對(duì)航跡進(jìn)行平滑來滿足無人機(jī)飛行要求，仿真結(jié)果表明，本文提出的融合算法優(yōu)化了路徑長(zhǎng)度，縮短了運(yùn)算時(shí)間。然而，該算法只針對(duì)單個(gè)無人機(jī)的航跡規(guī)劃，而在未來無人機(jī)技術(shù)的發(fā)展中，越來越多的案例將是多無人機(jī)協(xié)同飛行，這也將是下一步工作需要解決的問題。