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

Abstract

An unmanned aerial vehicle obstacle avoidance motion planning method based on heuristic environment composition comprises the steps that 1, environment data are acquired in real time through depth and positioning sensors, a local grid map with an unmanned aerial vehicle as the center is established, and distance information of grids in a set range near an obstacle is dynamically updated; 2, in the grid map, generating an initial track from a starting point to an end point by adopting a track search algorithm, marking a detected obstacle as an interest point, constructing an interest point map, and approximately setting distance information of grids outside a range; 3, combining the grid map and the interest point map to form a heuristic map, and optimizing the safety, smoothness and feasibility of the trajectory; and 4, if a new obstacle is detected in the optimization process, updating the heuristic map, and re-optimizing the trajectory until no new interest point exists. The method is suitable for autonomous obstacle avoidance trajectory planning of the multi-rotor unmanned aerial vehicle in a complex unknown environment, consumption of computing resources is reduced, and safety and flexibility of the obstacle avoidance trajectory are improved.

Description

Technical Field

[0001] The invention belongs to the technical field of autonomous obstacle avoidance planning of multi-rotor unmanned aerial vehicles, and in particular to a method for obstacle avoidance motion planning of unmanned aerial vehicles based on heuristic environment composition. Background Art

[0002] Multi-rotor drones have been widely used in disaster relief, logistics distribution, facility inspection and other fields due to their high maneuverability, vertical take-off and landing capabilities, and diversified load adaptation characteristics. Autonomous operation capabilities rely on the core technology of obstacle avoidance motion planning, that is, by integrating the perception and positioning information of sensors such as depth cameras, LiDAR, IMU / GNSS, etc., to generate collision-free flight trajectories that meet dynamic constraints in real time in unknown obstacle environments. However, in the face of a three-dimensional dense obstacle environment and limited onboard computing resources, how to balance trajectory safety and real-time performance is still a technical difficulty that needs to be overcome.

[0003] The current mainstream method models obstacle avoidance motion planning as a nonlinear optimization problem with constraints, and generates a safe, smooth trajectory that conforms to the dynamic characteristics of the drone through a solver. In recent years, the main obstacle avoidance motion planning algorithms include Fast-Planner, Faster, EGO-Planner, etc. Fast-Planner first constructs a grid map containing distance information, then uses hybrid A* to generate an initial desired trajectory, and finally improves the smoothness, safety and feasibility of the trajectory by solving the nonlinear optimization problem. Among them, the smoothness of the trajectory is characterized by high-order derivatives such as velocity and acceleration, and its calculation depends only on the parameters of the trajectory itself; while safety is characterized by the distance between the trajectory point and the obstacle. The larger the distance value, the higher the safety, and real-time acquisition of environmental distance information is required.

[0004] Obstacle avoidance motion planning mainly relies on the distance information from obstacles to keep the trajectory away from obstacles. Among them, using the occupancy grid map to pre-calculate the distance information is the mainstream method. However, these methods face a dilemma. First, they have the problem of missing gradient information. When the gradient disappears, the optimization of trajectory safety will stop, and it is difficult to further improve the safety of the trajectory. Second, there is an obvious trade-off between the update range of map distance information and onboard resources. Although narrowing the update range can reduce the computational cost, it will limit the safety of the trajectory and may make the trajectory close to the obstacle; on the contrary, the consumption of computing resources increases exponentially with the update range. Increasing the update range will bring a heavy burden to the hardware resources and affect the real-time performance of the system. Therefore, it is urgent to study lightweight obstacle avoidance motion planning methods to reduce the resource consumption of updating distance information and improve the safety of autonomous flight of multi-rotor drones.

[0005] Compared with the prior art, the differences are as follows:

[0006] Technical Comparison with Patent CN115908737A, "An Incremental 3D Composition Method for UAV Intelligent Obstacle Avoidance"

[0007] Patent CN115908737A inflates obstacles on the grid map to overcome the collision risk caused by the UAV size, but does not update the distance information. We update the distance information of the grids near the obstacles on the grid map and combine the heuristic map of the point-of-interest map to provide an approximate distance estimate for all spaces, which is applicable to the obstacle avoidance motion planning based on the distance gradient, ensuring the basic obstacle avoidance distance and improving the safety of the obstacle avoidance trajectory.

[0008] Technical Comparison with Patent CN119148742A, "A UAV Obstacle Avoidance System Based on Radar Control"

