Incremental three-dimensional mapping method for intelligent obstacle avoidance of unmanned aerial vehicle

Abstract

An incremental three-dimensional mapping method for intelligent obstacle avoidance of unmanned aerial vehicles, 1, a three-dimensional sliding grid map model is established; 2, pretreatment of odometer and depth information, obtain odometer, obtain depth information, time alignment of odometer and depth, down sampling and coordinate transformation of depth, convert depth information under carrier coordinate system into point cloud of world coordinate system; 3, build occupancy grid map, fuse the measured occupancy information into the occupancy grid map; 4, use breadth-first search algorithm BFS to expand the map, in each occupancy grid map update, use lower array to record the index of the grid state in the occupancy grid map from free to occupied, use raise array to record the index of the grid state in the occupancy grid map from occupied to free. The method overcomes the problem of large time consumption of large-scale and large-size map expansion, is suitable for three-dimensional mapping for autonomous obstacle avoidance of unmanned aerial vehicles, improves the three-dimensional mapping efficiency, and further improves the obstacle avoidance success rate.

Description

Technical Field

[0001] This invention belongs to the field of autonomous obstacle avoidance technology for unmanned aerial vehicles (UAVs), and proposes an incremental 3D mapping method for intelligent obstacle avoidance of UAVs, particularly relating to an incremental 3D mapping method for intelligent obstacle avoidance of UAVs. Background Technology

[0002] Unmanned aerial vehicles (UAVs), characterized by high agility and strong payload capacity, have been widely used in recent years in fields such as power line inspection, autonomous exploration, and emergency rescue. In unknown and complex environments, real-time autonomous mapping and planning are key technologies for UAVs to achieve these applications. Typically, UAVs use sensors such as stereo vision and LiDAR to acquire information about surrounding obstacles and express information such as the occupancy probability or distance of objects in space through a map building system. Trajectory planning is a multi-objective optimization problem, characterized by point-to-point planning and soft distance constraints. Therefore, expanding objects by a certain size according to the size of the UAV based on the original map is one of the important means to ensure the safety of the UAV.

[0003] Map inflation expands the original map by a certain size, creating virtual object regions and thus limiting the area that the drone's center of mass can actually reach. Map inflation is generally a necessary step in map planning and can improve the safety of flight trajectories. First, mainstream planning algorithms abstract the drone as a point mass, not considering the drone's shape during the search and optimization process. Second, when the drone's control precision and depth measurement accuracy are not high, a desired trajectory that gets too close to an object may lead to flight accidents. Finally, collision constraints in trajectory optimization often exist in the form of "soft constraints." Even if a large safety distance threshold is selected, it may still result in a desired trajectory that gets too close to an object.

[0004] When the expansion size and local update range are small, the additional computational cost introduced by map expansion is negligible. However, for drones with larger wheelbases, which require larger update ranges and expansion sizes, the time consumption of classic expansion algorithms increases significantly.

[0005] The existing technology is as follows:

[0006] Application No.: CN201910655674.8, Application Title: A Method, Apparatus, and UAV for Obstacle Avoidance by a UAV Based on Target Tracking. This invention relates to the field of autonomous obstacle avoidance technology for UAVs, and particularly to a method, apparatus, and UAV for obstacle avoidance by a UAV based on target tracking. The method includes: determining the expected forward velocity of the UAV, which is used to maintain an initial horizontal distance between the UAV and the target; acquiring a depth map of the environment in front of the UAV, and determining a grid map centered on the UAV based on the depth map; determining the optimal flight direction of the UAV and the minimum distance between the UAV and the obstacle based on the grid map; determining the optimal flight speed of the UAV based on the minimum distance, the expected forward velocity, and the optimal flight direction; and controlling the UAV to fly at the optimal flight speed along the optimal flight direction to avoid obstacles in front of the UAV. Through the above method, this invention can perform path planning for the entire flight space, improving the accuracy of autonomous obstacle avoidance during target tracking.

