基于粒子群算法的固定時間多約束無人機軌跡規劃

邵士凱　石偉龍　杜云

摘要：針對粒子群算法在軌跡規劃時，將無人機視為質點，未考慮無人機的飛行時間、角度等參數的不足，提出一種數值方法結合粒子群算法的軌跡規劃求解方法。首先，考慮到對每個時刻控制變量進行優化會耗費大量的時間，將無人機的飛行時間離散為一定數量的切比雪夫配點，在這些離散的配點處優化控制變量以減小計算負擔;其次，將角速度作為控制變量，運用曲線擬合求解出角速度與時間的函數，經過積分求出無人機的角度、位置與時間的函數;再次，將結果代入粒子群優化模型并結合無人機運動學模型進行優化求解，根據分配的時間計算出最終的角速度、角度以及位置坐標;最后，在復雜環境下進行無人機軌跡規劃仿真，通過與已有方法的對比，驗證所提求解方法的有效性和可行性。結果表明，所提出的軌跡求解方法可以求出包括位置在內的各個運動學參數，規劃出光滑的軌跡并且成功避開前進過程中的障礙物。所提方法有效提升了軌跡規劃的求解維度，對實現智能自主化飛行有一定的參考價值。

關鍵詞：航空、航天科學技術基礎學科其他學科;軌跡規劃;無人機;粒子群算法;運動學模型;曲線擬合

中圖分類號：TN958.98文獻標識碼：A

DOI：10.7535/hbkd.2022yx03005

Fixed time multi-constraint UAV trajectory planning based on particle swarm optimization

SHAO Shikai SHI Weilong DU Yun

（School of Electrical Engineering，Hebei University of Science and Technology，Shijiazhuang，Hebei 050018，China）

Abstract：In view of the disadvantage that UAV is usually regarded as a particle in trajectory planning with particle swarm optimization algorithm，the flight time，angle and other parameters of UAV are ignored，a trajectory planning method combining numerical method with particle swarm optimization algorithm was proposed.Firstly，considering that the optimization of each control variable will cost a lot of time，the flight time of UAV was discretized as a certain number of Chebyshev points，and control variables were optimized at these discrete collocations to reduce the computational burden.Secondly，the angular velocity was taken as the control variable，the function of angular velocity and time was solved by curve fitting，and the function of angular and position of UAV with time was obtained by integration.Thirdly，the angular velocity，angle and position were calculated according to the allocated time.Finally，the simulation of UAV trajectory planning in complex environment was carried out，and the effectiveness and feasibility of the proposed method were verified by comparing the Monte-Carlo simulation results with the results of existing methods.The results show that the proposed method can calculate the kinematic parameters including position，plan a smooth trajectory and successfully avoid the obstacles in the process of forward.Therefore，the combination of particle swarm optimization algorithm and numerical method can solve the kinematics parameters of UAV and provide certain reference value for the improvement of the solution dimension of trajectory planning and the realization of intelligent autonomous flight.

Keywords： basic science and technology of aeronautics and astronautics other disciplines;trajectory planning;UAV;particle swarm optimization （PSO）;kinematic model;curve fitting

隨著科技的不斷創新，無人機已經廣泛應用于軍事和民用任務，如情報、監視、偵察、救援和商業表演[1-3]。作為無人機研究領域的重要方向之一，無人機軌跡規劃備受關注。

經過眾多學者的不斷探索，無人機軌跡規劃算法的研究成效顯著?；趫D形的算法是一種簡單的路徑規劃方法，如：Voronoi圖算法[4]、A*算法[5]、概率路線圖算法[6]、快速探索隨機樹算法[7]以及人工勢場算法[8]。然而，這些算法很少考慮無人機的運動學和動力學約束，因此通常不能應用于實際情況。另外，這類算法都是基于代價圖實現的，需要提前生成和存儲代價圖，而代價圖的生成非常耗時。近年來，隨著群體智能技術的發展，基于生物群體的進化算法取得了很大的進步[9]，它們保持了較強的搜索能力，能夠以更高效、更靈活的方式搜索最優解。利用這種方法進行無人機路徑規劃越來越受到研究者的重視。常用的算法主要有人工蜂群算法（artificial bees colony algorithm）[10]、[JP2]蟻群算法（ant colony algorithm）[11]、遺傳算法（genetic algorithm）[12]和粒子群算法（PSO）[13]。在這些算法中，粒子群算法相對于其他算法具有通用性強、搜索效率高、迭代公式和算法規則簡單、易于仿真實現等優點[14-15]，應用于無人機航線規劃求解后取得了一定成果[16-18]。

