An unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition

Abstract

A kind of unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition, comprising the following steps: 1, real-time environment data is obtained by depth and positioning sensor, establish local grid map with unmanned aerial vehicle as center, dynamically update the distance information of grid in the set range near obstacle.2, in the grid map, initial trajectory from starting point to terminal point is generated by using trajectory search algorithm, the detected obstacle is marked as interest point, and the distance information of grid outside the approximate set range is set by constructing interest point map;3, combine grid map and interest point map, form heuristic map, optimize the safety, smoothness and feasibility of trajectory;4, if new obstacle is detected in optimization process, update heuristic map, and re-optimize trajectory until no new interest point is added.The method is suitable for multi-rotor unmanned aerial vehicle autonomous obstacle avoidance trajectory planning in complex unknown environment, reduces the consumption of computing resources, improves the safety and flexibility of obstacle avoidance trajectory.

Description

Technical Field

[0001] This invention relates to the field of autonomous obstacle avoidance planning technology for multi-rotor unmanned aerial vehicles (UAVs), specifically a method for UAV obstacle avoidance motion planning based on heuristic environmental mapping. Background Technology

[0002] Multi-rotor drones, with their high maneuverability, vertical takeoff and landing capabilities, and diverse payload adaptability, have been widely applied in disaster relief, logistics delivery, and facility inspection. Their autonomous operation capability relies on obstacle avoidance motion planning, a core technology that integrates perception and positioning information from sensors such as depth cameras, LiDAR, and IMU / GNSS to generate collision-free flight trajectories that meet dynamic constraints in real time within unknown obstacle environments. However, balancing trajectory safety and real-time performance remains a critical technical challenge when facing dense three-dimensional obstacle environments and limited onboard computing resources.

[0003] Current mainstream methods model obstacle avoidance motion planning as a constrained nonlinear optimization problem, generating safe, smooth trajectories that conform to the dynamic characteristics of UAVs through solvers. In recent years, obstacle avoidance motion planning algorithms have mainly included Fast-Planner, Faster, and EGO-Planner. Fast-Planner first constructs a grid map containing distance information, then uses a hybrid A* algorithm to generate an initial desired trajectory, and finally improves the smoothness, safety, and feasibility of the trajectory by solving a nonlinear optimization problem. Trajectory smoothness is characterized by higher-order derivatives such as velocity and acceleration, and its calculation depends only on the trajectory's own parameters; while safety is characterized by the distance between trajectory points and obstacles, with larger distance values ​​indicating higher safety, requiring real-time acquisition of environmental distance information.

[0004] Obstacle avoidance motion planning primarily relies on distance information to obstacles to guide the trajectory away from them. Pre-calculating distance information using a grid map is a mainstream method. However, these methods face a dilemma. First, they suffer from missing gradient information. When the gradient disappears, trajectory safety optimization ceases, making further improvements difficult. Second, there is a clear trade-off between the map distance information update range and onboard resources. Reducing the update range lowers computational costs but limits trajectory safety, potentially causing the trajectory to approach obstacles; conversely, computational resource consumption increases exponentially with the update range, placing a heavy burden on hardware resources and impacting real-time system performance. Therefore, there is an urgent need to research lightweight obstacle avoidance motion planning methods to reduce resource consumption for updating distance information and improve the autonomous flight safety of multi-rotor UAVs.

[0005] The differences compared to existing technologies are as follows:

[0006] Technical Comparison with Patent CN115908737A "An Incremental 3D Mapping Method for Intelligent Obstacle Avoidance of Unmanned Aerial Vehicles"

[0007] Patent CN115908737A expands obstacles on a grid map to overcome the collision risk caused by the size of the drone, but does not update distance information. In contrast, we update the distance information of grids near the obstacle on the grid map and combine it with a heuristic map from the point of interest map to provide an approximate distance estimate for all spaces, which is applicable to obstacle avoidance motion planning based on distance gradient. This ensures the basic obstacle avoidance distance and improves the safety of the obstacle avoidance trajectory.

[0008] Technical Comparison with Patent CN119148742A "A Radar-Controlled Unmanned Aerial Vehicle Obstacle Avoidance System"

