基于滚动规划框架的多无人机协同轨迹快速生成方法

2021-05-27徐广通邹汝平孙景亮

无人系统技术 2021年2期

徐广通，邹汝平，王祝，孙景亮，龙腾

徐广通1,2，邹汝平3，王祝4，孙景亮1,2，龙腾1,2

（1. 北京理工大学宇航学院，北京 100081；2. 飞行器动力学与控制教育部重点实验室，北京 100081；3. 西安现代控制技术研究所，西安 710065；4. 华北电力大学自动化系，保定 071003）

面向多无人机协同轨迹快速规划的需求，提出基于滚动规划框架轨迹高效求解方法，将协同轨迹规划问题分解为若干短时域规划子问题，通过降低求解维度，提升协同轨迹规划效率。在考虑飞行性能与避障/避撞约束的基础上，设计了终端状态启发的目标函数，确保滚动规划结果能够准确收敛到终端状态。定制基于剩余距离的速度调节机制，通过动态调速保证规划结果的时间一致性。使用序列凸优化方法求解短时域轨迹规划问题，进一步提升规划效率。典型任务想定下的数值仿真验证结果表明：所提方法能够在满足避障/避撞与性能约束的前提下，快速生成短时域协同轨迹（耗时小于1s），具有工程实用性。

多无人机；协同轨迹规划；滚动规划框架；凸规划；速度调节机制

1 引言

近年来，多无人机协同理论得到了广泛的研究，已在区域搜索、目标追踪等军事和包裹运输、农业植保等民事领域应用[1-3]。轨迹规划作为多无人机协同的关键技术之一，可生成满足动力学、边界、初始及终端状态、威胁规避、机间避碰等约束的轨迹，引导无人机安全高效执行既定任务[4]。

协同轨迹规划因其高维特征，传统的轨迹规划方法（混合整数规划[5]、快速扩展随机树[6]、伪谱法[7]）难以对其进行快速求解。滚动时域规划可将原轨迹规划问题分解为短时域规划子问题，通过滚动求解一系列低维度问题，提升规划效率。滚动时域规划已成功应用于航天器编队构成[8]、火箭着陆制导[9]及多飞行器协同[10]等领域。Kuwata等人[11]首先使用滚动规划架构求解多无人机轨迹规划问题，并利用混合整数规划方法对短时域轨迹规划问题进行求解，但是随着无人机数量增加，该方法求解效率明显降低。Van Parys等人[12]提出分布式滚动规划方法，使用交替方向乘子法求解短时域规划问题，然而该方法没有考虑机间避碰约束。Morgan等人[8]首先结合滚动规划框架与序列凸优化方法，高效求解大规模航天器轨迹规划问题。在此基础上，Morgan等人[13]将滚动规划方法与序列凸优化应用于多旋翼无人机协同轨迹规划，Morgan所提方法逐步降低滚动时域长度，在前期滚动规划中仍需考虑较长的时域，因此降低了整体规划效率。Luis等人[14]发展出改进的滚动规划方法，可将多旋翼无人机轨迹问题拆解为若干短时域规划问题，但该方法使用二阶积分动力学模型，难以处理复杂动力学约束下无人机协同轨迹问题，无法保证多无人机时间一致性。

为高效求解多无人机协同轨迹规划问题，本文提出基于滚动规划框架的轨迹快速求解方法，构建一系列短时域规划问题，并发展终端状态启发的目标函数，保证滚动规划逐渐收敛至终端状态。考虑无人机飞行动力学特性，开发基于剩余距离的速度调节机制，减少剩余距离短的无人机的飞行速度上边界，保证多无人机同时抵达终端位置。使用序列凸优化方法对短时域规划问题进行求解，进一步降低求解耗时。设计编队重构典型任务想定，开展数值仿真试验，试验结果表明所提方法可快速生成满足飞行约束的轨迹，验证了所提方法的合理性和时效性。

2 问题描述

考虑飞行动力学、状态与控制边界、初始与终端状态、威胁规避及机间避碰约束，多无人机轨迹规划可生成时间最优轨迹，引导无人机快速同时抵达任务区域。本节通过离散化和凸化[15]，将协同轨迹规划问题建立为凸优化问题。

2.1 无人机动力学模型

其中：p,i与p,i表示无人机水平位置；p,i表示无人机飞行高度；为重力加速度。

2.2 约束条件

无人机飞行轨迹需满足初始与终端状态约束、状态与控制边界约束，分别如式(2)与式(3)：

其中：,0、,f、min、max、min与max表示初始状态、终端状态、状态下边界、状态上边界、控制下边界以及控制上边界；0与f分别表示初始和终端时间。

无人机在飞行过程中需规避环境中的威胁。本文将任务环境中存在的探测雷达、防空导弹阵地等威胁建立为半球形威胁，并建立威胁规避约束如式(4)，保证无人机与威胁保持一定的安全距离。

2.3 凸优化问题

使用离散化和凸化技术，构建协同轨迹规划凸优化问题。将初始与终端状态约束、状态与控制边界约束分别离散为式(6)和式(7)，其中表示离散区间个数。

威胁规避约束转化为如式(9)和式(10)的仿射约束，保证离散点之间仍可满足威胁规避约束：

机间避碰约束凸化为如式(11)的仿射约束：

通过离散化与凸化，构建协同轨迹规划凸优化问题（P1）如式(12)：

3 基于滚动规划框架的轨迹生成方法

本节介绍基于滚动规划框架的轨迹快速规划方法，设计终端状态启发的目标函数，将P1分解为若干短时域规划问题，并通过基于剩余距离的速度调整机制保证多无人机轨迹时间一致性。

