基于改進(jìn)動(dòng)態(tài)規(guī)劃的無人機(jī)搜尋航跡規(guī)劃研究

錢 宇，祝禎祎

(中國(guó)民用航空飛行學(xué)院，四川 廣漢 618307)

1 引言

近幾年來，世界范圍內(nèi)出現(xiàn)越來越多的自然災(zāi)害，例如洪水、地震、臺(tái)風(fēng)等，這些災(zāi)害覆蓋面積廣、房屋摧毀數(shù)量多。無人機(jī)作為一種新興無人駕駛技術(shù)的載體，具有重量輕、航時(shí)長(zhǎng)、機(jī)動(dòng)性能好等優(yōu)勢(shì)，在救援的過程當(dāng)中發(fā)揮越來越重要的作用[1]。為了滿足無人機(jī)快速高效地進(jìn)行定點(diǎn)搜尋工作[2]，需要根據(jù)已知的障礙物環(huán)境和其物理性能約束規(guī)劃一條耗時(shí)最少、航程最短的航跡。

針對(duì)無人機(jī)航跡規(guī)劃技術(shù)，國(guó)內(nèi)外學(xué)者提出了很多高效的航跡規(guī)劃算法，主要分為傳統(tǒng)經(jīng)典算法和現(xiàn)代智能算法[3]。傳統(tǒng)經(jīng)典算法包括人工勢(shì)場(chǎng)法[4](Artificial Potential Field，APF)、模擬退火算法[5](Simulated Annealing Algorithm，SAA)等。這類方法容易陷入局部最優(yōu)解，在規(guī)劃空間較大時(shí)搜索效率低，相比之下，現(xiàn)代智能算法收斂程度更好，時(shí)間復(fù)雜度更小，應(yīng)用范圍更廣。其中包括A*算法[6]、遺傳算法[7](Genetic Algorithm，GA)、蟻群算法[8](ACO)等。經(jīng)過幾十年的發(fā)展，這些算法在某些具體場(chǎng)景下仍然有一定的局限性，因此提出了許多改進(jìn)算法。程澤新等[9]將遺傳算法用于無人機(jī)航跡規(guī)劃中，對(duì)進(jìn)化變異的策略進(jìn)行改進(jìn)并結(jié)合啟發(fā)式搜索方程，可以加快收斂的速度并更有效地獲取可行路徑，但是由于是基于概率的選擇機(jī)制，仍然存在陷入局部最優(yōu)的可能性。劉永琦等[10]提出在三維環(huán)境中修剪擴(kuò)展節(jié)點(diǎn)，降低傳統(tǒng)A*算法的網(wǎng)格節(jié)點(diǎn)計(jì)算量，提高了路徑搜索速度，然而并未詳細(xì)闡釋改進(jìn)A*算法的搜索流程。

動(dòng)態(tài)規(guī)劃算法(Dynamic Programming，DP)是由美國(guó)數(shù)學(xué)家R.E.Bellma求解多決策問題時(shí)提出的一種優(yōu)化算法，特點(diǎn)在于搜索效率高，結(jié)果穩(wěn)定可靠，從而受到廣泛的關(guān)注。它將一個(gè)問題分解成若干個(gè)互相聯(lián)系的階段，對(duì)每個(gè)階段作出決策，逐個(gè)求解，但是在實(shí)際應(yīng)用中性能會(huì)受到影響。譚峻強(qiáng)等[11]詳細(xì)地闡述了動(dòng)態(tài)規(guī)劃算法的基本思想，并將其用于求解最短運(yùn)輸距離，耗費(fèi)時(shí)間短，結(jié)果清晰明了；但是當(dāng)問題的規(guī)劃空間變大時(shí)，時(shí)間復(fù)雜度也會(huì)相應(yīng)地增大。孫健等[12]在突發(fā)威脅的環(huán)境下，結(jié)合動(dòng)態(tài)規(guī)劃算法和切線法可以實(shí)時(shí)地獲得一條合理規(guī)避障礙物的航跡，但是飛行過程中需要多次切換方向并且存在多余的航跡點(diǎn)，整個(gè)飛行航跡距離相應(yīng)地變長(zhǎng)。

