安全走廊约束的无人机轨迹序列凸优化方法

doi:10.16182/j.issn1004731x.joss.23-1026

摘要/Abstract

摘要：

针对无人机时间最优轨迹规划的序列凸优化方法存在初值敏感与收敛能力弱等不足，提出了一种基于安全飞行走廊约束的序列凸优化方法（safe flight corridor-sequential convex programming，SFC-SCP）。通过前端路径规划获得避障路径，用以构建安全飞行走廊，即为每个轨迹点构建一个不含障碍的凸多边形安全区域，将非凸避障约束转换为线性不等式约束，以提高规划收敛能力。后端使用序列凸优化方法将安全走廊约束下的非线性轨迹优化问题转换为一系列凸规划子问题进行逐次求解，降低问题的复杂度。其中，轨迹优化的初始解基于前端规划的避障路径构建，以缓解局部优化算法的初值敏感问题。仿真对比结果表明SFC-SCP算法在规划成功率、计算时效性和飞行时间最优性等方面相比于SCP有较大提升。

关键词: 四旋翼, 轨迹规划, 安全飞行走廊, 序列凸优化, 时间最优轨迹

Abstract:

To address the issues of sensitivity to initial values and weak convergence of sequential convex programming(SCP) based time-optimal trajectory planning for UAVs, a SCP method using safety flight corridor, denoted as SFC-SCP(safe flight corridor-sequential convex programming) is proposed. According to the obstacle avoidance path obtained from the front-end path planning, a safe flight corridor is constructed by forming a convex polygon safe flight area without obstacles for each trajectory point. The non-convex obstacle avoidance constraint is converted into linear inequality constraints to improve convergence ability. The rear-end SCP method is used to transform the nonlinear trajectory optimization problem under the safety corridor constraint into a series of convex sub-problems for successive solutions, reducing the complexity of the problem. Among SCP iteration, the initial solution of trajectory optimization is constructed based on the obstacle avoidance path planned by the front-end to alleviate initial sensitivity of the local optimization algorithm. The simulation comparison results show that SFC-SCP algorithm has better performance than SCP in terms of planning success rate, computational efficiency, and flight time optimality.

Key words: quadrotor, trajectory planning, safe flight corridor(SFC), sequential convex programming(SCP), time optimal trajectory

中图分类号:

TP242

王祝,张振鹏,张梦通等 . 安全走廊约束的无人机轨迹序列凸优化方法[J]. 系统仿真学报, 2025, 37(1): 134-144.

Wang Zhu,Zhang Zhenpeng,Zhang Mengtong,et al . Sequential Convex Programming Using Safe Flight Corridor for Trajectory Planning of UAVs[J]. Journal of System Simulation, 2025, 37(1): 134-144.

图/表 18

图1

图2

图3

图4

图5

图6

表1

图7

图8

图9

表2

图10

图11

图12

表3

图13

图14

表4

参考文献 29

