中利用無限循環(huán)將程序控制在主程序函數(shù)中，就出現(xiàn)了前面實(shí)驗(yàn)結(jié)果中令人迷惑的情況。

注：他是一個膽大心細(xì)的人，觀察還挺仔細(xì)的。

一類新算法研究智能飛行器航跡規(guī)劃問題

摘要: 智能飛行器航跡規(guī)劃問題是一個大范圍多目標(biāo)多約束的三維規(guī)劃問題，這類問題可以歸屬于路徑規(guī)劃問題，在滿足相應(yīng)條件的同時要求在較短的時間內(nèi)以較短的路程到達(dá)目的地。本文把航跡的約束條件轉(zhuǎn)化到實(shí)際問題中，通過對A*算法的改進(jìn)，建立起符合飛行器航跡規(guī)劃的兩種算法模型。通過兩種方案算法的比較，在兩種情況下，算法程序?qū)崿F(xiàn)得到航跡規(guī)劃結(jié)果表和路徑圖。算法的有效性和復(fù)雜度分析結(jié)果表明，給出的求解算法是十分有效的。

1. 引言

智能飛行器飛行操作的多約束環(huán)境下的航跡快速規(guī)劃優(yōu)化技術(shù)，是研究智能飛行器控制的一個重要問題。但是由于系統(tǒng)結(jié)構(gòu)的設(shè)置產(chǎn)生的限制，這類飛行器的定位系統(tǒng)，對自身進(jìn)行精準(zhǔn)定位無法進(jìn)行，定位誤差如果累計到一定程度，就可能導(dǎo)致整體任務(wù)失敗。所以在飛行過程中對定位誤差進(jìn)行校正，是智能飛行器航跡規(guī)劃中一項(xiàng)重要步驟 [1]。本文研究智能飛行器在系統(tǒng)定位精度限制下的航跡快速規(guī)劃問題，即如何在軌跡規(guī)劃的過程中，將定位誤差限制在可接受范圍內(nèi)，保證任務(wù)的順利完成。

假定飛行器的出發(fā)點(diǎn)為A點(diǎn)，目的地為B點(diǎn)。其航跡約束如下：

(1) 飛行器在空間飛行過程中需要實(shí)時定位，其定位誤差包括垂直誤差和水平誤差。飛行器每飛行1m，垂直誤差和水平誤差將各增加 δδ 個專用單位,，以下簡稱單位。到達(dá)終點(diǎn)時垂直誤差和水平誤差均應(yīng)小于個單位，并且為簡化問題，假設(shè)當(dāng)垂直誤差和水平誤差均小于 θθ 個單位時，飛行器仍能夠按照規(guī)劃路徑飛行。

(2) 飛行器在飛行過程中需要對定位誤差進(jìn)行校正。飛行區(qū)域中存在一些安全位置(稱之為校正點(diǎn))可用于誤差校正，當(dāng)飛行器到達(dá)校正點(diǎn)即能夠根據(jù)該位置的誤差校正類型進(jìn)行誤差校正。校正垂直和水平誤差的位置可根據(jù)地形在航跡規(guī)劃前確定?？尚Ｕ娘w行區(qū)域分布位置依賴于地形，無統(tǒng)一規(guī)律。若垂直誤差、水平誤差都能得到及時校正，則飛行器可以按照預(yù)定航線飛行，通過若干個校正點(diǎn)進(jìn)行誤差校正后最終到達(dá)目的地。

(3) 在出發(fā)地A點(diǎn)，飛行器的垂直和水平誤差均為0。

(4) 飛行器在垂直誤差校正點(diǎn)進(jìn)行垂直誤差校正后，其垂直誤差將變?yōu)?，水平誤差保持不變。

(5) 飛行器在水平誤差校正點(diǎn)進(jìn)行水平誤差校正后，其水平誤差將變?yōu)?，垂直誤差保持不變。

(6) 當(dāng)飛行器的垂直誤差不大于 α1α1 個單位，水平誤差不大于 α2α2 個單位時才能進(jìn)行垂直誤差校正。