[0009] Patent CN119148742A uses radar scan information to construct a 3D model of the environment, while our algorithm supports processing depth information from sensors such as radar and millimeter waves. The designed point of interest model describes the obstacles that can actually affect the trajectory in each planning process. By describing a smaller number of obstacles, a more effective distance field is described, which reduces the time spent on obstacle avoidance motion planning and improves the safety of the UAV obstacle avoidance system.

[0010] Patent CN119148742A uses an obstacle avoidance path recommendation module, while we use the obstacle avoidance path as an optimization variable to construct an optimization model, and the designed expected path is more flexible.

[0011] Technical Comparison with Patent CN109032162A "An Unmanned Aerial Vehicle Obstacle Avoidance System and Control Method Based on LiDAR"

[0012] Patent CN109032162A uses information from LiDAR scanning to construct terrain maps, while our algorithm supports processing information from depth sensors such as radar and millimeter waves. The designed point of interest model describes the obstacles that can actually affect the trajectory in each planning process. By describing a smaller number of obstacles, a more effective distance field is described, reducing the time spent on obstacle avoidance motion planning and improving the safety of the UAV obstacle avoidance system.

[0013] The path planning objective of patent CN109032162A is to find the shortest route, while our algorithm comprehensively considers obstacle avoidance distance, trajectory smoothness, and trajectory feasibility, and the optimized expected trajectory has better safety.

[0014] Technical Comparison with Patent CN109032182A "An Unmanned Aerial Vehicle Obstacle Avoidance System and Control Method Based on Millimeter-Wave Radar"

[0015] Patent CN119148742A uses millimeter-wave radar scan information to construct terrain maps, while our algorithm supports processing information from depth sensors such as radar and millimeter waves. The designed point of interest model describes the obstacles that can actually affect the trajectory in each planning process. By describing a smaller number of obstacles, a more effective distance field is described, which reduces the time spent on obstacle avoidance motion planning and improves the safety of the UAV obstacle avoidance system.

[0016] Technical Comparison with Patent CN114529800A "A Method, System, Device and Medium for Obstacle Avoidance of Rotary-Wing Unmanned Aerial Vehicles"

[0017] Patent CN114529800A uses the SLAM method and RGB-D camera depth map to construct a semantic map, while our algorithm directly processes the RGB-D camera depth map to construct a three-dimensional occupied grid map, updates the distance information of grids near obstacles, consumes low computing resources, and has high obstacle avoidance efficiency.

[0018] Technical Comparison with Patent CN106767785B "A Navigation Method and Device for a Dual-Loop Unmanned Aerial Vehicle"

[0019] Patent CN106767785B achieves localization based on information from a downward-looking binocular camera and a forward-looking camera constructs obstacle information in the environment. Our algorithm does not depend on specific sensor types. It establishes a local grid map centered on the drone through depth and positioning information, dynamically updates the distance information of grids within a set range near obstacles, and combines the grid map with the point of interest map to form a heuristic map, optimizing the safety, smoothness, and feasibility of the trajectory. Summary of the Invention

[0020] To address the above problems, this invention proposes a UAV obstacle avoidance motion planning method based on heuristic environment mapping. This method overcomes the problem of high resource consumption caused by updating large-scale distance information, is applicable to obstacle avoidance motion planning of multi-rotor UAVs, reduces the range of distance information updates in the grid map, and improves the safety of the desired trajectory.

[0021] To achieve the above objectives, the technical solution adopted by the present invention is as follows:

[0022] A method for obstacle avoidance motion planning for unmanned aerial vehicles (UAVs) based on heuristic environmental mapping includes the following steps:

[0023] (1) Real-time acquisition of environmental data through depth and positioning sensors to establish a local grid map centered on the UAV and dynamically update the distance information of grids within a set range near obstacles;

[0024] (2) Within the grid map, the initial trajectory from the starting point to the ending point is generated using a trajectory search algorithm. The detected obstacles are marked as points of interest, and a point of interest map is constructed. The distance information of the grid outside the set range is approximated.

[0025] (3) Combine grid maps and point-of-interest maps to form heuristic maps and optimize the safety, smoothness and feasibility of trajectories;

[0026] (4) If a new obstacle is detected during the optimization process, the heuristic map is updated and the trajectory is re-optimized until no new points of interest are added.

[0027] As a further improvement to the present invention, step (1) is as follows:

