固定翼無人機在線航跡規劃方法

劉佳　秦小林　許洋　張力戈

摘 要：在不確定環境下，針對固定翼無人機（UAV）航跡規劃問題，提出了一種基于滾動時域控制的模糊粒子群優化算法與改進人工勢場法相結合的在線航跡規劃方法。首先，對凸多邊形障礙物進行最小外接圓擬合;然后，根據靜態威脅，將規劃問題轉化為一系列時域窗口內的在線子問題，利用模糊粒子群算法實時優化求解以實現靜態避障;當環境中存在動態威脅時，使用改進人工勢場法對航跡進行調整完成動態避障。為了滿足固定翼無人機的動態約束，同時提出固定翼UAV的碰撞檢測法，可提前判斷障礙物是否為真正威脅源，以此減少轉彎頻率和幅度，降低飛行代價。仿真實驗結果表明，所提方法在固定翼UAV航跡規劃中能有效提升規劃速度、穩定性與實時避障能力，且克服了傳統人工勢場容易陷入局部最優的缺點。

關鍵詞：滾動時域控制;模糊粒子群;人工勢場;固定翼無人機;航跡規劃

中圖分類號： V249;TP301文獻標志碼：A

On-line path planning method of fixed-wing unmanned aerial vehicle

LIU Jia1，2， QIN Xiaolin1，2*， XU Yang1，2， ZHANG Lige1，2

（1. Chengdu Institute of Computer Application， Chinese Academy of Sciences， Chengdu Sichuan 610041， China;

2. School of Computer and Control Engineering， University of Chinese Academy of Sciences， Beijing 100049， China）

Abstract： By the combination of fuzzy particle swarm optimization algorithm based on receding horizon control and improved artificial potential field， an on-line path planning method for achieving fixed-wing Unmanned Aerial Vehicle （UAV） path planning in uncertain environment was proposed. Firstly， the minimum circumscribed circle fitting was performed on the convex polygonal obstacles. Then， aiming at the static obstacles， the path planning problem was transformed into a series of on-line sub-problems in the time domain window， and the fuzzy particle swarm algorithm was applied to optimize and solve the sub-problems in real time， realizing the static obstacle avoidance. When there were dynamic obstacles in the environment， the improved artificial potential field was used to accomplish the dynamic obstacle avoidance by adjusting the path. In order to meet the dynamic constraints of fixed-wing UAV， a collision detection method for fixed-wing UAV was proposed to judge whether the obstacles were real threat sources or not in advance and reduce the flight cost by decreasing the turning frequency and range. The simulation results show that， the proposed method can effectively improve the planning speed， stability and real-time obstacle avoidance ability of fixed-wing UAV path planning， and it overcomes the shortcoming of easy to falling into local optimum in traditional artificial potential field method.

Key words： receding horizon control; fuzzy particle swarm; artificial potential field; fixed-wing Unmanned Aerial Vehicle （UAV）; path planning

0 引言

無人機因其低成本、可自主飛行等優良特性，在軍事和民用領域得到了廣泛應用。無人機航跡規劃是一個多目標優化問題，定義為：在一定環境和任務目標下，尋找從起始點到目標點的最優飛行路線，同時避開各種威脅源[1]。

