基于滾動時域的無人機動態航跡規劃

王文彬，秦小林,3，張力戈，張國華

（1. 中國科學院 成都計算機應用研究所，四川 成都 610041; 2. 中國科學院大學 計算機與控制學院，北京100080; 3. 廣州大學 智能軟件研究院，廣東 廣州 510006）

無人機(UAV)航跡規劃(trajectory planning)是指在初始狀態、任務目標、威脅區和一些已知環境信息的情況下獲得性能最優的規劃問題，是任務規劃(mission planning)系統的關鍵技術之一。其在任務規劃系統中起著非常重要的作用，也是實現無人機智能導航并且完成任務的技術保障。基于飛行安全的需要，綜合考慮障礙物、無人機自身性能和飛行時間等約束條件，所規劃出的航跡既要盡可能減少無人機在飛行中墜毀的概率，又要使得綜合性能指標最小，并根據實際需要進行飛行中的局部調整。傳統的航跡規劃方法是基于預先給定的地圖生成一條具有最小代價的航跡，包括A*算法[1]、人工勢場法[2]、蟻群算法[3]、概率地圖方法[4](PRM)和快速擴展隨機樹[5](RRT)等。這些傳統的航跡規劃方法一般都用于離線規劃，用于在線規劃時，需要很長的時間或者極大的內存才能規劃出一條最優或次優路徑。特別地，無法對環境的變化做出快速的反應，并且在規劃的過程中很少涉及無人機自身的動力學約束，比如速度、最大加速度的限制等。因此，很少考慮無人機的安全性和航跡的可行性。

近十年來，滾動時域控制的思想也被用于解決航跡規劃問題[6-9]，采用滾動時域優化策略可以對帶有輸入和狀態約束的線性系統進行最優控制，易于處理約束以及多變量的優化問題。滾動時域控制[8-13]用于航跡規劃，不要求一次規劃到達目標，將規劃分成多個階段，成功地繞開障礙物，從而極大地減少了規劃的時間，使得該方法可用于在線航跡規劃。分布式滾動時域控制方法被提出[14-16]，進一步減少了多無人機航跡規劃的規劃時間。然而，上述工作主要集中于采用混合整數線性規劃(mixed integer linear programming，MILP)求解航跡規劃最優化問題，方法面臨以下兩個問題：處理帶復雜約束問題可擴展性不強和求解時間隨著問題規模指數增加?；谝陨瞎ぷ鞯牟蛔悖疚奶岢隽嘶赗HC-FPSO算法以解決帶有動力學約束的多旋翼無人機航跡規劃問題。從整個航跡規劃看，單次規劃屬于局部優化，因此需要一個全局的代價圖(終端罰函數)來表示航跡端點到目標點的代價估計。區別于文獻[8]，本文提出的基于VORONOI圖的代價圖可以使得規劃出的航跡盡可能地遠離障礙物，提高了無人機的生存機率。

文獻[17]通過空戰人工勢場確定其威力，將合威力引入PSO算法。通過人工勢場啟發粒子群算法在當前飛行方向的可機動范圍內進行尋優，重點加強對合威力方向的尋優。區別于文獻[17]，本文將人工勢場法加入到目標函數使無人機遠離障礙物，增加了無人機的安全性。此外，結合兩種方法的優勢，減少了計算量且在環境發生改變時，只需更新勢場。RHC-APF是一種非常優秀的航跡規劃算法，易于擴展。在優化過程中，計算每個粒子的約束違背量，根據約束違背量來更新粒子的速度，改進后的算法相比文獻[18]在計算時間方面有很大提高，單次規劃的平均時間減少42.43%，并且在求解結果的穩定性方面具有一致性。仿真結果也表明了RHC-FPSO算法的有效性。

1 相關工作

1.1 滾動時域控制原理

滾動時域控制[19](RHC)是一種基于模型的反饋控制策略。在每一采樣時刻，采集系統當前狀態作為初始狀態，根據系統狀態空間模型和約束，在線求解一個有限時域的最優控制問題，將求解最優控制的第1個控制信號實際作用到系統中，重復以上過程。對于含狀態約束和輸入約束等限制的系統，滾動時域控制是一種有效的控制方法。隨著時間的推移，當無人機執行任務時不斷地靠近目標，在此過程中，每一次的輸入都是通過求解一個有限時域內帶約束的優化問題。因此，相比固定時域能夠極大地減少計算時間，滿足在線航跡規劃。

