邵士凱　石偉龍　杜云

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

關(guān)鍵詞：航空、航天科學(xué)技術(shù)基礎(chǔ)學(xué)科其他學(xué)科;軌跡規(guī)劃;無人機;粒子群算法;運動學(xué)模型;曲線擬合

中圖分類號：TN958.98文獻標(biāo)識碼：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

隨著科技的不斷創(chuàng)新，無人機已經(jīng)廣泛應(yīng)用于軍事和民用任務(wù)，如情報、監(jiān)視、偵察、救援和商業(yè)表演[1-3]。作為無人機研究領(lǐng)域的重要方向之一，無人機軌跡規(guī)劃備受關(guān)注。

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

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

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

1問題描述與模型建立

1.1問題描述

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

1.2無人機運動學(xué)模型的建立

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

2求解策略

2.1配點分布

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

2.2粒子群算法

2.2.1公式描述

2.2.2自適應(yīng)參數(shù)調(diào)整

2.2.3混沌初始化

2.3運動學(xué)模型的求解

2.3.1軌跡規(guī)劃評價函數(shù)

2.3.2公式描述

2.3.3求解策略與步驟

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

2.3.4求解流程

3仿真結(jié)果與分析

具體仿真環(huán)境：CPU為intel Corei5處理器，內(nèi)存為8 GB，操作系統(tǒng)為windows 7 64位，主頻2.1 GHz，在MATLAB R2016b環(huán)境下實現(xiàn)無人機軌跡規(guī)劃。

3.1仿真條件設(shè)定

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

3.2軌跡規(guī)劃仿真結(jié)果與分析

3.3仿真比較

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

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

4結(jié)語

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

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

參考文獻/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]何燕.基于動態(tài)加權(quán)A*算法的無人機航跡規(guī)劃[J].河北科技大學(xué)學(xué)報，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]甄然，甄士博，吳學(xué)禮.一種基于人工勢場的無人機航跡規(guī)劃算法[J].河北科技大學(xué)學(xué)報，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]趙新秋，段思雨，馬學(xué)敏.基于閾值搜索的多目標(biāo)人工蜂群算法[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]張斌，錢正祥.基于蟻群算法的無人機航跡規(guī)劃技術(shù)及研究現(xiàn)狀[J].戰(zhàn)術(shù)導(dǎo)彈技術(shù)，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，F(xiàn)ISTER I，YANG Xinshe，et al.A comprehensive review of firefly algorithms[J].Swarm and Evolutionary Computation，2013，13：34-46.

[13]杜云，賈慧敏，邵士凱，等.面向多目標(biāo)偵察任務(wù)的無人機航線規(guī)劃[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，F(xiàn)EI 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曲線的高超聲速飛行器路徑規(guī)劃[J].彈箭與制導(dǎo)學(xué)報，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水下目標(biāo)協(xié)同搜索與打擊航跡規(guī)劃[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]曹詩卉，時滿宏，亓迎川，等.基于混沌粒子群算法的無人機航跡規(guī)劃[J].空軍預(yù)警學(xué)院學(xué)報，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.