目前，現有航跡規劃方法可分為幾何方法、啟發式搜索方法、勢場法等。其中幾何方法包括Voronoi圖[2]、概率圖模型[3]、可視圖模型等。這類算法首先對環境進行幾何建模，然后依據一定的最優策略，選擇某種搜索算法得到可行解;但當任務空間發生變化，需對任務空間重新遍歷，不適合動態航跡規劃。文獻[4]在三維環境下構建連續可微的位形空間，生成連接各個障礙物的廣義可視圖，并利用搜索算法搜索出一條可行路徑，在障礙物發生改變時，只需更新部分信息;但仍未解決在線規劃問題。文獻[5]提出一種改進的Voronoi圖算法，得到有效、無碰撞的可飛航跡;但未對未知環境下無人機避障問題進行討論。勢場法中典型的方法是人工勢場法[6]，其優點是計算量小、速度快，但容易陷入局部最優。盡管有學者在原來基礎上提出了虛擬力[7]和帶干擾的流動力學系統[8]，但是局部最小的問題依舊存在。為此，文獻[9]提出一種在三維環境下將改進的擾動流體動態系統與灰狼優化理論結合的航跡規劃算法，使得規劃出的航跡平滑可飛，能有效解決局部最優問題。文獻[10]通過引入相對速度斥力勢場解決了人工勢場局部極小的問題，通過設計了斥力增益模糊控制器，有效地完成了無人機動態避障。啟發式搜索法包括A*（A-Star）算法[11]、粒子群算法、遺傳算法[12]等經典算法;但這類算法隨著搜索空間的擴大，計算復雜度會呈爆炸式增長。文獻[13]提出了一種稀疏A*算法，該算法在修剪搜索空間的同時使得規劃路徑滿足約束條件，以此提高規劃路徑的效率。文獻[14]針對機器人避障提出了一種混合粒子群算法，實驗表明此算法性能和收斂速度要優于標準粒子群算法。文獻[15]提出一種慣性權值依據迭代次數變化而變化的改進粒子群算法，該算法明顯提高了航跡優化精度和穩定性;但因未過多考慮無人機的約束條件，不適用于實際工程應用。文獻[16]提出了基于滾動時域控制的模糊粒子群優化（Receding Horizon Control-Fuzzy Particle Swarm Optimization， RHC-FPSO）算法以解決帶有動力學約束的多旋翼無人機航跡規劃問題，該方法是在已知全局環境信息的前提下需要計算全局代價圖，當環境突然發生變化時，使用RHC-APF（Receding Horizon Control-Artificial Potential Field）作出調整，但是該方法不適用于局部環境已知的固定翼無人機航跡規劃。為提高固定翼無人機的生存能力，本文中提出了靜態避障和動態避障相結合的在線航跡規劃算法。首先，采用基于帶有約束的滾動時域控制策略的模糊粒子群優化（Restricted-RHC-FPSO， R-RHC-FPSO）方法完成已知局部環境信息下的靜態避障;然后，構造分段線性人工勢場法（Piecewise Linear Artificial Potential Field， PLAPF），完成動態避障。實驗結果表明，本文提出的算法能夠滿足不確定環境下固定翼無人機實時避障的要求和無人機動力學約束，達到良好的動態航跡規劃效果。

1 問題描述

1.1 滾動時域控制

滾動時域控制（Receding Horizon Control， RHC）也稱為模型預測控制（Model Predictive Control， MPC），是一種基于滾動預測方式，在線求解最優的控制方案[17]。假設當前時間點為k，從當前最新狀態xpk出發，預測時域為[k，k+N]，Upk定義為在時刻k預測N步的控制輸入序列，即Upk=[upk|kT，upk|1 + kT，…，upk|N-1 + kT]T，系統執行第一步控制輸入，并更新當前最新狀態，重復此過程，直到達到目標。

1.2 模型建立

結合RHC原理以及文獻[18]中提出的無人機動力學模型得到基于RHC的固定翼無人機模型為：

minu（·） JN=∑N-1i=0li（x（k+i|k），u（k+i|k），xf）+

f（x（k+N+1|k），xf）（1）

x（k+i+1|k）=p（k+i+1|k）

v（k+i+1|k）=

Ap（k+i|k）

v（k+i|k）+Bu（k+i|k）;i=0，1，…，N-1（2）

式（1）為目標函數，式（2）為無人機的狀態空間模型。其中：A=I2ΔtI2

O2I2;B=12（Δt）2I2ΔtI2T;uk+i表示在第k+i時刻的控制輸入;p（）、v（）、u（）分布表示無人機位置、速度和加速度;xf是目標集， f（）為終端懲罰項。 f（）表示為：

