Three-dimensional space path planning method for UAV based on 3D lidar sensor

Abstract

The present invention discloses a three-dimensional space path planning method for a drone based on a 3D laser radar sensor, comprising: 1. inputting the target point to be reached according to the needs of the user, and then constructing a three-dimensional grid map according to the point cloud data of the laser radar; 2. planning a path using a hybrid A* algorithm according to the obtained map information; 3. performing secondary optimization on the path using the MinimumSnap algorithm to obtain a path with better performance; 4. as the drone flies, the point cloud data keeps changing, the three-dimensional grid map is updated and the path is recalculated, and the new path is tracked until the target point is found. The present invention applies the traditional hybrid A* algorithm to the path planning of the drone, and performs secondary optimization on the planned path, thereby improving the smoothness and safety of the path, and realizing autonomous navigation in an unknown environment by continuously updating the map.

Description

Technical Field

[0001] The present invention relates to the technical fields of robot path planning technology, unmanned aerial vehicle control, etc., and specifically to a three-dimensional space path planning method for unmanned aerial vehicles based on a 3D laser radar sensor. Background Art

[0002] With the rapid development of drone technology, drone applications are becoming more and more widespread. Due to the wide application of drones in various fields, the variety of usage scenarios and the complexity of tasks, drones often need to perform tasks in dynamic scenes full of obstacles. Existing technologies such as big data and cloud computing cannot solve the problem of path finding and positioning. This requires drones to have the ability to navigate autonomously and fly safely, and to make decisions on their own using information collected by sensors, cameras or global positioning systems (GPS). Path planning is an important technology for autonomous navigation. Although there are many studies on path planning, there are still two problems in its application to drones. First, although traditional algorithms can obtain a traversable path, they only consider the geometric information of the environment and often ignore the kinematic model of the drone itself. Therefore, the trajectory quality obtained is low and is not suitable for direct use as a control command for drones; second, the autonomous navigation of drones often occurs in a dynamic unknown environment, which requires the path planning of drones to meet the requirements of being able to continuously regenerate safe and smooth trajectories in a short time. Summary of the invention

[0003] In order to overcome the shortcomings of the above-mentioned prior art, the present invention provides a three-dimensional space path planning method for an unmanned aerial vehicle based on a 3D lidar sensor, so as to realize the path planning of the unmanned aerial vehicle for scenes in dynamic unknown environments, thereby efficiently solving the problem of low trajectory smoothness in the prior art.

[0004] In order to achieve the above-mentioned purpose, the present invention adopts the following technical scheme:

[0005] The three-dimensional space path planning method of a UAV based on a 3D radar laser sensor of the present invention is characterized by being performed according to the following steps:

[0006] Step 1, taking the 3D laser radar integrated on the UAV as the origin, using the 3D laser radar to obtain point cloud data within the collection area;

[0007] Step 2: pre-processing the collected point cloud data using a point cloud expansion processing algorithm to obtain new point cloud data;

[0008] Step 3, mapping the new point cloud data to a three-dimensional grid map; wherein the state of each grid in the three-dimensional grid map is represented by a Boolean value, if the Boolean value is true, it indicates that the corresponding grid is in an occupied state, if the Boolean value is false, it indicates that the corresponding grid is in an idle state, and the unmapped grids in the three-dimensional grid map are recorded as unknown states;

[0009] Step 4, based on the three-dimensional grid map, using a hybrid A* algorithm to calculate the optimal path of the drone from the starting grid to the end grid;

[0010] Step 4.1: Use formula (1) to construct the motion model of the drone:

[0011] p(t)=[p x (t),p y (t),p z (t)] T (1)

[0012] In formula (1), p x (t), p y (t), p z (t) respectively represent the three-dimensional coordinate value of the grid where the UAV is located at time t in the three-dimensional grid map; p(t) represents the motion trajectory of the UAV at time t; let the dimension parameter μ∈{x,y,z}, and use formula (2) to calculate the motion trajectory p of the dimension parameter μ at time t μ (t);

[0013]

[0014] In formula (2), a k represents the kth coefficient in the polynomial; K represents the order of trajectory definition;

[0015] Step 4.2 uses equations (3) and (4) to calculate the state vector x(t) and control input u(t) of the drone at time t:

[0016]

[0017]

[0018] In formula (3) and formula (4), T represents transposition, represents the derivative of the motion trajectory p(t), and n represents the order of the partial derivative; represents a real number; p (n) (t) represents the nth order derivative of the motion trajectory p(t);

