Path planning method of quad-rotor unmanned aerial vehicle

Abstract

The invention discloses a path planning method for a quad-rotor unmanned aerial vehicle. The method comprises the following steps: acquiring a voxel map through a visual sensor of a quadrotor unmanned aerial vehicle, establishing a Voronoi map according to the voxel map, and solving three-dimensional points of the Voronoi map near an obstacle in the voxel map through a quadratic programming solver to obtain a plurality of path points; updating the plurality of path points through the ESDF gradient field to obtain a front-end path point set; establishing a multi-order B spline curve according to the front-end path point set; extracting control points according to a preset constraint condition through a B spline curve to obtain a plurality of control point groups, and aggregating the plurality of control point groups to obtain an aggregated control point group; performing path search on the aggregation control point group based on an A * algorithm to obtain an initial path trajectory; and optimizing the initial path trajectory through a preset rule to obtain a final path trajectory. Compared with the prior art, the path planning trajectory of the quad-rotor unmanned aerial vehicle provided by the invention is higher in quality and efficiency.

Description

Technical Field

[0001] The present invention is applicable to the technical field of unmanned aerial vehicles, and in particular relates to a path planning method for a quad-rotor unmanned aerial vehicle. Background Art

[0002] Quadrotors are a common type of drone with high flexibility. They can detect tasks that are helpful to humans in real time during common detection tasks, such as generating terrain, building three-dimensional maps, and generating an executable motion path. At the same time, in recent years, the low-altitude economy, such as search and rescue, package delivery, and industrial inspection, has become increasingly widespread. The autonomous obstacle avoidance and motion planning of quadrotors have played a vital role in this field. How to reasonably and effectively actively avoid obstacles for quadrotors and generate an effective motion path has become essential.

[0003] Common quadcopter planning problems are generally divided into the front-end geometric path search for a collision-free path, which is handed over to the back-end to generate a path that the drone can execute. Under this form of search and generation, the path generated by the front-end will not fully consider the needs of the back-end, that is, it cannot avoid some problems in the generated trajectory in advance, and the back-end will consider the relevant dynamic models. This kind of hierarchical planning requires strong response requirements for re-planning of the drone when encountering unexpected obstacles. At the same time, the local optimization path is allocated to a dynamically feasible trajectory for a given time. The front-end search for feasible paths does not take into account the parameters that need to be optimized by the back-end, especially when facing non-static replanning problems.

[0004] Without knowing the environment information in advance, the real-time generation of local maps is an issue that UAVs need to consider when doing path planning. The surrounding environment obtained by the front-end sensor is converted into its own local map according to the UAV's own algorithm, and then used to generate the local trajectory of the UAV. The traditional real-time construction of a global ESDF map is the mainstream method currently used by UAVs, but it will cause the generation of too large a map when searching for a path that is too far away, and at the same time, it will cause too much useless information. The Voronoi map has a reliable topological route in the process of knowing the global information, but Voronoi cannot handle some extreme situations on the local map.

[0005] In the traditional quadcopter trajectory generation problem, there are trajectory planning generation methods such as Bezier curve and minimum snap energy optimization, genetic algorithm, particle swarm algorithm, B-spline curve, etc. However, in the current mainstream UAV trajectory planning generation methods, the dynamic model of the UAV is rarely considered in trajectory generation.

[0006] B-spline curve is a common method for generating UAV trajectories. This curve method can make good use of the path points generated by the front end, and flexibly use them as control points of the B-spline curve to regenerate a trajectory suitable for UAV flight. The quality of the control points also directly affects the quality of the B-spline curve. At the same time, for high-order B-spline curves, it will be more difficult to solve, and the optimal trajectory generated will also violate the dynamic requirements of the UAV. The introduction of the motion dynamics model can fundamentally take into account whether the trajectory executed by the UAV can meet the dynamic constraints in the process of generating trajectories and replanning, and also fundamentally avoid the relationship between path planning and time parameterization.

[0007] Therefore, a new path planning method for quadrotor drones is urgently needed to solve the above technical problems. Summary of the invention

[0008] The present invention provides a path planning method for a quad-rotor unmanned aerial vehicle, aiming to improve the quality and efficiency of the path planning of the unmanned aerial vehicle.

[0009] The present invention provides a path planning method for a quad-rotor unmanned aerial vehicle, the path planning method comprising the following steps:

[0010] S1. Obtain a voxel map through a visual sensor of a quadrotor drone, establish a Voronoi diagram based on the voxel map, and solve the three-dimensional points of the Voronoi diagram near obstacles in the voxel map through a quadratic programming solver to obtain multiple path points;

[0011] S2. Update the plurality of path points by using the ESDF gradient field to obtain a front-end path point set;