[0009] Patent CN119148742A constructs a 3D model of the environment using the information scanned by the radar. Our algorithm supports processing the depth information of sensors such as radar and millimeter wave. The designed point-of-interest model describes the obstacles that can actually affect the trajectory during each planning process. A more effective distance field is described by a small number of obstacles, reducing the time consumption of the obstacle avoidance motion planning and improving the safety of the UAV obstacle avoidance system.

[0010] Patent CN119148742A adopts an obstacle avoidance path recommendation module, while we construct an optimization model with the obstacle avoidance path as the optimization variable, and the designed desired path is more flexible.

[0011] Technical Comparison with Patent CN109032162A, "A UAV Obstacle Avoidance System and Control Method Based on Lidar"

[0012] Patent CN109032162A constructs a topographic map using the information scanned by the lidar. Our algorithm supports processing the information of depth sensors such as radar and millimeter wave. The designed point-of-interest model describes the obstacles that can actually affect the trajectory during each planning process. A more effective distance field is described by a small number of obstacles, reducing the time consumption of the obstacle avoidance motion planning and improving the safety of the UAV obstacle avoidance system.

[0013] The path planning objective adopted by Patent CN109032162A is the shortest route, while our algorithm comprehensively considers the obstacle avoidance distance, trajectory smoothness and trajectory feasibility, and the optimized desired trajectory has better safety.

[0014] Technical Comparison with Patent CN109032182A, "A UAV Obstacle Avoidance System and Control Method Based on Millimeter Wave Radar"

[0015] Patent CN119148742A constructs a topographic map using the information scanned by a millimeter-wave radar. Our algorithm supports processing the information of depth sensors such as radar and millimeter-wave. The designed point-of-interest model describes the obstacles that can actually affect the trajectory during each planning process. A more effective distance field is described by a small number of obstacles, reducing the time-consuming of obstacle avoidance motion planning and improving the safety of the UAV obstacle avoidance system.

[0016] Technical comparison with patent CN114529800A "A method, system, device and medium for obstacle avoidance of a rotor UAV"

[0017] Patent CN114529800A constructs a semantic map using the SLAM method and the depth map of an RGB-D camera. Our algorithm directly processes the depth map of the RGB-D camera, constructs a three-dimensional occupancy grid map, and updates the distance information of the grids near the obstacles, consuming low computing resources and having high obstacle avoidance efficiency.

[0018] Technical comparison with patent CN106767785B "A navigation method and device for a dual-loop UAV"

[0019] Patent CN106767785B realizes positioning according to the information of a downward-looking binocular camera, and the forward-looking camera constructs the obstacle information in the environment. Our algorithm does not depend on the specific type of sensor. Through depth and positioning information, a local grid map centered on the UAV is established, and the distance information of the grids within a set range near the obstacles is dynamically updated. Combining the grid map and the point-of-interest map forms a heuristic map to optimize the safety, smoothness and feasibility of the trajectory. Summary of the invention

[0020] In view of the above problems, the present invention proposes a UAV obstacle avoidance motion planning method based on heuristic environmental composition, aiming to provide a method different from traditional photovoltaic systems. This method overcomes the problem of high resource consumption caused by updating distance information in a large range, is applicable to the obstacle avoidance motion planning of multi-rotor UAVs, reduces the range of updating distance information in the grid map, and improves the safety of the expected trajectory.

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

[0022] A UAV obstacle avoidance motion planning method based on heuristic environmental composition, comprising the following steps:

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

[0024] (2) Inside the grid map, use the trajectory search algorithm to generate the initial trajectory from the starting point to the ending point, mark the detected obstacles as points of interest, construct a point-of-interest map, and approximately set the distance information of the grids outside the specified range.

[0025] (3) Combine the grid map and the point-of-interest map to form a heuristic map, and optimize the safety, smoothness, and feasibility of the trajectory.

[0026] (4) If new obstacles are detected during the optimization process, update the heuristic map and re-optimize the trajectory until there are no new points of interest.

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

[0028] Read the depth information D of the current depth sensor of the drone, such as a depth camera or LiDAR. t ; Read the pose information (R t , T t ) of the current positioning sensor of the drone, where R t ∈ SO(3) is the rotation matrix describing the pitch, roll, and yaw angles of the drone, and T t ∈ R 3 is the translation vector describing the centroid position of the drone. Combine D t and (R t , T t ) to construct a local grid map G map centered on the drone, and update the distance information of the grids and obstacles within the specified range d set near the obstacles.

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