為了解決航跡規(guī)劃耗時(shí)長(zhǎng)以及冗余節(jié)點(diǎn)的問題，研究首先利用雙向動(dòng)態(tài)規(guī)劃算法把搜索空間重新規(guī)劃為兩個(gè)對(duì)稱區(qū)域，減少參與多階段決策的狀態(tài)點(diǎn)總數(shù)；之后基于優(yōu)化后的搜索區(qū)域，在區(qū)間內(nèi)驗(yàn)證度量函數(shù)滿足四邊形不等式并找到使節(jié)點(diǎn)之間歐氏距離最小的決策范圍，進(jìn)一步減少冗余節(jié)點(diǎn)的數(shù)量，以期得到一種在航跡最優(yōu)性和規(guī)劃速度上效果更好的規(guī)劃算法。

2 無人機(jī)航跡規(guī)劃建模

無人機(jī)從起始點(diǎn)到終止點(diǎn)的搜尋過程中，為了減少耗費(fèi)的時(shí)間，需要在空間中規(guī)劃一條最優(yōu)的飛行路徑。選擇在三維空間中對(duì)無人機(jī)的靜態(tài)航跡進(jìn)行仿真建模。假設(shè)條件如下：

1)無人機(jī)的飛行方向不會(huì)指向初始點(diǎn)。

2)只能在規(guī)定的空間內(nèi)進(jìn)行路徑規(guī)劃。

3)忽略無人機(jī)的大小，將其簡(jiǎn)化為質(zhì)點(diǎn)。

4)搜尋過程中只考慮障礙物的威脅。

2.1 三維空間柵格化

為了能夠計(jì)算出滿足約束條件的航跡規(guī)劃

表達(dá)式，需要對(duì)無人機(jī)搜索的整個(gè)空間進(jìn)行離散化

{(x，y，z)|a≤x，y，z≤bx，y，z∈Z}

(1)

在三維坐標(biāo)系中，令無人機(jī)經(jīng)過的航跡點(diǎn)qi(i=1，2…n)的坐標(biāo)為(xi，yi，zi)；障礙物區(qū)域表示為(xj，yj，zj，r)，整個(gè)規(guī)劃空間如圖1所示。其中空心正方體表示為空間離散后的節(jié)點(diǎn)，紅色實(shí)心正方體A和B分別表示無人機(jī)搜尋過程的起始點(diǎn)和終止點(diǎn)，球體代表空間中的障礙物區(qū)域。

圖1 無人機(jī)航跡規(guī)劃空間柵格化

2.2 約束條件

研究考慮最小航跡長(zhǎng)度、最大轉(zhuǎn)彎角、最大爬升角/下降角、最低飛行高度等約束。約束條件如下

式中，Lmin為無人機(jī)最小航跡長(zhǎng)度，θmax為無人機(jī)最大轉(zhuǎn)彎角，φmax為無人機(jī)最大爬升角/下降角，hmin為最低飛行高度。qi為空間航跡點(diǎn)，(xi，yi，zi)為航跡點(diǎn)的坐標(biāo)，d(qi，qi+1)為相鄰航跡點(diǎn)(qi，qi+1)的歐氏距離。

2.3 代價(jià)函數(shù)

航跡規(guī)劃的代價(jià)函數(shù)[13]是評(píng)價(jià)航跡優(yōu)劣的一個(gè)重要的標(biāo)準(zhǔn)。影響三維航跡規(guī)劃的主要因素包括航跡長(zhǎng)度代價(jià)L、飛行高度代價(jià)H和威脅區(qū)的威脅代價(jià)T，分別表示為

(6)

(7)

(8)

這里Rmax表示威脅區(qū)的最大作用距離，k表示威脅區(qū)的個(gè)數(shù)， (xj，yj，zj)表示威脅區(qū)中心坐標(biāo)。

航跡總代價(jià)J與相應(yīng)的權(quán)系數(shù)w1、w2和w3表示為

(9)

可以得到無人機(jī)航跡規(guī)劃模型

min(w1L+w2T+w3H)

(10)

3 航跡規(guī)劃算法

動(dòng)態(tài)規(guī)劃算法通常將問題劃分為若干個(gè)相互關(guān)聯(lián)的階段，通過找到使每個(gè)階段都達(dá)得最優(yōu)效果的決策序列，來獲得整個(gè)問題的最優(yōu)解。利用動(dòng)態(tài)規(guī)劃算法進(jìn)行無人機(jī)搜尋任務(wù)航跡規(guī)劃，在優(yōu)先考慮整體的思路下，確定每個(gè)狀態(tài)的最優(yōu)值，最終可以得到整條搜尋航跡的最優(yōu)策略。