[0012] S3, establishing a multi-order B-spline curve according to the front-end path point set;

[0013] S4, extracting control points according to preset constraints through the B-spline curve to obtain multiple control point groups, and aggregating the multiple control point groups to obtain an aggregated control point group;

[0014] S5, performing path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory;

[0015] S6. Optimizing the initial path trajectory according to preset rules to obtain a final path trajectory.

[0016] Preferably, the B-spline curve is defined as L(t), and the B-spline curve satisfies the following conditions:

[0017]

[0018] Among them, p irepresents the i-th control point, N i，k (t) is the basis function of B-spline, k is the order of B-spline, and t is the time.

[0019] Preferably, the B-spline curve includes a trajectory curve executed by the quadrotor drone, a trajectory curve being executed by the quadrotor drone, and a replanning curve of the quadrotor drone.

[0020] Preferably, the preset constraint condition is: the total cost between the node vectors of the B-spline curve cannot exceed a preset threshold, and the total cost is defined as C k，T (θ A ), the total cost satisfies the following conditions:

[0021]

[0022] in, represents the cost function, which is used to calculate the cost of the control point, k represents the order of the B-spline, T represents the node interval time, θ A represents the control point group at the time corresponding to all the node vectors of the B-spline curve, m represents the number of node vectors in the B-spline curve, represents the corresponding k+1 control points in the i-th control point group; the cost function satisfies:

[0023]

[0024] Among them, λ 1 and λ 2 represents the weight coefficient, represents the energy function, f s Indicates smoothness.

[0025] Preferably, step S5 includes the following sub-steps:

[0026] S51, extracting the control point with the smallest cost among the aggregated control points to obtain a first control point;

[0027] S52, determining whether the first control point is near the end control point, if so, obtaining the path trajectory and proceeding to step S54; if not, updating the control points in the control point group of the adjacent voxels of the first control point and proceeding to step S53;

[0028] S53, screening the updated control points according to the cost and the spatial dimension to obtain the path trajectory, and proceeding to step S54;

[0029] S54, performing a time-minimizing search on the path trajectory by using a heuristic function to obtain the initial path trajectory.

[0030] Preferably, the heuristic function is h(s), and the heuristic function satisfies the following conditions:

[0031]

[0032] Among them, ρ represents the weight coefficient, T v Indicates the minimum time of velocity on the x-axis, y-axis, and z-axis, p g represents the target position point, p 0 Indicates the starting point, v max Indicates maximum speed.

[0033] Preferably, the preset rule is:

[0034] S61, judging whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, if so, spatially constraining the control points exceeding the preset upper limit; if not, discarding them;

[0035] S62, cube-expanding the control points on the initial path trajectory; if the quad-rotor drone after cube-expanding does not collide with an obstacle after a preset time, no additional control points are added to change the trajectory; if the quad-rotor drone after cube-expanding collides with more than two obstacles, a preset number of control points are added to the control points after cube-expanding to change the trajectory, and cube-expanding the added control points at the same time;

[0036] S63, repeating step S61-step S62 until all control points in the initial path trajectory are completed to obtain the final path trajectory.

[0037] Preferably, after the adjacent control points in the initial path trajectory are cube-shaped and expanded, the following conditional formula is satisfied:

[0038] ||v k，T,i-1 -v k，T，i || 2 <α i-1 +α i ;

[0039] Among them, α i represents the diagonal length of the ith control point after cube expansion, α i-1 represents the diagonal length of the i-1th control point after cube expansion, v k，T，i-1 and v k，T，i represents two adjacent control points, k represents the order of the B-spline curve, and T represents the minimization time.

[0040] Compared with the prior art, the present invention obtains a voxel map through the visual sensor of a quadrotor drone, establishes a Voronoi diagram based on the voxel map, solves the three-dimensional points of the Voronoi diagram near obstacles in the voxel map through a quadratic programming solver, and obtains multiple path points; updates multiple path points through an ESDF gradient field to obtain a front-end path point set; establishes a multi-order B-spline curve based on the front-end path point set; extracts control points through the B-spline curve according to preset constraints to obtain multiple control point groups, and aggregates multiple control point groups to obtain aggregated control point groups; performs path search on the aggregated control point groups based on the A* algorithm to obtain an initial path trajectory; optimizes the initial path trajectory through preset rules to obtain a final path trajectory. In this way, the path planning trajectory of the quadrotor drone proposed by the present invention is higher in quality and efficiency. BRIEF DESCRIPTION OF THE DRAWINGS

[0041] The present invention will be described in detail below in conjunction with the accompanying drawings. The above and other aspects of the present invention will become clearer and easier to understand through the detailed description made in conjunction with the following drawings. In the accompanying drawings:

[0042] Figure 1 It is a flowchart of a path planning method for a quad-rotor drone provided in an embodiment of the present invention. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solution and advantages of the present invention more clearly understood, the present invention is further described in detail below in conjunction with the accompanying drawings and embodiments. It should be understood that the specific embodiments described herein are only used to explain the present invention and are not intended to limit the present invention.

[0044] Please refer to Figure 1 The present invention provides a path planning method for a quad-rotor UAV, the path planning method comprising the following steps:

[0045] S1. Acquire a voxel map through a visual sensor of a quadrotor drone, establish a Voronoi diagram based on the voxel map, and solve the three-dimensional points of the Voronoi diagram near obstacles in the voxel map through a quadratic programming solver to obtain multiple path points.

[0046] In an embodiment of the present invention, a voxel map is constructed by a front-end depth camera, an improved Voronoi diagram is constructed on the map, and an ESDF gradient field is constructed at a specific three-dimensional point, so as to construct a reliable and safe topological front-end path.

[0047] The Voronoi diagram is a mathematical structure used to describe a method of dividing a plane from a certain set of points into multiple regions, each region containing all the points on the plane that are closest to that point.

[0048] The Voronoi diagram can be used not only for global maps, but also for local maps. In fact, constructing and using the Voronoi diagram in the local map of a drone has some unique advantages and considerations. In this invention, the quadcopter is regarded as a convex polyhedron, and the following are defined:

[0049] B = {b 1 , b 2 , ..., b n} is defined as the set of vertices on the convex polyhedron of the drone, S = {S 1 , S 2 , ..., S n} is the set of all obstacles. The obstacles here can be the set of different convex polyhedrons of the same obstacle.

[0050] The Voronoi diagram is defined in the present invention as follows: for obstacles, it is a connected three-dimensional area, and at the same time there is a set of sets in space, and the set contains specific three-dimensional points in space that satisfy:

[0051]

[0052] Here || or|| 2 It refers to the actual Euclidean distance between two points, taking into account the error of depth information. o represents a point on the surface of an obstacle or embedded in the obstacle, and S i It is represented as the ith obstacle, and r is the depth information point after filtering. Based on this, the Voronoi region is defined as:

[0053]

[0054] In addition, if the X and Y dimensions are two 3D points,

[0055] Then there is

[0056] The distance between the point and the nearest obstacle is smaller than the distance to other obstacles. i Corresponds to only one point p i .

[0057] And in three-dimensional space, the boundary of each Voronoi region is composed of a set of planes, which are defined as Voronoi surfaces:

[0058]

[0059] The voronoi surface is composed of r i , r jDefined by two points, all points on the surface are equidistant from the two three-dimensional points, and x is the set of all points on the Voronoi surface.

[0060] Assume that the obstacles captured by the depth camera are a set S, and we need to find a 3D point that maximizes the distance from the point to the nearest obstacle. As long as we find one point, we can find the positions of the remaining points by analogy. First, we define the objective function to be solved, and limit the x of the range to be solved to a Voronoi area, that is, solve the 3D points in the Voronoi area near the obstacle:

[0061]

[0062] At the same time, the quadratic programming solver is used to solve the problem, and the distance function uses the Euclidean distance:

[0063]

[0064] Considering the continuity of the UAV's motion, the constraints are set as follows:

[0065] dist(x,S i )≥d max ;

[0066] x∈H i，j ;

[0067] In order to ensure that each obstacle is at the maximum distance from the search point, and the search point is on the Voronoi surface, the point at the front of the drone can maximize the minimum distance. max It needs to be at least greater than the distance from the center to the edge of the obstacle. However, in reality, obstacles are always irregular, they may be convex or concave, and there is no way to use a fixed value to determine the distance. Here we define d max =max||b i -b j ||, while i≠j, where b i , b j are vertices on the convex polyhedron of the drone. Finally, the set of path points is obtained by using the quadratic programming solver:

[0068] X={x 1 , x 2 , ..., x n}.

[0069] S2. Update the plurality of path points through the ESDF gradient field to obtain a front-end path point set.

[0070] In an embodiment of the present invention, the distance is adjusted by using an evaluation function and an ESDF (Euclidean Signed Distance Field) gradient field.

[0071] Because the Voronoi diagram itself is a topological structure, when the polynomial optimization method is used to find the optimal solution, it does not take into account that the straight line connection between points will directly pass through obstacles. The front-end path found by this topological structure is not feasible for back-end trajectory optimization. At the same time, the path points after the solution mentioned above only meet the constraint condition, that is, maximize the minimum distance s. Because the X set obtained by the solver may still be inside the obstacle, the voxel map is used to construct the ESDF gradient field to guide the points where the front-end set collides away from the obstacle.