[0019] Step 4.3, use formula (5) to get the cost traj(T1) of the UAV's motion trajectory in period T1:

[0020]

[0021] Step 4.4, apply the Pontryagin minimum principle to solve equation (5) to obtain the estimated cost traj of any path of the drone from the state of the starting grid to the state of the current grid in the three-dimensional grid map: * (T1);

[0022] Step 4.5, using formula (6), calculate the actual cost g of any path of the drone from the state of the starting grid to the state of the current grid in the three-dimensional grid map: c :

[0023]

[0024] Formula (6), J represents the number of grids from the starting grid to the current grid, u j represents the control input of the UAV at the jth grid; τ j represents the time when the UAV passes through the jth grid;

[0025] Step 4.6, use formula (7) to calculate the total cost f of any path of the drone from the state of the starting grid to the state of the target grid c :

[0026] f c =g c +traj * (T1) (7)

[0027] Step 4.7, calculate the total cost of all paths from the state of the starting grid to the state of the target grid according to the process of 4.4 to 4.6, and select the path with the smallest total cost as the optimal path;

[0028] Step 5, smoothing the optimal path and generating a new path;

[0029] Step 5.1: divide the optimal path into h segments according to the three-dimensional coordinate points of each grid that the optimal path passes through, and use the polynomial curve shown in formula (8) to represent each segment trajectory:

[0030]

[0031] In formula (8), p i is the parameter vector of the trajectory of the i-th section in the optimal path; t i represents the moment when the UAV passes the end point of the trajectory of the i-th section;

[0032] Step 5.2: Use equation (9) to construct the parameter vector p consisting of h road segment trajectories:

[0033]

[0034] Step 5.3, use formula (10) to construct the optimization objective function minf(p) of the optimal path:

[0035]

[0036] In formula (10), Q represents a quadratic matrix;

[0037] Step 5.4, using a quadratic matrix solution method to solve the optimization objective function minf(p), obtain a parameter vector p composed of h road segment trajectories, and thus obtain a new optimized trajectory;

[0038] Step 6: Setting the map update time to regularly update the map and continuously track the new optimization trajectory;

[0039] Step 6.1, traverse the three-dimensional coordinate points of each grid in the new optimized trajectory, find the grid closest to the UAV, and use equations (11) and (12) to obtain the velocity v and acceleration a of the UAV tracking the grid closest to the new optimized trajectory;

[0040] v=v e (11)

[0041] a=a e +k p (p e -p cur )+k d (v e -v cur ) (12)

[0042] In formulas (11) and (12), p e 、v e 、a e represents the position, velocity and acceleration of the three-dimensional coordinate point of the grid closest to the drone in the new optimized trajectory; p cur 、v cur 、a cur Indicates the position, velocity and acceleration of the drone at the three-dimensional coordinate point of the current grid; k p , k d is a constant coefficient;

[0043] Step 6.2: Send the three-dimensional coordinate position p of the grid point closest to the drone. e , speed v e and acceleration a e, so that the UAV tracks the nearest grid in the new optimized trajectory, and then returns to step 6.1 to execute until the map update time is reached, and then returns to step 1 to execute until the target grid is reached.

[0044] Compared with the prior art, the present invention has the following beneficial effects:

[0045] 1. The present invention collects point cloud information from the 3D lidar sensor on the drone and establishes a three-dimensional grid map, and performs secondary optimization based on the trajectory generated by the hybrid A*. In actual flight, a safe and smooth trajectory can be quickly generated. Compared with traditional drone path planning algorithms, the path smoothness is improved, and path planning in a dynamic unknown environment is achieved by continuously updating the map and tracking the path.

[0046] 2. The collection of three-dimensional point cloud information is realized by integrating 3D laser radar on the drone, so that the environmental information around the drone can be collected quickly and accurately.

[0047] 3. In step 2 of the present invention, the point cloud data is firstly subjected to morphological expansion processing. The expansion operation is mainly used for denoising and maintaining the original shape, and its effect is like expanding the edge of the point cloud image outward, thereby avoiding collision with obstacles caused by measurement errors of the sensor.

[0048] 4. In step 3 of the present invention, the new point cloud data is mapped to the three-dimensional grid map. A Boolean value is used to represent the state of the three-dimensional grid map, and the unmapped grid represents the unexplored area. This distinguishes the explored area from the unexplored area, and realizes the compact arrangement of the three-dimensional grid map in the memory, reduces the time to access the map, and improves the efficiency of searching the map.