(7) 當(dāng)飛行器的垂直誤差不大于 β1β1 個單位，水平誤差不大于 β2β2 個單位時才能進(jìn)行水平誤差校正。

(8) 飛行器在轉(zhuǎn)彎時受到結(jié)構(gòu)和控制系統(tǒng)的限制，無法完成即時轉(zhuǎn)彎(飛行器前進(jìn)方向無法突然改變)，假設(shè)飛行器的最小轉(zhuǎn)彎半徑為200 m。

圍繞在上述軌跡約束條件下，本文為智能飛行器建立航跡規(guī)劃一般模型和算法。本文針對參考文獻(xiàn) [1] 中的數(shù)據(jù)，規(guī)劃分別滿足約束條件(1)~(7)和(1)~(8)時，飛行器運(yùn)行的最優(yōu)航跡。另外，飛行器的飛行環(huán)境可能隨時間動態(tài)變化，雖然校正點(diǎn)在飛行前已經(jīng)確定，但飛行器在部分校正點(diǎn)進(jìn)行誤差校正時存在無法達(dá)到理想校正的情況(即將某個誤差精確校正為0)，例如天氣等不可控因素導(dǎo)致飛行器到達(dá)校正點(diǎn)也無法進(jìn)行理想的誤差校正。若假設(shè)飛行器在部分校正點(diǎn)(文獻(xiàn) [1] 中附件1和附件2中F列標(biāo)記為“1”的數(shù)據(jù))能夠成功將某個誤差校正為0的概率是80%，如果校正失敗，則校正后的剩余誤差為min (error, 5)個單位(其中error為校正前誤差，min為取小函數(shù))，本文針對此情況重新規(guī)劃航跡。

2. 飛行器航跡規(guī)劃模型

在給定初始點(diǎn)A到終點(diǎn)B條件下，為確保測量飛機(jī)從A點(diǎn)通過校正點(diǎn)到達(dá)B點(diǎn)的全程距離最小 [2]。設(shè) titi 時刻，飛行器當(dāng)前位置與可達(dá)域校正點(diǎn)的距離為 A(ti)A(ti) [3]，結(jié)合約束條件，建立飛行器航跡規(guī)劃模型：

按照本文參數(shù)特點(diǎn)，本文采用水平、垂直誤差交替校正的方式，為了將飛行器的誤差控制在較小的范圍內(nèi)，應(yīng)當(dāng)先校正垂直誤差，然后校正水平誤差，之后依次輪流交替，直至到達(dá)終點(diǎn)B。

為求解最優(yōu)航跡問題，我們設(shè)計了兩套方案：

方案一：每次選擇可達(dá)域中最近的校正點(diǎn)，此方案能在某誤差得到校正的同時將另一誤差控制在最小范圍，使得各方向誤差距離極限仍有較大空間(記為“誤差余量”)能最大程度的保障飛行器抵達(dá)終點(diǎn)。但是由于每次選擇最短距離前進(jìn)，過于保守，航跡規(guī)劃過程中會遇到某點(diǎn)S1可達(dá)域?yàn)榭斩鵁o法繼續(xù)的情況。實(shí)際上，前面的規(guī)劃中若某些點(diǎn)能選擇大一點(diǎn)的距離前進(jìn)，校正相同次數(shù)后或能到達(dá)S2，而S2可達(dá)域非空，可以繼續(xù)規(guī)劃航跡，即也許可以避開S1。

方案二：考慮飛行器當(dāng)前位置到可達(dá)域校正點(diǎn)的距離和可達(dá)域校正點(diǎn)到達(dá)終點(diǎn)的直線距離，計算兩距離之和可得多個組合，假設(shè)各個校正點(diǎn)為當(dāng)前位置，計算其是否存在可達(dá)域，去除不存在可達(dá)域的校正點(diǎn)對應(yīng)的組合，在剩余組合中取最小組合。該方案屬于A*算法的應(yīng)用，既考慮了當(dāng)前飛行距離也預(yù)估了后續(xù)飛行距離，從全局考慮了飛行路徑，便于找到較優(yōu)路徑。但是可能遇到可達(dá)域?yàn)榭盏那闆r，此時無法繼續(xù)規(guī)劃路徑，因而不能抵達(dá)終點(diǎn)。

