中利用無限循環將程序控制在主程序函數中，就出現了前面實驗結果中令人迷惑的情況。

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

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

摘要: 智能飛行器航跡規劃問題是一個大范圍多目標多約束的三維規劃問題，這類問題可以歸屬于路徑規劃問題，在滿足相應條件的同時要求在較短的時間內以較短的路程到達目的地。本文把航跡的約束條件轉化到實際問題中，通過對A*算法的改進，建立起符合飛行器航跡規劃的兩種算法模型。通過兩種方案算法的比較，在兩種情況下，算法程序實現得到航跡規劃結果表和路徑圖。算法的有效性和復雜度分析結果表明，給出的求解算法是十分有效的。

1. 引言

智能飛行器飛行操作的多約束環境下的航跡快速規劃優化技術，是研究智能飛行器控制的一個重要問題。但是由于系統結構的設置產生的限制，這類飛行器的定位系統，對自身進行精準定位無法進行，定位誤差如果累計到一定程度，就可能導致整體任務失敗。所以在飛行過程中對定位誤差進行校正，是智能飛行器航跡規劃中一項重要步驟 [1]。本文研究智能飛行器在系統定位精度限制下的航跡快速規劃問題，即如何在軌跡規劃的過程中，將定位誤差限制在可接受范圍內，保證任務的順利完成。

假定飛行器的出發點為A點，目的地為B點。其航跡約束如下：

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

(2) 飛行器在飛行過程中需要對定位誤差進行校正。飛行區域中存在一些安全位置(稱之為校正點)可用于誤差校正，當飛行器到達校正點即能夠根據該位置的誤差校正類型進行誤差校正。校正垂直和水平誤差的位置可根據地形在航跡規劃前確定。可校正的飛行區域分布位置依賴于地形，無統一規律。若垂直誤差、水平誤差都能得到及時校正，則飛行器可以按照預定航線飛行，通過若干個校正點進行誤差校正后最終到達目的地。

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

(4) 飛行器在垂直誤差校正點進行垂直誤差校正后，其垂直誤差將變為0，水平誤差保持不變。

(5) 飛行器在水平誤差校正點進行水平誤差校正后，其水平誤差將變為0，垂直誤差保持不變。

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

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

(8) 飛行器在轉彎時受到結構和控制系統的限制，無法完成即時轉彎(飛行器前進方向無法突然改變)，假設飛行器的最小轉彎半徑為200 m。

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

2. 飛行器航跡規劃模型

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

按照本文參數特點，本文采用水平、垂直誤差交替校正的方式，為了將飛行器的誤差控制在較小的范圍內，應當先校正垂直誤差，然后校正水平誤差，之后依次輪流交替，直至到達終點B。

為求解最優航跡問題，我們設計了兩套方案：

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

方案二：考慮飛行器當前位置到可達域校正點的距離和可達域校正點到達終點的直線距離，計算兩距離之和可得多個組合，假設各個校正點為當前位置，計算其是否存在可達域，去除不存在可達域的校正點對應的組合，在剩余組合中取最小組合。該方案屬于A*算法的應用，既考慮了當前飛行距離也預估了后續飛行距離，從全局考慮了飛行路徑，便于找到較優路徑。但是可能遇到可達域為空的情況，此時無法繼續規劃路徑，因而不能抵達終點。

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

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

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

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

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

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

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

3. 改進的A*算法

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

A*算法是一種啟發式搜索算法。通過在搜索空間不斷評估路徑的估價函數值來啟發式搜索節點來構造最優路徑。通常A*算法的常用估價函數表示為

其中，n為當前節點， f(n)f(n) 是從初始點經由節點為n到目標點的估價函數， g(n)g(n) 是在狀態空間中從初始節點到節點n的實際代價， h(n)h(n) 是從節點n到目標節點的估計代價。

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

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

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

2) 通過算法來使經過校正區域進行校正的次數盡可能少，并討論分析所用算法的有效性和復雜度。

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

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

2) 通過算法來使經過校正區域進行校正的次數盡可能少，并討論分析所用算法的有效性和復雜度。

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

算法1

Step1：計算當前點A的可達域;

Step2：若可達域非空，轉Step3，否則，標記當前點為失敗點，轉Step5;

Step3：若終點在可達域中，結束程序，逆序輸出軌跡棧元素即可得到航跡規劃。否則，轉Step4;

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

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

Figure 1. 1 (data 1) track route map

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

Figure 2. 1 (data 2) track route map

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

算法2

Step1：計算當前點A的可達域，若終點是在可達域轉Step2;否則轉Step3;

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

Step3：計計算當前點到達可達域各點的誤差，在此誤差值下，假設可達域各點Bi為當前點;

Step4：計計算Bi是否存在可達域，將存在可達域的點加入集合P;

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

Figure 3. 2 (data 1) track route map

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

Figure 4. 2 (data 2) track route map

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

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

Figure 5. 3 (data 1) track route map

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

Figure 6. 3 (data 2) track route map

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

Table 1. Final track route 1

表1. 最終航跡路徑一

Table 2. Track planning results Table 1: Data 1

表2. 航跡規劃結果表一：數據1

Table 3. Track planning results Table 1: Data 2

表3. 航跡規劃結果表一：數據2

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

表4. 算法1和2在數據1和2上的對比

Table 5. Final track route 2

表5. 最終航跡路徑二

Table 6. Track planning results Table 2: Data 1

表6. 航跡規劃結果表二：數據1

Table 7. Track planning results Table 2: Data 2

表7. 航跡規劃結果表二：數據2

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

表8. 航跡規劃結果表三：數據1和數據2

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

表9. 算法1~4的時空復雜度

算法3

Step1：計算當前點A的可達域;

Step2：若可達域非空，轉Step3，否則，標記當前點為失敗點，轉Step5;

Step3：若終點在可達域中，結束程序，逆序輸出軌跡棧元素即可得到航跡規劃。否則，轉Step4;

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

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

算法4

Step1：計算當前點A的可達域，若終點是在可達域轉Step2;否則轉Step3;

Step2：結束程序，逆序輸出軌跡棧元素即可得到航跡規劃;

Step3：計算當前點到達可達域各點的誤差，在此誤差值下，假設可達域各點Bi為當前點;

Step4：計算Bi是否存在可達域，將存在可達域的點加入集合P;

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

4. 數值結果

本節利用已知數據來驗證算法1~4的有效性。我們的數值實驗是應用Python和Matlab軟件進行計算。

5. 總結

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

審核編輯：湯梓紅