Sequential Convex Programming Using Safe Flight Corridor for Trajectory Planning of UAVs

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

Abstract

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

CLC Number:

TP242

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

Figures/Tables 18

Fig. 1

Fig. 2

Fig. 3

Fig. 4

Fig. 5

Fig. 6

Table 1

Fig. 7

Fig. 8

Fig. 9

Table 2

Fig. 10

Fig. 11

Fig. 12

Table 3

Fig. 13

Fig. 14

Table 4

References 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]	Cheng Jie, Zheng Yuan, Li Chenglong, Jiang Bo. Multi-UAV Collaborative Trajectory Planning Algorithm for Urban Ultra-low-altitude Air Transportation Scenario [J]. Journal of System Simulation, 2024, 36(1): 50-66.
[2]	Guangjian Tian, Jiyang Dai, Jin Ying, Ning Wang. Multi-UAV Trajectory Planning Based on Adaptive Segmented Potential Field Method [J]. Journal of System Simulation, 2022, 34(11): 2368-2376.
[3]	Fuzhen Zhang, Yaoqin Zhu. Task Allocation Method for Multi-UAV Cooperative Reconnaissance in Complex Environment [J]. Journal of System Simulation, 2022, 34(10): 2293-2302.
[4]	Zhao Jing, Wang Peng, Ding Xiaoqian, Jiang Guoping, Xu Fengyu, Sun Yanfei. Fault Tolerant Control and Simulation of Quadrotor Based on Adaptive Observer [J]. Journal of System Simulation, 2022, 34(1): 1-10.
[5]	Zeng Sijie, Yan Liang, Duan Xiaojun. Planning and Analysis on UAV Trajectory Based on PCE Method [J]. Journal of System Simulation, 2022, 34(1): 145-152.
[6]	Duan Xianyin, Zhang Can, Zhu Zerun, Sun Zhaoyang, Jiang Guozhang, Xiang Feng. Trajectory Planning of 6-DOF Manipulator Based on Inverse Multi-objective Optimization [J]. Journal of System Simulation, 2021, 33(9): 2128-2137.
[7]	Jiang Zerui, Yang Lijun, Li Jun, Jiao Xiaolong, Zheng Hang. Kinematics Analysis and Simulation of Automatically Tracking Dental Surgery Lamp [J]. Journal of System Simulation, 2021, 33(12): 2864-2879.
[8]	Ye Shuai, Jiang Guoping, Zhou Yingjiang, Liu Shang. Fixed-time Event-triggered Formation Control for Multiple UAVs [J]. Journal of System Simulation, 2021, 33(10): 2420-2431.
[9]	Xiu Supu, Wen Yuanqiao, Xiao Changshi, Yuan Haiwen, Zhan Wenqiang. Design and Simulation on Autonomous Landing of a Quad Tilt Rotor [J]. Journal of System Simulation, 2020, 32(9): 1676-1685.
[10]	Zhou Nan, Wang Liang, Ai Jianliang. Modeling and Simulation of Quadrotor UAV with Vector Thrust Based on Backstepping [J]. Journal of System Simulation, 2020, 32(6): 1117-1125.
[11]	You Bo, Wang Mingrui, Li Zhi, Ding Liang. Trajectory Planning of Wheeled Mobile Robot Based on Model Predictive Control [J]. Journal of System Simulation, 2020, 32(4): 591-600.
[12]	Chen Haiyun, Chen Huazhou, Liu Qiang. Multi-UAV 3D Formation Path Planning Based on Improved Artificial Potential Field [J]. Journal of System Simulation, 2020, 32(3): 414-420.
[13]	Zhang Siyuan, Li Xianying, Shen Xiaoyun. ADS-B IN based Conflict Prediction and Conflict-free Trajectory Planning for Multi-aircraft [J]. Journal of System Simulation, 2019, 31(8): 1627-1635.
[14]	Wang Biao, Tang Chaoying, Kong Daqing. Quadrotor Attitude Control Based on an Improved ADRC [J]. Journal of System Simulation, 2018, 30(8): 3124-3129.
[15]	Sun Lei, Zhang Lishuang, Zhou Lu, Zhang Xuebo. A New Method on Trajectory Planning for Mobile Robots Using Bezier Curves [J]. Journal of System Simulation, 2018, 30(3): 962-968.