[0072] Specific steps:

[0073] 1. Initialize the voxel map. Here, define the unknown areas and obstacles in the voxel map as occupied, and the idle areas as unoccupied. The following initializes the state of each voxel:

[0074]

[0075] 2. During the visual scanning process of the quadrotor drone, if a 3D point is found to be on the surface or inside an obstacle, the updated point is calculated using the gradient descent method in the voxel unit where the point is located.

[0076]

[0077] is the ESDF gradient of the voxel where the 3D point is located, x i,update is the updated point.

[0078] In three-dimensional space, it is necessary to consider the neighbors of the voxel in six directions, and at the same time find the voxel with the minimum state in the direction around the current voxel, recorded as state min (v i ).

[0079] state min (v i ) defines a signed distance field:

[0080] but The ESDF gradient is calculated according to state(v i ) to derive the x-axis, y-axis, and z-axis directions to obtain the ESDF gradient, and define the following formula:

[0081]

[0082] If it is found that the straight line between each point is blocked by an obstacle, the straight line segment covered by the obstacle is truncated, and the ESDF gradient of the voxel at both ends of the truncated straight line is calculated respectively, and then the gradient descent method is used to calculate the updated point.

[0083] 3. Update the points where the X set changes. Define the evaluation function:

[0084]

[0085] Repeat the judgment in the second step above until the front-end depth information is no longer updated. At the same time, the larger the evaluation function is, the closer the distance from the overall curve to the obstacle is, and finally the front-end path point set is obtained.

[0086] S3. Establish a multi-order B-spline curve according to the front-end path point set.

[0087] In the embodiment of the present invention, the B-spline curve is defined as L(t), and the B-spline curve satisfies the following conditions:

[0088]

[0089] Among them, p i represents the i-th control point, Ni, k(t) is the basis function of B-spline, k represents the order of B-spline, and t represents the time.

[0090] In an embodiment of the present invention, the B-spline curve includes a trajectory curve executed by the quadrotor drone, a trajectory curve being executed by the quadrotor drone, and a replanning curve of the quadrotor drone.

[0091] Specifically, we first define the B-spline curve: we use the B-spline curve parameterization as the underlying output of the entire framework, and use high-order uniform B-splines. The node vector determines the shape of the basis function, and the basis function determines the control effect of each control point on the local trajectory curve. Define the node vector as {t 0 , t 1 , ..., t m}, assuming that there are n+1 control points, the number of node vectors m is: m=n+k+1, where k is the B-spline order, and the order of the B-spline defined in the present invention is 3rd order.

[0092] And the function of the k-order B-spline curve is specified:

[0093]

[0094] where p i Represents the i-th control point, and assigns the front path point set X to the control point p i =x i , Ni，k (t) is the basis function of B-spline, and according to the basis function definition:

[0095]

[0096] After defining the uniform B-spline curve, we can see that the basis functions are all functions of time, so the node vector, that is, the span of each time, can be redefined.

[0097] For each node vector, a small change Δt is defined by the user. This small change is a fixed time change value. Define u = (tt i )Δt is used to represent the time from the i-th period to the i+1-th period, which is represented as the span of the i-th node.

[0098] After redefining time as u, each control point is stacked and compressed, defined as P i-k =[p i-k , p i-k+1 , ...p i ]The energy function is defined as:

[0099]

[0100] Where Q l for The Hessian matrix of . On this basis, the functions and characteristics of uniform B-splines are defined.

[0101] In re-planning, the optimization of local control points is more important. The calculation of any point of the B-spline curve can be represented by a single time span u defined containing k+1 control points, and three layered curves are made for the local control effect, namely the executed trajectory, the executing trajectory, and the re-planned trajectory after optimization.

[0102] Among the three defined trajectories, since the control points define different intervals and result in different node vectors, the three trajectories will not affect each other. In order to prevent the speed and acceleration of the drone from changing suddenly, a bidirectional linked list is used to update the three trajectories in real time. Here, the current control point is defined as the jth control point, and the trajectory curve before and after R moments is, that is, the executed trajectory:

[0103]

[0104] The execution trajectory is:

[0105]

[0106] The replanned curve is:

[0107]

[0108] After local control, the planner will perform a historical perception event on the globally generated control points. The main function of the event is to re-plan the trajectory to be optimized, put the control points that meet the dynamics of the quadcopter into the executable array, and put the control points that do not meet the dynamics into the non-executable array. And each time in the global re-planning, the two sets of arrays will be cleared in real time. When the quadcopter executes the plan later, if the point does not meet the dynamics in the last re-planning, but meets the dynamics model in this re-planning, it can also be added to the executable array, which enhances the executability.