[0028] Read the depth information from the drone's current depth sensors, such as depth cameras or LiDAR. Read the pose information of the drone's current positioning sensor. ,in It is a rotation matrix that describes the pitch, roll, and yaw angles of the UAV. It is a translation vector describing the position of the UAV's center of mass, fused and Constructing a local grid map centered on drones Update the range near obstacles Distance information between the inner grid and obstacles.

[0029] As a further improvement to the present invention, step (2) is as follows:

[0030] In a raster map containing distance information Inside, a trajectory search algorithm is used to find the path from the starting point. To the finish line initial trajectory And the detected obstacles are recorded as points of interest. Build a point of interest map ,approximate Distance information for unupdated areas; a heuristic map is a combination of a raster map and a point-of-interest map, i.e. spatial point The distance information is as follows:

[0031]

[0032] in, It is a spatial point exist The distance value, It is a spatial point exist The distance value.

[0033] As a further improvement to the present invention, the heuristic map in step (2) The construction includes the following steps:

[0034] (2-1) In When executing the trajectory search algorithm, the coordinates of the occupied grids traversed by the motion primitives are stored in the interest point set. Maintain interest in the middle. Uniqueness;

[0035] (2-2) Based on the set of interest points obtained from the search , get in In the context of space, the distance information between any point and an obstacle is:

[0036] .

[0037] As a further improvement of the present invention, step (2-2) uses a kd-tree data structure to represent the point of interest map. This accelerates the distance information calculation process.

[0038] As a further improvement to the present invention, step (3) is as follows:

[0039] In heuristic maps In this study, an optimization problem is established that considers the difficulty of obstacle avoidance and control of multi-rotor UAV trajectories.

[0040]

[0041] Among them, optimization variables Indicates the starting point To the finish line The initial trajectory, the objective function The difficulty of trajectory control and the distance of the trajectory from the obstacle The weighted sum, and Indicates weight, express exist Mid-range value, The distance penalty function is represented as a quadratic function, and the constraint indicates that the trajectory velocity and acceleration do not exceed the maximum velocity. and maximum acceleration Physical limitations determine the feasibility of the trajectory. and Let the velocity and acceleration of the trajectory be represented by , respectively. An optimization algorithm is used to solve for the optimal trajectory. .

[0042] As a further improvement to the present invention, the discretization of the optimization problem in step (3) involves the optimization variables... Discretization yields several trajectory points :

[0043]

[0044] An optimization algorithm based on the quasi-Newton method is used to solve discrete optimization problems.

[0045] As a further improvement to the present invention, step (4) is as follows:

[0046] Detecting the optimal trajectory Is the nearest obstacle at each trajectory point...? In the middle. If there is a recently encountered obstacle that has not been recorded, record it as a point of interest and update. and Repeat step (3). If the optimal trajectory does not exist, output the optimal trajectory. .

[0047] As a further improvement to the present invention, the loop is optimized in step (4):

[0048] (4-1) will Discretization yields several trajectory points. Query whether the most recently occupied grid cell for each trajectory point is in the set of points of interest. If not in In the middle, then update ,optimization Points of interest set The elements contain at most All occupied grid cells in the middle;

[0049] (4-2) If It has been updated, and the discrete optimization problem in step (3) is solved again.

[0050] Beneficial Effects: This invention discloses a UAV obstacle avoidance motion planning method based on heuristic environmental mapping. This method integrates depth and pose information collected by the UAV to construct a local grid map centered on the UAV, updating only the distance information of grid cells within a set range near obstacles. A trajectory search algorithm is used to find an initial trajectory from the starting point to the ending point, and detected obstacles are recorded as points of interest (POIs), constructing an POI map that approximates the distance information of grid cells outside the set obstacle range. A heuristic map model composed of the local grid map and the POI map is established to optimize the safety, smoothness, and feasibility of the trajectory. If new POIs are discovered during optimization, the POI map and heuristic map are reconstructed, and the trajectory is optimized again until no new POIs appear. By using the heuristic information of POIs, the range requiring distance information updates is effectively reduced, lowering the resource consumption of obstacle avoidance motion planning for multi-rotor UAVs and improving the safety and flexibility of the desired obstacle avoidance trajectory. Attached Figure Description

[0051] Figure 1 This is a flowchart of the method disclosed in this invention;