上述智能算法雖可快速規劃出可行軌跡，但仍有不足之處，如易陷入局部最優、未考慮無人機的實際飛行性能需求等，導致其軌跡規劃結果只具備粗略的指引作用。為提高軌跡的求解精度，一些學者采用數值方法將求解的參數維度由三維拓展到四維，進而，求解結果可直接作為無人機系統的參考指令，大大降低了飛行控制系統的復雜度。文獻[19]針對高超聲速飛行器，在多約束條件下，設計了基于Dubins曲線的軌跡規劃算法，仿真結果表明所提方法可產生滿足約束的飛行軌跡。文獻[20]提出了一種基于偽譜法的軌跡規劃方法，采用非線性規劃方法有效解決了軌跡優化問題。但數值方法仍存在較大局限性，隨著問題維數的提升、復雜程度的提高，數值方法會出現耗時長、收斂速度慢，甚至無法求得有效解等問題。因此，綜合群智能算法和數值法的優點，將群智能算法與數值方法結合實現軌跡規劃是一個值得研究的問題。文獻[21]將控制變量和時間分別離散取值并結合粒子群算法提出了一種新的軌跡規劃求解方法，有效解決了軌跡規劃的優化問題。然而，文獻[21]主要是將飛行時間平均分配，控制輸入接近分段常數值，而且采用二維的運動學模型，因此該求解方法存在一些局限性。文獻[22]針對無人機與水下無人航行器協同攻擊目標的問題提出了一種對控制變量進行優化的粒子群算法，最終提出的路徑規劃方法能夠生成協同路徑，所得結果與理論最優解非常接近，但是在軌跡優化的求解方法上只提出了一個大體框架以及一些仿真驗證，沒有闡明具體的求解方法。

針對粒子群算法以及已有方法的不足，本文提出一種新的軌跡規劃求解方法，在固定時間且多約束的條件下，將傳統的僅包含位置的三維航線規劃擴展為包含飛行時間和多種狀態約束的四維軌跡規劃。

1問題描述與模型建立

1.1問題描述

針對無人機在復雜環境下的軌跡規劃問題，考慮到目標點、障礙區域已由衛星與高空雷達探測得知，且本文假設無人機勻速飛行。因此，影響任務完成的關鍵在于：1）如何將飛行時間合理分配給各個配點;2）如何求解粒子群算法與無人機運動學模型的耦合關系，獲取多維度航線指令，為飛控系統提供良好參考;3）如何保證整體航線的最優性，如距離最短、性能指標最優等。無人機在復雜環境下軌跡規劃示例如圖1所示。

1.2無人機運動學模型的建立

在軌跡規劃問題中，通常將載運工具視為質點，采用質點運動模型，計算其行動軌跡，甚至在一些文獻中只考慮了質點運動學模型[23]。這是因為相較于控制器模塊，軌跡規劃模塊相當于外回路，需要為內回路提供控制指令信號，所以需要有較快的運算速度。軌跡規劃問題中載運工具通常視為剛體，其運動模型由1組變系數非線性微分方程描述，求解此方程組需要耗費一定時間。為了進一步節約時間，本文采用簡化后的無人機運動學模型[24]，見式（1）。

2求解策略

2.1配點分布

在軌跡規劃過程中，對每個時刻的控制變量優化取值會耗費大量時間，從而降低算法的效率。為了平衡計算量和效率，在軌跡規劃階段設置了配點，即將UAV飛行的時間離散為一定數量的切比雪夫點，它們是優化控制變量的時刻。切比雪夫點計算公式[25]見式（2）。

2.2粒子群算法

2.2.1公式描述

2.2.2自適應參數調整

2.2.3混沌初始化

2.3運動學模型的求解

2.3.1軌跡規劃評價函數

2.3.2公式描述

2.3.3求解策略與步驟

無人機在進行軌跡規劃時，面臨各種參數的調試運行，僅僅求出航跡的位置信息并不能滿足實際的需求。因此，若想為無人機提供更好的飛行參考指令，需結合無人機運動學模型求出各個運動學參數。具體步驟如下：

2.3.4求解流程

3仿真結果與分析

具體仿真環境：CPU為intel Corei5處理器，內存為8 GB，操作系統為windows 7 64位，主頻2.1 GHz，在MATLAB R2016b環境下實現無人機軌跡規劃。

3.1仿真條件設定

首先設定地形環境。為使仿真更具有真實性，將仿真環境設定為三維地形圖，具體包括山峰威脅區域和雷達威脅區域，山峰用起伏的曲面表示，雷達用半球體表示。具體參數設定如表1所示。

3.2軌跡規劃仿真結果與分析

3.3仿真比較

