Multi-rotor unmanned aerial vehicle trajectory planning method based on active perception

Abstract

The invention discloses a multi-rotor unmanned aerial vehicle trajectory planning method based on active perception. The method comprises the following steps: S1, acquiring mapping of an environment and positioning information of an unmanned aerial vehicle; s2, obtaining a collision-free initial front-end search path of the unmanned aerial vehicle from the starting point p0 to the ending point pz by using an RRT * algorithm; s3, aiming at the RRT * trajectory generated in the S2, using a minco algorithm to optimize so as to obtain a smooth and obstacle-avoiding trajectory meeting dynamic constraints, comprising the following sub-steps: S3.1, constructing a problem form of an objective function; s3.2, the constraint problem in the S3.1 is converted into an unconstrained problem; s3.3, a solver is used for solving; s4, through graph search problem modeling, searching for balance smoothness and obtaining a yaw angle initial sequence of unknown space information; and S5, obtaining an optimized yaw angle by using the same back-end optimization method. The active perception of the environment is improved, and the flight safety of the unmanned aerial vehicle is improved.

Description

Technical Field

[0001] The present invention belongs to the technical field of multi-rotor unmanned aerial vehicles, and in particular relates to a multi-rotor unmanned aerial vehicle trajectory planning method based on active sensing. Background Art

[0002] With the vigorous development of computer technology, unmanned systems have become an important trend in the development of science and technology. UAVs themselves have the characteristics of small size, light weight, strong adaptability, high concealment and low risk factor. With the continuous maturity of UAV technology, UAVs are widely used in military and civil fields. The tasks of unmanned systems are diverse and complex. Its biggest feature is that there is no human involved in the system loop. The autonomous navigation of unmanned systems without human support will face a huge problem, namely the problem of autonomous movement. In the process of performing tasks, human system navigation can make decisions through human observation and judgment, but in unmanned systems, these tasks will be determined by the system itself, and path planning technology is the key foundation of unmanned autonomous flight technology. The essence of UAV path planning is to consider the constraints of UAV dynamics, environment, etc. to plan a smooth and directly executable trajectory for UAV. The quality of path planning results will directly affect the real-time performance of UAV in completing tasks. Therefore, in order to ensure the safety of UAV flight and the perfection of tasks, it is very necessary to conduct in-depth research on the trajectory planning technology of multi-rotor UAVs.

[0003] The path planning of general quadrotor drones is processed by a coupling method combining these two methods. The commonly used path planning algorithms include global path planning algorithms and local path planning algorithms. The global path planning algorithm includes algorithms based on sampling and graphics methods; the local path planning algorithm includes algorithms based on differential flatness and MPC; algorithms based on sampling such as RRT*, PRM, etc., and algorithms based on graphics methods such as A*, Dijstra algorithm, etc. In general, the global path planning algorithm has a global optimal solution and can handle the complexity of the environment, but the global method is very slow in high-dimensional space and it is difficult to meet dynamic constraints. The local path planning algorithm has local optimality and can handle complex dynamic constraints, but it is necessary to establish a drone model, a high-order front-end path search of the problem, and a back-end trajectory planning method to combine trajectory planning. In addition, for the trajectory planning problem of quadrotor drones, in order to cope with the unknown environment in navigation, the strategies of some trajectory planning methods lack the awareness of the environment, and the drone cannot fly safely at a fast speed in an environment with dense obstacles. Therefore, on the basis of the existing Minco-based trajectory planning, an active perception part is added to solve the unknown environment and improve the safety of drone flight. Summary of the invention

[0004] In order to overcome the shortcomings of the prior art, the present invention provides a multi-rotor UAV trajectory planning method based on active perception. Specifically, in a static environment, a collision-free initial front-end path to the target point is globally planned for the UAV, and then the Minco algorithm is used for optimization on this basis to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints, and the yaw angle is optimized in two steps to improve the active perception of the environment.

[0005] The present invention provides the following technical solutions to solve the above technical problems:

[0006] A multi-rotor UAV trajectory planning method based on active sensing, the method comprising the following steps:

[0007] S1, obtain the environment map and the positioning information of the UAV;