[0052] Figure 2 This is a local grid map designed according to the present invention;

[0053] Figure 3 This is a point-of-interest map designed according to the present invention;

[0054] Figure 4 This invention provides a heuristic map, trajectory search motion primitives and initial path, and a schematic diagram of the optimal trajectory. Detailed Implementation

[0055] The technical solutions of the present invention will be clearly and completely described below with reference to the accompanying drawings in the embodiments of the present invention. The following embodiments are used to illustrate the present invention, but are not intended to limit the scope of the present invention.

[0056] To make the objectives, technical solutions, and advantages of this invention clearer, specific embodiments of this invention will be described below in conjunction with the accompanying drawings.

[0057] This invention discloses a method for obstacle avoidance motion planning of unmanned aerial vehicles (UAVs) based on heuristic environment mapping, comprising the following steps:

[0058] The flowchart of the method disclosed in this invention is as follows: Figure 1 As shown, the local grid map designed in this invention is as follows: Figure 2 As shown, the point-of-interest map designed in this invention is as follows: Figure 3 As shown, the heuristic map, trajectory search motion primitives and initial path, and optimal trajectory diagram designed in this invention are illustrated as follows: Figure 4 As shown.

[0059] Step (1) Read the depth information from the UAV's current depth sensors, such as a depth camera or LiDAR. Read the pose information of the drone's current positioning sensor. ,in It is a rotation matrix that describes the pitch, roll, and yaw angles of the UAV. This is a translation vector describing the position of the UAV's center of mass. (Fusion) and Constructing a local grid map centered on drones Update the range near obstacles Distance information between the inner grid and obstacles.

[0060] Step (2) In the raster map containing distance information Inside, a trajectory search algorithm is used to find the path from the starting point. To the finish line initial trajectory And the detected obstacles are recorded as points of interest. Build a point of interest map ,approximate Distance information for areas that have not been updated. A heuristic map is a combination of a raster map and a point-of-interest map, i.e. Spatial point The distance information is as follows:

[0061]

[0062] in, It is a spatial point exist The distance value, It is a spatial point exist The distance value.

[0063] Step (3) in the heuristic map In this study, an optimization problem is established that considers the difficulty of obstacle avoidance and control of multi-rotor UAV trajectories.

[0064]

[0065] Among them, optimization variables Indicates the starting point To the finish line The initial trajectory. Objective function. The difficulty of trajectory control and the distance of the trajectory from the obstacle The weighted sum, and Indicates weight, express exist Mid-range value, Distance penalty functions are represented as quadratic functions. Constraints indicate that the trajectory velocity and acceleration do not exceed the maximum velocity. and maximum acceleration Physical limitations determine the feasibility of the trajectory. and Let represent the velocity and acceleration of the trajectory, respectively. An optimization algorithm is used to solve for the optimal trajectory. .

[0066] Step (4) Detect the optimal trajectory Is the nearest obstacle at each trajectory point...? In the middle. If there is a recently encountered obstacle that has not been recorded, record it as a point of interest and update. and Repeat step (3). If the optimal trajectory does not exist, output the optimal trajectory. .

[0067] Heuristic map in step (2) The construction includes the following steps:

[0068] (2-1) In When executing the trajectory search algorithm, the coordinates of the occupied grids traversed by the motion primitives are stored in the interest point set. Maintain interest in the middle. The uniqueness of.

[0069] (2-2) Based on the set of interest points obtained from the search , get in In the context of space, the distance information between any point and an obstacle is:

[0070]

[0071] In step (2-2), a kd-tree data structure is used to represent the point of interest map. This accelerates the distance information calculation process.

[0072] Discretization of the optimization problem in step (3). For the optimization variables... Discretization yields several trajectory points :

[0073]

[0074] An optimization algorithm based on the quasi-Newton method is used to solve discrete optimization problems.

[0075] Loop optimization in step (4):

[0076] (4-1) will Discretization yields several trajectory points. Query whether the most recently occupied grid cell for each trajectory point is in the set of points of interest. In the middle. If not In the middle, then update ,optimization Set of points of interest The elements contain at most All occupied grid cells.

[0077] (4-2) If It has been updated, and the discrete optimization problem in step (3) is solved again.