[0109] No additional control points are added to the global control points to optimize replanning because the dynamics framework favors safe and reliable trajectories over performance.

[0110] S4. Extracting control points according to preset constraints through the B-spline curve to obtain multiple control point groups, and aggregating the multiple control point groups to obtain an aggregated control point group.

[0111] In the embodiment of the present invention, the preset constraint condition is: the total cost between the node vectors of the B-spline curve cannot exceed a preset threshold, and the total cost is defined as C k，T (θ A ), the total cost satisfies the following conditions:

[0112]

[0113] in, represents the cost function, which is used to calculate the cost of the control point, k represents the order of the B-spline, T represents the node interval time, θ A represents the control point group at the time corresponding to all the node vectors of the B-spline curve, m represents the number of node vectors in the B-spline curve, represents the corresponding k+1 control points in the i-th control point group; the cost function satisfies:

[0114]

[0115] Among them, λ 1 and λ 2 represents the weight coefficient, represents the energy function, f s Indicates smoothness.

[0116] Specifically, since ordinary geometric planning cannot understand the dynamic characteristics of quadrotor drones, although the geometric path can be solved to generate the optimal and shortest path, this geometric path can only be converted into an executable motion trajectory for the quadrotor drone by re-planning a new motion trajectory in the back end, or by generating a collision-free executable trajectory according to a given geometric path; the former lacks real-time performance, and the latter does not meet the kinematic characteristics of the drone.

[0117] In traditional UAV dynamics planning calculations, collision detection and boundary value issues need to be considered. If the local characteristics (replanning capabilities) of the quadcopter can be enhanced for the above problems, then reducing the unnecessary motion trajectories of the quadcopter is desirable.

[0118] Here are the control point group definition steps:

[0119] 1. In the X set obtained from the Voronoi diagram at the front end, a three-dimensional control point group is constructed. The control point group corresponds to k continuous control points. Continuity means that it must be continuous in the voxel unit. Similarly, three-dimensional diagonals are also possible. Here, a third-order B-spline curve is taken, and each control point group contains 4 continuous control points. The control points here are not the final control points, but pending, and are called placeable control points here. The control points here are control points that may become motion trajectories in the future.

[0120] 2. The control point group θ is defined as:

[0121] θ={p 0 , p 1 ..., p k , p k+1};

[0122] Among them, p k belongs to the kth control point. At the same time, each p k ∈P each {p k-1 , p k}∈E, E is the relationship set of each edge.

[0123] In replanning, each replanned path will be given a start and end point, where the start and end points are defined as control point groups:

[0124] starting point:

[0125] end:

[0126] The defined control point group has the following characteristics: 1. Markov assumption, 2. spatial continuity.

[0127] Markov assumption: For adjacent control point groups, the control points that can be placed in them can be repeated. Because of the local controllability of B-spline and the discreteness of the state space, the defined control point groups are continuous. The number of control points that can be placed between each continuous control point group can be adjusted, and adjacent k+1 control points are aggregated into one control point group.

[0128] In adjacent control point groups, it is defined that there are k identically placeable control points in two adjacent control point groups. On this basis, the Markov assumption can be guaranteed, ensuring that the current state of the quadrotor drone can be better considered in trajectory planning during replanning.

[0129] Because the B-spline curve is defined based on the time span of the control points and the definition of the control point group, each control point group in the k-order B-spline curve contains k+1 control points, which also means that each control point group can be regarded as a local trajectory. Since each adjacent control point group contains k identical control points, it shows that the trajectory generated by the control points in the defined control point group is naturally continuous.

[0130] 3. In the defined control point group, relevant constraints must be made to ensure that the defined assumptions are valid. To represent the control point group containing k control points in the j-th control point group.

[0131] For a continuous trajectory, it is necessary to define the positive cost associated with the control point group:

[0132]

[0133] It can be known that the cost is the cost function of all node vectors The cumulative sum of k represents the order of B-spline, T is the node interval time, θ A is the control point group at the time corresponding to all node vectors, The control points that can be placed are the k+1 control points corresponding to the i-th control point group explained above. The node interval time is defined as follows:

[0134] T=u i -u i-1 ;

[0135] The cost function is defined as: the time the node vector spends at that location plus the smoothness and energy function, which is specifically expressed as:

[0136]

[0137] The cost function adds time T to generate trajectories in a shorter time to avoid generating trajectories for too long.1 is the weight coefficient; f s The smoothness expression is the square of the velocity and acceleration corresponding to the control point group of the node vector gap, that is, minimizing f s :

[0138]

[0139] So far, the control point group and the function to be solved have been defined.