f（x（k+N+1），xf）=d（x（k+N+1），xvis）+Cif（3）

其中：d（）表示航跡端點到障礙物的代價，用航跡端點到障礙物最小外接圓的切線距離表示;xvis表示用航跡端點到障礙物最小外接圓的切點;Cif為障礙物到目標點的最小代價。

一般的輸出約束滿足包括轉彎角約束、速度約束、加速度約束，以及障礙物約束。

change≤max

‖v（k+i|k）‖≤vmax

‖auav‖≤amax

p（k+i|k）O （4）

式中：O∈R2代表不可飛區域;max、vmax、amax分別為固定翼無人機最大轉彎角、最大速度和最大加速度約束。

1.3 碰撞檢測

為保證固定翼無人機滿足轉彎角約束條件同時減小無人機轉彎頻率，本文在文獻[19]的碰撞檢測方式上作出了改進，如圖1所示，給出如下定義。

定義1 如果d-Robi≤ρo，此時視障礙物i為潛在威脅源。d表示無人機當前位置Puav與障礙物最小外接圓圓心Pobi之間的距離，ρo表示無人機檢測半徑。

定義2 如果vro在沖突域NPuavM中或ψor滿足式（5），此時將障礙物i視為真正的威脅源，將繼續向此方向飛行視為不可飛區域。

∠MPuavx′≤ψor≤∠NPuavx′

ψm-|β-|≤ψor≤ψm+|β+|（5）

其中：vro表示無人機和障礙物i的相對速度;ψor表示相對速度vro與x坐標的夾角;NPuav和MPuav為無人機當前位置和障礙物i最小外接圓的切線;β-和β+分別表示d和MPuav以及d與NPuav之間的夾角。

此種檢測算法結合滾動時域預測方式能使無人機提前判斷障礙物是否構成威脅，及早進行規避操作。因為轉彎過程比直飛需要更大的推力，所以應避免出現轉彎角度過大的情況，這是與多旋翼無人機碰撞檢測算法的不同之處。

2 R-RHC-FPSO靜態避障

2.1 模糊粒子群算法

PSO算法是一種基于種群搜索策略的自適應隨機優化算法，各粒子速度和位置更新公式為：

Vj（t+1）=wVj（t）+c1r1（Lj（t）-Xj（t））+

c2r2（G（t）-Xj（t））

Xj（t+1）=Xj（t）+Vj（t）（6）

其中：Lj（t）是每個粒子的局部最優解;G（t）是全局最優解;w為慣性權重;t為當前迭代次數;c1、c2表示學習因子;r1和r2為區間[0，1]的隨機數。因為控制時域為N，所以粒子速度Vj（t）和位置Xj（t）是N個2維向量組成的矩陣。

慣性權重具有能夠平衡算法的全局搜索和局部搜索能力的作用，而慣性權重的自適應調節能夠加快算法收斂到最優值[20]，本文中慣性權重更新規則如下：

w（t）=wmax， t<23T

wmax-wmax-wminKk，t≥23T （7）

其中：t為當前迭代次數;T為總迭代次數。慣性權重w（t）的值隨著迭代次數的變化而變化，在迭代總次數的前2/3時期，慣性權重值設置為最大，增強全局搜索能力;在迭代后期，慣性權重值逐漸變小，增強局部搜索能力。

wmin和wmax分別為w（t）的最小和最大值，其中：wmax不是一個固定的值，而是根據粒子的適應度值作出更新，如果粒子滿足約束，即為可行解，為方便起見，此時wmax設置為1;如果粒子不滿足約束時，wmax（t， j）=1-Ft， j（u）maxj Ft， j（u），Ft， j（u）表示粒子j在第t次迭代的代價。

通過對wmax的動態調整，使得滿足約束的粒子的搜索能力增強，不滿足約束的粒子搜索能力得到弱化。實驗結果表明，這種更新方式所費時間更短。

2.2 R-RHC-FPSO算法流程