圖5 a）反映了適應度函數值的變化過程，可知在同等條件下本文方法的適應度函數值會以較快的速度收斂到最優值，因此相較于文獻[21]所述方法，本文方法有著更快的收斂速度。圖5 b）為軌跡的俯視圖，可知運用本文方法，無人機可以成功到達終點并規劃出可行且平滑的軌跡，而文獻[21]所述的方法產生的軌跡發生了偏折，影響到軌跡規劃的效果。

為了探究軌跡偏折的原因，2種方法的角速度和角度的詳細信息如圖6所示。由圖6 a）可知，文獻[21]所述方法將時間離散等分，每個時間段的控制輸入為分段常數值，而本文方法的飛行時間離散為一定數量的切比雪夫點，適用于在軌跡規劃的末端時刻對多個控制變量進行處理的終端約束問題。同時，本文方法求解的控制輸入為一條曲線，與文獻[21]中的分段常值輸入相比，本文的控制輸入更接近實際情景。此外，在圖6 b）中，文獻[21]所述方法產生的角度連接線是一條折線，這也是直接導致軌跡波折的原因，而本文方法所求的角度值接近于一條曲線，計算數據相對穩定。因此，本文方法相對文獻[21]中的方法，在算法收斂和生成平滑軌跡的問題上更具有優勢。

4結語

粒子群算法在軌跡規劃過程中將無人機作為質點考慮，因此求解結果只包含位置信息，求解維度比較單一，然而在實際飛行過程中，還需要涉及無人機的角度、角速度等參數信息。針對這一不足之處，本文提出通過求解無人機的運動學模型，得出各個運動學參數，使傳統的僅包含位置的三維軌跡規劃擴展為包含飛行時間和多種狀態約束的四維軌跡規劃，在提升求解維度的同時，避免了數值法計算的復雜性。

本文采用的運動學模型還較為簡單，今后尚需在現有工作基礎上繼續進行深入研究，建立更合適的四旋翼無人機運動學模型，并探索更精確、快速的軌跡規劃求解方法。

參考文獻/References：

[1]GEORGE J，SUJIT P B，SOUSA J B.Search strategies for multiple UAV search and destroy missions[J].Journal of Intelligent & Robotic Systems，2011，61（1）：355-367.

[2]WAI R J，PRASETIA A S.Adaptive neural network control and optimal path planning of UAV surveillance system with energy consumption prediction[J].IEEE Access，2019，7：126137-126153.

[3]TOMIC T，SCHMID K，LUTZ P，et al.Toward a fully autonomous UAV：Research platform for indoor and outdoor urban search and rescue[J].IEEE Robotics & Automation Magazine，2012，19（3）：46-56.

[4]PEHLIVANOGLU Y V.A new vibrational genetic algorithm enhanced with a voronoi diagram for path planning of autonomous UAV[J].Aerospace Science and Technology，2012，16（1）：47-55.

[5]何燕.基于動態加權A*算法的無人機航跡規劃[J].河北科技大學學報，2018，39（4）：349-355.HE Yan.UAV route planning based on improved dynamic weighted A* algorithm[J].Journal of Hebei University of Science and Techno-logy，2018，39（4）：349-355.

[6]BAUMANN M，LONARD S，CROFT E A，et al.Path planning for improved visibility using a probabilistic road map[J].IEEE Transactions on Robotics，2010，26（1）：195-200.

[7]KOTHARI M，POSTLETHWAITE I.A probabilistically robust path planning algorithm for UAVs using rapidly-exploring random trees[J].Journal of Intelligent & Robotic Systems，2013，71（2）：231-253.

[8]甄然，甄士博，吳學禮.一種基于人工勢場的無人機航跡規劃算法[J].河北科技大學學報，2017，38（3）：278-284.ZHEN Ran，ZHEN Shibo，WU Xueli.An improved route planning algorithm for unmanned aerial vehicle based on artificial potential field[J].Journal of Hebei University of Science and Technology，2017，38（3）：278-284.

[9]MA Haiping，SHEN Shigen，YU Mei，et al.Multi-population techniques in nature inspired optimization algorithms：A comprehensive survey[J].Swarm and Evolutionary Computation，2019，44：365-387.

[10]趙新秋，段思雨，馬學敏.基于閾值搜索的多目標人工蜂群算法[J].控制與決策，2020，35（8）：1793-1802.ZHAO Xinqiu，DUAN Siyu，MA Xuemin.A multi-objective artificial bee colony based on limit search strategy[J].Control and Decision，2020，35（8）：1793-1802.

[11]張斌，錢正祥.基于蟻群算法的無人機航跡規劃技術及研究現狀[J].戰術導彈技術，2012（4）：58-62.ZHANG Bin，QIAN Zhengxiang.Research status of route planning of UAV based on ant colony algorithm[J].Tactical Missile Techno-logy，2012（4）：58-62.

