Path planning method for quadrotor drone

Abstract

The present invention provides a path planning method for a quadrotor drone. The present invention obtains a voxel map through a visual sensor of the 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 to obtain multiple path points; updates the 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 the multiple control point groups to obtain an aggregated control point group; performs a path search on the aggregated control point group based on an A* algorithm to obtain an initial path trajectory; and optimizes the initial path trajectory according to preset rules to obtain a final path trajectory. Compared with the prior art, the path planning trajectory of the quadrotor drone proposed by the present invention is higher in quality and efficiency.

Description

Technical Field

[0001] The present invention is applicable to the technical field of unmanned aerial vehicles (UAVs), and in particular relates to a path planning method for a quad-rotor UAV. 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, 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 avoid obstacles and generate an effective motion path for quadrotors 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 then handed over to the back-end to generate a path that the drone can execute. Under this search and generation form, the path generated by the front-end does not fully consider the needs of the back-end, that is, it cannot avoid some problems in generating the trajectory in advance, and the back-end is left to consider the relevant dynamic models. Such hierarchical planning requires strong response requirements for re-planning the drone when encountering unexpected obstacles. At the same time, the local optimized path is allocated to a dynamically feasible trajectory within a given time. The front-end search for a feasible path does not take into account the parameters that need to be optimized by the back-end, especially when facing non-static replanning problems.

[0004] When environmental information is unknown, real-time generation of local maps is a key consideration for drone path planning. The surrounding environment captured by front-end sensors is converted into a local map based on the drone's algorithms, which is then used to generate the drone's local trajectory. Traditional real-time global ESDF maps are the mainstream approach currently used by drones. However, searching for paths that are too far can result in overly large maps and an excess of useless information. Voronoi maps provide reliable topological routes when global information is known, but they cannot handle some extreme cases when used on local maps.

[0005] In the traditional quadrotor UAV trajectory generation problem, Bezier curves and minimum snap energy optimization, genetic algorithms, particle swarm algorithms, B-spline curves and other trajectory planning generation methods are used. However, among the current mainstream UAV trajectory planning generation methods, the UAV's dynamic model is rarely considered in trajectory generation.

[0006] B-spline curves are a common method for generating drone trajectories. This curve method effectively utilizes path points generated by the front-end, flexibly using them as control points for the B-spline curve to regenerate a trajectory suitable for drone flight. The quality of the control points directly affects the quality of the B-spline curve. Furthermore, solving high-order B-spline curves can be difficult, and the resulting optimal trajectory can violate the drone's dynamic requirements. The proposed motion dynamics model fundamentally considers whether the drone's trajectory satisfies dynamic constraints during trajectory generation and replanning, fundamentally avoiding the relationship between path planning and time parameterization.

[0007] Therefore, there is an urgent need for a new path planning method for quadrotor drones to solve the above technical problems. Summary of the Invention

[0008] The present invention provides a path planning method for a quadrotor unmanned aerial vehicle (UAV), aiming to improve the quality and efficiency of the UAV's path planning.

[0009] The present invention provides a path planning method for a quadrotor 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 using a quadratic programming solver to obtain multiple path points;

[0011] S2. Update the plurality of path points 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 using 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. Perform path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory;

[0015] S6. Optimize 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 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:

[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 represent weight coefficients, 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 from 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 voxels adjacent to the first control point and proceeding to step S53;

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

[0029] S54 , performing a time-minimizing search on the path trajectory 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, p0 represents the starting position point, v max Indicates maximum speed.

[0033] Preferably, the preset rules are:

[0034] S61, determining whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, and if so, applying spatial constraints to the control points that exceed the preset upper limit; if not, discarding them;

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

[0036] S63, repeating steps S61 and 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 cubed and expanded, they satisfy the following conditional formula:

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

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

[0040] Compared with the existing technology, the present invention obtains a voxel map through the visual sensor of the quadcopter 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 the multiple path points through the 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 the multiple control point groups to obtain an aggregated control point group; performs a path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory; and optimizes the initial path trajectory according to preset rules to obtain a final path trajectory. In this way, the path planning trajectory of the quadcopter 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 with reference to the accompanying drawings. The above and other aspects of the present invention will become clearer and easier to understand through the detailed description made with reference to the following drawings. In the accompanying drawings:

[0042] Figure 1 This is a flowchart of a path planning method for a quadrotor drone provided by an embodiment of the present invention. DETAILED DESCRIPTION

[0043] In order to make the purpose, technical solutions and advantages of the present invention more clearly understood, the present invention will be further described in detail below with reference to 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 secure topological front-end path.

[0047] The Voronoi diagram is a mathematical structure used to describe how a plane is divided into multiple regions by a set of specific points, 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 paper, the quadcopter is regarded as a convex polyhedron and the following definitions are defined:

[0049] B={b1,b2,...,b n} is defined as each vertex set on the convex polyhedron of the drone, S={S1,S2,...,S n} is the set of all obstacles. The obstacles here can be a set of different convex polyhedrons of the same obstacle.

[0050] The Voronoi diagram is defined in the present invention as follows: for an obstacle, it is a connected three-dimensional region, and there is a set of sets in space, which contain specific three-dimensional points in the space that satisfy:

[0051]

[0052] Here, || or ||2 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 the obstacle or embedded in the obstacle, and S i It is represented as the i-th obstacle, and r is the depth information point after filtering. Based on this, the Voronoi region is defined as follows:

[0053]

[0054] In addition, if the X and Y dimensions are two three-dimensional points,

[0055] Then there is

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

[0057] And in three-dimensional space, the intersection 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 j Defined 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. 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 solution range to a Voronoi region, that is, solve the 3D points in the Voronoi region near the obstacle:

[0061]

[0062] At the same time, the quadratic programming solver is used to solve the problem, and the distance function used is 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 quadratic programming solver is used to solve the path point set:

[0068] X={x1,x2,...,x n}.

[0069] S2. Update the plurality of path points using 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, polynomial optimization methods, when finding the optimal solution, do not take into account the possibility that straight lines connecting points will directly pass through obstacles. Therefore, the front-end path found by this topological structure is not feasible for back-end trajectory optimization. Furthermore, the path points after the solution mentioned above only satisfy the constraint condition, namely, maximizing the minimum distance s. Because the X set obtained by the solver may still be inside an obstacle, a voxel map is used to construct an ESDF gradient field to guide the points where the front-end set collides away from obstacles.

[0072] Specific steps:

[0073] 1. Initialize the voxel map. Here, define unknown areas and obstacles in the voxel map as occupied, and free 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 find the voxel with the minimum state in the direction around the current voxel, which is 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 ) state to derive the three directions of x-axis, y-axis, and z-axis 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 set X 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 based on the front-end path point set.