[0140] Because the number of control points in the control point groups of B-spline curves of different orders is different, the number of control points contained in the control point group will affect the quality of the aggregated control point group. Here, p is used k,i，j To indicate that there is a jth control point to be placed in the i-th segment node vector in the k-order B-spline curve, the Markov property defined by the above control point group is guaranteed.

[0141] At the same time, this C k，T (θ A ) Check whether the cost between node vectors exceeds the set cost, and finally perform aggregation.

[0142] The control points of the control point group aggregation can be transformed into a deterministic graph search problem, namely:

[0143] G = (P, E);

[0144] Where P is the set of control point positions, and E is the edge adjacent to the control point, that is, the set of constraint equations for the control point. In three-dimensional space, after being subdivided into voxel units, there will be 26 adjacent units around each voxel unit. The benefit of defining this map is actually to enable the quadcopter to clearly subdivide each control point into each unit when searching for a path.

[0145] After the deterministic graph search, we need to find the shortest path between the aggregated control point groups to form a continuous trajectory. In three-dimensional space, each voxel unit has 26 adjacent voxel units nearby, and the control point group aggregation satisfies the Markov property.

[0146] S5, performing path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory;

[0147] In the embodiment of the present invention, step S5 includes the following sub-steps:

[0148] S51, extracting the control point with the smallest cost among the aggregated control points to obtain a first control point;

[0149] S52, determining whether the first control point is near the end control point, if so, obtaining the path trajectory and proceeding to step S54; if not, updating the control points in the control point group of the adjacent voxels of the first control point and proceeding to step S53;

[0150] S53, screening the updated control points according to the cost and the spatial dimension to obtain the path trajectory, and proceeding to step S54;

[0151] S54, performing a time-minimizing search on the path trajectory by using a heuristic function to obtain the initial path trajectory.

[0152] In the embodiment of the present invention, the heuristic function is h(s) (a heuristic function for minimizing the solution time), and the heuristic function satisfies the following conditions:

[0153]

[0154] Among them, ρ represents the weight coefficient, that is, the relative importance of time minimization in the objective function of the trajectory, T v Indicates the minimum time of velocity on the x-axis, y-axis, and z-axis, p g represents the target position point, p 0 Indicates the starting point, v max Indicates maximum speed.

[0155] Specifically, the present invention uses improved A* to search for the shortest path of aggregated control point groups. In three-dimensional space, by aggregating adjacent control point groups, the same placeable control point may also exist in multiple control point groups. For high-dimensional control point groups, if the control point occupies two or more control point groups, the control point is regarded as the same, that is, from high dimension to low dimension, but the actual physical meaning of the control point remains unchanged, and they are essentially points in space.

[0156] The improved A* algorithm can transform the search in high-dimensional space into the search in low-dimensional space. s The control point group starts at the first placeable v k，T，i Among the control points, for all the control point groups occupying the control point, determine whether to treat the control point as a low-dimensional state. At the same time, the aggregation between control point groups is related to how the control point groups are searched. The more control points the control point groups aggregate, the faster the search will be, but at the same time, the quality of the search will also be reduced.

[0157] The present invention no longer uses the traditional Euclidean distance or other heuristic functions expressed by distance methods. The designed heuristic function considers more whether the trajectory can generate a reliable and safe trajectory in a shorter time. Here, only the derivative of the position, that is, the speed time s, is considered:

[0158]

[0159] After the control point groups are aggregated, a continuous trajectory is obtained. In order to guide the search direction, the time of each trajectory is minimized, constraints are set for the start and end control points of each control point group, and the positions and speeds of the start and end points are set.

[0160]

[0161] After defining the starting state and the ending state, the maximum time of the three axes about the speed and position in the three-dimensional space can be calculated to obtain the vector about time The minimum value in the vector is taken as the lower bound. Therefore, the final objective function expression is:

[0162]

[0163] Where S(u) is as follows:

[0164]

[0165] Solving the quadratic programming problem yields That is, the time minimized in this trajectory. The heuristic function can be set as:

[0166]

[0167] Based on this heuristic function, the A* algorithm is used to search for the optimal solution in terms of time.

[0168] The points obtained by the final search can be used as the control points of the last step before safety constraints. At this time, the control points that the quadrotor drone can explore and undergo a series of optimization and transformation steps can generate trajectories for the quadrotor drone, and finally obtain the searched continuous trajectory, that is, the initial path trajectory.

[0169] S6. Optimizing the initial path trajectory according to preset rules to obtain a final path trajectory.

[0170] In the embodiment of the present invention, the preset rule is:

[0171] S61, judging whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, if so, spatially constraining the control points exceeding the preset upper limit; if not, discarding them;