[0030] Inside the grid map G map containing distance information, use the trajectory search algorithm to find the initial trajectory x(t): R → R 3 from the starting point s ∈ R 3 to the ending point e ∈ R 3 , and record the detected obstacles as points of interest i ∈ R 3 , construct a point-of-interest map I map , approximately set the distance information of the unupdated area of G map . The heuristic map is the union of the grid map and the point-of-interest map, that is, H map = G map ∪ I map . The distance information of the spatial point x ∈ R 3 is:

[0031] d H (x) = min{d G (x), dI (x)}

[0032] where d G (x) is the distance value of the spatial point x in G map and d I (x) is the distance value of the spatial point x in I map .

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

[0034] (2-1) When executing the trajectory search algorithm in G map , store the coordinates of the occupied grid passed by the motion primitive in the set of interest points I, and maintain the uniqueness of the interest point i;

[0035] (2-2) According to the obtained set of interest points I, obtain the distance information between any point in space and the obstacle in I map :

[0036] d I (x) = min{||x - i||, i ∈ I}.

[0037] As a further improvement of the present invention, the k-d tree data structure is used to represent the interest point map I map in the step (2-2) to accelerate the calculation process of the distance information.

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

[0039] In the heuristic map H map , establish an optimization problem considering the trajectory obstacle avoidance and control difficulty of the multi-rotor UAV:

[0040]

[0041] where the optimization variable x(t) represents the initial trajectory from the starting point s ∈ R 3 to the end point e ∈ R 3 , the objective function F is the weighted sum of the trajectory control difficulty and the distance of the trajectory from the obstacle , λ s and λ o represent weights, d H (x(t)) represents the distance value of x(t) in H map , φ(·) represents a distance penalty function such as a quadratic function, and the constraint represents that the trajectory speed and acceleration do not exceed the maximum speed v max and the maximum acceleration a max physical limitations, which is the feasibility of the trajectory, and respectively represent the speed and acceleration of the trajectory, and an optimization algorithm is used to solve the optimal trajectory

[0042] As a further improvement of the present invention, in step (3), for the discretization of the optimization problem, the optimization variable x(t) is discretized to obtain a number of trajectory points x 1 , x 2 , …, x N :

[0043]

[0044] An optimization algorithm using the quasi - Newton method is used to solve the discrete optimization problem

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

[0046] Detect the optimal trajectory Whether the nearest obstacle of each trajectory point is within I map If there is an unrecorded nearest obstacle, it is denoted as an interest point, and I map and H map are updated, and step (3) is repeated. If not, the optimal trajectory is output

[0047] As a further improvement of the present invention, in step (4), cyclic optimization:

[0048] (4 - 1) Discretize to obtain a number of trajectory points (x 1 , x 2 , …, x N ), query whether the nearest occupied grid of each trajectory point is in the interest point set I. If it is not in I, then update I T , optimize H map , and the elements of the interest point set I at most contain all the occupied grids in G map ; map

[0049] (4 - 2) If H map is updated, solve the discrete optimization problem of the optimization problem in step (3) again

[0050] Beneficial effects: The present invention discloses a method for obstacle avoidance motion planning of an unmanned aerial vehicle (UAV) based on heuristic environmental mapping. This method integrates the depth information and pose information collected by the UAV to construct a local grid map centered on the UAV, and only updates the distance information of the grids within a set range near the obstacles. A trajectory search algorithm is used to find the initial trajectory from the starting point to the ending point, and the detected obstacles are recorded as points of interest to construct a point-of-interest map, approximating the distance information of the grids outside the set range of the obstacles. A heuristic map model composed of the local grid map and the point-of-interest map is established to optimize the safety, smoothness, and feasibility of the trajectory. If new points of interest are found during the optimization process, the point-of-interest map and the heuristic map are reconstructed, and the trajectory is optimized again until no new points of interest appear. Through the heuristic information of the points of interest, the range that needs to update the distance information is effectively reduced, the resource consumption of the multi-rotor UAV obstacle avoidance motion planning is reduced, and the safety and flexibility of the expected obstacle avoidance trajectory are improved. Description of the Drawings

[0051] Figure 1 is the flowchart of the method disclosed in the present invention;

[0052] Figure 2 is the local grid map designed by the present invention;

[0053] Figure 3 is the point-of-interest map designed by the present invention;