無人機(jī)航跡規(guī)劃中，在初始狀態(tài)確定，而結(jié)束狀態(tài)不確定的情況下，自底向上的逆序法效率更高；相反在結(jié)束狀態(tài)確定，而初始狀態(tài)不確定的情況下，自頂向下的順序法效率更高[14]。因此在航跡規(guī)劃中確定了初始狀態(tài)和結(jié)束狀態(tài)的基礎(chǔ)上，順序法和逆序法都可以使用。研究采用逆序法進(jìn)行建模分析。

無人機(jī)從初始點(diǎn)A搜尋到終止點(diǎn)B的總航跡，可以劃分為n個(gè)階段，每個(gè)階段表示為k(k的取值為1，2…n)，第k階段的狀態(tài)為fk；第k階段狀態(tài)為fk的決策變量表示成vk(fk)。若確定了第k階段的狀態(tài)fk和選擇的決策vk(fk)，則k+1階段的狀態(tài)fk+1也可以確定，常用式子fk+1=M(fk，vk(fk))表示。規(guī)劃過程表示為：

圖2 搜尋規(guī)劃流程圖

航跡點(diǎn)和距離信息存儲(chǔ)在動(dòng)態(tài)規(guī)劃表中，從終點(diǎn)開始利用逆序法向前進(jìn)行規(guī)劃，依次找到花費(fèi)最小的前向節(jié)點(diǎn)。將指標(biāo)函數(shù)中第k階段的狀態(tài)fk到第k+1階段的狀態(tài)fk+1之間的花費(fèi)定義為兩點(diǎn)之間的歐氏距離d(fk+1，fk)，第一階段到第k階段花費(fèi)度量值之和w表示為

(11)

已知階段1的狀態(tài)A和階段n的狀態(tài)B，單向動(dòng)態(tài)規(guī)劃的逆序法模型

(12)

gk(fk)表示第k階段末狀態(tài)fk到結(jié)束點(diǎn)B的最優(yōu)指標(biāo)函數(shù)值；gn(fn)代表結(jié)束點(diǎn)狀態(tài)B。同理，順序法的動(dòng)態(tài)規(guī)劃模型可以表示為

(13)

其中，gk+1(fk+1)表示為初始狀態(tài)A到第k+1階段末狀態(tài)fk+1的最優(yōu)指標(biāo)函數(shù)值，g0(f0)代表初始點(diǎn)狀態(tài)A。當(dāng)搜尋出從A到B的最優(yōu)航跡時(shí)，其中每一段子航跡也是最優(yōu)的，滿足動(dòng)態(tài)規(guī)劃的最優(yōu)子結(jié)構(gòu)性質(zhì)；函數(shù)通過作用于后部子過程，得到一個(gè)在以往計(jì)算中的最優(yōu)狀態(tài)，不會(huì)對(duì)下一步的決策起到影響，滿足了無后效性原則；通過將航跡規(guī)劃得到的狀態(tài)存儲(chǔ)在內(nèi)存表中，略增加了空間復(fù)雜度，但解決了子問題的重疊性。

4 算法改進(jìn)策略

4.1 狀態(tài)總數(shù)優(yōu)化

動(dòng)態(tài)規(guī)劃算法在搜尋過程中，需要遍歷整個(gè)搜索空間中的狀態(tài)點(diǎn)尋找最短航跡，當(dāng)問題規(guī)模較小時(shí)， 可以取得很好的效果， 但是當(dāng)規(guī)劃空間增大，問題中狀態(tài)總數(shù)增多時(shí)， 規(guī)劃耗時(shí)變長(zhǎng)。針對(duì)此問題，雙向動(dòng)態(tài)規(guī)劃算法在保證全局最優(yōu)的同時(shí)，也提高了算法的規(guī)劃速度。

圖3 雙向動(dòng)態(tài)規(guī)劃算法分析

如圖3所示，單向動(dòng)態(tài)規(guī)劃的計(jì)算空間區(qū)域?yàn)槲迕骟wOEFGH，但是雙向動(dòng)態(tài)規(guī)劃的計(jì)算空間區(qū)域?yàn)槲迕骟wOABCD與五面體IABCD之和；圖中灰色區(qū)域是雙向動(dòng)態(tài)規(guī)劃相對(duì)于單向動(dòng)態(tài)規(guī)劃少計(jì)算的狀態(tài)區(qū)域；令順序法和逆序法的最優(yōu)交匯處在第k階段，選取狀態(tài)點(diǎn)的過程需要滿足

qk∈N∩qk∈B=1