[0049] 5. In step 4 of the present invention, the actual cost and estimated cost in the hybrid A* algorithm are redesigned to obtain the optimal path, realizing path planning that takes into account the kinematic constraints of the UAV itself, and changing the UAV's motion space from discrete space to continuous space, thereby improving the performance of UAV path planning.

[0050] 6. In step 5 of the present invention, the path generated by the hybrid A* is smoothed, thereby improving the path smoothness.

[0051] 7. In step 6 of the present invention, the three-dimensional grid map is updated regularly, and the optimized path trajectory is continuously tracked until the target point is found, thereby realizing path planning in a dynamic unknown environment, thereby improving the safety of autonomous navigation of the UAV. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] Figure 1 It is a schematic diagram of the process of the present invention;

[0053] Figure 2 A schematic diagram of a three-dimensional grid map keying method according to the present invention;

[0054] Figure 3 is a schematic diagram of a path planning method of the present invention;

[0055] Figure 4 This is a schematic diagram of the path optimization of the present invention. DETAILED DESCRIPTION

[0056] In this embodiment, Figure 1 As shown, a three-dimensional space path planning method for a UAV based on a 3D radar laser sensor includes the following steps:

[0057] Step 1: Taking the 3D laser radar integrated on the UAV as the origin, the 3D laser radar is used to obtain point cloud data within the collection area;

[0058] Step 2: pre-processing the collected point cloud data using a point cloud expansion processing algorithm to obtain new point cloud data;

[0059] Step 2.1, in order to avoid the collision with obstacles caused by the measurement error of the sensor, the point cloud data needs to be morphologically expanded as shown in Formula 1. The expansion operation is mainly used for denoising and maintaining the original shape. Its effect is like expanding the edge of the point cloud image outward.

[0060]

[0061] Step 2.2, in equation (1), set A represents the image and set B represents the convolution kernel. A⊕B means that the expansion of B to A is to convolve the point cloud image with the kernel.

[0062] Step 3, mapping the new point cloud data to the three-dimensional grid map; wherein the state of each grid in the three-dimensional grid map is represented by a Boolean value, if the Boolean value is true, it indicates that the corresponding grid is in an occupied state, if the Boolean value is false, it indicates that the corresponding grid is in an idle state, and the unmapped grids in the three-dimensional grid map are recorded as unknown states;

[0063] Step 3.1, such as Figure 2 As shown, after obtaining the expanded point cloud image, the point cloud data is mapped to a three-dimensional grid map.

[0064]

[0065] In formula 2, x i ,y i ,z i Indicates the point cloud position; x o ,y o ,zo represents the starting point of the map; k represents the resolution of the map; x, y, z represent the coordinates of the corresponding grid nodes after the point cloud is mapped. Finally, through the rasterization of the space, the expansion operation of the point cloud and the mapping of the map, a map that can be used for the search algorithm is constructed.

[0066] Step 4, such as Figure 3 As shown, based on the three-dimensional grid map, the hybrid A* algorithm is used to calculate the optimal path of the drone from the starting grid to the end grid;

[0067] Step 4.1, use three independent one-dimensional time parameterized polynomial functions to represent the trajectory. Use formula (1) to construct the UAV motion model:

[0068] p(t)=[p x (t),p y (t),p z (t)] T (3)

[0069] In formula (3), p x (t), p y (t), p z (t) represents the three-dimensional coordinate value of the grid where the UAV is located at time t in the three-dimensional grid map; p(t) represents the motion trajectory of the UAV at time t; let the dimension parameter μ∈{x,y,z}, and use formula (4) to calculate the motion trajectory p of the dimension parameter μ at time t μ (t);

[0070]

[0071] In formula (4), a k represents the kth coefficient in the polynomial; K represents the order of trajectory definition;

[0072] Step 4.2 From the perspective of the quadrotor system, it corresponds to a linear time-invariant (LTI) system. The state vector x(t) and control input u(t) of the drone at time t are calculated using equations (4) and (5) respectively:

[0073]

[0074]

[0075] In formula (5) and formula (6), T represents transposition, represents the derivative of the motion trajectory p(t), and n represents the order of the partial derivative; represents a real number; p (n) (t) represents the nth order derivative of the motion trajectory p(t);

[0076] Step 4.3 Define the state space model:

[0077]

[0078]

