CN-120066078-B - Unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition

CN120066078BCN 120066078 BCN120066078 BCN 120066078BCN-120066078-B

Abstract

A method for planning obstacle avoidance movement of an unmanned aerial vehicle based on heuristic environmental composition comprises the following steps of 1, acquiring environmental data in real time through depth and positioning sensors, establishing a local grid map with the unmanned aerial vehicle as a center, and dynamically updating distance information of grids in a set range near an obstacle. 2. The method comprises the steps of generating an initial track from a starting point to an end point in a grid map by adopting a track searching algorithm, marking the detected obstacle as an interest point, constructing an interest point map, approximately setting distance information of grids outside a range, forming a heuristic map by combining the grid map and the interest point map, optimizing the safety, smoothness and feasibility of the track, and 4, updating the heuristic map and re-optimizing the track if a new obstacle is detected in the optimizing process until no new interest point is added. The method is suitable for autonomous obstacle avoidance track planning of the multi-rotor unmanned aerial vehicle in a complex unknown environment, reduces the consumption of computing resources, and improves the safety and flexibility of the obstacle avoidance track.

Inventors

WANG LIHUI
LI YONG
MENG JIAN
CHEN XIYUAN

Assignees

东南大学

Dates

Publication Date: 20260508
Application Date: 20250226

Claims (6)

1. A heuristic environment composition-based unmanned aerial vehicle obstacle avoidance motion planning method is characterized by comprising the following steps: (1) Acquiring environmental data in real time through a depth and positioning sensor, establishing a local grid map with the unmanned aerial vehicle as a center, and dynamically updating the distance information of grids in a set range near the obstacle; the step (1) is specifically as follows: reading depth information including a depth camera or LiDAR from a current depth sensor of an unmanned aerial vehicle Reading pose information of current positioning sensor of unmanned aerial vehicle Wherein Is a rotation matrix describing the pitch, roll and yaw angles of the drone, Is a translation vector describing the position of the mass center of the unmanned plane, and is fused And Constructing a local grid map centered on a drone Updating the setting range around the obstacle Distance information of the inner grid and the obstacle; (2) Generating an initial track from a starting point to an end point in the grid map by adopting a track searching algorithm, marking the detected obstacle as an interest point, constructing an interest point map, and approximately setting the distance information of the grids outside the range; The step (2) is specifically as follows: in a grid map containing distance information In, a track searching algorithm is adopted to search for a slave starting point To the end point Is set to the initial trajectory of (1) And the detected obstacle is recorded as an interest point Constructing a map of points of interest Approximation of Distance information of the area is not updated, and the heuristic map is the combination of the grid map and the interest point map, namely Spatial point The distance information of (2) is: ; Wherein, the Is a spatial point At the position of Is used for the distance value of (a), Is a spatial point At the position of Distance value of (2); Heuristic map in the step (2) The construction of (2) comprises the following steps: (2-1) at When the track searching algorithm is executed, the coordinates of the occupied grids passed by the motion elements are stored in the interest point set In maintaining points of interest Is the uniqueness of (3); (2-2) Point of interest set from search Is obtained at Distance information between any point in space and an obstacle: ; (3) Combining the grid map and the interest point map to form a heuristic map, and optimizing the safety, smoothness and feasibility of the track; (4) If a new obstacle is detected in the optimization process, updating the heuristic map and re-optimizing the track until no new interest points are added.
2. The unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition of claim 1, wherein the step (2-2) adopts a k-d tree data structure to represent a point of interest map The distance information calculation process is accelerated.
3. The unmanned aerial vehicle obstacle avoidance movement planning method based on heuristic environment composition according to claim 1, wherein the step (3) is specifically as follows: In heuristic map In the method, an optimization problem of avoiding barriers and controlling difficulty of the track of the multi-rotor unmanned aerial vehicle is established and considered: ; wherein the variables are optimized Representing the starting point To the end point Initial trajectory, objective function of (2) Is the difficulty of track control And the distance of the track from the obstacle Is used in the field of the digital camera, And The weight is represented by a weight that, Representation of At the position of A value of the medium distance is obtained, Representing the distance penalty function includes a quadratic function, and constraining represents that the track speed and acceleration do not exceed the maximum speed And maximum acceleration The physical limit, which is the feasibility of the track, And Respectively representing the speed and the acceleration of the track, and solving the optimal track by adopting an optimization algorithm 。
4. The unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environmental composition according to claim 3, wherein the discretization of the optimization problem in step (3) is performed on optimization variables Discretizing to obtain a plurality of track points : ; And solving a discrete optimization problem by adopting an optimization algorithm of a quasi-Newton method.
5. The unmanned aerial vehicle obstacle avoidance movement planning method based on heuristic environment composition according to claim 1, wherein the step (4) is specifically as follows: detecting an optimal trajectory Whether the nearest obstacle of each track point is at If there is a recent obstacle that has not been recorded, record as a point of interest, and update And Repeating the step (3), and if the optimal track does not exist, outputting the optimal track 。
6. The unmanned aerial vehicle obstacle avoidance movement planning method based on heuristic environmental composition of claim 5, wherein in step (4), the cyclic optimization is performed: (4-1) will Discretizing to obtain a plurality of track points Querying whether each track point is in the most recently occupied grid or not If not in In (C), update Optimizing Point of interest set At most comprise elements of All of the occupied grids; (4-2) if Is updated to solve the discrete optimization problem of the optimization problem in the step (3) again.

