徐廣通，鄒汝平，王 祝，孫景亮，龍 騰

基于滾動規(guī)劃框架的多無人機協(xié)同軌跡快速生成方法

徐廣通1,2，鄒汝平3，王 祝4，孫景亮1,2，龍 騰1,2

（1. 北京理工大學(xué)宇航學(xué)院，北京 100081；2. 飛行器動力學(xué)與控制教育部重點實驗室，北京 100081；3. 西安現(xiàn)代控制技術(shù)研究所，西安 710065；4. 華北電力大學(xué)自動化系，保定 071003）

面向多無人機協(xié)同軌跡快速規(guī)劃的需求，提出基于滾動規(guī)劃框架軌跡高效求解方法，將協(xié)同軌跡規(guī)劃問題分解為若干短時域規(guī)劃子問題，通過降低求解維度，提升協(xié)同軌跡規(guī)劃效率。在考慮飛行性能與避障/避撞約束的基礎(chǔ)上，設(shè)計了終端狀態(tài)啟發(fā)的目標(biāo)函數(shù)，確保滾動規(guī)劃結(jié)果能夠準(zhǔn)確收斂到終端狀態(tài)。定制基于剩余距離的速度調(diào)節(jié)機制，通過動態(tài)調(diào)速保證規(guī)劃結(jié)果的時間一致性。使用序列凸優(yōu)化方法求解短時域軌跡規(guī)劃問題，進一步提升規(guī)劃效率。典型任務(wù)想定下的數(shù)值仿真驗證結(jié)果表明：所提方法能夠在滿足避障/避撞與性能約束的前提下，快速生成短時域協(xié)同軌跡（耗時小于1s），具有工程實用性。

多無人機；協(xié)同軌跡規(guī)劃；滾動規(guī)劃框架；凸規(guī)劃；速度調(diào)節(jié)機制

1 引 言

近年來，多無人機協(xié)同理論得到了廣泛的研究，已在區(qū)域搜索、目標(biāo)追蹤等軍事和包裹運輸、農(nóng)業(yè)植保等民事領(lǐng)域應(yīng)用[1-3]。軌跡規(guī)劃作為多無人機協(xié)同的關(guān)鍵技術(shù)之一，可生成滿足動力學(xué)、邊界、初始及終端狀態(tài)、威脅規(guī)避、機間避碰等約束的軌跡，引導(dǎo)無人機安全高效執(zhí)行既定任務(wù)[4]。

協(xié)同軌跡規(guī)劃因其高維特征，傳統(tǒng)的軌跡規(guī)劃方法（混合整數(shù)規(guī)劃[5]、快速擴展隨機樹[6]、偽譜法[7]）難以對其進行快速求解。滾動時域規(guī)劃可將原軌跡規(guī)劃問題分解為短時域規(guī)劃子問題，通過滾動求解一系列低維度問題，提升規(guī)劃效率。滾動時域規(guī)劃已成功應(yīng)用于航天器編隊構(gòu)成[8]、火箭著陸制導(dǎo)[9]及多飛行器協(xié)同[10]等領(lǐng)域。Kuwata等人[11]首先使用滾動規(guī)劃架構(gòu)求解多無人機軌跡規(guī)劃問題，并利用混合整數(shù)規(guī)劃方法對短時域軌跡規(guī)劃問題進行求解，但是隨著無人機數(shù)量增加，該方法求解效率明顯降低。Van Parys等人[12]提出分布式滾動規(guī)劃方法，使用交替方向乘子法求解短時域規(guī)劃問題，然而該方法沒有考慮機間避碰約束。Morgan等人[8]首先結(jié)合滾動規(guī)劃框架與序列凸優(yōu)化方法，高效求解大規(guī)模航天器軌跡規(guī)劃問題。在此基礎(chǔ)上，Morgan等人[13]將滾動規(guī)劃方法與序列凸優(yōu)化應(yīng)用于多旋翼無人機協(xié)同軌跡規(guī)劃，Morgan所提方法逐步降低滾動時域長度，在前期滾動規(guī)劃中仍需考慮較長的時域，因此降低了整體規(guī)劃效率。Luis等人[14]發(fā)展出改進的滾動規(guī)劃方法，可將多旋翼無人機軌跡問題拆解為若干短時域規(guī)劃問題，但該方法使用二階積分動力學(xué)模型，難以處理復(fù)雜動力學(xué)約束下無人機協(xié)同軌跡問題，無法保證多無人機時間一致性。