在當前時刻k，依據模糊粒子群更新規則式（6）優化求解式（1），在優化過程中粒子計算得到的控制輸入和式（2）不滿足式（4）中任一條件時，將該粒子視為不可行粒子，在得到最優控制輸入u（k+i|k）（i=0，1…，N-1）后執行第一步控制輸入u（k+1|k）并更新當前狀態，系統進入下一時刻k+1，循環執行此步驟，直到達到目標。具體步驟如下所示：

1）設置粒子群種群數M，并初始化每個粒子的速度Vj=（Vj，1，Vj，2，…，Vj，N），位置Xj=（Xj，1，Xj，2，…，Xj，N）。

2）判斷當前迭代次數t是否小于最大迭代次數T，如果是轉到步驟3）;否則轉到步驟6）。

3）遍歷粒子群，如果粒子j滿足約束式（4），按照式（3）計算每個粒子的代價值;否則，粒子j的代價為BigM+C，其中BigM為一個很大的數，C為相應的約束違背量。

4）依據代價值最小原則更新全局最優解G（t）和每個粒子局部最優解Lj（t）。

5）按照式（6）更新粒子j的速度Vj、位置Xj，如果粒子不滿足終止條件，轉步驟3）;否則轉到6）。

6）將全局最優解G（t）的第一個控制輸入模型式（2）中，計算無人機下一時刻的狀態。

7）如果無人機到達目標點，結束;否則轉到步驟2）。

3 PLAPF動態避障

如果環境中存在動態障礙物時，繼續使用R-RHC-FPSO靜態避障算法，需實時計算代價圖，計算成本大，而人工勢場法因其計算量小、規劃時間短的優良特性，可以和快速規避動態障礙物的目標結合，完成動態避障任務。傳統人工勢場法是通過構造虛擬的引力場和斥力場[21]來尋找一個無碰撞的路徑。傳統人工勢場法在靜態威脅環境下可以快速規劃出一條無碰撞路徑，但是如果環境中存在動態威脅物時，其規劃能力就減弱了，而且會出現局部極小和目標不可達的情況。為此，通過在分段線性人工勢場中引入相對速度勢場來解決這兩個問題。

3.1 引力勢場

引力勢場由無人機到目標點的勢場和無人機自身速度勢場構成，即：

Uatt（p，v） = εpρ2goal（p） + εv‖v‖2（8）

其中：εp、εv為引力增益因數。分段線性的引力可由該點勢函數的負梯度計算得到：

Fatt=0，ρgoal（p）

Fmaxρgoal（p）-rRdetect，r≤ρgoal（p）≤ρo

Fmax，ρgoal（p）>ρo （9）

其中Fmax=2εp ρgoal（p）ρgoal（p）+2εv‖v‖。當目標位置在無人機檢測范圍外時，將最大引力作用于無人機，并將其推向目標;當目標位置位于無人機檢測范圍內同時無人機與目標點的距離大于等于最小安全距離時，此時的引力和ρgoal（p）成正比函數，當ρgoal（p）越小時，引力越小，此種方法可防止無人機在到達目標點時由于引力過大產生目標不可達的情況;當固定翼無人機距離目標的距離小于最小安全距離r時，沒有外力作用于無人機，即視為到達目標。

3.2 斥力勢場

斥力勢場由障礙物自身產生的勢場和無人機與障礙物之間的相對速度勢場構成，即：

Urepi（p，v）=ηp（1ρi（p）-1ρo）+ηvovro（10）

其中：ηp、ηvo為斥力增益因數;vro=（v-vobs）Tero表示無人機與障礙物的相對速度。分段線性的斥力可由斥力勢函數的負梯度計算得到：

Frepi（p，v）=Frepi_max， ρi（p）≤r且vro>0

Frepi_max（1-（ρi（p）-r）ρo），

r<ρi（p）≤ρo且vro>0

0，ρi（p）>ρo或vro≤0（11）

其中：

Frepi_max=-p[ηp（1ρi（p）-1ρo）+ηvovro]-