(14)

其中qk表示為第k階段選擇的狀態(tài)點(diǎn)，順序狀態(tài)點(diǎn)存儲(chǔ)在內(nèi)存表N中，逆序狀態(tài)點(diǎn)存儲(chǔ)在內(nèi)存表B中。

4.2 狀態(tài)轉(zhuǎn)移過程優(yōu)化

動(dòng)態(tài)規(guī)劃算法通過計(jì)算每個(gè)階段中所有符合約束的狀態(tài)點(diǎn)來尋找最短路徑；狀態(tài)轉(zhuǎn)移過程就是，把原問題分解為兩個(gè)子問題，利用求解過的前一狀態(tài)和此狀態(tài)上的決策得到當(dāng)前的狀態(tài)。狀態(tài)轉(zhuǎn)移所涉及的狀態(tài)數(shù)目影響了動(dòng)態(tài)規(guī)劃算法的時(shí)間效率。因此在優(yōu)化決策量的基礎(chǔ)上減少所涉及的狀態(tài)數(shù)目，提高運(yùn)算量。

將狀態(tài)轉(zhuǎn)移中度量值定義為w(i，j)，航跡規(guī)劃過程中的狀態(tài)轉(zhuǎn)移方程表示為

(15)

度費(fèi)用，fk(i，j)表示區(qū)間(i，j)上的最短路徑。在區(qū)域OABCD中，根據(jù)式(11)度量函數(shù)w(i，j)屬于單調(diào)遞增，區(qū)間(i，j)包含于(i′，j′)，可以驗(yàn)證其滿足包含關(guān)系單調(diào)性

w(i，j)≤w(i′，j′)，(i，j)?(i′，j′)

(16)

在式(16)的基礎(chǔ)上對(duì)距離函數(shù)w進(jìn)行分類討論，得到度量函數(shù)w(i，j)滿足四邊形不等式：

w(a，c)+w(b，d)≤w(b，c)+w(a，d)a≤b≤c≤d

(17)

根據(jù)式(16)和(17)，通過歸納法可以得到，最短路徑函數(shù)也滿足四邊形不等式

fk(a，c)+fk(b，d)≤fk(b，c)+fk(a，d)

(18)

Pk∈{(x，y，z)|a≤x，y，z≤bx，y，z∈Z}為第k階段決策集合，令p(a，b)為fk在(a，b)區(qū)間內(nèi)取最小值時(shí)的決策，通過反證法可以得到

p(a，b-1)≤p(a，b)≤p(a+1，b)

(19)

根據(jù)式(19)，在保證航跡距離最短的情況下，雙向規(guī)劃的決策范圍大大縮小，進(jìn)一步排除了冗余的節(jié)點(diǎn)，存儲(chǔ)計(jì)算過程更加高效。優(yōu)化后的狀態(tài)轉(zhuǎn)移函數(shù)可以表示為

(20)

其中狀態(tài)轉(zhuǎn)移方程的區(qū)間長(zhǎng)度L=j-i，v(i+1，j)和v(i，j-1)的長(zhǎng)度為L(zhǎng)-1，區(qū)間的長(zhǎng)度為n個(gè)，對(duì)于確定的長(zhǎng)度L，其包含的狀態(tài)有n個(gè)，因此時(shí)間復(fù)雜度為O(n2)。改進(jìn)之前，根據(jù)式(15)可知算法的時(shí)間復(fù)雜度為O(n3)。在改進(jìn)過程中，分析狀態(tài)之間的關(guān)系，得出最優(yōu)決策具有單調(diào)性質(zhì)，因此在計(jì)算過程中減少了狀態(tài)轉(zhuǎn)移考慮的狀態(tài)數(shù)，提高了算法的時(shí)間效率。

4.3 改進(jìn)動(dòng)態(tài)規(guī)劃算法流程

改進(jìn)動(dòng)態(tài)規(guī)劃算法計(jì)算流程如下：

1)初始化數(shù)據(jù)，包括搜索空間、障礙物坐標(biāo)和無人機(jī)約束函數(shù)。

2)三維空間柵格化，若搜尋點(diǎn)不在障礙物區(qū)域內(nèi)，通過式(14)確定順序法和逆序法交匯階段k。

3)在順序法的基礎(chǔ)上，結(jié)合改進(jìn)狀態(tài)轉(zhuǎn)移函數(shù)式(20)搜尋前k階段的路徑點(diǎn)，利用d(fk+1，fk)計(jì)算距離并把信息全部存儲(chǔ)在內(nèi)存表N中。逆序法同理，將數(shù)據(jù)都存儲(chǔ)在內(nèi)存表B中。