[0079] Step 4.4 The complete solution of the state equation is expressed as:

[0080]

[0081] Step 4.5, use formula (10) to get the cost traj(T1) of the UAV's motion trajectory in period T1:

[0082]

[0083] Step 4.6, apply the Pontryagin minimum principle to solve equation (10) and obtain the estimated cost traj of any path from the state of the starting grid to the state of the current grid in the three-dimensional grid map of the UAV * (T1);

[0084] Step 4.7, use formula (11) to calculate the actual cost g of any path from the state of the starting grid to the state of the current grid in the three-dimensional grid map of the drone: c :

[0085]

[0086] Formula (11), J represents the number of grids from the starting grid to the current grid, u j represents the control input of the UAV at the jth grid; τ j represents the time when the UAV passes through the jth grid;

[0087] Step 4.8, use formula (12) to calculate the total cost f of any path of the drone from the state of the starting grid to the state of the target grid c :

[0088] f c =g c +traj * (T1) (12)

[0089] Step 4.9, calculate the total cost of all paths from the state of the starting grid to the state of the target grid according to the process of 4.4 to 4.6, and select the path with the smallest total cost as the optimal path;

[0090] Step 5, smoothing the optimal path and generating new path points;

[0091] Step 5.1, decompose the trajectory generated in step 4. Since the calculation of three-dimensional trajectory is very complicated, in order to simplify the calculation, it is necessary to decompose the three-dimensional trajectory into x, y, and z dimensional trajectories.

[0092] Step 5.2, such as Figure 4 As shown, according to the three-dimensional coordinate points of each grid passed by the optimal path, the optimal path is divided into h segments of trajectory, and the polynomial curve shown in formula (8) is used to represent the trajectory of each segment:

[0093]

[0094] In formula (13), p i is the parameter vector of the trajectory of the i-th section in the optimal path; t i represents the moment when the UAV passes the end point of the trajectory of the i-th section;

[0095] Step 5.3, use formula (14) to calculate the parameter vector p composed of multiple trajectories:

[0096]

[0097] In step 5.4, the trajectory must satisfy a series of constraints, the two ends of the continuous trajectory have no collision, and the trajectory is smooth. In this way, the problem can be described as a constrained optimization problem.

[0098]

[0099]

[0100]

[0101]

[0102]

[0103] The trajectory optimization problem is modeled as a mathematical quadratic programming problem.

[0104] Step 5.5. Construct constraint objectives. Setting the position, velocity, acceleration or higher of a certain point to a specific value can form an equality constraint as shown in equation (18). Since the intermediate point needs to be passed, an equality constraint is also constructed for the position of the intermediate point. The position, velocity and acceleration between adjacent segments can form an equality constraint.

[0105] Position constraints:

[0106]

[0107]

[0108] In formula (18), t i represents the moment when the UAV passes the end point of the i-th trajectory, h is the number of segments into which the optimal path is segmented, n represents the order of the trajectory, and p i , v i , a i Represents the position, velocity, and acceleration constraints of the drone when it passes the end point of the i-th trajectory.

[0109] Step 5.4, using the quadratic matrix solution method to solve the optimization objective function minf(p), obtain the parameter vector p composed of the h road segment trajectories, and thus obtain a new optimized trajectory;

[0110] Step 5.5, finally, the p values ​​of the three dimensions x, y, and z are synthesized into the optimized trajectory.

[0111] Step 6: Set the map update time to update the map regularly and continuously track new optimization trajectories;

[0112] Step 6.1, traverse the three-dimensional coordinate points of each grid in the new optimized trajectory, find the grid closest to the UAV, and use equations (11) and (12) to obtain the velocity v and acceleration a of the UAV tracking the grid closest to the new optimized trajectory;

[0113] v=v e (19)

[0114] a=a e +k p (p e -p cur )+k d (v e -v cur ) (20)

[0115] In formulas (19) and (20), p e 、v e 、a e represents the position, velocity and acceleration of the three-dimensional coordinate point of the grid closest to the drone in the new optimized trajectory; p cur 、v cur 、a cur Indicates the position, velocity and acceleration of the drone at the three-dimensional coordinate point of the current grid; k p , k d is a constant coefficient;

[0116] Step 6.2: Send the three-dimensional coordinate position p of the grid point closest to the drone. e , speed v e and acceleration a e, so that the UAV tracks the nearest grid in the new optimized trajectory, and then returns to step 6.1 to execute until the map update time is reached, and then returns to step 1 to execute until the target grid is reached.