[0007] That application used a drone-centric grid map, ignoring a large amount of historical information. Our approach, however, uses a 3D sliding grid map centered on the drone. The true origin of the sliding map is the world coordinate system, moving with the drone's movement. This allows for large-scale environment modeling of the drone using the limited memory of its onboard computer. Simultaneously, we integrate historical depth map information into a single sliding grid map, preserving historical information and mitigating map errors caused by depth map measurement errors to a limited extent.

[0008] Application No.: CN201811049054.1, Application Title: A Visual Obstacle Avoidance Method for Unmanned Aerial Vehicles (UAVs) Based on Distributed Maps. This method constructs a distributed map through collaboration between the UAV and a ground station for visual obstacle avoidance flight. The UAV generates a new environmental map only when it detects entering a new environment by fusing IMU, VO, and GPS real-time pose perception. Simultaneously, the UAV receives flight commands from the ground station and controls its flight by changing the speed of its four motors to achieve visual obstacle avoidance. The ground station is used to optimize and store the environmental map constructed by the UAV and to visualize the environmental map. The ground station is also responsible for planning a safe flight path based on the current UAV pose and the environmental map, and then sending it to the UAV. This invention utilizes the collaboration between the UAV and the ground station and a distributed map architecture to accurately model the environment, making it suitable for visual obstacle avoidance of UAVs in complex environments.

[0009] That application uses an occupied grid map to represent obstacles in the environment. Our approach uses both an occupied grid map and an expanded map to represent obstacles. Drone obstacle avoidance planning is typically done as a point mass, without considering the drone's shape. Searching for a safe path solely on a grid map can lead to collisions. The rapidly expanded map, by adding a virtual occupied area for obstacles, overcomes the problems caused by point mass planning.

[0010] The application uses an occupancy probability value of 0 or 1. We, however, use a logarithmic occupancy probability value, ranging from -∞ to ∞, which can filter and fuse erroneous measurements from sensors such as depth, exhibiting good robustness.

[0011] Application No.: CN201910740462.X, Application Title: An Obstacle Avoidance Method for Quadrotor UAVs Based on Laser Detection. This method involves: firstly, using the Hector SLAM method to establish a static grid map of the flight site based on a laser radar; secondly, using the A* algorithm to plan the shortest path from the starting point to the target point; and thirdly, using the inertial measurement unit inside the Pixhawk flight control board to calculate the quadcopter UAV's odometer information; then, using an adaptive Monte Carlo positioning algorithm to obtain the coordinate transformation of the quadcopter UAV relative to the map; fourthly, using the laser radar to establish a local cost map during the UAV's flight; and finally, for dynamic obstacles appearing during flight, using a dynamic window algorithm to avoid them based on the established local cost map, and performing local path planning until the UAV returns to the global shortest path obtained by the A* algorithm. This invention has the advantages of being simple and convenient, having fast positioning speed, strong autonomy, and good real-time performance.

[0012] This obstacle avoidance plan utilizes a static grid map of the flight site built using LiDAR. We primarily describe the method for constructing an expanded map based on this grid map, effectively overcoming collision problems caused by UAV size, control errors, and depth measurement errors.

[0013] Application No.: CN201711465023.X, Application Title: A Method and System for Obstacle Avoidance by Unmanned Aerial Vehicles (UAVs). The method involves a millimeter-wave radar detecting an obstacle ahead, followed by the UAV capturing video images of the obstacle using a front-facing camera. The type of obstacle is analyzed from the video images, and the shape and distance of the obstacle from the UAV are determined by combining the video images and the millimeter-wave radar detection results. A grid map is constructed based on the obstacle shape and distance, and a flight path is planned on the grid map using the A-Star algorithm, adjusting the flight path according to a safety radius. The UAV then flies along the flight path, avoiding or traversing the obstacle. Beneficial effects: By utilizing a front-facing camera and millimeter-wave radar to acquire information on obstacle shape, type, and distance from the UAV, obstacle detection becomes more accurate and comprehensive, resulting in high accuracy in obstacle avoidance. The UAV can achieve automatic obstacle avoidance with a high degree of intelligence.