[0008] When planning a drone, select a visual sensor to build a map of the environment and obtain real-time distance information between the drone and obstacles;

[0009] S2, use the RRT* algorithm to obtain the distance from the starting point p0 to the end point p z A collision-free initial front search path;

[0010] When planning the path of a drone in three-dimensional space, the entire continuous space is discretized into a grid map, and the RRT* path search algorithm is used to search for the shortest path from the starting point to the target point;

[0011] S3. Optimize the RRT* trajectory generated in S2 using the minco algorithm to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints, including the following sub-steps:

[0012] S3.1, problem form for constructing the objective function;

[0013] S3.2, transform the constrained problem in S3.1 into an unconstrained problem;

[0014] S3.3, solve using solver;

[0015] S4, for the trajectory optimized in S3, find the initial sequence of yaw angles that balance smoothness and obtain unknown spatial information through graph search problem modeling;

[0016] S5. For the optimal path searched in S4, a smooth and dynamically feasible yaw angle trajectory is planned. Further, the optimized expression of the trajectory in S3.1 is:

[0017]

[0018] T i>0 (2)

[0019]

[0020] in represents the third-order derivative of the UAV trajectory planning, ρ represents the weight coefficient, which is used to balance the smoothing term and time cost, T i represents the time of executing the trajectory, and H represents the function of multiple constraints.

[0021] Furthermore, the process of step S3.2 is as follows:

[0022] Minco is a trajectory parameterization method with waypoints and time as parameters, and its expression is:

[0023]

[0024] Among them, p(t) is M segments of m-dimensional polynomial trajectory, and the trajectory representation of each segment is:

[0025]

[0026] where c i represents the coefficient of the polynomial, β(t) represents the column vector of time series, and q and T distribution represent the intermediate points and duration of the trajectory;

[0027] In formula (1) Calculate the objective function J e The gradient form is The constraint of formula (2) can be obtained by The form of is eliminated and converted into the problem of τ; the multi-constraint function in formula (3) includes obstacle avoidance constraint and feasibility constraint. A segment of trajectory is represented by i. Each segment of trajectory is discretized into j discrete points in the form of time averaging. The state of each discrete point is calculated according to the equation of the trajectory, and the corresponding penalty function is written according to the form of constraint; the feasibility constraint terms include the constraint terms on velocity and acceleration, which are converted into the following form: The obstacle avoidance term is expressed as the square of the distance difference between the distance from the point to the nearest obstacle and the minimum distance threshold. Its specific expression is:

[0028] Furthermore, the S3.3 is to calculate the gradient of the penalty function, and then solve it through the corresponding solver; the quasi-Newton method l-bfgs is used to perform unconstrained optimization solution on the nonlinear function.

[0029] The process of step S4 is as follows:

[0030] For the trajectory optimized by S3.3 above, selectM The yaw angle at the starting position is determined by the current state of the quadrotor. The corresponding graph search yaw angle node is created through the trajectory. Each uniformly distributed position corresponds to a corresponding node k ij , j∈[0,1,...,J] each different node corresponds to the corresponding position and yaw angle ξ and information gain g; for each position node, an edge to the next position node is created. At this time, the graph node of the entire yaw angle search sequence is created, and then a yaw angle sequence with the minimum search cost is found through the graph search algorithm. The cost is expressed as a balance between smoothness and information gain: For information gain, the potential information gain is estimated as the number of unmapped voxels that conform to the camera model used and are not blocked by occupied voxels. Ray casting is performed on each voxel in the camera's field of view to verify its visibility. In order to ensure that all space can be evenly covered by the camera sensor, each voxel contributes the same information gain to the drone. However, for point-to-point navigation, the voxels closer to the trajectory and current position have a greater impact on the drone. Therefore, the information gain of a visible voxel centered on n is biased: Ig(n) = exp{-α l L l (n,p(t))-α s L s (n,p(t))},L l (n,p(t)) and L s (n,p(t)) is the lateral and longitudinal distance to the trajectory p(t).

[0031] Preferably, in S5, a smooth and dynamically feasible yaw angle trajectory is optimized according to the minco backend trajectory optimization algorithm used in S3, wherein there is no constraint on the obstacle avoidance term in the constraint function.