為高效求解多無人機協(xié)同軌跡規(guī)劃問題，本文提出基于滾動規(guī)劃框架的軌跡快速求解方法，構(gòu)建一系列短時域規(guī)劃問題，并發(fā)展終端狀態(tài)啟發(fā)的目標(biāo)函數(shù)，保證滾動規(guī)劃逐漸收斂至終端狀態(tài)?？紤]無人機飛行動力學(xué)特性，開發(fā)基于剩余距離的速度調(diào)節(jié)機制，減少剩余距離短的無人機的飛行速度上邊界，保證多無人機同時抵達終端位置。使用序列凸優(yōu)化方法對短時域規(guī)劃問題進行求解，進一步降低求解耗時。設(shè)計編隊重構(gòu)典型任務(wù)想定，開展數(shù)值仿真試驗，試驗結(jié)果表明所提方法可快速生成滿足飛行約束的軌跡，驗證了所提方法的合理性和時效性。

2 問題描述

考慮飛行動力學(xué)、狀態(tài)與控制邊界、初始與終端狀態(tài)、威脅規(guī)避及機間避碰約束，多無人機軌跡規(guī)劃可生成時間最優(yōu)軌跡，引導(dǎo)無人機快速同時抵達任務(wù)區(qū)域。本節(jié)通過離散化和凸化[15]，將協(xié)同軌跡規(guī)劃問題建立為凸優(yōu)化問題。

2.1 無人機動力學(xué)模型

其中：p,i與p,i表示無人機水平位置；p,i表示無人機飛行高度；為重力加速度。

2.2 約束條件

無人機飛行軌跡需滿足初始與終端狀態(tài)約束、狀態(tài)與控制邊界約束，分別如式(2)與式(3)：

其中：,0、,f、min、max、min與max表示初始狀態(tài)、終端狀態(tài)、狀態(tài)下邊界、狀態(tài)上邊界、控制下邊界以及控制上邊界；0與f分別表示初始和終端時間。

無人機在飛行過程中需規(guī)避環(huán)境中的威脅。本文將任務(wù)環(huán)境中存在的探測雷達、防空導(dǎo)彈陣地等威脅建立為半球形威脅，并建立威脅規(guī)避約束如式(4)，保證無人機與威脅保持一定的安全距離。

2.3 凸優(yōu)化問題

使用離散化和凸化技術(shù)，構(gòu)建協(xié)同軌跡規(guī)劃凸優(yōu)化問題。將初始與終端狀態(tài)約束、狀態(tài)與控制邊界約束分別離散為式(6)和式(7)，其中表示離散區(qū)間個數(shù)。

威脅規(guī)避約束轉(zhuǎn)化為如式(9)和式(10)的仿射約束，保證離散點之間仍可滿足威脅規(guī)避約束：

機間避碰約束凸化為如式(11)的仿射約束：

通過離散化與凸化，構(gòu)建協(xié)同軌跡規(guī)劃凸優(yōu)化問題（P1）如式(12)：

3 基于滾動規(guī)劃框架的軌跡生成方法

本節(jié)介紹基于滾動規(guī)劃框架的軌跡快速規(guī)劃方法，設(shè)計終端狀態(tài)啟發(fā)的目標(biāo)函數(shù)，將P1分解為若干短時域規(guī)劃問題，并通過基于剩余距離的速度調(diào)整機制保證多無人機軌跡時間一致性。

3.1 滾動規(guī)劃問題

P1轉(zhuǎn)化為短時域凸優(yōu)化問題（P2），如式(13)：

其中：h表示終端狀態(tài)啟發(fā)項權(quán)重系數(shù)；k表示滾動規(guī)劃時域索引；表示第k次滾動規(guī)劃的初始狀態(tài)，即第滾動規(guī)劃的終端狀態(tài)。

3.2 基于剩余距離的速度調(diào)節(jié)機制

3.3 算法流程

基于滾動框架的軌跡快速生成方法偽代碼如算法1所示，具體流程敘述如下：