加上約束條件(8)之后，在飛行器遇到障礙物時，并且正處于全局最優(yōu)路徑上的時候，采用局部A*路徑規(guī)劃算法 [4]，將障礙物區(qū)域表示為一個球體 [5]。則半圓危險度表示為：

當(dāng)飛行器未遇到障礙物時，危險度為0，飛行器的誤差則與到障礙物中心的距離有關(guān)，距離越近越危險但誤差相對小。(1)式中， RmaxRmax 是障礙物的最大半徑， dvdv 是飛行器到障礙物中心的距離。

那么加上約束條件(8)之后，規(guī)劃模型為：

本文中轉(zhuǎn)彎帶來的副作用僅是縮小了校正點(diǎn)的工作域，因此方案一、二的算法規(guī)劃航跡適用兩種情況。

另外，飛行器的飛行環(huán)境可能隨時間動態(tài)變化，雖然校正點(diǎn)在飛行前已經(jīng)確定，但飛行器在部分校正點(diǎn)進(jìn)行誤差校正時存在無法達(dá)到理想校正的情況(即將某個誤差精確校正為0)。因此，部分校正點(diǎn)存在校正失效的可能，會增大該方向誤差的值，進(jìn)而減少其誤差余量，將影響后續(xù)校正點(diǎn)的選擇，會改變航跡甚至使得航跡規(guī)劃失敗。因校正失效的概率較低(只有20%)，根據(jù)不同情況可以用兩種方法處理失效的校正：

1) 每次校正后加上概率誤差。

2) 極端情況，每次有可能校正失效時都按照校正失效處理，即必然失效。

3. 改進(jìn)的A*算法

下面我們將在經(jīng)典的A*算法 [6] - [12] 的基礎(chǔ)上，提出了一種改進(jìn)的A*算法，用于解決本文的飛行器航跡規(guī)劃問題。

A*算法是一種啟發(fā)式搜索算法。通過在搜索空間不斷評估路徑的估價函數(shù)值來啟發(fā)式搜索節(jié)點(diǎn)來構(gòu)造最優(yōu)路徑。通常A*算法的常用估價函數(shù)表示為

其中，n為當(dāng)前節(jié)點(diǎn)， f(n)f(n) 是從初始點(diǎn)經(jīng)由節(jié)點(diǎn)為n到目標(biāo)點(diǎn)的估價函數(shù)， g(n)g(n) 是在狀態(tài)空間中從初始節(jié)點(diǎn)到節(jié)點(diǎn)n的實(shí)際代價， h(n)h(n) 是從節(jié)點(diǎn)n到目標(biāo)節(jié)點(diǎn)的估計代價。

A*搜索算法的搜索效率由搜索方向和搜索步長決定。為了提升搜索效率，需要從搜索方向、搜索節(jié)點(diǎn)確定改進(jìn)。路徑的優(yōu)劣則主要依賴于估價函數(shù)的設(shè)計。所以我們從搜索節(jié)點(diǎn)和方向方面改進(jìn)，結(jié)合本文要解決的問題，我們改進(jìn)了校正點(diǎn)的選擇，例如每次選擇可達(dá)域中最近的校正點(diǎn)，并且本文需要加入水平、垂直的校正，在算法的搜索方向改進(jìn)中，考慮如果當(dāng)前校正了水平誤差，則水平誤差暫時無憂，能最快降低垂直誤差的機(jī)會就是下一次校正，如此交替進(jìn)行，這樣就 能確保兩個誤差都盡可能小，所以改進(jìn)后的算法的設(shè)計對極端情況承受能力較強(qiáng)，并將改進(jìn)的算法的具體步驟設(shè)計在下文給出，除非另有說明，否則我們總是在本文中使用算法命名的符號。

第一方面，我們針對所有文獻(xiàn) [1] 中的數(shù)據(jù)，來分別規(guī)劃滿足約束條件(1)~(7)時，飛行器運(yùn)行的最優(yōu)航跡，并綜合性考慮以下兩個優(yōu)化目標(biāo)：