Claims

1. A three-dimensional space path planning method for unmanned aerial vehicles based on 3D radar laser sensors, characterized in that Proceed as follows: Step 1, taking the 3D laser radar integrated on the UAV as the origin, using the 3D laser radar to obtain point cloud data within the collection area; Step 2, pre-processing the collected point cloud data using the point cloud expansion processing algorithm shown in formula (1a) to obtain new point cloud data; (1a) In formula (1a), A represents the point cloud image, and B represents the convolution kernel; represents convolution; Step 3, mapping the new point cloud data to a three-dimensional grid map; wherein the state of each grid in the three-dimensional grid map is represented by a Boolean value, if the Boolean value is true, it indicates that the corresponding grid is in an occupied state, if the Boolean value is false, it indicates that the corresponding grid is in an idle state, and the unmapped grids in the three-dimensional grid map are recorded as unknown states; Step 4, based on the three-dimensional grid map, using a hybrid A* algorithm to calculate the optimal path of the drone from the starting grid to the end grid; Step 4.1: Use formula (1) to construct the motion model of the drone: (1) In formula (1), They respectively represent the three-dimensional coordinate values ​​of the grid where the drone is located at time t in the three-dimensional grid map; represents the motion trajectory of the drone at time t; let the dimension parameter μ ∈ {x, y, z}, and use formula (2) to calculate the motion trajectory of the dimension parameter μ at time t ; (2) In formula (2), represents the kth coefficient in the polynomial; K represents the order of trajectory definition; Step 4.2 Use equations (3) and (4) to calculate the state vector of the drone at time t: and control input : (3) (4) In formula (3) and formula (4), represents transpose, Represents the motion trajectory The derivative of represents the order of partial derivative; represents a real number; Represents the motion trajectory No. Derivatives; Step 4.3, use formula (5) to get the UAV The cost of the movement trajectory of the time period : (5) Step 4.4, apply the Pontryagin minimum principle to solve equation (5) to obtain the estimated cost of any path of the drone from the state of the starting grid to the state of the current grid in the three-dimensional grid map: ; Step 4.5, using formula (6), calculate the actual cost of any path of the drone from the state of the starting grid to the state of the current grid in the three-dimensional grid map: : (6) Formula (6), J represents the number of grids passed from the starting grid to the current grid, represents the control input of the UAV at the jth grid; represents the time when the UAV passes through the jth grid; Step 4.6, use formula (7) to calculate the total cost of any path of the drone from the state of the starting grid to the state of the target grid : (7) Step 4.7, calculate the total cost of all paths from the state of the starting grid to the state of the target grid according to the process of 4.4 to 4.6, and select the path with the smallest total cost as the optimal path; Step 5, smoothing the optimal path and generating a new path; Step 5.1: divide the optimal path into h segments according to the three-dimensional coordinate points of each grid that the optimal path passes through, and use the polynomial curve shown in formula (8) to represent each segment trajectory: (8) In formula (8), is the parameter vector of the trajectory of the i-th section in the optimal path; represents the moment when the UAV passes the end point of the trajectory of the i-th section; Step 5.2: Use equation (9) to construct the parameter vector consisting of h road segment trajectories: : (9) Step 5.3: Use formula (10) to construct the optimization objective function of the optimal path : (10) In formula (10), represents a quadratic matrix; Step 5.4, using the quadratic matrix solution method to optimize the objective function Solve and obtain the parameter vector composed of h road segment trajectories , thus obtaining a new optimization trajectory; Step 6: Setting the map update time to regularly update the map and continuously track the new optimization trajectory; Step 6.1, traverse the three-dimensional coordinate points of each grid in the new optimized trajectory, find the grid closest to the drone, and use equations (11) and (12) to obtain the speed of the drone tracking the grid closest to the new optimized trajectory: and acceleration ; (11) (12) In formulas (11) and (12), , , Represents the position, velocity and acceleration of the three-dimensional coordinate point of the grid closest to the drone in the new optimized trajectory; Indicates the position, velocity and acceleration of the drone at the three-dimensional coordinate point of the current grid; is a constant coefficient; Step 6.2: Send the 3D coordinates of the nearest grid point to the drone. ,speed and acceleration , so that the UAV tracks the nearest grid in the new optimized trajectory, and then returns to step 6.1 to execute until the map update time is reached, and then returns to step 1 to execute until the target grid is reached.

Images