[0172] S62, cube-expanding the control points on the initial path trajectory; if the quad-rotor drone after cube-expanding does not collide with an obstacle after a preset time, no additional control points are added to change the trajectory; if the quad-rotor drone after cube-expanding collides with more than two obstacles, a preset number of control points are added to the control points after cube-expanding to change the trajectory, and cube-expanding the added control points at the same time;

[0173] S63, repeating step S61-step S62 until all control points in the initial path trajectory are completed to obtain the final path trajectory.

[0174] In the embodiment of the present invention, after the adjacent control points in the initial path trajectory are cube-shaped and expanded, the following conditional formula is satisfied:

[0175] ||v k，T,i-1 -v k，T,i || 2 <α i-1 +α i ;

[0176] Among them, α i represents the diagonal length of the ith control point after cube expansion, α i-1 represents the diagonal length of the i-1th control point after cube expansion, v k，T，i-1 and v k，T，i represents two adjacent control points, k represents the order of the B-spline curve, and T represents the minimization time.

[0177] Specifically, even after the improved A* algorithm is used to aggregate control point groups in a high-dimensional state space, it can ensure that the trajectory of adjacent control point groups aggregated at the current moment is collision-free. However, when the trajectory is executed, the generated trajectory is still insufficient when facing sudden obstacles or places that cannot be perceived by the front-end vision.

[0178] By using the convex hull of the generated B-spline curve, the present invention inserts control points at places where collisions may occur in the execution trajectory of the quad-rotor drone, thereby increasing the node vector and changing the local trajectory. However, the problem with inserting control points is that the present invention proposes to aggregate control points in the control point group, and adding too many control points will cause a huge change in the number of control point groups, which will result in a certain amount of calculation when the improved A* algorithm is re-searched locally, and will also lead to an inability to solve the problem.

[0179] Therefore, the present invention makes the following adjustments to the control points of the control point group on the B-spline curve formed at the third point:

[0180] 1. Judge the control point on the current trajectory and calculate all the control point groups it occupies. If the control point group occupied by the control point exceeds the set starting upper limit, impose spatial constraints on it. If it does not exceed, it will be discarded.

[0181] 2. The voxel where the control point is located is expanded into a cube. The size of the cube is determined by the maximum distance between the vertices of the drone's convex polyhedron. This ensures that the trajectory generated by the drone is within the convex hull and that no collision with obstacles occurs.

[0182] 3. If the drone expands in a cube-like form over a period of time in the future and does not collide with any obstacles, it can pass through without adding additional control points to change its trajectory.

[0183] 4. If the expanded cube of the quadcopter control point collides with more than two obstacles, certain control points are added to the control point to change its trajectory, and the cube is expanded at the added control points.

[0184] 5. Repeat the above steps until a suitable track is found.

[0185] The specific expression is as follows:

[0186] Use Γ={v k，i,0 , v k，i,1 , ..., v k,i，j} to indicate the center of the cube, that is, the location of the control point. Use α={α 0 , α 1 , ..., α T} to describe the length of the side of the cube.

[0187] At the same time, in the present invention, the adjacent cubes need to satisfy the following conditions after expansion:

[0188] ||v k,T,i-1 -v k，T，i || 2 <α i-1 +α i ;

[0189] Among them, α i Determined by the vertex distance of the convex polyhedron of the body, define D B ={D 1 , D 2 , ..., D n}, where D i for || b i -b j ||, i≠j, and D B The array is from D min To D maxSort in ascending order, so traverse all α i =D i , and finally used as the length of the side where the cube is located. This is how the expanded cube is constructed.

[0190] When this constraint is met, this spatial constraint can be well involved in the adjacent control point group, which essentially satisfies that there are no obstacles or collisions when each track segment takes over the next track segment. At the same time, adding limited control points after the fourth point is generated can also add certain constraints to the upper and lower tracks.

[0191] Compared with the prior art, the present invention obtains a voxel map through the visual sensor of a quadrotor drone, establishes a Voronoi diagram based on the voxel map, solves the three-dimensional points of the Voronoi diagram near obstacles in the voxel map through a quadratic programming solver, and obtains multiple path points; updates multiple path points through an ESDF gradient field to obtain a front-end path point set; establishes a multi-order B-spline curve based on the front-end path point set; extracts control points through the B-spline curve according to preset constraints to obtain multiple control point groups, and aggregates multiple control point groups to obtain aggregated control point groups; performs path search on the aggregated control point groups based on the A* algorithm to obtain an initial path trajectory; optimizes the initial path trajectory through preset rules to obtain a final path trajectory. In this way, the path planning trajectory of the quadrotor drone proposed by the present invention is higher in quality and efficiency.