1) 通過算法來預(yù)判每個最優(yōu)路徑，使航跡長度盡可能小;

2) 通過算法來使經(jīng)過校正區(qū)域進(jìn)行校正的次數(shù)盡可能少，并討論分析所用算法的有效性和復(fù)雜度。

第二方面，我們針對所有文獻(xiàn) [1] 附件中的數(shù)據(jù)(參數(shù)與第一問的相同)，分別規(guī)劃滿足條件(1)~(8)時飛行器的航跡，并綜合性考慮以下兩個優(yōu)化目標(biāo)：

1) 通過算法來預(yù)判每個最優(yōu)路徑，使航跡長度盡可能小;

2) 通過算法來使經(jīng)過校正區(qū)域進(jìn)行校正的次數(shù)盡可能少，并討論分析所用算法的有效性和復(fù)雜度。

最后根據(jù)一、二兩種方案設(shè)計出算法1和2，并根據(jù)通過軟件實(shí)現(xiàn)后得出的結(jié)果，畫出兩個方面的兩個數(shù)據(jù)集的航跡規(guī)劃路徑圖，見圖1~6，將結(jié)果(即飛行器從起點(diǎn)出發(fā)所經(jīng)過的誤差校正點(diǎn)編號，和校正前的誤差)依次填入航跡規(guī)劃的結(jié)果表中，見表1~8，并得出算法的時空復(fù)雜度，證明算法是有效可行的，見表9。

算法1

Step1：計算當(dāng)前點(diǎn)A的可達(dá)域;

Step2：若可達(dá)域非空，轉(zhuǎn)Step3，否則，標(biāo)記當(dāng)前點(diǎn)為失敗點(diǎn)，轉(zhuǎn)Step5;

Step3：若終點(diǎn)在可達(dá)域中，結(jié)束程序，逆序輸出軌跡棧元素即可得到航跡規(guī)劃。否則，轉(zhuǎn)Step4;

Step4：選擇可達(dá)域中最近的點(diǎn)Aclose作為后繼點(diǎn)，將可達(dá)域存入可達(dá)域棧C中，將上一校正點(diǎn)Apre校正后的水平誤差pre_h和垂直誤差pre_v存入對應(yīng)棧，以pre_h和pre_v為基礎(chǔ)加上Apre到A點(diǎn)所產(chǎn)生的誤差增量來更新對應(yīng)誤差h、v，將后繼點(diǎn)加入軌跡棧，標(biāo)記后繼點(diǎn)為當(dāng)前點(diǎn)，轉(zhuǎn)Step1;

Step5：將軌跡棧中棧頂元素出棧(此元素與當(dāng)前點(diǎn)一致，均為失敗點(diǎn))，再從棧頂出棧一個元素，標(biāo)記為當(dāng)前點(diǎn)，將可達(dá)域棧棧頂元素出棧，去除可達(dá)域中失敗點(diǎn)的信息(確保此點(diǎn)不會再有機(jī)會選中)之后作為新的可達(dá)域，將水平誤差棧和垂直誤差棧棧頂元素出棧，替換當(dāng)前水平誤差h和垂直誤差v，轉(zhuǎn)Step2。

Figure 1. 1 (data 1) track route map

圖1. 一(數(shù)據(jù)1)航跡路徑圖

Figure 2. 1 (data 2) track route map

圖2. 一(數(shù)據(jù)2)航跡路徑圖

算法2

Step1：計算當(dāng)前點(diǎn)A的可達(dá)域，若終點(diǎn)是在可達(dá)域轉(zhuǎn)Step2;否則轉(zhuǎn)Step3;

Step2：計結(jié)束程序，逆序輸出軌跡棧元素即可得到航跡規(guī)劃;

Step3：計計算當(dāng)前點(diǎn)到達(dá)可達(dá)域各點(diǎn)的誤差，在此誤差值下，假設(shè)可達(dá)域各點(diǎn)Bi為當(dāng)前點(diǎn);

Step4：計計算Bi是否存在可達(dá)域，將存在可達(dá)域的點(diǎn)加入集合P;