滾動時域優化的基本思想是：首先，根據對應的目標函數和約束條件，RHC利用多旋翼無人機的狀態空間模型，預測未來規劃時域內的控制輸入，將其作用于系統中；然后，測量下一時刻的狀態，根據當前的狀態進行下一步優化，隨著時間的推進反復滾動執行。具體過程如圖1所示。

從以上對模型預測控制的介紹和分析，可以看出：RHC是根據當前的狀態和目標函數進行不斷地迭代，求解該時刻的有限時段的最優輸入，采用的是局部優化，不是一個不變的全局最優目標。因此，需要代價圖使得規劃時跳出局部最優值，完成全局優化。

圖 1 滾動時域控制示意圖Fig. 1 The diagram of receding horizon control

1.2 人工勢場法

人工勢場法[20]是由Khatib在1986提出的一種虛擬力場法。其方法是將研究對象所處的環境用勢場來定義，通過位置信息來控制對象的避障運動。將研究對象在實際環境中的運動轉換為在虛擬勢場的運動，在該勢場中目標點對研究對象產生引力，環境中的障礙物產生斥力。引力和斥力的合力決定了研究對象的運動方向和運動速度。人工勢場相比其他的算法具有計算量小、描述簡單、實時性高、應用廣泛。

根據人工勢場的理論，對于靜態或者動態的環境，都可以計算相應的人工勢能圖。對于任意狀態，研究對象的位姿用表示，則勢場可以用表示，目標轉態位姿用表示，因此相應的吸引勢為，障礙物的位姿用表示，排斥勢，因此空間中的任意位姿的勢能場都可以表示為

目標點對研究對象產生的勢能大小與兩者的距離相關，距離大、勢能大，反之亦然。當二者之間的距離為零時，表示到達目標點，因此引力勢能與距離成正比關系，可表示為

與引力勢場類似，障礙物對研究對象產生斥力場。斥力場勢能大小與障礙物和研究對象的距離有關，距離越近，斥力勢能越大，反之越小。因此可以表示為

2 問題描述

以多旋翼無人機為例，討論其由初始狀態出發，在規避障礙物和滿足相應的約束條件下，以最小代價到達目標區域。其線性時不變系統動力學描述為[14]

一般的目標函數，采用如下形式：

因此基于RHC框架的優化完整的描述為

文獻[10]使用MILP優化目標函數(7)，如果存在非線性約束條件或者目標函數，通過將其轉化為多約束條件進行線性化，將會導致約束條件呈指數增長，會損失一部分求解精度，可擴展性不強。并且對于復雜的環境和目標函數，求解速度進一步降低。PSO可以快速求解一個可行解，因此，使用PSO結合RHC可以平衡求解時間和求解精度。實驗結果表明，基于改進的PSO方法(FPSO)能夠有效地提高計算效率。

3 RHC-FPSO航跡規劃

本文將整個航跡規劃分成兩個階段，第1階段為代價評估階段，根據當前環境生成代價圖，當環境發生改變時，重新計算代價圖，使用基于VORONOI圖的代價圖來表示航跡端點到目標點的代價估計；第2階段為在線航跡規劃階段，在問題描述中，已經將航跡規劃表示成一個優化問題，因此本階段主要是基于RHC-FPSO在線航跡規劃的一個優化過程。

3.1 代價圖

根據第1節分析可知，從航跡規劃全局看，滾動時域優化是局部優化，因此容易陷入局部最優。如圖2所示，這種情況下無人機陷入了局部最優，目標不可達，因此需要估計預測航跡端點到目標點的代價。代價估計階段根據給出的障礙物和目標預先計算，當探測到環境發生改變將會重新計算。航跡規劃階段使用預先計算的代價圖來估計航跡端點到目標點的代價。

圖 2 不含代價圖的航跡規劃示意圖Fig. 2 The diagram of trajectory planning without cost map

文獻[8]提出的代價圖表示航跡端點到目標點的時間估計值?；舅枷霝椋涸诓豢紤]無人機自身的動力學約束條件下，對于多邊形的障礙物，連接開始點、障礙物頂點和目標點，如果這些點的連線沒有穿過障礙物，則添加一條邊，這樣形成的圖稱為可視圖。對于可視圖使用Dijkstra[21]單源最短路徑算法，搜索從目標點到各個點的最短路徑。由第3節問題描述可知，航跡端點為，因此基于代價圖的終端懲罰項可以表示如下：