[0192] It should be noted that, in this article, the terms "include", "comprises" or any other variations thereof are intended to cover non-exclusive inclusion, so that a process, method, article or device including a series of elements includes not only those elements, but also other elements not explicitly listed, or also includes elements inherent to such process, method, article or device. In the absence of further restrictions, an element defined by the sentence "comprises a ..." does not exclude the existence of other identical elements in the process, method, article or device including the element.

[0193] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only the preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation manner. The above-mentioned specific implementation manner is only illustrative rather than restrictive. Under the enlightenment of the present invention, ordinary technicians in this field can also make many forms and equivalent changes without departing from the scope of protection of the purpose of the present invention and the claims, all of which are within the protection of the present invention.

Claims

1. A path planning method for a quad-rotor drone, characterized in that: The path planning method comprises the following steps: S1. Obtain a voxel map through a visual sensor of a quadrotor drone, establish a Voronoi diagram based on the voxel map, and solve the three-dimensional points of the Voronoi diagram near obstacles in the voxel map through a quadratic programming solver to obtain multiple path points; S2. Update the plurality of path points by using the ESDF gradient field to obtain a front-end path point set; S3, establishing a multi-order B-spline curve according to the front-end path point set; S4, extracting control points according to preset constraints through the B-spline curve to obtain multiple control point groups, and aggregating the multiple control point groups to obtain an aggregated control point group; S5, performing path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory; S6. Optimizing the initial path trajectory according to preset rules to obtain a final path trajectory.

2. The path planning method for a quadrotor drone as claimed in claim 1, characterized in that: The B-spline curve is defined as L(t), and the B-spline curve satisfies the following conditions: Among them, p i represents the i-th control point, N i,k (t) is the basis function of B-spline, k is the order of B-spline, and t is the time.

3. The path planning method for a quad-rotor drone as claimed in claim 2, characterized in that: The B-spline curve includes a trajectory curve executed by the quadrotor drone, a trajectory curve being executed by the quadrotor drone, and a replanning curve of the quadrotor drone.

4. The path planning method for a quad-rotor drone as claimed in claim 1, characterized in that: The preset constraint condition is that the total cost between the node vectors of the B-spline curve cannot exceed a preset threshold, and the total cost is defined as C k,T (θ A ), the total cost satisfies the following conditions: in, represents the cost function, which is used to calculate the cost of the control point, k represents the order of the B-spline, T represents the node interval time, θ A represents the control point group at the time corresponding to all the node vectors of the B-spline curve, m represents the number of node vectors in the B-spline curve, represents the corresponding k+1 control points in the i-th control point group; the cost function satisfies: Among them, λ1 and λ2 represent weight coefficients, represents the energy function, f s Indicates smoothness.

5. The path planning method for a quad-rotor drone as claimed in claim 1, characterized in that: Step S5 includes the following sub-steps: S51, extracting the control point with the smallest cost among the aggregated control points to obtain a first control point; S52, determining whether the first control point is near the end control point, if so, obtaining the path trajectory and proceeding to step S54; if not, updating the control points in the control point group of the adjacent voxels of the first control point and proceeding to step S53; S53, screening the updated control points according to the cost and the spatial dimension to obtain the path trajectory, and proceeding to step S54; S54, performing a time-minimizing search on the path trajectory by using a heuristic function to obtain the initial path trajectory.

6. The path planning method for a quad-rotor drone as claimed in claim 5, characterized in that: The heuristic function is h(s), and the heuristic function satisfies the following conditions: Among them, ρ represents the weight coefficient, T v Indicates the minimum time of velocity on the x-axis, y-axis, and z-axis, p g represents the target position point, p0 represents the starting position point, v max Indicates maximum speed.

7. The path planning method for a quad-rotor drone as claimed in claim 1, characterized in that: The preset rules are: S61, judging whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, if so, spatially constraining the control points exceeding the preset upper limit; if not, discarding them; S62, cube-expanding the control points on the initial path trajectory; if the quad-rotor drone after cube-expanding does not collide with an obstacle after a preset time, no additional control points are added to change the trajectory; if the quad-rotor drone after cube-expanding collides with more than two obstacles, a preset number of control points are added to the control points after cube-expanding to change the trajectory, and cube-expanding the added control points at the same time; S63, repeating step S61-step S62 until all control points in the initial path trajectory are completed to obtain the final path trajectory.

8. The path planning method for a quad-rotor drone as claimed in claim 7, characterized in that: After the adjacent control points in the initial path trajectory are cubed and expanded, the following conditional formula is satisfied: ‖v k,T,i-1 -v k,T,i ‖2<α i-1 +α i ; Among them, α i represents the diagonal length of the ith control point after cube expansion, α i-1 represents the diagonal length of the i-1th control point after cube expansion, v k,T,i-1 and v k,T,i represents two adjacent control points, k represents the order of the B-spline curve, and T represents the minimization time.

Images