[0014] That application used a grid map construction method based on millimeter-wave radar and video images, while our method uses a point cloud-based grid map construction method. Since current millimeter-wave sensors lack the ability to describe obstacle shapes, obstacle size mainly relies on video image analysis, which is time-consuming. We directly use point clouds to effectively describe obstacle shapes and distances.

[0015] This application directly uses a wall-following algorithm to adjust the distance between the drone and obstacles. We, on the other hand, provide an inflated map, enabling other users to perform distance field calculations based on this map, thus providing a foundation for designing more sophisticated paths. Summary of the Invention

[0016] To address the above issues, this application proposes an incremental 3D mapping method for intelligent obstacle avoidance in unmanned aerial vehicles (UAVs). This method overcomes the problem of time-consuming large-scale map expansion, is suitable for 3D mapping of UAVs for autonomous obstacle avoidance, improves 3D mapping efficiency, and thus increases the obstacle avoidance success rate.

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

[0018] This invention provides an incremental 3D mapping method for intelligent obstacle avoidance in unmanned aerial vehicles (UAVs), comprising the following steps:

[0019] (1) Establish a three-dimensional sliding grid map model and store map information using an array;

[0020] By moving the map origin, a sliding grid map can be implemented, allowing a point p in space to be mapped. w The mapping relationship between information and array indices is shown in the formula:

[0021]

[0022] in, This represents the origin of the map, which moves as the drone moves; r represents the map resolution.

[0023] Then, based on how the array is implemented in the computer, the corresponding index is obtained, and the access method is shown in the formula:

[0024] index=size(0)×size(1)×voxel(0)+size(1)×voxel(1)+voxel(2)

[0025] Where index represents the array index and size represents the side length of the map;

[0026] (2) Preprocess odometry and depth information;

[0027] Obtain the depth map, denoted as D∈R H×W By aligning the odometry and depth time, and then downsampling and transforming the depth, the depth information in the vehicle coordinate system is converted into a point cloud in the world coordinate system.

[0028] p w =RKD u，v，dd +t

[0029] Where K represents the intrinsic parameter matrix of the depth camera, and D u，v，d The index represents the depth map, and the odometry (R, t) represents the coordinate mapping relationship from the vehicle coordinate system to the world coordinate system;

[0030] (3) Construct an occupied grid map;

[0031] Assuming the map observation process exhibits Markov property and that measurements between voxels are independent, ray projection traverses the entire depth map, reducing the occupancy probability of voxels along the ray's path and increasing the occupancy probability of the voxel ultimately hit. Bayesian filtering updates the voxel occupancy probability as follows:

[0032]

[0033] Among them, D t x represents the depth map observed at time t. t P(m) represents the pose of the UAV at time t; t ) and P(m t-1 ) represents the confidence level of the occupied raster map at times t and t-1; p(m0) represents the initial confidence level of the map; P(m|D t x t ) is the measurement model, representing the change in map probability caused by the depth measurement at time t;

[0034] Using logarithmic probability Rewrite the above equation:

[0035] L(m t )=L(m t-1 )+L(m|D t x t )-L(m0)

[0036] Considering the dynamic characteristics of objects in the working environment, maximum and minimum constraints are added to the above equation.

[0037] L(m t )=min{max[L(m t-1 )+L(m|D t x t )-L(m0), L min ], L max}

[0038] The prior probability of the map is set to 0.5, i.e., L(m0) = 0, indicating that the entire map has not yet been detected. The measurement model L(m|D t x t ) represents the log-update probability after observing a grid cell;