o[ηp（1ρi（p）-1ρo）+ηvovro] （12）

只有當障礙物進入無人機檢測范圍并且無人機與障礙物i的相對速度大于0時，該障礙物才會對無人機產生斥力作用，并且障礙物i產生的斥力會隨著ρi（p）的減小而增大，當ρi（p）小于等于最小安全距離r時，此時作用于無人機的斥力最大。假設無人機在檢測范圍內檢測到障礙物數為Num，則無人機在當前位置受到的總斥力為：

Frep（p，v）=∑Numi=1Frepi（p，v） （13）

因此，通過上述改進可以保證無人機能夠快速規避動環境中的動態障礙物，同時解決了APF的局部極小和目標不可達的問題。

4 仿真結果及分析

為驗證所提算法的有效性，在Matlab R2016a編程環境下對固定翼無人機航跡規劃進行仿真，仿真環境區域為50m×50m，按照1∶100比例縮放，仿真參數如表1所示。

表格（有表名）表1 仿真參數

Tab. 1 Simulation parameters

參數描述數值N滾動時域控制步數6K粒子群最大迭代次數100M粒子群數量30Δt/s采樣時間1vmax/（km·s-1）最大速度0.6amax/（km·s-1）最大加速度0.2max/（°）最大轉彎角45r/km最小安全距離0.5pinit起始位置（0，50）pgoal目標位置（50，0）

4.1 實驗一

將本文所提出的碰撞檢測方式（R-RHC-FPSO）和傳統碰撞檢測方式（RHC-FPSO）進行仿真實驗對比，結果如圖2所示。

圖2中，（a）和（b）分別是傳統的檢測算法即在檢測半徑以內的障礙物都視為威脅源與本文所提碰撞檢測算法在無動態威脅環境下的航跡和航向角變化對比。從圖2（a）中Ⅰ和Ⅱ可看出，本文所提檢測算法能夠提前判斷繼續向前飛行可能會發生沖突，及時轉變方向;而普通檢測算法只有在無人機與障礙物距離小于安全距離時，才進行規避，明顯看出普通檢測算法得到的航跡圖存在轉彎角過大的問題。同樣從圖2（b）可以看出，本文所提檢測方式在航向角穩定性上更具有優勢，平均單步航向角轉變要小于普通檢測算法規劃出的航跡航向角轉變。

[7]DONG Z， CHEN Z， ZHOU R， et al. A hybrid approach of virtual force and A* search algorithm for UAV path re-planning [C]// Proceedings of the 6th Industrial Electronics and Applications. Piscataway： IEEE， 2011： 1140-1145.

[8]WANG H， LYU W， YAO P， et al. Three-dimensional path planning for unmanned aerial vehicle based on interfered fluid dynamical system [J]. Chinese Journal of Aeronautics， 2015， 28（1）： 229-239.

[9]姚鵬，王宏倫.基于改進流體擾動算法與灰狼優化的無人機三維航路規劃[J].控制與決策，2016，31（4）：701-708.（YAO P， WANG H L. Three-dimensional path planning for UAV based on improved interfered fluid dynamical system and grey wolf optimizer [J]. Control and Decision， 2016， 31（4）： 701-708.）

[10]姚遠，周興社，張凱龍，等.基于稀疏A* 搜索和改進人工勢場的無人機動態航跡規劃[J].控制理論與應用，2010，27（7）：953-959.（YAO Y， ZHOU X S， ZHANG K L， et al. Dynamic trajectory planning for unmanned aerial vehicle based on sparse A* search and improved artificial potential field [J]. Control Theory and Applications， 2010， 27（7）： 953-959.）

[11]FAN K C， LUI P C. Solving find path problem in mapped environments using modified A* algorithm [J]. IEEE Transactions on Systems， Man， and Cybernetics， 1994， 24（9）： 1390-1396.

[12]CHEN G， CRUZ J B. Genetic algorithm for task allocation in UAV cooperative control [C]// Proceedings of the 2003 AIAA Guidance， Navigation， and Control Conference and Exhibit. Reston： AIAA， 2003： Article No. 5582.