1	Han Sangchul, Chon Sanguk, Kim JungYeong, et al. Snake Robot Gripper Module for Search and Rescue in Narrow Spaces[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 1667-1673.
2	Saeed Hamood Alsamhi, Shvetsov Alexey V, Kumar Santosh, et al. UAV Computing-assisted Search and Rescue Mission Framework for Disaster and Harsh Environment Mitigation[J]. Drones, 2022, 6(7): 154.
3	Rossini Micol, Biagio Di Mauro, Garzonio Roberto, et al. Rapid Melting Dynamics of an Alpine Glacier with Repeated UAV Photogrammetry[J]. Geomorphology, 2018, 304: 159-172.
4	Kim Hongjun, Kook Kim Byung. Energy-optimal Transport Trajectory Planning and Online Trajectory Modification for Holonomic Robots[J]. Asian Journal of Control, 2021, 23(5): 2185-2200.
5	Foehn Philipp, Romero Angel, Scaramuzza Davide. Time-optimal Planning for Quadrotor Waypoint Flight[J]. Science Robotics, 2021, 6(56): eabh1221.
6	Yu Xiang, Zhou Xiaobin, Guo Kexin, et al. Safety Flight Control for a Quadrotor UAV Using Differential Flatness and Dual-loop Observers[J]. IEEE Transactions on Industrial Electronics, 2022, 69(12): 13326-13336.
7	Tordesillas J, Lopez B T, Everett M, et al. FASTER: Fast and Safe Trajectory Planner for Navigation in Unknown Environments[J]. IEEE Transactions on Robotics, 2022, 38(2): 922-938.
8	Yao Zhihong, Jiang Haoran, Cheng Yang, et al. Integrated Schedule and Trajectory Optimization for Connected Automated Vehicles in a Conflict Zone[J]. IEEE Transactions on Intelligent Transportation Systems, 2022, 23(3): 1841-1851.
9	Letizia Nunzio A, Salamat Babak, Tonello Andrea M. A Novel Recursive Smooth Trajectory Generation Method for Unmanned Vehicles[J]. IEEE Transactions on Robotics, 2021, 37(5): 1792-1805.
10	Gao Fei, Wu William, Gao Wenliang, et al. Flying on Point Clouds: Online Trajectory Generation and Autonomous Navigation for Quadrotors in Cluttered Environments[J]. Journal of Field Robotics, 2019, 36(4): 710-733.
11	Byrd R H, Jean Charles Gilbert, Nocedal J. A Trust Region Method Based on Interior Point Techniques for Nonlinear Programming[J]. Mathematical Programming, 2000, 89(1): 149-185.
12	王祝, 刘莉, 龙腾, 等. 基于罚函数序列凸规划的多无人机轨迹规划[J]. 航空学报, 2016, 37(10): 3149-3158.
	Wang Zhu, Liu Li, Long Teng, et al. Trajectory Planning for Multi-UAVs Using Penalty Sequential Convex Pro-gramming[J]. Acta Aeronautica et Astronautica Sinica, 2016, 37(10): 3149-3158.
13	Misra G, Bai Xiaoli. Optimal Path Planning for Free-Flying Space Manipulators via Sequential Convex Programming[J]. Journal of Guidance, Control, and Dynamics, 2017, 40(11): 3019-3026.
14	刘延杰, 朱圣英, 崔平远. 序列凸优化的小天体附着轨迹优化[J]. 宇航学报, 2018, 39(2): 177-183.
	Liu Yanjie, Zhu Shengying, Cui Pingyuan. Soft Landing Trajectory Optimization on Asteroids by Convex Programming[J]. Journal of Astronautics, 2018, 39(2): 177-183.
15	Zhang Zhe, Li Jianxun, Wang Jun. Sequential Convex Programming for Nonlinear Optimal Control Problems in UAV Path Planning[J]. Aerospace Science and Technology, 2018, 76: 280-290.
16	Wang Zhu, Liu Li, Long Teng. 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 Guangtong, Long Teng, Wang Zhu, et al. Trust-region Filtered Sequential Convex Programming for multi-UAV Trajectory Planning and Collision Avoidance[J]. ISA Transactions, 2022, 128, Part B: 664-676.
18	Li Wenbo, Li Wentao, Cheng Lin, et al. Trajectory Optimization with Complex Obstacle Avoidance Constraints via Homotopy Network Sequential Convex Programming[J]. Aerospace, 2022, 9(11): 720.
19	Zhang Zhe, Jin Gumin, Li Jianxun. Penalty Boundary Sequential Convex Programming Algorithm for Non-convex Optimal Control Problems[J]. ISA Transactions, 2018, 72: 229-244.
20	Li Dongcheng, Yin Wangping, Wong W E, et al. Quality-oriented Hybrid Path Planning Based on A^*and Q-learning for Unmanned Aerial Vehicle[J]. IEEE Access, 2022, 10: 7664-7674.
21	Shao Shikai, He Chenglong, Zhao Yuanjie, et al. Efficient Trajectory Planning for UAVs Using Hierarchical Optimization[J]. IEEE Access, 2021, 9: 60668-60681.
22	Qin Zhenquan, Zhang Xuan, Zhang Xinwei, et al. The UAV Trajectory Optimization for Data Collection from Time-constrained IoT Devices: A Hierarchical Deep Q-network Approach[J]. Applied Sciences, 2022, 12(5): 2546.
23	Deits R, Tedrake R. Computing Large Convex Regions of Obstacle-free Space Through Semidefinite Programming[M]//H Levent Akin, Amato N M, Isler V, et al. Algorithmic Foundations of Robotics XI: Selected Contributions of the Eleventh International Workshop on the Algorithmic Foundations of Robotics. Cham: Springer International Publishing, 2015: 109-124.
24	Gao Fei, Wang Luqi, Zhou Boyu, et al. Teach-repeat-replan: A Complete and Robust System for Aggressive Flight in Complex Environments[J]. IEEE Transactions on Robotics, 2020, 36(5): 1526-1545.
25	Park Jungwon, Kim Dabin, Chan Kim Gyeong, et al. Online Distributed Trajectory Planning for Quadrotor Swarm with Feasibility Guarantee Using Linear Safe Corridor[J]. IEEE Robotics and Automation Letters, 2022, 7(2): 4869-4876.
26	Liu Sikang, Watterson M, Mohta K, et al. Planning Dynamically Feasible Trajectories for Quadrotors Using Safe Flight Corridors in 3-D Complex Environments[J]. IEEE Robotics and Automation Letters, 2017, 2(3): 1688-1695.
27	Betts J T. Practical Methods for Optimal Control and Estimation Using Nonlinear Programming[M]. 2nd ed. Philadelphia: Society for Industrial and Applied Mathematics, 2010: 279-282.
28	徐广通, 王祝, 曹严, 等. 动态优先级解耦的无人机集群轨迹分布式序列凸规划[J]. 航空学报, 2022, 43(2): 415-426.
	Xu Guangtong, Wang Zhu, Cao Yan, et al. Dynamic-priority-decoupled UAV Swarm Trajectory Planning Using Distributed Sequential Convex Programming[J]. Acta Aeronautica et Astronautica Sinica, 2022, 43(2): 415-426.
29	王祝, 徐广通, 龙腾. 基于定制内点法的多无人机协同轨迹规划[J]. 自动化学报, 2023, 49(11): 2374-2385.
	Wang Zhu, Xu Guangtong, Long Teng. Customized Interior-point Method for Cooperative Trajectory Planning of Multiple Unmanned Aerial Vehicles[J]. Acta Automatica Sinica, 2023, 49(11): 2374-2385.

参数名称	量值
初始状态/m	s₀=[5,9,1,0,0,0]
终端状态/m	s_f=[87,92,8,0,0,0]
状态量边界/m	s_max=[+∞,+∞,+∞,20,20,20] s_min=[-∞,-∞,-∞,-20,-20,-20]
控制量边界/(m/s²)	u_max=[10,10,10] u_min=[-10,-10,-10]
栅格分辨率/m	step=3
路径点间离散数	l=7
走廊信赖域/m	η=5
轨迹点信赖域/m	T_r=[10,10,10]
迭代误差/m	ε=0.01

算法	飞行时间/s	算法耗时/ms
SCP	10.59	116
A*-SCP	11.33	86
SFC-SCP	11.38	59

算法	飞行时间/s	算法耗时/ms
SCP	NAN	NAN
A*-SCP	12.07	170
SFC-SCP	13.27	54

算法	成功率/%	平均飞行时间/s	平均求解耗时/ms
SCP	29	10.79	99.1
A*-SCP	89	11.81	96.3
SFC-SCP	100	11.93	43.6

[1]	徐健, 宋鑫, 刘秀平, 陈博, 闫焕营. 基于数字孪生的装配机器人建模及系统实现[J]. 系统仿真学报, 2023, 35(7): 1497-1507.
[2]	赵静, 王鹏, 丁筱茜, 蒋国平, 徐丰羽, 孙雁飞. 基于观测器的四旋翼容错控制及仿真研究[J]. 系统仿真学报, 2022, 34(1): 1-10.
[3]	曾思劼, 晏良, 段晓君. 基于PCE方法的无人机轨迹规划与分析[J]. 系统仿真学报, 2022, 34(1): 145-152.
[4]	段现银, 张灿, 朱泽润, 孙朝阳, 蒋国璋, 向峰. 逆解多目标优化的六自由度机械手轨迹规划[J]. 系统仿真学报, 2021, 33(9): 2128-2137.
[5]	蒋泽睿, 杨立军, 李俊, 焦晓龙, 郑航. 自动跟踪口腔手术灯的运动学分析与仿真[J]. 系统仿真学报, 2021, 33(12): 2864-2879.
[6]	叶帅, 蒋国平, 周映江, 刘尚. 基于事件触发的多无人机固定时间编队控制[J]. 系统仿真学报, 2021, 33(10): 2420-2431.
[7]	修素朴, 文元桥, 肖长诗, 元海文, 詹文强. 倾转四旋翼自主降落的设计与仿真[J]. 系统仿真学报, 2020, 32(9): 1676-1685.
[8]	周楠, 王亮, 艾剑良. 基于反步法的矢量推力旋翼机建模及仿真研究[J]. 系统仿真学报, 2020, 32(6): 1117-1125.
[9]	尤波, 王明睿, 李智, 丁亮. 基于模型预测控制的轮式移动机器人轨迹规划[J]. 系统仿真学报, 2020, 32(4): 591-600.
[10]	谌海云, 陈华胄, 刘强. 基于改进人工势场法的多无人机三维编队路径规划[J]. 系统仿真学报, 2020, 32(3): 414-420.
[11]	廖瑛, 季海雨, 薛高雄. 基于伪谱法的巡航导弹最优闭环制导方法[J]. 系统仿真学报, 2018, 30(9): 3464-3471.
[12]	王彪, 唐超颖, 孔大庆. 基于改进自抗扰技术的四旋翼姿态控制[J]. 系统仿真学报, 2018, 30(8): 3124-3129.
[13]	孙雷, 张丽爽, 周璐, 张雪波. 一种基于Bezier曲线的移动机器人轨迹规划新方法[J]. 系统仿真学报, 2018, 30(3): 962-968.
[14]	潘健, 刘昌龙. 基于ESO的LQR控制器在无人机姿态控制中的研究[J]. 系统仿真学报, 2018, 30(2): 753-759.
[15]	曹凯, 黄肖肖, 于云, 刘春. 自主车辆避障安全路径的可达集建模[J]. 系统仿真学报, 2016, 28(3): 526-533.