[0039] (4) Use the BFS algorithm to expand the map. In each update of the occupied grid map, use the lower array to record the index of the grid state in the occupied grid map that changes from free to occupied, and use the raise array to record the index of the grid state in the occupied grid map that changes from occupied to free. Finally, use the BFS algorithm to traverse the areas near the elements of the lower and raise arrays respectively, and set the collision state respectively to realize incremental expansion map update.

[0040] As a further improvement of the present invention, step (2) preprocesses the odometer and depth information to obtain a depth map through stereo vision, lidar and millimeter-wave radar.

[0041] As a further improvement of the present invention, the calculation of the downsampling step size in step (2) is as follows:

[0042] In the depth map D∈R H×W Let D1 = (u1, v1, d1) and D2 = (u2, v2, d2) be any two points. The difference between their actual coordinates in the world coordinate system is:

[0043]

[0044] Assuming the depth values ​​in the depth map are continuous, i.e., ignoring depth jumps at object edges, the downsampling parameters are calculated based on the maximum measurement value of the depth map:

[0045]

[0046] Where Δu and Δv are the horizontal and vertical downsampling step sizes of the depth map, respectively, and f x and f y It is the focal length parameter of the depth camera, d max is the user-specified maximum depth, and r is the map resolution.

[0047] As a further improvement to the present invention, the specific algorithm for processing the raise and lower arrays in step (4) is as follows:

[0048] The update algorithm for the raise array is as follows;

[0049] Input: the `raise` array;

[0050] Step 1. In the occupancy probability update step, add the grid cells whose state has become free to the raise array;

[0051] Step 2. For all elements in the raise array, clear the inflation;

[0052] Step 3. Apply the BFS algorithm to each element in the raise array; set the expansion range to 2*L according to the obstacle avoidance requirements; then specify the neighbor index of the grid to facilitate BFS expansion; if a grid in the occupied state is encountered during the expansion process, add its index to the temp data, otherwise clear the expansion.

[0053] Output: lower array & intermediate expansion map;

[0054] The update algorithm for the lower array;

[0055] Input: the lower array

[0056] Step 1. In the occupancy probability update step, add the grid cells whose state becomes occupied to the lower array; and add all elements of the temp data when updating the raise data to the lower array;

[0057] Step 2. For all elements in the lower array, inflate the array;

[0058] Step 3. Apply the BFS algorithm to each element in the lower array; set the expansion range to L according to the obstacle avoidance requirements; then specify the neighbor index of the grid to facilitate BFS expansion.

[0059] Output: Inflated map.

[0060] Beneficial effects:

[0061] This invention discloses an incremental UAV planning map expansion method based on BFS. The method designs a 3D sliding grid map model, utilizing GNSS, SLAM, stereo vision, LiDAR, and millimeter-wave radar to obtain odometry and depth maps. Then, the odometry and depth are time-aligned, and the depth is downsampled and transformed to convert the depth information in the carrier coordinate system into a point cloud in the world coordinate system. Next, using ray casting and Bayesian filtering algorithms, the measured occupancy information is fused into the occupancy grid map, achieving fusion of multiple depth observations and improving map accuracy. In each update of the occupancy grid map, the `lower` array records the index of a grid state changing from free to occupied, and the `raise` array records the index of a grid state changing from occupied to free. Finally, the BFS algorithm is used to update the expansion state of grids in the specified expansion area near `lower` and `raise`. Thus, the downstream planner queries the occupancy information in the expanded map layer, enabling autonomous flight of the UAV. Attached Figure Description

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

[0063] Figure 2 This is a schematic diagram of the sliding map disclosed in this invention. Detailed Implementation

[0064] 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.

[0065] The present invention discloses a flowchart as follows: Figure 1 The proposed incremental UAV map expansion method based on BFS includes the following steps:

[0066] Step 1: Establish a 3D sliding grid map model, such as... Figure 2 As shown, an array is used to store map information. A sliding grid map is implemented by moving the map origin. A point p in space... W The mapping relationship between information and array indices is shown in the formula:

[0067]

[0068] in, The origin of the map is represented by r, which moves as the drone moves.

[0069] Then, based on how the array is implemented in the computer, the corresponding index is obtained, and the access method is shown in the formula:

[0070] index=size(0)×size(1)×voxel(0)+size(1)×voxel(1)+voxel(2)

[0071] Here, index represents the array index, and size represents the side length of the map.

[0072] Step 2: Preprocess odometry and depth information. Obtain a depth map using stereo vision, LiDAR, millimeter-wave radar, etc., denoted as D∈R. H×W Align the odometry and depth time, then downsample the depth and perform coordinate transformation to convert the depth information in the carrier coordinate system into a point cloud in the world coordinate system.

[0073] (2-1) The formula for calculating coordinate transformation is as follows:

[0074] p w =RKD u，v，d +t

[0075] Where K represents the intrinsic parameter matrix of the depth camera, and D u，v，d The index represents the depth map, and the odometry (R, t) represents the coordinate mapping relationship from the carrier coordinate system to the world coordinate system.

[0076] (2-2) The selection of the downsampling step size is based on the following:

[0077] In the depth map D∈R H×W Let D1 = (u1, v1, d1) and D2 = (u2, v2, d2) be any two points. The difference between their actual coordinates in the world coordinate system is:

[0078]

[0079] Assuming the depth values ​​in the depth map are continuous, i.e., ignoring depth jumps at object edges, the downsampling parameters can be calculated based on the maximum measurement value of the depth map:

[0080]

[0081] Where Δu and Δu are the horizontal and vertical downsampling step sizes of the depth map, respectively, and f x and f y It is the focal length parameter of the depth camera, d max is the user-specified maximum depth, and r is the map resolution.

[0082] Step 3: Construct an occupancy grid map. Assuming the map observation process exhibits Markov property and that measurements between voxels are independent, ray projection traverses the entire depth map, reducing the occupancy probability of voxels along the ray's path and increasing the occupancy probability of the voxel ultimately hit. Specifically, Bayesian filtering updates the voxel occupancy probabilities:

[0083]

[0084] Among them, D t x represents the depth map observed at time t. t P(m) represents the pose of the UAV at time t; t ) and P(m t-1 ) represents the confidence level of the occupied raster map at times t and t-1; p(m0) represents the initial confidence level of the map; P(m|D t x t ) is the measurement model, representing the change in map probability caused by the depth measurement at time t.

[0085] Using logarithmic probability Rewrite the above equation:

[0086] L(m t )=L(m t-1 )+L(m|D t x t )-L(m0)

[0087] Considering the dynamic characteristics of objects in the working environment, maximum and minimum constraints are added to the above equation.

[0088] L(m t )=min{max[L(m t-1 )+L(m|D t x t )-L(m0), L min ], L max}

[0089] The prior probability of the map is set to 0.5, i.e., L(m0) = 0, indicating that the entire map has not yet been detected. The measurement model is L(m|D t x t ) represents the log-up update probability after observing a grid.

[0090] Step 4: Expand the map using the BFS algorithm. In each update of the occupied grid map, the `lower` array records the index of a grid cell that changes from free to occupied, and the `raise` array records the index of a grid cell that changes from occupied to free. Finally, using the BFS algorithm, traverse the regions near the elements in the `lower` and `raise` arrays respectively, setting collision states to achieve incremental map expansion and improve the efficiency of large-scale map expansion updates. The specific algorithm for processing the `raise` and `lower` arrays is as follows:

[0091]

[0092]