然而，基于文獻[8]提出的代價圖，選擇的可視點為障礙物的頂點，這會導致規劃出的航跡會沿著障礙物邊緣，降低無人機安全性。因此，我們引入基于VORONOI圖的可視圖，VORONOI圖的優點是使得可視點離障礙物的距離盡可能遠，VORONOI圖使得可視點的選擇不再是障礙物的頂點，而是障礙物之間的頂點連線的中點。

定義1 VORONOI圖[22]。任意兩點和之間的歐式距離，記作，假設平面P =為平面上的任意個互異的點，所對應的VORONOI圖是平面的一個子區域的劃分，因此整個平面被劃分成個單元，具有以下性質：任意一點位于點所對應的單元中，當且僅當對于任意的，都有。

根據定義1可知，VORONOI多邊形的每條邊上的點到相對應的兩個點的距離相等，即VORONOI多邊形上的點是到障礙物或威脅點的最遠點。因此可以得出，飛行器以VORONOI多邊形的頂點作為可視點，可以提高飛行器的安全系數。

定理 1 VORONOI圖定理[22]：若，則在與平面上任意個基點相對應的VORONOI圖中，頂點的數目不會超過，而且邊的數目不會超過。

由定理1可知，采用基于VORONOI圖的可視點的方法，相對于文獻[8]，可視點呈線性增長，并且小于。

3.2 基于RHC-FPSO在線航跡規劃

航跡規劃是復雜約束條件下的最優化問題，采用粒子群等智能優化算法進行航跡規劃時，往往算法迭代初期，控制輸入和狀態可能存在違反約束的情況，算法需要迭代若干次才能產生滿足要求的輸入，并在此基礎上進一步尋優。粒子群算法中種群最優個體影響著粒子搜索方向，如何縮短搜索到可行輸入的時間對提高算法的效率至關重要。因此，我們根據粒子違反約束條件的程度來更新粒子速度。

評價函數設計：評價函數用于給粒子群中每個個體計算適應度。所有約束條件都滿足的輸入粒子稱為可行個體，違反任何一項約束條件的個體稱為不可行個體。在進行個體評價時遵循以下準則：

1) 對于可行輸入來說，根據式(3)，代價小的輸入優于代價大的輸入；

2) 對于不可行航跡來說，約束違背小的輸入優于約束違背大的輸入；

3) 對于可行輸入與不可行輸入，可行輸入總是優于不可行輸入；

不可行輸入意味著在實際中輸入是不可行的，因此沒有必要計算它的代價，可以減少計算量。基于以上分析，對任意輸入，采用下面的評價函數。

圖 3 評價函數圖像Fig. 3 The image of evaluation function F(u)

3.3 基于RHC-APF在線航跡規劃

3.1節介紹的方法中，引入的VORONOI圖，構建比較復雜，而且當環境改變的時候，需要重新計算，不利于實時航跡規劃。本節結合APF實時計算和計算簡單的特點，提出RHC-APF算法。

1) 基于4.1提出的代價圖，會導致無人機靠近障礙物飛行。按照APF的思想，讓障礙物給無人機一個斥力場，因此此時的目標函數可以表示為式(16)的形式：

2) 代價圖的計算會增加計算負擔，而且不利于在線規劃，因為當環境發生改變時，需要重新計算代價圖，特別突發事件，會降低實時性。人工勢場法只需增加障礙物的一個勢場，當目標發生改變時，也只需改變目標函數的勢場，并且勢場的計算非常簡單。將RHC和APF結合后能夠克服APF的不足，同時增加了實時性。此時的目標函數為

3.4 算法實現流程

1) 首先初始化無人機參數，設置最大速度、最大加速等限制，設置滾動時域，以及無人機采樣間隔。

表 1 仿真參數和值Table 1 Simulation parameters and values

3) 按照式(11)和式(12)分別計算每個粒子的適應度和約束違背量。

4) 更新粒子群最優解和每個粒子的最優解。

5) 如果粒子滿足約束條件，按照式(13)和式(14)更新粒子的位置和速度。否則，按照式(13)和式(15)更新粒子的位置和速度。