[13]SZCZERBA R J， GALKOWSKI P， GLICKTEIN I S， et al. Robust algorithm for real-time route planning [J]. IEEE Transactions on Aerospace and Electronic Systems， 2000， 36（3）： 869-878.

[14]KUO P H， LI T H S， CHEN G Y， et al. A migrant-inspired path planning algorithm for obstacle run using particle swarm optimization， potential field navigation， and fuzzy logic controller [J]. The Knowledge Engineering Review， 2017， 32： Article No. e5.

[15]方群，徐青.基于改進粒子群算法的無人機三維航跡規劃[J].西北工業大學學報，2017，35（1）：66-73.（FANG Q， XU Q. 3D route planning for UAV based on improved PSO algorithm [J]. Journal of Northwestern Polytechnical University， 2017， 35（1）： 66-73.）

[16]王文彬，秦小林，張力戈，等.基于滾動時域的無人機動態航跡規[J].智能系統學報，2018，13（4）：524-533.（WANG W B， QIN X L， ZHANG L G， et al. Dynamic UAV trajectory planning based on receding horizon [J]. CAAI Transactions on Intelligent Systems， 2018， 13（4）： 524-533.）

[17]MAYNE D Q， RAWLINGS J B， RAO C V， et al. Constrained model predictive control： Stability and optimality [J]. Automatica， 2000， 36（6）： 789-814.

[18]KUWATA Y， RICHARDS A， SCHOUWENAARS T， et al. Decentralized robust receding horizon control for multi-vehicle guidance [C]// Proceedings of the 2006 American Control Conference. Piscataway： IEEE， 2006： 2047-2052.

[19]THANH H L N N， PHI N N， HONG S K， et al. Simple nonlinear control of quadcopter for collision avoidance based on geometric approach in static environment [J]. International Journal of Advanced Robotic Systems， 2018， 15（2）： 1-7.

[20]張長勝，孫吉貴，歐陽丹彤.一種自適應離散粒子群算法及其應用研究[J].電子學報，2009，37（2）：299-304.（ZHANG C S， SUN J G， OUYANG D T. A self-adaptive discrete particle swarm optimization algorithm [J]. Acta Electronica Sinica， 2009， 37（2）： 299-304.）

[21]LATOMBE J C. Robot Motion Planning [M]. Berlin： Kluwer Academic Publishers， 1991： 19-20.

This work is partially supported by the National Natural Science Foundation of China （61402537）， the Light of West China Program of Chinese Academy of Sciences， the Sichuan Science and Technology Program （2018GZDZX0041）.

LIU Jia， born in 1995， M. S. candidate. Her research interests include path planning of unmanned aerial vehicle， machine learning.

QIN Xiaolin， born in 1980， Ph. D， research follow. His research interests include automatic reasoning， cluster intelligence.

XU Yang， born in 1994， M. S. candidate. His research interests include machine learning， optimization algorithm.

ZHAGN Lige， born in 1995， Ph. D. candidate. His research interests include machine learning， optimization algorithm.

收稿日期：2019-05-22;修回日期：2019-07-23;錄用日期：2019-07-23。

基金項目：國家自然科學基金資助項目（61402537）;中國科學院“西部青年學者”項目;四川省科技計劃項目（2018GZDZX0041）。

作者簡介：劉佳（1995—），女，寧夏銀川人，碩士研究生，主要研究方向：無人機航跡規劃、機器學習; 秦小林（1980—），男，重慶人，研究員，博士生導師，博士，主要研究方向：自動推理、集群智能; 許洋（1994—），男，重慶人，碩士研究生，主要研究方向：機器學習、優化算法;張力戈（1995—），男，山西原平人，博士研究生，主要研究方向：機器學習、優化算法。

文章編號：1001-9081（2019）12-3522-06DOI：10.11772/j.issn.1001-9081.2019050863