[0054] Figure 4 is the schematic diagram of the heuristic map, trajectory search motion primitive, initial path, and optimal trajectory designed by the present invention. Detailed Embodiments

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

[0056] To make the purpose, technical solution, and advantages of the present invention clearer, the specific implementation cases of the present invention will be described below in conjunction with the accompanying drawings.

[0057] The present invention discloses a method for obstacle avoidance motion planning of an unmanned aerial vehicle based on heuristic environmental mapping, including the following steps:

[0058] Among them, the flowchart of the method disclosed in the present invention is as shown in Figure 1 shown, the local grid map designed by the present invention is as shown in Figure 2 shown, the point-of-interest map designed by the present invention is as shown in Figure 3 shown, and the schematic diagram of the heuristic map, trajectory search motion primitive, initial path, and optimal trajectory designed by the present invention is as shown in Figure 4 shown.

[0059] Step (1): Read the depth information D of the current depth sensor of the drone, such as a depth camera or LiDAR. t ; Read the pose information (R t , T t ) of the current positioning sensor of the drone, where R t ∈ SO(3) is the rotation matrix describing the pitch, roll, and yaw angles of the drone, and T t ∈ R 3 is the translation vector describing the centroid position of the drone. Fuse D t and (R t , T t ) to construct a local grid map G map centered on the drone, and update the distance information between the grids and obstacles within a set range d set .

[0060] Step (2): In the grid map G map containing distance information, use a trajectory search algorithm to find an initial trajectory x(t): R → R 3 from the starting point s ∈ R 3 to the ending point e ∈ R 3 , and mark the detected obstacles as points of interest i ∈ R 3 , construct a point-of-interest map I map , and approximate the distance information of the unupdated area of G map . The heuristic map is the union of the grid map and the point-of-interest map, i.e., H map = G map ∪ I map . The distance information of a spatial point x ∈ R 3 is:

[0061] d H (x) = min{d G (x), d I (x)}

[0062] where d G (x) is the distance value of the spatial point x in G map , and d I (x) is the distance value of the spatial point x in I map .

[0063] Step (3): In the heuristic map H map , establish an optimization problem considering the trajectory obstacle avoidance and control difficulty of a multi-rotor drone:

[0064]

[0065] where the optimization variable x(t) represents the initial trajectory from the starting point s ∈ R 3 to the ending point e ∈ R 3The initial trajectory. The objective function F is the trajectory control difficulty and the distance of the trajectory from the obstacle is the weighted sum, where λ s and λ o represent weights, and d H (x(t)) represents the distance value of x(t) in H map . φ(·) represents a distance penalty function such as a quadratic function. The constraints indicate that the trajectory speed and acceleration do not exceed the maximum speed v max and the maximum acceleration a max due to physical limitations, which is the feasibility of the trajectory. and represent the speed and acceleration of the trajectory respectively. An optimization algorithm is used to solve for the optimal trajectory

[0066] Step (4) detects the optimal trajectory to check if the nearest obstacle to each trajectory point is within I map . If there is an unrecorded nearest obstacle, it is denoted as a point of interest and I map and H map are updated, and step (3) is repeated. If not, the optimal trajectory is output

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

[0068] (2-1) When executing the trajectory search algorithm in G map , the coordinates of the occupied grid passed by the motion primitive are stored in the point-of-interest set I, maintaining the uniqueness of the point of interest u.

[0069] (2-2) Based on the obtained point-of-interest set I, the distance information between any point in space and the obstacle in I map is obtained:

[0070] d I (x) = min{||x - i||, i ∈ I}

[0071] In step (2-2), the k-d tree data structure is used to represent the point-of-interest map I map to accelerate the distance information calculation process.

[0072] In step (3), the discretization of the optimization problem. The optimization variable x(t) is discretized to obtain a number of trajectory points x 1 , x 2 , …, x N :

[0073]

[0074] An optimization algorithm using the quasi - Newton method is used to solve the discrete optimization problem.

[0075] Loop optimization in step (4):

[0076] (4 - 1) Discretize to obtain a number of trajectory points (x 1 , x 2 , …, x N ). Query whether the grid occupied most recently by each trajectory point is in the set of interest points I. If it is not in I, then update I T . Optimize H map . The elements of the set of interest points I contain at most all the occupied grids in G map . map

[0077] (4 - 2) If H map is updated, solve the discrete optimization problem in step (3) again.

[0078] The above - mentioned is only the preferred embodiment of the present invention, and it is not any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope claimed by the present invention.