[0032] The present invention provides a multi-rotor UAV trajectory planning method based on active perception. First, a collision-free initial front-end path to the target point is globally planned for the UAV. Then, the Minco algorithm is used for optimization to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints. At the same time, in order to improve the active perception of the environment, the yaw angle is optimized in two steps. The simulation results show that the present invention is simple in design and can obtain an optimized, smooth and obstacle-avoiding trajectory in a complex obstacle environment.

[0033] The beneficial effects of the present invention are: improving active perception of the environment and improving the safety of UAV flight. BRIEF DESCRIPTION OF THE DRAWINGS

[0034] Figure 1 It is a flowchart of the multi-rotor UAV trajectory planning method based on active perception.

[0035] Figure 2 It is a schematic diagram of communication between simulation and experimental platform. DETAILED DESCRIPTION

[0036] In order to make the purpose, technical solution and advantages of the present invention clearer, the technical solution of the present invention is further described in detail below with reference to the accompanying drawings and specific embodiments.

[0037] Reference Figure 1 , a multi-rotor UAV trajectory planning method based on active perception, comprising the following steps: S1, obtaining the environment map and the positioning information of the UAV; S2, using the RRT* algorithm to obtain the trajectory of the UAV from the starting point p0 to the ending point p z An initial front-end search path without collision; S3, for the RRT* trajectory generated in S2, use the minco algorithm to optimize to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints, including the following sub-steps: S3.1, construct the problem form of the objective function; S3.2, transform the constrained problem into an unconstrained problem; S3.3, use the solver to solve; S4, for the optimized trajectory, find the initial sequence of yaw angles that balance smoothness and obtain unknown spatial information through graph search problem modeling; S5, for the optimal path searched in S4, plan a smooth, dynamically feasible yaw angle trajectory;

[0038] In an embodiment of the present invention, in step S1, detection is carried out by a visual sensor installed on a quadrotor drone to provide mapping information of an unknown environment. The visual sensor can be selected from existing visual sensors in the art, such as a binocular camera, a depth camera, etc., which can construct a map of the environment and obtain the distance information between the drone and obstacles in real time.

[0039] When planning a drone, you can choose existing visual sensors in the field, such as binocular cameras, depth cameras, etc., which can build a map of the environment and obtain real-time distance information between the drone and obstacles. Since the field of view of the camera sensor is limited, it will lead to dangerous flight, so subsequent yaw angle trajectory optimization is required.

[0040] In step S2, the RRT* algorithm is used to obtain the distance from the starting point p0 to the end point p0 of the drone. z A collision-free initial front search path.

[0041] When planning the path of a drone in three-dimensional space, the entire continuous space is discretized into a grid map, and the RRT* path search algorithm is used to search for the shortest path from the starting point to the target point. Since the path search only considers the position of the robot's starting point and the target point, and does not consider factors such as the current speed, acceleration, and the dynamic constraints of the drone, the found path will not meet the dynamic constraints of the drone. For example, the discontinuous path curvature will cause the underlying controller to be unable to track the trajectory. Therefore, subsequent trajectory optimization work is required.

[0042] In step S3, the RRT* trajectory generated in S2 is optimized using the minco algorithm to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints, including the following sub-steps:

[0043] S3.1. The optimized expression of the UAV trajectory is:

[0044]

[0045] T i >0 (2)

[0046]

[0047] in represents the third-order derivative of the UAV trajectory planning, ρ represents the weight coefficient, which is used to balance the smoothing term and time cost, T i It represents the time of executing the trajectory, and H represents the function of multiple constraints, which mainly includes the constraints of smoothness term, obstacle avoidance term and feasible term.

[0048] S3.2, Minco is a trajectory parameterization method with waypoints and time as parameters, and its expression is:

[0049]

[0050] Among them, p(t) is M segments of m-dimensional polynomial trajectory, and the trajectory representation of each segment is:

[0051]

[0052] where c i represents the coefficient of the polynomial, β(t) represents the column vector of time series, q and T represent the intermediate points and duration of the trajectory, respectively.