Step5：計計算點(diǎn)A到P中各點(diǎn)的距離以及P中各點(diǎn)到終點(diǎn)的距離之和，以上一校正點(diǎn)Apre校正后的水平誤差pre_h和垂直誤差pre_v為基礎(chǔ)加上Apre到A點(diǎn)所產(chǎn)生的誤差增量來更新對應(yīng)誤差h、v，取最小距離點(diǎn)作為后繼點(diǎn)，將后繼點(diǎn)加入軌跡棧，標(biāo)記后繼點(diǎn)為當(dāng)前點(diǎn)，進(jìn)入Step1。

Figure 3. 2 (data 1) track route map

圖3. 二(數(shù)據(jù)1)航跡路徑圖

Figure 4. 2 (data 2) track route map

圖4. 二(數(shù)據(jù)2)航跡路徑

第三方面，我們解決飛行器在部分校正點(diǎn)進(jìn)行誤差校正時存在無法達(dá)到理想校正的情況，假設(shè)飛行器到達(dá)該校正點(diǎn)時即可知道在該點(diǎn)處是否能夠校正成功，但不論校正成功與否，均不能改變規(guī)劃路徑，因此，部分校正點(diǎn)存在校正失效的可能，會增大該方向誤差的值，進(jìn)而減少其誤差余量，將影響后續(xù)校正點(diǎn)的選擇，會改變航跡甚至使得航跡規(guī)劃失敗。因校正失效的概率較低，只有20%，根據(jù)不同情況可以用兩種方法處理失效的校正，并設(shè)計出算法3和算法4，它是以算法1和算法2為基礎(chǔ)調(diào)整校正時的誤差更新機(jī)制。

Figure 5. 3 (data 1) track route map

圖5. 三(數(shù)據(jù)1)航跡路徑圖

Figure 6. 3 (data 2) track route map

圖6. 三(數(shù)據(jù)2)航跡路徑圖

Table 1. Final track route 1

表1. 最終航跡路徑一

Table 2. Track planning results Table 1: Data 1

表2. 航跡規(guī)劃結(jié)果表一：數(shù)據(jù)1

Table 3. Track planning results Table 1: Data 2

表3. 航跡規(guī)劃結(jié)果表一：數(shù)據(jù)2

Table 4. Comparison of Algorithms 1 and 2 on Data 1 and 2

表4. 算法1和2在數(shù)據(jù)1和2上的對比

Table 5. Final track route 2

表5. 最終航跡路徑二

Table 6. Track planning results Table 2: Data 1

表6. 航跡規(guī)劃結(jié)果表二：數(shù)據(jù)1

Table 7. Track planning results Table 2: Data 2

表7. 航跡規(guī)劃結(jié)果表二：數(shù)據(jù)2

Table 8. Track planning results Table 3: Data 1 and Data 2

表8. 航跡規(guī)劃結(jié)果表三：數(shù)據(jù)1和數(shù)據(jù)2

Table 9. Space-time complexity of Algorithm 1 - 4

表9. 算法1~4的時空復(fù)雜度

算法3

Step1：計算當(dāng)前點(diǎn)A的可達(dá)域;

Step2：若可達(dá)域非空，轉(zhuǎn)Step3，否則，標(biāo)記當(dāng)前點(diǎn)為失敗點(diǎn)，轉(zhuǎn)Step5;

Step3：若終點(diǎn)在可達(dá)域中，結(jié)束程序，逆序輸出軌跡棧元素即可得到航跡規(guī)劃。否則，轉(zhuǎn)Step4;

Step4：選擇可達(dá)域中最近的點(diǎn)Aclose作為后繼點(diǎn)，將可達(dá)域存入可達(dá)域棧C中，將上一校正點(diǎn)Apre校正后的水平誤差pre_h和垂直誤差pre_v存入對應(yīng)棧，以pre_h和pre_v為基礎(chǔ)加上Apre到A點(diǎn)所產(chǎn)生的誤差增量來更新對應(yīng)誤差h、v，若后繼點(diǎn)為可能失效點(diǎn)，將對應(yīng)誤差加上bia進(jìn)行二次校正，將后繼點(diǎn)加入軌跡棧，標(biāo)記后繼點(diǎn)為當(dāng)前點(diǎn)，轉(zhuǎn)Step1;