[0087] In an 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 B-spline order, 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 adopt high-order uniform B-spline. 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 {t0, t1, ..., 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 3.

[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 , N i，k (t) is the basis function of B-spline, and is defined according to the basis function:

[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 custom small change Δt is used. 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-spline are defined.

[0101] During replanning, the optimization of local control points is particularly important. The calculation of any point on the B-spline curve can be represented by a single time span u defined with k+1 control points. The local control effect is layered into three curves: the executed trajectory, the executing trajectory, and the optimized replanned trajectory.

[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 UAV's speed and acceleration from changing suddenly, a bidirectional linked list is used to update the three trajectories in real time. Here, the current 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 performs a history perception event on the globally generated control points. This event's primary purpose is to replan the trajectory to be optimized, placing control points that conform to the quadcopter's dynamics into an executable array and those that do not into a non-executable array. Both arrays are reset to zero in real time during each global replan. When the quadcopter subsequently executes the plan, if a point did not conform to the dynamics model in the previous replan but conforms to the dynamics model in this replan, it can also be added to the executable array, enhancing its feasibility.

[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 using 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 represent weight coefficients, represents the energy function, f s Indicates smoothness.

[0116] Specifically, because ordinary geometric planning cannot understand the dynamic characteristics of quadrotors, 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 by replanning a new motion trajectory in the backend or generating a collision-free executable trajectory according to the given geometric path. The former lacks real-time performance, and the latter does not meet the kinematic characteristics of the drone.

[0117] Traditionally, collision detection and boundary value issues are considered in drone dynamics planning. If the local characteristics (replanning capabilities) of quadrotors can be enhanced to address these issues, then reducing unnecessary motion trajectories is desirable.

[0118] Here are the steps to define the control point group:

[0119] 1. Construct a 3D control point group from the X set obtained from the Voronoi diagram at the front end. Each control point group corresponds to k consecutive control points. Continuity means that the control points must be continuous within the voxel unit. Similarly, special 3D diagonals can also be used. Here, a 3rd-order B-spline curve is used, so each control point group contains 4 consecutive control points. The control points here are not the final control points, but are pending. They are called placeable control points here. The control points here are control points that may become the motion trajectory in the future.

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

[0121] θ={p0,p1...,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. Due to the local controllability of B-splines 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 a control point group.

[0128] In adjacent control point groups, it is defined that there are k identical 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 this location plus the smoothness and energy function, specifically expressed as:

[0136]

[0137] The time T is added to the cost function to generate trajectories in a shorter time and avoid the trajectory generation time being too long. At the same time, λ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 lead to the quality of the aggregated control point group. Here, p is used k,i，j To indicate that there is a j-th control point to be placed in the i-th segment node vector of the k-th 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 edges adjacent to the control points, i.e., the set of constraint equations for the control points. In three-dimensional space, after being subdivided into voxel units, each voxel unit has 26 adjacent units surrounding it. The benefit of defining this map is that it allows the quadcopter to clearly subdivide each control point into each unit when searching for a path.

[0145] After a deterministic graph search, we need to find the shortest path between the clustered 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. Perform path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory;

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

[0148] S51, extracting the control point with the smallest cost from 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 voxels adjacent to the first control point and proceeding to step S53;

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

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

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

[0153]

[0154] Where ρ 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, p0 represents the starting position point, v max Indicates maximum speed.

[0155] Specifically, the present invention uses an improved A* to search for the shortest path of an aggregated control point group. 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 considered to be 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, from θ 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 the 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 affected.

[0157] In this invention, the traditional Euclidean distance or other heuristic functions for distance expression are no longer used. The designed heuristic function considers more whether the trajectory can generate a reliable and safe trajectory in a short time. Here, only the derivative of position, namely 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 velocities 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 is calculated, and the vector about time can be obtained. 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 to minimize 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 point obtained by the final search can be used as the control point of the last step before the safety constraint. At this time, the control point that the quadrotor drone can explore and after a series of optimization and transformation steps can generate a trajectory for the quadrotor drone, and finally obtain the searched continuous trajectory, that is, the initial path trajectory.

[0169] S6. Optimize 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, determining whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, and if so, applying spatial constraints to the control points that exceed the preset upper limit; if not, discarding them;

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

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

[0174] In an embodiment of the present invention, adjacent control points in the initial path trajectory satisfy the following conditional expression after cube expansion:

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

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

[0177] Specifically, even after using the improved A* algorithm 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 executing the trajectory, the generated trajectory is still insufficient when facing sudden obstacles or places that cannot be perceived by the front-end vision.

[0178] By leveraging the convex hull of generated B-spline curves, the present invention inserts control points into areas of the quadrotor drone's trajectory where collisions are likely to occur, thereby increasing node vectors and altering the local trajectory. However, the problem with inserting control points is that while the present invention proposes grouping control points, adding too many control points can lead to a significant change in the number of control point groups. This results in a significant computational overhead when re-searching the local modified A* algorithm, and can also lead to unsolvable problems.

[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. Determine 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, it will be spatially constrained. If it does not exceed the set starting upper limit, it will be discarded.

[0181] 2. Expand the voxel where the control point is located 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 will not collide with obstacles.

[0182] 3. If the drone expands into a cube within a certain period of time 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 two or more obstacles, then certain control points are added to this 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 trajectory is found.

[0185] The specific expression is as follows:

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

[0187] At the same time, in the present invention, 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 ={D1, D2, ..., D n}, where D i b i -b j ||, i≠j, and D B The array is from D min to D max Sort in ascending order, so traverse all α i =D i, and finally used as the side length to describe the cube. This is how we construct the expanded cube.

[0190] When these constraints are met, they can be effectively applied to adjacent control point groups, effectively ensuring that each trajectory segment can connect to the next without any obstacles or collisions. Furthermore, adding a limited number of control points after the fourth point can also impose certain constraints on the upper and lower trajectories.

[0191] Compared with the existing technology, the present invention obtains a voxel map through the visual sensor of the quadcopter 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 the multiple path points through the 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 the multiple control point groups to obtain an aggregated control point group; performs a path search on the aggregated control point group based on the A* algorithm to obtain an initial path trajectory; and optimizes the initial path trajectory according to preset rules to obtain a final path trajectory. In this way, the path planning trajectory of the quadcopter drone proposed by the present invention is higher in quality and efficiency.

[0192] It should be noted that, in this document, the terms "comprises," "includes," or any other variations thereof are intended to encompass non-exclusive inclusion, such that a process, method, article, or apparatus comprising a series of elements includes not only those elements but also other elements not explicitly listed, or elements inherent to such process, method, article, or apparatus. In the absence of further limitations, an element defined by the phrase "comprising a ..." does not exclude the presence of other identical elements in the process, method, article, or apparatus comprising the element.

[0193] The embodiments of the present invention are described above in conjunction with the accompanying drawings. What is disclosed is only a preferred embodiment of the present invention. However, the present invention is not limited to the above-mentioned specific implementation methods. The above-mentioned specific implementation methods are merely illustrative and not restrictive. Under the guidance 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, which are all within the protection of the present invention.

Claims

1. A path planning method for a quadrotor 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 using a quadratic programming solver to obtain multiple path points; S2. Update the plurality of path points 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 using 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. Perform 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; 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.

2. The path planning method for a quadrotor drone according to claim 1, wherein: 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 quadrotor drone according to claim 2, wherein: 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 quadrotor drone according to claim 1, wherein: Step S5 includes the following sub-steps: S51, extracting the control point with the smallest cost from 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 voxels adjacent to the first control point and proceeding to step S53; S53, screening the updated control points according to the cost and spatial dimension to obtain the path trajectory, and proceeding to step S54; S54 , performing a time-minimizing search on the path trajectory using a heuristic function to obtain the initial path trajectory.

5. The path planning method for a quadrotor drone according to claim 4, wherein: The heuristic function is h(s), which 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.

6. The path planning method for a quadrotor drone according to claim 1, wherein: The preset rules are: S61, determining whether the control point group occupied by the control points on the initial path trajectory exceeds a preset upper limit, and if so, applying spatial constraints to the control points that exceed the preset upper limit; if not, discarding them; S62, cube-expanding the control points on the initial path trajectory; if the quadrotor drone after cube expansion does not collide with an obstacle after a preset time, no additional control points are added to change the trajectory; if the quadrotor drone after cube expansion collides with two or more obstacles, a preset number of control points are added to the control points after cube expansion to change the trajectory, and cube expansion is performed on the added control points; S63, repeating steps S61 and S62 until all control points in the initial path trajectory are completed to obtain the final path trajectory.

7. The path planning method for a quadrotor drone according to claim 6, wherein: 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 i-th control point after cube expansion, α i-1 Indicates the diagonal length of the i-1th control point after cube expansion, v k,T,i-1 and v k,T,i Denote two adjacent control points, k denotes the order of the B-spline curve, and T denotes the minimization time.

Images