In recent years，with the rapid increase in the demand of UAV for rescue and disaster relief in unknown and complex environments such as earthquake ruins，fire scene，rugged mountains and forests，higher requirements are put forward for the autonomous navigation of UAV.In order to ensure that the UAV can quickly respond to unforeseen risks when flying at high speed in unknown environment，the online trajectory planning module in autonomous navigation is very important.Gradient based planningmethod has the outstanding advantages of high success rate and fast planning speed，and has gradually become the mainstream method of UAV online trajectory planning.
However，the traditional gradient based planning method needs to construct Euclidean Signed Distance Field（ESDF）in advance，which leads to the problems of high redundancy of obstacle information and limited planning efficiency.To solve these problems，this paper proposes an online trajectory planning method based on regional fast optimization.
Online trajectory planning of UAV is generally based on state estimation and voxel mapping module.Updated maps and pose information of UAV are fed to trajectory generation module to generate initial trajectory，and then enter trajectory optimization module to generate optimal trajectory，which is sent to trajectory server，and the corresponding flight controller can control UAV.In this paper，in order to transform the local planning in unknown environment into the local fast optimization problem of initial trajectory，uniform B-spline is used to further parameterize the initial trajectory.According to the current motion state and environmental information of UAV，a more efficient trajectory optimization strategy is designed to quickly optimize the initial trajectory to a high-quality trajectory that meets the requirements of safety，smoothness and dynamic feasibility.
Trajectory optimization is divided into two stages：the first stage is fast trajectory optimization in collision area.Collision detection is carried out continuously on the initial trajectory.A pair of control points Q in and Q out are used to record the first and last positions of each collision area trajectory，and a“collision set”Q col composed of collision control points is found.Afterwards，the A* path search algorithm is used to search for the optimal path，which is to find a safe guiding path from Q in to Q out and obtain the set of path points A.To push each collision control point Q i in the“collision set”Q col away from the current obstacle at the fastest speed and shortest distance，a collision control point replacement strategy is proposed.By searching for the corresponding path point AQ for each collision control point Q i in the path point set A，the replacement operation is performed，and it is used as the new control point Q inew.During the replacement process，only the position of the collision control points on the initial trajectory was adjusted to minimize the impact on the entire trajectory，allowing for more flexible adjustment of the collision area trajectory and achieving fast trajectory optimization.The second stage is multi-objective trajectory optimization，which prepares for further trajectory optimization by defining and extracting local obstacle information related to trajectory planning and calculating collision cost.Then，considering trajectory safety，smoothness and dynamic feasibility，multi-objective optimization function is established to further optimize trajectory.
Simulation results show that this method has a significant improvement in planning time compared with existing algorithms：in short-distance planning in simple scenarios，compared with the frontier method，the total planning time，trajectory initialization time and trajectory optimization time are reduced by 36.1%，33.1% and 37.7%，respectively；In the long-distance planning under complex scenes，compared with the classical gradient based planning method，the trajectory planning time is shortened by 86.56% on average，and the planning efficiency is greatly improved.Compared with the cutting-edge gradient based planning method，the efficiency of trajectory optimization is further improved as the complexity of the environment becomes higher.And the method proposed in this paper can effectively carry out online planning in different complex environments，and has strong robustness and scalability.
主管单位：重庆市教育委员会