6) 如果粒子群不滿足終止條件，轉3)。下一時刻的狀態。

8) 如果到達目標點，完成航跡規劃。否則轉2)。

4 仿真結果與分析

本文的基本算法已在MATLAB 2016中進行了實現。

多旋翼無人機其動力學可以由具有速度和加速度約束的質點動力學模型近似[9]。

輸出約束：一般的輸出約束包括障礙物約束(19)，速度約束(20)，加速度約束(21)

仿真環境：處理器：PC (Intel (R)) i3-3240 CPU 3.40 GHz；操作系統：Windows 7；仿真軟件：MATLAB 2016。表1為仿真的實驗參數，參數設置參考文獻[9，18]。本文實驗部分為 (–25 m，25 m)到(–15 m，12 m)的一個二維規劃環境。

實驗一 VORONOI代價圖與傳統代價圖對比

圖4為根據文獻[8]的代價圖和本文提出的基于VORONOI圖的代價圖的對比。黑色方塊表示障礙區域，按照VORONOI圖的定義，所選取的可視點都是距離障礙區最遠的點，并且地圖邊框也是不可飛區域，可視點如圖4所示。文獻[8]的可視點為多邊形的頂點。

圖 4 基于不同代價圖航跡規劃示意圖Fig. 4 The diagram of trajectory planning based on different cost maps

從實驗結果可知，文獻[8]的代價圖會使得航線盡量靠近障礙物的頂點，在有擾動的情況下，很可能導致飛行器與障礙物發生碰撞。本文提出的代價圖可以使得規劃出的航跡遠離障礙物，提高了無人機生存概率，但是規劃出的航跡會比文獻[8]的航跡代價高，因此可以綜合考慮各種因數，選擇合適的代價圖。

實驗二 RHC-FPSO算法與傳統航跡規劃算法對比

圖5將傳統經典的航跡規劃方法SAS和RRT與本文提出的算法進行了對比。稀疏A*搜索(SAS)算法是啟發式搜索算法A*的一種改進，首先將規劃環境表示為網格的形式，通過預先確定的代價函數尋找最小代價的航跡。本文在擴展節點時，通過把約束條件結合到搜索算法中去，減少了搜索空間，進一步縮短了規劃時間。RRT通過對規劃環境中的采樣點進行碰撞檢測來獲取空間中的障礙物的信息,可以避免對規劃環境進行建模。RRT首先選擇開始點作為隨機樹的根節點，通過隨機探索方向點來擴展隨機樹，直到擴展到目標點，然后從目標點回溯到根節點，完成航跡規劃。因此SAS算法在空間表示精度下是全局最優解，而RRT是局部最優解。

圖 5 3種航跡規劃方法對比圖Fig. 5 Comparison of three trajectory planning methods

從圖5和表2可以看出，在以距離為代價的航跡規劃算法中，稀疏A*搜索算法是3種算法里面航跡規劃最短的算法，RRT是隨機算法，是算法里面航跡最長的，并且不是最優路徑，RHCFPSO算法不是距離最短的算法。從圖5可知，本文算法規劃出的航跡不是一些直線段的組合，主要是因為需要滿足無人機的各種約束，本實驗主要是指速度和加速度約束。

表 2 航跡規劃比較Table 2 Comparison of trajectory planning

實驗三 RHC-FPSO與RHC-PSO算法對比

考慮多旋翼無人機，因其具有懸停功能，因此最小速度可以為零，設初始位置為(25, 0)，目標為(–25, –3)。實驗結果如圖6所示，為將關于未來步內的預測航跡的計算結果分解開，RHC-FPSO每次預測未來的6個航路點，而將第1個控制輸入作為實際的輸入值，其他的預測航路點作為參考航路點。圖7為將每次預測值的第1個控制輸入作為實際輸入后規劃的完整航跡，可以看出在滿足約束條件的前提下，規劃出一條穿過障礙物區實際可飛的航跡。

圖 6 RHC-FPSO的預測航跡示意圖Fig. 6 The prediction of trajectory based on RHC-FPSO

圖 7 RHC-FPSO的航跡Fig. 7 The trajectory based on RHC-FPSO