Step5：將軌跡棧中棧頂元素出棧(此元素與當(dāng)前點(diǎn)一致，均為失敗點(diǎn))，再從棧頂出棧一個元素，標(biāo)記為當(dāng)前點(diǎn)，將可達(dá)域棧棧頂元素出棧，去除可達(dá)域中失敗點(diǎn)的信息(確保此點(diǎn)不會再有機(jī)會選中)之后作為新的可達(dá)域，將水平誤差棧和垂直誤差棧棧頂元素出棧，替換當(dāng)前水平誤差h和垂直誤差v，轉(zhuǎn)Step2。

算法4

Step1：計算當(dāng)前點(diǎn)A的可達(dá)域，若終點(diǎn)是在可達(dá)域轉(zhuǎn)Step2;否則轉(zhuǎn)Step3;

Step2：結(jié)束程序，逆序輸出軌跡棧元素即可得到航跡規(guī)劃;

Step3：計算當(dāng)前點(diǎn)到達(dá)可達(dá)域各點(diǎn)的誤差，在此誤差值下，假設(shè)可達(dá)域各點(diǎn)Bi為當(dāng)前點(diǎn);

Step4：計算Bi是否存在可達(dá)域，將存在可達(dá)域的點(diǎn)加入集合P;

Step5：計算點(diǎn)A到P中各點(diǎn)的距離以及P中各點(diǎn)到終點(diǎn)的距離之和，以上一校正點(diǎn)Apre校正后的水平誤差pre_h和垂直誤差pre_v為基礎(chǔ)加上Apre到A點(diǎn)所產(chǎn)生的誤差增量來更新對應(yīng)誤差h、v，若后繼點(diǎn)為可能失效點(diǎn)，將對應(yīng)誤差加上bia進(jìn)行二次校正，取最小距離點(diǎn)作為后繼點(diǎn)，將后繼點(diǎn)加入軌跡棧，標(biāo)記后繼點(diǎn)為當(dāng)前點(diǎn)，進(jìn)入Step1。

4. 數(shù)值結(jié)果

本節(jié)利用已知數(shù)據(jù)來驗(yàn)證算法1~4的有效性。我們的數(shù)值實(shí)驗(yàn)是應(yīng)用Python和Matlab軟件進(jìn)行計算。

5. 總結(jié)

本文使用了一種新的改進(jìn)的A*算法，算法模型在第一方面中，很好地降低兩個誤差增長所帶來的風(fēng)險，如當(dāng)前校正了水平誤差，則水平誤差暫時無憂，能最快降低垂直誤差的機(jī)會就是下一次校正，如此交替進(jìn)行，能確保兩個誤差都盡可能小，算法的設(shè)計對極端情況承受能力較強(qiáng)，得出的路徑總長較短，耗費(fèi)的時間較少，見表1~4。第二方面中，考慮轉(zhuǎn)彎帶來的副作用僅是縮小了校正點(diǎn)的工作域，因此還可利用問題1的算法規(guī)劃航跡，此時需將各個數(shù)據(jù)對應(yīng)的參數(shù)減小0.63個單位，在問題1算法的基礎(chǔ)上規(guī)劃航跡，減少了工作量，并能得出很好的路徑，見表5~7。在第三方面中，考慮到部分校正點(diǎn)存在校正失效的可能，在正常情況和極端情況，每次有可能校正失效時都按照校正失效處理，大大增加了算法的可行性和覆蓋性，得出安全可行的路徑，見表8。總體來說，算法1~4在空間存在有解航跡時必能保證抵達(dá)終點(diǎn)，可以快速找到較優(yōu)軌跡甚至最優(yōu)軌跡，兩個算法模型優(yōu)勢互補(bǔ)，既能兼顧快速尋優(yōu)的需求，也能保障有解必達(dá)終點(diǎn)的安全性。

審核編輯：湯梓紅