Claims

1. A UAV obstacle avoidance motion planning method based on heuristic environment composition, characterized by: The following steps are involved: (1) Real-time acquisition of environmental data through depth and positioning sensors, establishment of a local grid map centered on the drone, and dynamic update of distance information of the grid within a set range near obstacles; (2) In the grid map, the trajectory search algorithm is used to generate the initial trajectory from the start point to the end point, the detected obstacles are marked as points of interest, and a point of interest map is constructed to approximate the distance information of the grid outside the set range; (3) Combine the grid map with the POI map to form a heuristic map to optimize the safety, smoothness, and feasibility of the trajectory; (4) If new obstacles are detected during the optimization process, the heuristic map is updated and the trajectory is reoptimized until there are no new points of interest.

2. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 1, characterized in that: Step (1) is as follows: Read the depth information of the drone's current depth sensor such as a depth camera or LiDAR t ; Read the current position information of the drone's positioning sensor (R t ,T t ), where R t ∈SO(3) is the rotation matrix describing the pitch, roll and yaw angles of the drone, T t ∈R 3 is the translation vector describing the center of mass position of the drone, fused with D t and (R t ,T t ), construct a local grid map G centered on the drone map , update the set range d near the obstacle set Distance information between inner grid and obstacles.

3. The method for obstacle avoidance motion planning of unmanned aerial vehicle based on heuristic environment composition according to claim 1, characterized in that: Step (2) is as follows: In the grid map G containing distance information map Inside, a trajectory search algorithm is used to find the starting point s∈R 3 To the end point e∈R 3 The initial trajectory x(t):R→R 3 , and the detected obstacle is recorded as the interest point i∈R 3 , build a map of points of interest I map , approximate G map The distance information of the unupdated area, the heuristic map is the union of the grid map and the point of interest map, that is, H map =G map ∪I map , a spatial point x∈R 3 The distance information is: d H (x)=min{d G (x),d I (x)} Among them, d G (x) is the spatial point x in G map The distance value, d I (x) is the spatial point x in I map The distance value.

4. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 3, characterized in that: The heuristic map H in step (2) map The construction includes the following steps: (2-1) In G map When executing the trajectory search algorithm, the coordinates of the occupied grids passed by the moving primitive are stored in the interest point set I to maintain the uniqueness of the interest point i; (2-2) According to the set of interest points I obtained by the search, we can get map The distance information between any point in space and the obstacle: d I (x)=min{||x-i||,i∈I}。 5. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 4, characterized in that: In the step (2-2), a kd tree data structure is used to represent the point of interest map I map , speeding up the distance information calculation process.

6. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 1, characterized in that: Step (3) is as follows: In the heuristic map H map In this paper, an optimization problem considering the obstacle avoidance and control difficulty of multi-rotor UAV trajectory is established: Among them, the optimization variable x(t) represents the starting point s∈R 3 To the end point e∈R 3 The initial trajectory, the objective function F is the trajectory control difficulty and the distance of the trajectory from the obstacle The weighted sum of s and λ o represents the weight, d H (x(t)) represents the value of x(t) in H map In the middle distance value, φ(·) represents the distance penalty function such as a quadratic function, and the constraint indicates that the trajectory speed and acceleration do not exceed the maximum speed v max and the maximum acceleration a max Physical limitations are the feasibility of the trajectory, and Represent the velocity and acceleration of the trajectory respectively, and use the optimization algorithm to solve the optimal trajectory 7. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 6, characterized in that: In step (3), the optimization problem is discretized to obtain several trajectory points x1, x2, …, x N : The quasi-Newton method is used to solve the discrete optimization problem.

8. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 1, characterized in that: Step (4) is as follows: Detecting the optimal trajectory Is the nearest obstacle to each trajectory point within I map If there is a nearest obstacle that has not been recorded, it is recorded as a point of interest and I is updated. map and H map , repeat step (3), if it does not exist, output the optimal trajectory 9. The method for obstacle avoidance motion planning of a UAV based on heuristic environment composition according to claim 8, characterized in that: Loop optimization in step (4): (4-1) Discretize and get several trajectory points (x1, x2, ..., x N ) T , query the grid occupied by each trajectory point recently, whether it is in the interest point set I, if not in I, update I map , optimize H map , the elements of interest point set I contain at most G map All occupied grids in ; (4-2) If H map is updated, and the discrete optimization problem of the optimization problem in step (3) is solved again.

Images