针对无人机集群轨迹规划高维强耦合特征导致计算复杂度高的难题，提出了动态优先级解耦的序列凸规划方法（DPD-SCP），将耦合的集群轨迹规划问题拆分为若干单机凸规划子问题，通过分布式求解提高集群轨迹规划的计算效率与可扩展性。设计飞行时间驱动的动态优先级解耦机制，降低飞行时间短无人机优先级，挖掘其轨迹调整潜力，消除集群相互规避导致的迭代振荡问题，提升集群轨迹迭代的收敛速度。定制时间一致约束更新准则，避免集群飞行时间非正常增长情况，并理论证明了DPD-SCP方法能够生成满足动力学、避碰与时间一致约束的集群轨迹。仿真结果表明：所提的DPD-SCP方法的求解效率显著优于耦合SCP、串行优先级解耦SCP以及并行解耦SCP方法。
In this paper, a Dynamic-Priority-Decoupled Sequential Convex Programming method (DPD-SCP) is proposed to alleviate the high-computational complexity burden for UAV swarm trajectory planning caused by high-dimensional and strong-coupling features. DPD-SCP splits a coupled swarm trajectory planning problem into several single-UAV convex programming subproblems, and the computational efficiency and scalability are enhanced by utilizing distributed computation. The flight-time-driven dynamic priority decoupled mechanism is designed to improve the convergence rate of swarm trajectory iterations. In this decoupled mechanism, the priority of UAVs with short flight time is lowered to explore the UAV's trajectory adjustment potential and eliminate the oscillation problem due to mutual avoidance of swarms. The time-consistency constraint update criterion is customized to avoid abnormal growth of swarm flight time. Furthermore, it is theoretically validated that DPD-SCP can generate the swarm trajectories that can satisfy the constraints of dynamics, collision avoidance, and time consistency. The simulation results show that the efficiency of DPD-SCP is significantly higher than that of the coupled SCP, serial-priority-decoupled SCP, and parallel-decoupled SCP methods.
[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] MOHANAN M G, SALGOANKAR A. A survey of robotic motion planning in dynamic environments[J].Robotics and Autonomous Systems, 2018, 100:171-185.
[3] GOERZEN C, KONG Z, METTLER B. A survey of motion planning algorithms from the perspective of autonomous UAV guidance[J].Journal of Intelligent and Robotic Systems, 2009, 57(1-4):65-100.
[4] MORGAN D, CHUNG S J, HADAEGH F Y. Swarm assignment and trajectory optimization using variable-swarm, distributed auction assignment and model predictive control[C]//AIAA Guidance, Navigation, and Control Conference. Reston:AIAA, 2015.
[5] GREGOIRE J, AČG ÁP M, FRAZZOLI E. Locally-optimal multi-robot navigation under delaying disturbances using homotopy constraints[J].Autonomous Robots, 2018, 42(4):895-907.
[6] VAN DEN BERG J P, OVERMARS M H. Prioritized motion planning for multiple robots[C]//2005 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway:IEEE Press, 2005:430-435.
[7] ROBINSON D R, MAR R T, ESTABRIDIS K, et al. An efficient algorithm for optimal trajectory generation for heterogeneous multi-agent systems in non-convex environments[J].IEEE Robotics and Automation Letters, 2018, 3(2):1215-1222.
[8] VELAGAPUDI P, SYCARA K, SCERRI P. Decentralized prioritized planning in large multirobot teams[C]//2010 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway:IEEE Press, 2010:4603-4609.
[9] AČG ÁP M, NOVÁK P, KLEINER A, et al. Prioritized planning algorithms for trajectory coordination of multiple mobile robots[J].IEEE Transactions on Automation Science and Engineering, 2015, 12(3):835-849.
[10] AUGUGLIARO F, SCHOELLIG A P, D'ANDREA R. Generation of collision-free trajectories for a quadrocopter fleet:A sequential convex programming approach[C]//2012 IEEE/RSJ International Conference on Intelligent Robots and Systems. Piscataway:IEEE Press, 2012:1917-1922.
[11] 王祝, 刘莉, 龙腾, 等. 基于罚函数序列凸规划的多无人机轨迹规划[J].航空学报, 2016, 37(10):3149-3158. WANG Z, LIU L, LONG T, et al. Trajectory planning for multi-UAVs using penalty sequential convex pro-gramming[J].Acta Aeronautica et Astronautica Sinica, 2016, 37(10):3149-3158(in Chinese).
[12] CHEN Y F, CUTLER M, HOW J P. Decoupled multiagent path planning via incremental sequential convex programming[C]//2015 IEEE International Conference on Robotics and Automation (ICRA). Piscataway:IEEE Press, 2015:5954-5961.
[13] 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.
[14] 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.
[15] LU P. Convex-concave decomposition of nonlinear equality constraints in optimal control[J].Journal of Guidance, Control, and Dynamics, 2020, 44(1):4-14.
[16] FOUST R, CHUNG S J, HADAEGH F Y. Optimal guidance and control with nonlinear dynamics using sequential convex programming[J].Journal of Guidance, Control, and Dynamics, 2019, 43(4):633-644.
[17] BETTS J T. Practical methods for optimal control and estimation using nonlinear programming[M]. Philadelphia:Society for Industrial and Applied Mathematics, 2010:279-282.
[18] JONGEN H T, MEER K, TRIESCH E. Optimization theory[M]. Boston:Kluwer Academic Publishers, 2004:5.
[19] LIU X F, LU P. Solving nonconvex optimal control problems by convex optimization[J].Journal of Guidance, Control, and Dynamics, 2014, 37(3):750-765.
[20] MAO Y Q, DUERI D, SZMUK M, et al. Successive convexification of non-convex optimal control problems with state constraints[J].IFAC-PapersOnLine, 2017, 50(1):4063-4069.
[21] STURM J F. Using SeDuMi 1.02, A Matlab toolbox for optimization over symmetric cones[J].Optimization Methods and Software, 1999, 11(1-4):625-653.
[22] GRANT M, BOYD S. CVX:Matlab software for disciplined convex programming, version 2.0[EB/OL]. (2011-06-17)[2015-10-09]. http://cvxr.com/cvx.
CJA