步驟2（第3和4行）：構(gòu)建短時域凸優(yōu)化問題。根據(jù)無人機剩余距離判斷是否調(diào)整時域長度，并利用式(14)計算新的滾動時域長度。利用算法參數(shù)和任務(wù)信息構(gòu)建如式(13)所示的短時域凸優(yōu)化問題(P2)。

步驟3（第5~7行）：求解短時域凸優(yōu)化問題。使用凸優(yōu)化方法[17]求解(P2)獲得當(dāng)前時域的協(xié)同軌跡，并更新無人機飛行速度上邊界，將當(dāng)前規(guī)劃時域終點設(shè)置為下一規(guī)劃時域的起始點。

步驟4（第2和8行）：判斷算法收斂。算法不斷滾動求解，直到軌跡抵達終點，即滿足式(16)的收斂條件：

算法1 基于滾動規(guī)劃框架的軌跡快速生成方法 輸入：初始/終端狀態(tài)（s0, sf），初始控制u0邊界約束B，威脅集合O，收斂誤差，←1輸出：無人機軌跡 1, i=1, 2,, N 2while不滿足Eq. 3使用公式(14)計算得到TH 4P2←構(gòu)建短時域凸優(yōu)化問題(, B, O, TH) 5←求解P2 6使用公式(15)更新無人機速度上邊界 7更新滾動時域； 8end while

4 仿真結(jié)果及分析

本節(jié)開展典型想定下數(shù)值仿真試驗，通過對比Morgan設(shè)計滾動規(guī)劃方法[13]，驗證本文所提方法的合理性和時效性?；贛ATLAB R2017a環(huán)境進行數(shù)值仿真，使用凸優(yōu)化數(shù)值優(yōu)化器SeDuMi[18]求解短時域凸優(yōu)化問題(P2)，計算平臺選用配置Intel Core i7-7660 2.50GHz處理器和16GB內(nèi)存的筆記本電腦。

4.1 仿真想定及參數(shù)設(shè)置

設(shè)計多無人機編隊重構(gòu)想定，要求無人機從一字形編隊變換為雁形編隊，無人機初始與終端位置如表1所示，威脅位置如表2所示。無人機初始和終端速度、初始和終端航向角、初始和終端航跡傾角均為 0。無人機之間安全距離限制為200 m，狀態(tài)與控制邊界約束如式(17)：

表1 無人機初始/終端位置

表2 威脅信息

使用所提算法求解多無人機協(xié)同軌跡規(guī)劃問題，滾動時域離散點數(shù)量初始值K為6，算法收斂誤差如式(18)。Morgan所提滾動規(guī)劃方法參數(shù)設(shè)置同本文方法。

4.2 協(xié)同軌跡規(guī)劃結(jié)果

軌跡規(guī)劃結(jié)果如圖2和圖3所示，其中實線和虛線表示不同規(guī)劃時域的軌跡，空心圓表示不同時域之間的連接點，紅色實心圓表示威脅。從結(jié)果可以看出，算法通過8次滾動規(guī)劃為9架無人機生成滿足威脅規(guī)避約束的協(xié)同軌跡。

如圖4所示，無人機之間最小距離始終高于安全限制，表明規(guī)劃得到的軌跡滿足機間避碰約束。由于兩側(cè)無人機起始點與終點距離相對較近，速度調(diào)節(jié)機制通過降低相應(yīng)無人機的飛行速度，實現(xiàn)與其他無人機剩余距離的一致，保證了多無人機同時抵達終端位置。因此，數(shù)值仿真結(jié)果驗證了本文所提方法的有效性。

圖2 協(xié)同軌跡規(guī)劃結(jié)果

圖3 短時域軌跡

圖4 無人機之間最小距離

4.3 協(xié)同規(guī)劃效率分析