[0093] 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. An incremental 3D mapping method for intelligent obstacle avoidance in unmanned aerial vehicles (UAVs), comprising the following steps, characterized in that: (1) Establish a three-dimensional sliding grid map model and store map information using an array; By moving the map origin, a sliding grid map can be implemented, allowing a point in space to be viewed. The mapping relationship between information and array indices is shown in the formula: ； wherein, represents an origin of the map, which moves as the drone moves, represents a map resolution; Then, based on how the array is implemented in the computer, the corresponding index is obtained, and the access method is shown in the formula: ； wherein, denotes an index of an array, denotes a side length of a map; (2) Preprocess odometry and depth information; Obtain the depth map, denoted as By aligning the odometry and depth time, and then downsampling and transforming the depth, the depth information in the vehicle coordinate system is converted into a point cloud in the world coordinate system. ； in, This represents the intrinsic parameter matrix of the depth camera. Index representing depth map, odometry This indicates the coordinate mapping relationship from the carrier coordinate system to the world coordinate system; (3) Construct an occupied grid map; The map observation process exhibits Markov property and the measurements between voxels are independent. By traversing the entire depth map through ray projection, the occupancy probability of voxels along the ray's path is reduced, while the occupancy probability of the voxel ultimately hit is increased. Specifically, Bayesian filtering updates the voxel occupancy probability. ； in, This represents the depth map observed at time t. This represents the pose of the UAV at time t; and This represents the confidence level of the occupied raster map at times t and t-1; Indicates the initial confidence level of the map; It is a measurement model, representing the change in map probability caused by depth measurement at time t; Using log probabilities Rewriting the above equation: ； Considering the dynamic characteristics of objects in the working environment, maximum and minimum constraints are added to the above equation. ； The prior probability of the map is set to 0.5, i.e. , indicating that the entire map has not been detected, the measurement model is the log-likelihood of observing a cell. (4) Use the BFS algorithm to expand the map. In each update of the occupied grid map, use the lower array to record the index of the grid state in the occupied grid map that changes from free to occupied, and use the raise array to record the index of the grid state in the occupied grid map that changes from occupied to free. Finally, use the BFS algorithm to traverse the areas near the elements of the lower and raise arrays respectively, and set the collision state respectively to realize incremental expansion map update.

2. The incremental 3D mapping method for intelligent obstacle avoidance in unmanned aerial vehicles according to claim 1, characterized in that: Step (2) preprocesses the odometer and depth information to obtain a depth map using stereo vision, lidar, and millimeter-wave radar. 3.The incremental three-dimensional mapping method for intelligent obstacle avoidance of UAV according to claim 1, wherein: The downsampling step size in step (2) is calculated as follows: In depth map Choose any two points and The actual coordinate difference between these two points in the world coordinate system is: ； The depth values ​​in the depth map are continuous, meaning depth jumps at object edges are ignored. The downsampling parameters are calculated based on the maximum measurement value of the depth map. ； in, and It refers to the horizontal and vertical downsampling step size of the depth map. and These are the focal length parameters of the depth camera. It is the maximum depth specified by the user. That is the map resolution. 4.The incremental three-dimensional mapping method for intelligent obstacle avoidance of UAV according to claim 1, wherein: The specific algorithm for processing the raise and lower arrays in step (4) is as follows: The update algorithm for the raise array is as follows; Input: raise array; Step 1. In the occupancy probability update step, add the grid cells whose state has become free to the raise array; Step 2. For all elements in the `raise` array, clear the inflation; Step 3. Apply the BFS algorithm to each element in the raise array; set the expansion range to 2*L according to the obstacle avoidance requirements; then specify the neighbor index of the grid to facilitate BFS expansion; if a grid in the occupied state is encountered during the expansion process, add its index to the temp data, otherwise clear the expansion. Output: lower array & intermediate expansion map; The update algorithm for the lower array; Input: lower array Step 1. In the occupancy probability update step, add the grid cells whose state becomes occupied to the lower array; and add all elements of the temp data when updating the raise data to the lower array; Step 2. For all elements in the lower array, inflate the array; Step 3. Apply the BFS algorithm to each element in the lower array; set the expansion range to L according to the obstacle avoidance requirements; then specify the neighbor index of the grid to facilitate BFS expansion; Output: Inflated map.

Images