3.1 滚动规划问题

P1转化为短时域凸优化问题（P2），如式(13)：

其中：h表示终端状态启发项权重系数；k表示滚动规划时域索引；表示第k次滚动规划的初始状态，即第滚动规划的终端状态。

3.2 基于剩余距离的速度调节机制

3.3 算法流程

基于滚动框架的轨迹快速生成方法伪代码如算法1所示，具体流程叙述如下：

步骤2（第3和4行）：构建短时域凸优化问题。根据无人机剩余距离判断是否调整时域长度，并利用式(14)计算新的滚动时域长度。利用算法参数和任务信息构建如式(13)所示的短时域凸优化问题(P2)。

步骤3（第5~7行）：求解短时域凸优化问题。使用凸优化方法[17]求解(P2)获得当前时域的协同轨迹，并更新无人机飞行速度上边界，将当前规划时域终点设置为下一规划时域的起始点。

步骤4（第2和8行）：判断算法收敛。算法不断滚动求解，直到轨迹抵达终点，即满足式(16)的收敛条件：

算法1 基于滚动规划框架的轨迹快速生成方法输入：初始/终端状态（s0, sf），初始控制u0边界约束B，威胁集合O，收敛误差，←1输出：无人机轨迹 1, i=1, 2,, N 2while不满足Eq. 3使用公式(14)计算得到TH 4P2←构建短时域凸优化问题(, B, O, TH) 5←求解P2 6使用公式(15)更新无人机速度上边界 7更新滚动时域； 8end while

4 仿真结果及分析

本节开展典型想定下数值仿真试验，通过对比Morgan设计滚动规划方法[13]，验证本文所提方法的合理性和时效性。基于MATLAB R2017a环境进行数值仿真，使用凸优化数值优化器SeDuMi[18]求解短时域凸优化问题(P2)，计算平台选用配置Intel Core i7-7660 2.50GHz处理器和16GB内存的笔记本电脑。

4.1 仿真想定及参数设置

设计多无人机编队重构想定，要求无人机从一字形编队变换为雁形编队，无人机初始与终端位置如表1所示，威胁位置如表2所示。无人机初始和终端速度、初始和终端航向角、初始和终端航迹倾角均为 0。无人机之间安全距离限制为200 m，状态与控制边界约束如式(17)：

表1 无人机初始/终端位置

表2 威胁信息

使用所提算法求解多无人机协同轨迹规划问题，滚动时域离散点数量初始值K为6，算法收敛误差如式(18)。Morgan所提滚动规划方法参数设置同本文方法。

4.2 协同轨迹规划结果

轨迹规划结果如图2和图3所示，其中实线和虚线表示不同规划时域的轨迹，空心圆表示不同时域之间的连接点，红色实心圆表示威胁。从结果可以看出，算法通过8次滚动规划为9架无人机生成满足威胁规避约束的协同轨迹。

如图4所示，无人机之间最小距离始终高于安全限制，表明规划得到的轨迹满足机间避碰约束。由于两侧无人机起始点与终点距离相对较近，速度调节机制通过降低相应无人机的飞行速度，实现与其他无人机剩余距离的一致，保证了多无人机同时抵达终端位置。因此，数值仿真结果验证了本文所提方法的有效性。

图2 协同轨迹规划结果

图3 短时域轨迹

图4 无人机之间最小距离

4.3 协同规划效率分析

图5与图6分别为本文所提方法与Morgan方法滚动规划耗时。随着无人机数量增加，两种方法的规划耗时均增加。针对9架无人机协同轨迹规划问题，本文所提方法整体耗时（5.4 s），相比Morgan方法（11.0 s）降低了50.9%，具有明显的效率优势。随着滚动规划的进行，Morgan方法轨迹规划耗时呈下降趋势，9架无人机短时域滚动规划耗时从2.2 s降至0.84 s。然而，本文所提方法的短时域滚动规划耗时始终不大于1 s。考虑无人机最大飞行速度，规划时域长度，本文所提滚动求解方法满足实时在线轨迹规划的时效性要求，可根据动态变化的态势信息进行适应性调整。

图5 本文所提方法滚动规划耗时

图6 Morgan方法滚动规划耗时

5 结论

为提升多无人机协同轨迹规划效率，本文提出了基于滚动规划框架的协同轨迹高效求解方法，将整体规划问题拆分为一系列短时域规划问题，降低规划耗时。设计典型任务想定，通过数值仿真试验对所提方法的有效性进行验证。得出以下主要结论。

（1）构建短时域轨迹规划问题，定制终端状态启发的目标函数，引导滚动规划结果逐步收敛到终端状态。

（2）提出基于剩余距离的速度调节机制，调整无人机飞行速度上边界，保证多无人机同时抵达任务区域。

（3）仿真试验结果表明，短时域规划耗时小于1 s，具备实时在线轨迹规划的应用潜力。

[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] 符文星, 郭行, 闫杰. 智能无人飞行器技术发展趋势综述 [J]. 无人系统技术, 2019, 2 (4): 31-37.

[3] 张涛, 李清, 张长水, 等. 智能无人自主系统的发展趋势 [J]. 无人系统技术, 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

2096–5915(2021)02–33–07

10.19942/j.issn.2096–5915.2021.2.016

徐广通，邹汝平，王祝，等. 基于滚动规划框架的多无人机协同轨迹快速生成方法[J]. 无人系统技术，2021，4(2)：33–39.

2020–08–03；

2020–11–15

国家自然科学基金（61903033, 51675047）；中国博士后科学基金特别资助（站前）项目（2019TQ0037）

徐广通（1992–），男，博士研究生，主要研究方向为集群飞行器协同任务规划与控制。