圖8顯示了改進后的算法比文獻[15]在計算時間上有很大的改進。RHC-FPSO算法單次最大計算時間為0.440 0 s，最小計算時間為0.310 0 s，平均計算時間為0.369 5 s，RHC-PSO的最大、最小和平均時間分別為0.800 0 s、0.460 0 s和0.641 9 s。

圖 8 時間對比圖Fig. 8 Comparison of time

圖9和圖10顯示了RHC-FPSO與RHC-PSO在收斂速度和穩定上幾乎相當。圖10中，平均穩定性是按照式(22)定義的，分別為0.806 7和0.715 0，對于基于PSO的優化方法可以使用式(22)衡量。

式中*代表所有的值。

圖 9 代價迭代收斂對比Fig. 9 The convergence comparison of the costs

圖 10 穩定性對比圖Fig. 10 Comparison of stability

實驗四 滾動時域和人工勢場相結合(RHCAPF)

實驗四主要是結合滾動時域和人工勢場的優勢規劃航跡。實驗一中，需要計算代價圖，也是一個非常大的代價，而且當環境變化的時候，需要重新計算，代價比較高。如果采用APF的思想，使用勢能場作為優化目標，則不需要計算代價圖，極大地減少了計算時間，非常適合在線規劃。由于使用滾動時域，計算未來有限步的輸入，同時使用FPSO優化，成功地解決了APF的兩個主要問題。

從圖10和圖11可以得出，由于障礙物對無人機有一個斥力場，使得無人機遠離障礙區，提高了無人機的安全性，并且不再需要提前計算代價圖，很適合動態的在線規劃。

圖 11 RHC-APF的航跡Fig. 11 The trajectory based on RHC-APF

實驗五 動態環境中的航跡規劃

本文提出的航跡規劃算法是適合在動態環境下進行航跡規劃的。本實驗參數使用表1的參數設置。如圖12所示，黑色方框區域為飛行前已經知道的障礙物區域，虛線為飛行器在飛行過程中探測到的障礙物。與圖7對比可知，當飛行器靠近圖12中下面的淺色虛線的未知障礙物時，飛行探測器探測到新的障礙物，因此需要重新計算代價圖，然后根據新的代價圖以及當前狀態，進行下一個時刻的航跡規劃，此時還沒有探測到上面的障礙物，因此，飛行器往上面飛行。當探測到上面的障礙物時，飛行器已經不能按照之前的航跡飛行了，此時需要飛出凹型區域，如圖12所示。

圖 12 動態航跡規劃預測航跡示意圖Fig. 12 The prediction of dynamic trajectory planning

圖13顯示無人機完整的飛行軌跡，在圖11中由于檢測到新的障礙物，飛行器需要快速轉變方向。在轉變方向的時候，無人機的速度接近零，因此，整個的航跡如圖13所示。

從以上的5個實驗和實驗的分析可知，RHCFPSO在滿足障礙物約束和無人機自身約束的條件下，順利地穿過障礙物區域到達目標區域，并且在時間上有很大提高，是滿足在線計算要求的，實驗五顯示對新檢測到的障礙物，本文算法可以做出快速反應，規避障礙物。

5 結束語

首先，將無人機動力學模型進行線性近似，表示為狀態空間模型。然后，根據模型和滾動時域優化的思想，將航跡規劃表示為一個帶約束的優化問題。最后，使用粒子群優化算法進行優化，成功地規劃出滿足無人機約束和障礙物規避的實際可飛的航跡。將航跡規劃分為兩個階段，第1階段為預先代價估計階段，第2階段為在線航跡規劃。在代價估計階段，引入VORONOI圖，使得節點離障礙物的距離盡可能遠，進一步提高了無人機的生存概率。在第2階段，通過理論和仿真表明，采用FPSO方法在RHC框架下進行在線航跡規劃可以將無人機動力學約束條件很容易地引入到規劃問題中來，為帶有動態約束的無人機長航程航跡規劃提供一種新型的算法，為無人機規劃出一條滿足約束條件的實際可飛的航跡。滾動時域規劃減少了一定的計算量，是一種可行的優化方法。將滾動時域和人工勢場法相結合，可以不需要計算代價圖，減少了計算量，并且在環境發生改變時，不再需要計算代價圖，只需更新勢場。因此RHC-APF是非常適合動態在線航跡規劃，易于擴展。未來的工作應該把該方法擴展到多機動態三維航跡規劃中，以及進一步提高求解算法的速度。