圖5與圖6分別為本文所提方法與Morgan方法滾動規(guī)劃耗時。隨著無人機數(shù)量增加，兩種方法的規(guī)劃耗時均增加。針對9架無人機協(xié)同軌跡規(guī)劃問題，本文所提方法整體耗時（5.4 s），相比Morgan方法（11.0 s）降低了50.9%，具有明顯的效率優(yōu)勢。隨著滾動規(guī)劃的進行，Morgan方法軌跡規(guī)劃耗時呈下降趨勢，9架無人機短時域滾動規(guī)劃耗時從2.2 s降至0.84 s。然而，本文所提方法的短時域滾動規(guī)劃耗時始終不大于1 s?？紤]無人機最大飛行速度，規(guī)劃時域長度，本文所提滾動求解方法滿足實時在線軌跡規(guī)劃的時效性要求，可根據(jù)動態(tài)變化的態(tài)勢信息進行適應(yīng)性調(diào)整。

圖5 本文所提方法滾動規(guī)劃耗時

圖6 Morgan方法滾動規(guī)劃耗時

5 結(jié) 論

為提升多無人機協(xié)同軌跡規(guī)劃效率，本文提出了基于滾動規(guī)劃框架的協(xié)同軌跡高效求解方法，將整體規(guī)劃問題拆分為一系列短時域規(guī)劃問題，降低規(guī)劃耗時。設(shè)計典型任務(wù)想定，通過數(shù)值仿真試驗對所提方法的有效性進行驗證。得出以下主要結(jié)論。

（1）構(gòu)建短時域軌跡規(guī)劃問題，定制終端狀態(tài)啟發(fā)的目標(biāo)函數(shù)，引導(dǎo)滾動規(guī)劃結(jié)果逐步收斂到終端狀態(tài)。

（2）提出基于剩余距離的速度調(diào)節(jié)機制，調(diào)整無人機飛行速度上邊界，保證多無人機同時抵達任務(wù)區(qū)域。

（3）仿真試驗結(jié)果表明，短時域規(guī)劃耗時小于1 s，具備實時在線軌跡規(guī)劃的應(yīng)用潛力。

[1] Chung S-J, Paranjape A A, Dames P, et al. A survey on aerial swarm robotics [J]. IEEE Transactions on Robotics, 2018, 34 (4): 837-855.

[2] 符文星, 郭行, 閆杰. 智能無人飛行器技術(shù)發(fā)展趨勢綜述 [J]. 無人系統(tǒng)技術(shù), 2019, 2 (4): 31-37.

[3] 張濤, 李清, 張長水, 等. 智能無人自主系統(tǒng)的發(fā)展趨勢 [J]. 無人系統(tǒng)技術(shù), 2018, 1 (1): 11-22.

[4] H?nig W, Preiss J A, Kumar T S, et al. Trajectory planning for quadrotor swarms [J]. IEEE Transactions on Robotics, 2018, 34 (4): 856-869.

[5] Richards A, Schouwenaars T, How J P, et al. Spacecraft trajectory planning with avoidance constraints using mixed-integer linear programming [J]. Journal of Guidance, Control, and Dynamics, 2002, 25 (4): 755-764.

[6] Desaraju V R, How J P. Decentralized path planning for multi-agent teams with complex constraints [J]. Autonomous Robots, 2012, 32 (4): 385-403.

[7] Shirazi A, Ceberio J, Lozano J A. Spacecraft trajectory optimization: A review of models, objectives, approaches and solutions [J]. Progress in Aerospace Sciences, 2018, 102 (2018): 76-98.

[8] Morgan D, Chung S-J, Hadaegh F Y. Model predictive control of swarms of spacecraft using sequential convex programming [J]. Journal of Guidance, Control, and Dynamics, 2014, 37 (6): 1725-1740.

[9] Wang J, Cui N, Wei C. Optimal rocket landing guidance using convex optimization and model predictive control [J]. Journal of Guidance, Control, and Dynamics, 2019, 42 (5): 1078-1092.

[10] Wang Z, Liu L, Long T, et al. Efficient unmanned aerial vehicle formation rendezvous trajectory planning using Dubins path and sequential convex programming [J]. Engineering Optimization, 2019, 51 (8): 1412-1429.

[11] Kuwata Y, How J P. Cooperative distributed robust trajectory optimization using receding horizon MILP [J]. IEEE Transactions on Control Systems Technology, 2010, 19 (2): 423-431.

[12] Van Parys R, Pipeleers G. Distributed MPC for multi-vehicle systems moving in formation [J]. Robotics and Autonomous Systems, 2017, 97 (2017): 144-152.