[0053] In formula (1) Calculate the objective function J e The gradient form is The constraint of formula (2) can be obtained by The form of is eliminated and converted into the problem of τ; the multi-constraint function in formula (3) mainly includes obstacle avoidance constraint and feasibility constraint. A section of trajectory is represented by i. Each section of trajectory is discretized into j discrete points in the form of time averaging. The state of each discrete point is calculated according to the equation of the trajectory, and the corresponding penalty function is written according to the form of constraint; the feasibility constraint terms include the constraint terms on velocity and acceleration, which are converted into the following form: The obstacle avoidance term is expressed as the square of the distance difference between the distance from the point to the nearest obstacle and the minimum distance threshold. Its specific expression is:

[0054] S3.3, in order to calculate the gradient of the penalty function, it is solved by the corresponding solver; the quasi-Newton method l-bfgs is used to perform unconstrained optimization solution for the nonlinear function.

[0055] In step S4, for the trajectory optimized in S3, an initial sequence of yaw angles that balances smoothness and obtains unknown spatial information is sought through graph search problem modeling.

[0056] The process of step S4 is: for the trajectory optimized by S3.3 above, select M}, the yaw angle at the starting position is determined by the current state of the quadrotor, and the corresponding graph search yaw angle node is created through the trajectory. Each uniformly distributed position corresponds to the corresponding node k ij , j∈[0,1,...,J] each different node corresponds to the corresponding position and yaw angle and information gain g; for each position node, an edge to the next position node is created. At this time, the graph node of the entire yaw angle search sequence is created, and then a yaw angle sequence with the minimum search cost is found through the graph search algorithm. The cost is expressed as a balance between smoothness and information gain: For information gain, the potential information gain is estimated as the number of unmapped voxels that conform to the camera model used and are not blocked by occupied voxels. Ray casting is performed on each voxel in the camera's field of view to verify its visibility. In order to ensure that all space can be evenly covered by the camera sensor, each voxel contributes the same information gain to the drone. However, for point-to-point navigation, the voxels closer to the trajectory and current position have a greater impact on the drone. Therefore, the information gain of a visible voxel centered on n is biased: Ig(n) = exp{-α l L l (n,p(t))-α s L s (n,p(t))},L l (n,p(t)) and L s(n,p(t)) is the lateral and longitudinal distance to the trajectory p(t).

[0057] In step S5, a smooth and dynamically feasible yaw angle trajectory is planned for the optimal path searched in S4. The smooth and dynamically feasible yaw angle trajectory is optimized according to the minco backend trajectory optimization algorithm used in S3, wherein there is no constraint on the obstacle avoidance term in the constraint function.

[0058] The above is a detailed description and description of the implementation of the present invention in conjunction with the accompanying drawings, but it is not limited to the above-mentioned methods. Within the knowledge of those skilled in the art, as long as the concept of the present invention is the basis, various changes and improvements can be made. At the same time, the present invention is simulated in the visualization interface and qgc detection system in gazebo. The entire simulation and experimental platform communication structure block diagram is shown in the figure. Figure 2 As shown in the figure, the qgc monitoring system can directly perform takeoff, landing and hovering operations on the drone. The visualization in rviz can show the optimized trajectory of the drone in a complex obstacle environment. Experiments show that the method proposed in the present invention has a good effect in quadcopter trajectory planning.

[0059] The contents described in the embodiments of this specification are merely an enumeration of the implementation forms of the inventive concept and are for illustrative purposes only. The protection scope of the present invention should not be considered to be limited to the specific forms described in this embodiment, and the protection scope of the present invention also extends to equivalent technical means that can be thought of by ordinary technicians in this field based on the inventive concept.

Claims