[12]FISTER I，FISTER I，YANG Xinshe，et al.A comprehensive review of firefly algorithms[J].Swarm and Evolutionary Computation，2013，13：34-46.

[13]杜云，賈慧敏，邵士凱，等.面向多目標偵察任務的無人機航線規劃[J].控制與決策，2021，36（5）：1191-1198.DU Yun，JIA Huimin，SHAO Shikai，et al.UAV trajectory planning for multi-target reconnaissance missions[J].Control and Decision，2021，36（5）：1191-1198.

[14]SHAO Shikai，PENG Yu，HE Chenglong，et al.Efficient path planning for UAV formation via comprehensively improved particle swarm optimization[J].ISA Transactions，2020，97：415-430.

[15]LEE W，KIM D E.Adaptive approach to regulate task distribution in swarm robotic systems[J].Swarm and Evolutionary Computation，2019，44：1108-1118.

[16]KARIMI J，POURTAKDOUST S H.Optimal maneuver-based motion planning over terrain and threats using a dynamic hybrid PSO algorithm[J].Aerospace Science and Technology，2013，26（1）：60-71.

[17]HUANG Chen，FEI Jiyou.UAV path planning based on particle swarm optimization with global best path competition[J].International Journal of Pattern Recognition and Artificial Intelligence，2018，32（6）.DOI：10.1142/S0218001418590085.

[18]PHUNG M D，QUACH C H，DINH T H，et al.Enhanced discrete particle swarm optimization path planning for UAV vision-based surface inspection[J].Automation in Construction，2017，81：25-33.

[19]王荔豪，王愛華，柯楠，等.基于Dubins曲線的高超聲速飛行器路徑規劃[J].彈箭與制導學報，2015，35（3）：41-43.WANG Lihao，WANG Aihua，KE Nan，et al.Route planning for hypersonic vehicle based on Dubins curves[J].Journal of Projectiles，Rockets，Missiles and Guidance，2015，35（3）：41-43.

[20]ZHANG Limin，GAO Haitao，CHEN Zengqiang，et al.Multi-objective global optimal parafoil homing trajectory optimization via Gauss pseudospectral method[J].Nonlinear Dynamics，2013，72（1）：1-8.

[21]KAMEL M A，YU Xiang，ZHANG Youmin.Real-time fault-tolerant formation control of multiple WMRs based on hybrid GA-PSO algorithm[J].IEEE Transactions on Automation Science and Engineering，2021，18（3）：1263-1276.

[22]WU Yu.Coordinated path planning for an unmanned aerial-aquatic vehicle （UAAV） and an autonomous underwater vehicle （AUV） in an underwater target strike mission[J].Ocean Engineering，2019，182：162-173.

[23]SHEN Chao，SHI Yang，BUCKHAM B.Integrated path planning and tracking control of an AUV：A unified receding horizon optimization approach[J].IEEE/ASME Transactions on Mechatronics，2017，22（3）：1163-1173.

[24]吳宇，蘇析超，崔佳鵬，等.USV&AUV水下目標協同搜索與打擊航跡規劃[J].控制與決策，2021，36（4）：825-834.WU Yu，SU Xichao，CUI Jiapeng，et al.Coordinated path planning of USV&AUV for an underwater target[J].Control and Decision，2021，36（4）：825-834.

[25]FAHROO F，ROSS I M.Direct trajectory optimization by a Chebyshev pseudospectral method[J].Journal of Guidance，Control，and Dynamics，2002，25（1）：160-166.

[26]TIAN Dongping，SHI Zhongzhi.MPSO：Modified particle swarm optimization and its applications[J].Swarm and Evolutionary Computation，2018，41：49-68.

[27]YAN Yuting，ZHANG Ru，WANG Ji，et al.Modified PSO algorithms with “Request and Reset” for leak source localization using multiple robots[J].Neurocomputing，2018，292：82-90.

[28]曹詩卉，時滿宏，亓迎川，等.基于混沌粒子群算法的無人機航跡規劃[J].空軍預警學院學報，2016，30（6）：418-421.CAO Shihui，SHI Manhong，QI Yingchuan，et al.Path planning of UAV based on chaotic particle swarm optimization algorithm[J].Journal of Air Force Early Warning Academy，2016，30（6）：418-421.

[29]CHAI Runqi，SAVVARIS A，TSOURDOS A.Violation learning differential evolution-based hp-adaptive pseudospectral method for trajectory optimization of space maneuver vehicle[J].IEEE Transactions on Aerospace and Electronic Systems，2017，53（4）：2031-2044.