[13] Morgan D, Subramanian G P, Chung S-J, et al. Swarm assignment and trajectory optimization using variable-swarm, distributed auction assignment and sequential convex programming [J]. The International Journal of Robotics Research, 2016, 35 (10): 1261-1285.

[14] Luis C E, Schoellig A P. Trajectory generation for multiagent point-to-point transitions via distributed model predictive control [J]. IEEE Robotics and Automation Letters, 2019, 4 (2): 375-382.

[15] Liu X, Lu P. Solving nonconvex optimal control problems by convex optimization [J]. Journal of Guidance, Control, and Dynamics, 2014, 37 (3): 750-765.

[16] Wang Z, Liu L, Long T. Minimum-time trajectory planning for multi-unmanned-aerial-vehicle cooperation using sequential convex programming [J]. Journal of Guidance, Control, and Dynamics, 2017, 40 (11): 2976-2982.

[17] Xu G, Long T, Wang Z, et al. Matrix structure driven interior point method for quadrotor real-time trajectory planning [J].

IEEE Access, 2019, 7 (2019): 90941-90953.

[18] Sturm J F. Using SeDuMi 1.02, A MATLAB toolbox for optimization over symmetric cones [J]. Optimization Methods Softwares, 2008, 11 (1-4): 625-653.

Multiple Unmanned Aerial Vehicle Rapid Cooperative Trajectory Generation Method Using Receding Planning Framework

XU Guangtong1,2, ZOU Ruping3, WANG Zhu4, SUN Jingliang1,2, LONG Teng1,2

(1. School of Aerospace Engineering, Beijing Institute of Technology, Beijing 100081, China; 2. Key Laboratory of Dynamics and Control of Flight Vehicle, Ministry of Education, Beijing 100081, China; 3. Xi’an Modern Control Technology Research Institute, Xi’an 710065, China; 4. Department of Automation, North China Electric Power University, Baoding 071003, China)

To improve the efficiency of the multiple unmanned aerial vehicle cooperative trajectory planning, this paper proposes the rapid trajectory generation method using receding planning framework. The cooperative trajectory planning problem is divided into several short-horizon planning subproblems to reduce the problem dimension, which can save the runtime. Considering flight performance and obstacle/collision avoidance constraints, the final-state-heu- ristic objective function is designed to ensure that the solution of receding planning converges to the final state. The surplus-distance-based velocity adjustment mechanism is customized and the flight velocity is adjusted dynamically for guaranteeing the time consistency of cooperative trajectories. To further enhance the computational efficiency, the sequential convex programming method is used to solve the short-horizon trajectory planning problem. The simulation results on typical scenarios show that the proposed method can generate short-horizon cooperative trajectories in less than 1 second subject to obstacle/collision avoidance and flight performance constraints, which demonstrates the engineering practicability of the proposed method.

Multiple Unmanned Aerial Vehicles；Cooperative Trajectory Planning；Receding Planning Framework；Convex Programming；Velocity Adjustment Mechanism

V249.1

A

2096–5915(2021)02–33–07

10.19942/j.issn.2096–5915.2021.2.016

徐廣通，鄒汝平，王 祝，等. 基于滾動規(guī)劃框架的多無人機協(xié)同軌跡快速生成方法[J]. 無人系統(tǒng)技術(shù)，2021，4(2)：33–39.

2020–08–03；

2020–11–15

國家自然科學(xué)基金（61903033, 51675047）；中國博士后科學(xué)基金特別資助（站前）項目（2019TQ0037）

徐廣通（1992–），男，博士研究生，主要研究方向為集群飛行器協(xié)同任務(wù)規(guī)劃與控制。

鄒汝平（1962–），男，博士，研究員，主要研究方向為總體設(shè)計與制導(dǎo)技術(shù)。

王 祝（1991–），男，博士，講師，主要研究方向為集群飛行器協(xié)同任務(wù)規(guī)劃與控制。

孫景亮（1990–），男，博士后，主要研究方向為自適應(yīng)動態(tài)規(guī)劃、飛行器制導(dǎo)與控制。

龍 騰（1982–），男，博士，教授，主要研究方向為飛行器總體設(shè)計、多學(xué)科設(shè)計優(yōu)化理論與應(yīng)用。