[0078] The above description is merely a preferred embodiment of the present invention and is not intended to limit the present invention in any other way. Any modifications or equivalent changes made based on the technical essence of the present invention shall still fall within the scope of protection claimed by the present invention.

Claims

1. A method for obstacle avoidance motion planning of unmanned aerial vehicles based on heuristic environment mapping, characterized in that: Includes the following steps: (1) Real-time acquisition of environmental data through depth and positioning sensors to establish a local grid map centered on the UAV and dynamically update the distance information of grids within a set range near obstacles; Step (1) is as follows: Read depth information from the drone's current depth sensors, including depth cameras or LiDAR. ; Read the pose information of the drone's current positioning sensor. ,in It is a rotation matrix that describes the pitch, roll, and yaw angles of the UAV. It is a translation vector describing the position of the UAV's center of mass, fused and Constructing a local grid map centered on drones Update the range near obstacles Distance information between the inner grid and obstacles; (2) Within the grid map, the initial trajectory from the starting point to the ending point is generated using a trajectory search algorithm. The detected obstacles are marked as points of interest, and a point of interest map is constructed. The distance information of the grid outside the set range is approximated. Step (2) is as follows: In a raster map containing distance information Inside, a trajectory search algorithm is used to find the path from the starting point. To the finish line initial trajectory And the detected obstacles are recorded as points of interest. Build a point of interest map ,approximate Distance information for unupdated areas; a heuristic map is a combination of a raster map and a point-of-interest map, i.e. spatial point The distance information is as follows: ； in, It is a spatial point exist The distance value, It is a spatial point exist The distance value; The heuristic map in step (2) The construction includes the following steps: (2-1) In When executing the trajectory search algorithm, the coordinates of the occupied grids traversed by the motion primitives are stored in the interest point set. Maintain interest in the middle. Uniqueness; (2-2) Based on the set of interest points obtained from the search , get in In the context of space, the distance information between any point and an obstacle is: ； (3) Combine grid maps and point-of-interest maps to form heuristic maps and optimize the safety, smoothness and feasibility of trajectories; (4) If a new obstacle is detected during the optimization process, the heuristic map is updated and the trajectory is re-optimized until no new points of interest are added.

2. The UAV obstacle avoidance motion planning method based on heuristic environment mapping according to claim 1, characterized in that: In step (2-2), a kd-tree data structure is used to represent the point of interest map. This accelerates the distance information calculation process.

3. The UAV obstacle avoidance motion planning method based on heuristic environment mapping according to claim 1, characterized in that: Step (3) is as follows: In heuristic maps In this study, an optimization problem is established that considers the difficulty of obstacle avoidance and control of multi-rotor UAV trajectories. ； Among them, optimization variables Indicates the starting point To the finish line The initial trajectory, the objective function The difficulty of trajectory control and the distance of the trajectory from the obstacle The weighted sum, and Indicates weight, express exist Mid-range value, The distance penalty function includes a quadratic function, and the constraint indicates that the trajectory velocity and acceleration do not exceed the maximum velocity. and maximum acceleration Physical limitations determine the feasibility of the trajectory. and Let the velocity and acceleration of the trajectory be represented by , respectively. An optimization algorithm is used to solve for the optimal trajectory. .

4. The UAV obstacle avoidance motion planning method based on heuristic environment mapping according to claim 3, characterized in that: In step (3), the optimization problem is discretized, and the optimization variables are... Discretization yields several trajectory points : ； An optimization algorithm based on the quasi-Newton method is used to solve discrete optimization problems.

5. The UAV obstacle avoidance motion planning method based on heuristic environment mapping according to claim 1, characterized in that: Step (4) is as follows: Detecting the optimal trajectory Is the nearest obstacle at each trajectory point...? If there is a recently encountered obstacle that has not been recorded, it is recorded as a point of interest and updated. and Repeat step (3). If the optimal trajectory does not exist, output the optimal trajectory. .

6. The UAV obstacle avoidance motion planning method based on heuristic environment mapping according to claim 5, characterized in that: Loop optimization in step (4): (4-1) will Discretization yields several trajectory points. Query whether the most recently occupied grid cell for each trajectory point is in the set of points of interest. If not in In the middle, then update ,optimization Points of interest set The elements contain at most All occupied grid cells in the middle; (4-2) If It has been updated, and the discrete optimization problem in step (3) is solved again.

Images