1. A multi-rotor UAV trajectory planning method based on active perception, characterized in that: The method comprises the following steps: S1, obtain the environment map and the positioning information of the UAV; When planning a drone, select a visual sensor to build a map of the environment and obtain real-time distance information between the drone and obstacles; S2, use the RRT* algorithm to obtain the distance from the starting point p0 to the end point p z A collision-free initial front search path; When planning the path of a drone in three-dimensional space, the entire continuous space is discretized into a grid map, and the RRT* path search algorithm is used to search for the shortest path from the starting point to the target point; S3. Optimize the RRT* trajectory generated in S2 using the minco algorithm to obtain a smooth, obstacle-avoiding trajectory that satisfies dynamic constraints, including the following sub-steps: S3.1, problem form for constructing the objective function; S3.2, transform the constrained problem in S3.1 into an unconstrained problem; S3.3, solve using solver; S4, for the trajectory optimized in S3, find the initial sequence of yaw angles that balance smoothness and obtain unknown spatial information through graph search problem modeling; S5. Plan a smooth, dynamically feasible yaw angle trajectory for the optimal path searched in S4.

2. A multi-rotor UAV trajectory planning method based on active perception as claimed in claim 1, characterized in that: The optimized expression of the trajectory of S3.1 is: T i >0 (2) in represents the third-order derivative of the UAV trajectory planning, ρ represents the weight coefficient, which is used to balance the smoothing term and time cost, T i represents the time of executing the trajectory, and H represents the function of multiple constraints.

3. A multi-rotor UAV trajectory planning method based on active perception as claimed in claim 2, characterized in that: The process of step S3.2 is as follows: Minco is a trajectory parameterization method with waypoints and time as parameters, and its expression is: Among them, p(t) is M segments of m-dimensional polynomial trajectory, and the trajectory representation of each segment is: where c i represents the coefficient of the polynomial, β(t) represents the column vector of time series, and q and T distribution represent the intermediate points and duration of the trajectory; In formula (1) Calculate the objective function J e The gradient form is The constraint of formula (2) can be obtained by The form of is eliminated and converted into the problem of τ; the multi-constraint function in formula (3) includes obstacle avoidance constraint and feasibility constraint. A segment of trajectory is represented by i. Each segment of trajectory is discretized into j discrete points in the form of time averaging. The state of each discrete point is calculated according to the equation of the trajectory, and the corresponding penalty function is written according to the form of constraint; the feasibility constraint terms include the constraint terms on velocity and acceleration, which are converted into the following form: The obstacle avoidance term is expressed as the square of the distance difference between the distance from the point to the nearest obstacle and the minimum distance threshold. Its specific expression is:

4. A multi-rotor UAV trajectory planning method based on active perception as claimed in claim 3, characterized in that: S3.3 is to calculate the gradient of the penalty function, and then solve it through the corresponding solver; The quasi-Newton method l-bfgs is used to perform unconstrained optimization on nonlinear functions.

5. A multi-rotor UAV trajectory planning method based on active sensing as described in any one of claims 1 to 4, characterized in that: The process of step S4 is as follows: For the trajectory optimized by S3.3 above, select M }, the yaw angle at the starting position is determined by the current state of the quadrotor, and the corresponding graph search yaw angle node is created through the trajectory. Each uniformly distributed position corresponds to the corresponding node k ij , j∈[0,1,...,J] each different node corresponds to the corresponding position and yaw angle ξ and information gain g; for each position node, an edge to the next position node is created. At this time, the graph node of the entire yaw angle search sequence is created, and then a yaw angle sequence with the minimum search cost is found through the graph search algorithm. The cost is expressed as a balance between smoothness and information gain: For information gain, the potential information gain is estimated as the number of unmapped voxels that conform to the camera model used and are not blocked by occupied voxels. Ray casting is performed on each voxel in the camera's field of view to verify its visibility. In order to ensure that all space can be evenly covered by the camera sensor, each voxel contributes the same information gain to the drone. However, for point-to-point navigation, the voxels closer to the trajectory and current position have a greater impact on the drone. Therefore, the information gain of a visible voxel centered on n is biased: Ig(n) = exp{-α l L l (n,p(t))-α s L s (n,p(t))},,L l (n,p(t)) and L s (n,p(t)) is the lateral and longitudinal distance to the trajectory p(t).

6. A multi-rotor UAV trajectory planning method based on active sensing as claimed in any one of claims 1 to 4, characterized in that: In S5, a smooth and dynamically feasible yaw angle trajectory is optimized according to the minco backend trajectory optimization algorithm used in S3, wherein there is no constraint on the obstacle avoidance term in the constraint function.

Images