Description

Unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition Technical Field The invention relates to the technical field of autonomous obstacle avoidance planning of multi-rotor unmanned aerial vehicles, in particular to an unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition. Background The multi-rotor unmanned aerial vehicle has been applied in the fields of disaster relief, logistics distribution, facility inspection and the like in a large scale by virtue of the high maneuverability, the vertical take-off and landing capability and the diversified load adaptation characteristics. Autonomous operation capability depends on obstacle avoidance motion planning core technology, namely, collision-free flight tracks meeting dynamics constraint are generated in real time in an unknown obstacle environment by fusing sensing and positioning information of sensors such as a depth camera, liDAR, IMU/GNSS and the like. However, when the three-dimensional dense obstacle environment and the airborne computational resources are limited, how to balance the track safety and the real-time performance is still a technical difficulty to be broken through. The current mainstream method models obstacle avoidance motion planning as a constrained nonlinear optimization problem, and a safe, smooth and unmanned aerial vehicle dynamic characteristic-conforming track is generated through a solver. In recent years, obstacle avoidance motion planning algorithms mainly comprise Fast-Planner, faster, EGO-Planner and the like. Fast-Planner firstly constructs a grid map containing distance information, then adopts mixed A to generate an initial expected track, and finally solves the nonlinear optimization problem to improve the smoothness, safety and feasibility of the track. The track smoothness is characterized by higher derivatives such as speed, acceleration and the like, the calculation of the track smoothness only depends on the parameters of the track, the safety is characterized by the distance between the track points and the obstacle, and the greater the distance value is, the higher the safety is, and the environment distance information needs to be acquired in real time. Obstacle avoidance motion planning relies primarily on distance information from the obstacle to keep the trajectory away from the obstacle. Among them, the method of calculating distance information in advance using an occupied grid map is the mainstream. However, these methods face a dilemma. First, they have a problem of gradient information loss. When the gradient disappears, the optimization of the track safety is stopped, and the track safety is difficult to further improve. And secondly, an obvious trade-off relation exists between the map distance information updating range and the airborne resources. Reducing the update range can reduce the calculation cost, but can limit the safety of the track and possibly lead the track to approach to the obstacle, and conversely, the calculation resource consumption increases along with the index increase of the update range, and increasing the update range can bring heavy burden to hardware resources and affect the real-time performance of the system. Therefore, research on a lightweight obstacle avoidance motion planning method is needed, so that resource consumption for updating distance information is reduced, and the autonomous flight safety of the multi-rotor unmanned aerial vehicle is improved. Compared with the prior art, the method has the following differences: Compared with the technology of patent CN 115908737A' an incremental three-dimensional composition method for intelligent obstacle avoidance of unmanned aerial vehicle The patent CN115908737A expands the obstacle on the grid map to overcome the collision risk caused by the size of the unmanned aerial vehicle, but does not update the distance information, and the distance information of the grid nearby the obstacle is updated on the grid map, and the distance approximate estimation of all the spaces is provided by combining with the heuristic map of the interest point map, so that the obstacle avoidance motion planning based on the distance gradient is suitable, the basic obstacle avoidance distance is ensured, and the safety of the obstacle avoidance track is improved. Compared with the technology of a radar control-based unmanned aerial vehicle obstacle avoidance system in patent CN119148742A Patent CN119148742a adopts radar scanning information to construct a 3D model of the environment, while our algorithm supports processing depth information of sensors such as radar and millimeter wave, and the designed interest point model describes obstacles which can actually affect the track in each planning process, describes a more effective distance field through a small number of obstacles, reduces time consumption of obstacle avoidance motion pla