4)若內(nèi)存表N和B中存在相同的狀態(tài)集P，則把兩個(gè)表中存儲(chǔ)的狀態(tài)點(diǎn)和P連接起來，計(jì)算相應(yīng)的距離。若不存在，則返回步驟2)。

5)找出最小的距離，并輸出。

算法流程圖如圖4。

圖4 改進(jìn)動(dòng)態(tài)規(guī)劃算法流程圖

5 仿真與結(jié)果分析

為了驗(yàn)證算法的性能，研究利用遺傳算法、動(dòng)態(tài)規(guī)劃算法和改進(jìn)動(dòng)態(tài)規(guī)劃算法進(jìn)行了仿真計(jì)算。

仿真條件為：無人機(jī)的起始點(diǎn)為(1m，1m，1m)，終止點(diǎn)為(10m，10m，10m)；當(dāng)無人機(jī)在搜尋過程中，可以利用立體攝像機(jī)獲取障礙物的位置坐標(biāo)，最小航跡長(zhǎng)度為1m，最大轉(zhuǎn)彎角50°，最大爬升角60°。障礙物的位置信息如表1。

表1 障礙物信息表

仿真結(jié)果航跡對(duì)比如圖5、圖6，不同算法的航跡總長(zhǎng)度及計(jì)算時(shí)間如表2所示。

圖5 DP與GA算法航跡規(guī)劃對(duì)比圖

圖6 改進(jìn)DP與DP算法航跡規(guī)劃對(duì)比圖

圖5顯示出遺傳算法規(guī)劃了一條可行的搜尋航跡，但是相對(duì)于動(dòng)態(tài)規(guī)劃算法，不是一條最優(yōu)航跡，主要是遺傳算法在航跡規(guī)劃后期收斂速度變慢，容易陷入局部最優(yōu)，使得計(jì)算效率變低；無論是在規(guī)劃速度還是航跡最優(yōu)性都是動(dòng)態(tài)規(guī)劃算法更好，利用空間換取時(shí)間，存儲(chǔ)了每個(gè)狀態(tài)點(diǎn)之間的距離，但是還會(huì)產(chǎn)生冗余。圖6中，改進(jìn)動(dòng)態(tài)規(guī)劃算法計(jì)算的狀態(tài)點(diǎn)的數(shù)量相比于動(dòng)態(tài)規(guī)劃算法更少，解決了空間占據(jù)過多的問題，因此規(guī)劃速度更快，消耗時(shí)間更短；并且通過狀態(tài)轉(zhuǎn)移優(yōu)化策略減少了冗余節(jié)點(diǎn)，航跡長(zhǎng)度更優(yōu)。三種算法的仿真結(jié)果如表2。

表2 不同算法仿真結(jié)果對(duì)比

結(jié)果表示，在航跡點(diǎn)數(shù)量方面，改進(jìn)DP算法相對(duì)于GA算法減少7.9%，相對(duì)于DP算法減少5.4%；在時(shí)間效率方面，改進(jìn)DP算法相對(duì)于GA算法提高65%，相對(duì)于DP算法提高43%。仿真結(jié)果驗(yàn)證了改進(jìn)動(dòng)態(tài)規(guī)劃算法耗時(shí)短，搜尋距離小。

6 結(jié)論

1)改進(jìn)動(dòng)態(tài)規(guī)范算法減少了狀態(tài)轉(zhuǎn)移過程的計(jì)算量和存儲(chǔ)量，加快了規(guī)劃速度；優(yōu)化了航跡節(jié)點(diǎn)數(shù)量，縮短了整個(gè)搜尋的時(shí)間。改進(jìn)動(dòng)態(tài)規(guī)劃算法能夠很好的規(guī)劃一條代價(jià)最小、航路最優(yōu)的航跡。

2)針對(duì)動(dòng)態(tài)規(guī)劃算法存在維數(shù)爆炸，耗時(shí)長(zhǎng)的問題，結(jié)合雙向動(dòng)態(tài)規(guī)劃算法的并行搜索特點(diǎn)和決策變量的優(yōu)化策略得到一種改進(jìn)動(dòng)態(tài)規(guī)劃算法，并將其運(yùn)用在無人機(jī)災(zāi)后定點(diǎn)搜尋任務(wù)規(guī)劃中。