CN116466752A - Unmanned aerial vehicle track rapid intelligent planning method in unknown complex environment - Google Patents

Abstract

The invention discloses a fast intelligent planning method for a UAV trajectory in an unknown complex environment, which includes the following steps: 1. Front-end search, which is divided into two parts: the shortest safe path search and the directional expansion of control points satisfying dynamic constraints, which are sequentially performed to efficiently obtain initial B-spline curve control points and trajectory space-time information; 2. Back-end optimization, which assigns corresponding safe flight corridors to local trajectories according to the stage of the initial control points, limits the simplex containing the minimum volume of local trajectories through the minimum volume basis, and efficiently generates fast and safe final trajectories based on different optimization strategies in a known safe space. This method uses the trajectory spatio-temporal information obtained by the front-end search to simplify the back-end trajectory optimization problem into quadratic programming, which improves the efficiency of generating fast and safe trajectories.

Description

Fast intelligent planning method for UAV track in unknown complex environment

technical field

The invention belongs to the technical field of motion planning and autonomous navigation, and specifically relates to a fast and intelligent planning method for unmanned aerial vehicle track in an unknown complex environment.

Background technique

In recent years, research on the autonomous navigation of UAVs has achieved many results, but generating fast and safe trajectories in unknown and complex environments is still a difficult problem and challenge. In addition to the partial observability of the environment that makes it challenging to obtain fast trajectories, it also makes this task extremely difficult to plan safe trajectories in real-time rolling time domain only relying on limited onboard perception conditions. As shown in Figure 1, the entire three-dimensional space where the UAV is located Can be split into scrolling local maps centered on UAV _Lp /> Known Safe Spaces in /> and known to occupy space /> and unknown spaces outside the local map or not perceived /> i.e. /> How to consider the safety of unknown space is very important to balance flight safety and flight speed. The trade-off strategies of different planning methods in these two aspects are shown in Figure 2. In the figure, L _p and E are the starting point and end point of planning respectively, R is a point on the trajectory L _p → E, F is the end point of the safe trajectory in the corresponding method, ■ means the termination condition (the velocity and acceleration at the end of the trajectory are zero), /> Indicates no termination condition.

In addition to the above factors, generating fast trajectories in an unknown environment is also limited by many conditions. First of all, in an unknown environment, when the UAV only relies on the airborne sensors to perceive and plan the surrounding environment within a limited range, due to the lack of prior map or global trajectory information, it only relies on the local map The planner cannot guarantee that the planning will be successful, so the trajectory is forced to add a termination condition, so that even if the rolling time domain planning fails in the future, the flight safety of the UAV can be guaranteed. This strategy loses the flight speed to a certain extent. Secondly, the different strategies adopted for the time allocation problem involved in trajectory parameterization (how much time is allocated for each interval) will also affect the flight speed. Although the common strategy of solving the equation of motion in a closed form is efficient, the solution it provides is too conservative. Finally, an incomplete solution space can also limit the generation of fast trajectories, especially the interval assignment problem (which polyhedron each segment of the polynomial is in) involved in hard-constrained trajectory optimization methods. If local trajectories are restricted to a single convex polyhedron representing non-convex free space, a loss of solution space is bound to result. To solve this problem, one way is to introduce binary decision variables to allow the optimizer to freely choose the convex polyhedron where the local trajectory is located, but solving the optimization problem with mixed integers is computationally expensive.

In order to take into account the speed while ensuring the safety of the trajectory, FASTER creatively optimizes the trajectory in the known safe space and the unknown space respectively. In order to overcome the limitation of trajectory time and interval allocation, FASTER introduces more intervals than the number of polyhedrons obtained by convex decomposition of free space, and allocates all intervals to the same time obtained by the heuristic strategy, and expresses trajectory generation as a Mixed Integer Quadratic Program (MIQP) with high spatial degrees of freedom to obtain fast and secure trajectories. Although FASTER's ideas and effects are amazing, it still has many shortcomings. The biggest hidden danger is the problem of efficiency and the compromise for efficiency that makes FASTER tend to be conservative. Since the solution time of MIQP problem is too long, in order to improve efficiency, the maximum number of polyhedrons used for optimization has to be limited to reduce the integer decision variables and shorten the solution time. In addition, in order to improve the completeness of the solution space, that is, the volume of the convex polyhedron representing the free space is as large as possible, the path points used for the convex decomposition are interpolated to limit the distance between each pair. Under the influence of these two factors, when the convex polyhedron does not contain the unknown space, FASTER degenerates into the conservative method in Figure 2. Secondly, when FASTER obtains a fast trajectory, it needs to perform two optimizations in a known safe space and an unknown space, that is, it needs to perform two convex decompositions for different spaces, which will bring additional computational overhead. The above deficiencies have affected the planning efficiency of FASTER to a certain extent.

Contents of the invention

Aiming at the efficiency problem existing in the current advanced method FASTER, the present invention provides an efficient, fast and safe quadrotor UAV trajectory generation method with B-spline curve control points as the main body. In each plan, only in the known safe space Trajectory optimization based on fast and safe strategies is carried out in the interior respectively. Ideally, with rolling time-domain planning, the UAV can always execute fast and safe trajectories throughout the navigation process (Fig. 2L _p → R).

For realizing above-mentioned object, the technical scheme that the present invention takes is:

The method for fast and intelligent planning of UAV trajectory in an unknown complex environment is characterized in that it includes two parts: front-end search and back-end optimization:

For the front-end search, in order to ensure the C2 continuity of the trajectory and easy calculation, a cubic B-spline curve with p=3 will be used to search and optimize with the control points of the B-spline as the main body, relying on the local sliding map built around the current position L _p of the drone And according to the current state of the UAV, it includes position L _p , velocity L _v , acceleration L _a and performs re-planning;

Including the following steps:

Step 1: If the final target G _term is not in the local sliding map , then place the final target G _term in /> Directions are projected onto the map /> point G on

Step 2: Run the hop search algorithm from L _p to G and only keep it in the known safe space inside part

Step 3: Calculate the initial B-spline curve control points through the current state of the UAV;

Step 4: towards The vertices of are expanded sequentially, and the next control point is calculated according to the existing control points and dynamic constraints V _max and A _max ;

Back-end optimization, by expanding the control points based on the direction information reflected in the JPS search results, using the effective information reflected in the search results to optimize the trajectory to generate a smooth, safe and dynamically feasible trajectory, fixing the remaining two elements of the B-spline curve, and only optimizing the control points of the remaining unique elements. The constraints and objective functions of each part will be displayed in functional modules, and the optimization form will be given at the end;

Including the following steps:

Step 1: For those in a known safe space Perform a convex decomposition of the known safe space to obtain convex polyhedron />

Step 2: Determine whether the number of control points is sufficient. If not, it means that the obtained trajectory is short. At this time, the known safe space around the trajectory is small, and there is no need for quick optimization, so in this case, conservative backup optimization is performed;

Step 3: If the conditions are satisfied, perform fast optimization first to obtain a fast trajectory, then add termination conditions to it, and then perform safety optimization on the unfixed control points of the fast trajectory to obtain a safe trajectory with termination conditions;

Finally, a fast and safe final trajectory is output through front-end search and back-end optimization.

As a further improvement of the present invention, in the backend optimization process:

The objective function is as follows:

Smooth term f _jerk : calculated by the third derivative of the trajectory, used to measure the smoothness of the trajectory, where is the i-th basis matrix of the 3rd degree B-spline basis function;

Distance item f _goal : represented by the Euclidean distance between the control point and the end point G, used to measure the degree to which the trajectory is close to the end point.

The initial state item f _start : calculated from the initial velocity and acceleration of the trajectory and the corresponding state of the UAV. The purpose of this is to ensure the continuity of the re-planned trajectory on the one hand, and on the other hand to give q ₀ , q ₁ , q ₂ the necessary optimization space, where v ₀ , v ₁ , v ₂ are the control points of the velocity curve, and a ₀ , a ₁ are the control points of the acceleration curve;

As a further improvement of the present invention, in the backend optimization process:

The constraints of each part are as follows:

Dynamics constraint C _dynamics : For velocity constraints, the control point of the velocity curve is converted into the vertex of the minimum simplex to constrain. For the acceleration constraint, because the degree of acceleration curve is 1, the simplex formed by its own control points is already the smallest, so there is no need to convert it. At the same time, a ₀ is determined by the initial acceleration L _a , so there is no need to add constraints to it. is the quadratic curve basis matrix of the minimum volume basis, V _max and A _max are the maximum velocity and acceleration respectively;

Collision-free constraint C _colli-sio : According to Convex decomposition of the known safe space around, get convex polyhedron {(A _p , c _p )}, p=1, 2, ..., P. Then, based on the properties of the B-spline curve, a corresponding convex polyhedron is assigned to each local trajectory according to the stage of the key point, and the control point expanding toward V ₁ is in the first stage;

Initial state constraint C _initial : an equality constraint, used to ensure that the initial position of the trajectory is equal to L _p ;

The final state constraint C _final : the velocity and acceleration at the end of the mandatory trajectory are 0. For the third degree B-spline curve, only need to ensure that the last three control points are equal;

q _n-2 =q _n-1 =q _n 7)

Control point constraint C _cp : it is used to fix the specified control point, and keep the corresponding local fast trajectory, that is, L _p → R, unchanged during security optimization;

As a further improvement of the present invention, the optimization strategies in the back-end optimization process include three types, respectively fast optimization, safety optimization and standby optimization;

1) Fast optimization: the objective function is f _jerk +f _goal +f _start , the constraints are C _dynamics , C _{collision-free} , C _initial , and the fixed _seg is 0;

2) Security optimization: the objective function is f _jerk +f _goal , the constraints are C _dynamics , C _{collision-free} , C _initial , C _cp , and the fixed _seg is 1;

3) Backup optimization: the objective function is f _jerk +f _goal +f _start , the constraints are C _dynamics , C _{collision-free} , C _initial , C _final , and the fixed _seg is 0;

Where fixed _seg represents the number of fixed segment fast trajectories, when fixed _seg ≠ 0 means from q ₀ to A total of fixed _seg +3 control points remain unchanged and do not participate in optimization.

Beneficial effects: the B-spline curve is used to represent the trajectory and its control points are used as the main body of search and optimization, the trajectory space-time information obtained through the front-end search that satisfies the dynamic constraints eliminates the binary plastic decision variable used for interval allocation, and the back-end trajectory optimization problem is simplified to quadratic programming. Efficiently generate fast and safe final trajectories based on different optimization strategies in a known safe space. Compared with the current advanced technology, this method can efficiently generate fast and safe quadrotor UAV trajectories.

Description of drawings

Figure 1 is the space division and symbolic representation;

Figure 2 is a comparison of various planning algorithms;

Fig. 3 is the flowchart of method disclosed in the present invention;

Figure 4 is a schematic diagram of the method.

Detailed ways

The present invention will be further described below in conjunction with specific examples.

The invention discloses an efficient, fast and safe four-rotor UAV trajectory generation method in an unknown complex environment that eliminates plastic variables, including the following steps:

1. Front-end search, in order to ensure the C2 continuity of the trajectory and easy calculation, a three-time B-spline curve with p=3 will be used to search and optimize with the control points of the B-spline as the main body, relying on the local sliding map built around the current position L _p of the drone And according to the current state of the UAV, it includes position L _p , velocity L _v , acceleration L _a and performs re-planning;

Including the following steps:

Step 1: If the final target G _term is not in the local sliding map , then place the final target G _term in /> Directions are projected onto the map /> point G on

Step 2: Run the jump point search algorithm (Jump Point Search, JPS) from L _p to G, and only keep it in the known safe space part inside />

Step 3: Calculate the initial B-spline curve control points through the current state of the UAV;

Step 4: towards The vertices of are expanded sequentially, and the next control point is calculated according to the existing control points and dynamic constraints V _max and A _max .

2. Back-end optimization, through the front-end search, the time and space information of the trajectory is obtained to a certain extent. Obtaining control points by expanding based on the direction information reflected in the JPS search results is efficient but theoretically unsafe. Therefore, it is necessary to use the effective information reflected in the search results for trajectory optimization to generate smooth, safe and dynamically feasible high-quality trajectories. The optimization method is also to fix the remaining two elements of the B-spline curve, and only optimize the control point of the remaining unique element. In order to obtain the final fast and safe trajectory, it is necessary to perform fast and safe trajectory optimization on the trajectories that meet the conditions in turn. Otherwise, the backup optimization that belongs to the conservative method is carried out. Their principles are the same, but there are still differences in form. This article will demonstrate the constraints and objective functions of each part in functional modules, and finally give their optimization forms.

Including the following steps:

(2-1) Objective function;

Smooth term f _jerk : calculated by the third derivative of the trajectory, used to measure the smoothness of the trajectory, where is the i-th basis matrix of the 3rd degree B-spline basis function.

Distance item f _goal : represented by the Euclidean distance between the control point and the end point G, used to measure the degree to which the trajectory is close to the end point.

Initial state item f _start : Calculated from the initial velocity and acceleration of the trajectory and the corresponding state of the UAV. The purpose of this is to ensure the continuity of the replanned trajectory on the one hand, and on the other hand to give q ₀ , q ₁ , and q ₂ the necessary optimization space to improve the success rate of solving the optimization problem. In the formula, v ₀ , v ₁ , and v ₂ are the control points of the velocity curve, and a ₀ , a ₁ are the control points of the acceleration curve.

(2-2) constraints;

Dynamic constraints C _dynamics : For speed constraints, the control points of the speed curve are converted into the vertices of the minimum simplex for constraints, which can effectively improve the completeness of the solution space. As for the acceleration constraint, because the degree of the acceleration curve is 1, the simplex formed by its own control points is already the smallest, so there is no need to convert, and a ₀ is determined by the initial acceleration L _a , so there is no need to add constraints to it, where is the quadratic curve basis matrix of the minimum volume basis, and V _max and A _max are the maximum velocity and acceleration respectively.

Collision-free constraint C _colli-sio : According to Convex decomposition of the known safe space around, get convex polyhedron {(A _p , c _p )}, p=1, 2, ..., P. Then, based on the properties of the B-spline curve, a corresponding convex polyhedron is assigned to each segment of the local trajectory according to the stage of the key point. As shown in Figure 4, the control point expanding toward V ₁ is in the first stage.

Initial state constraint C _initial : an equality constraint, used to ensure that the initial position of the trajectory is equal to L _p .

Final state constraint C _final : the velocity and acceleration at the end of the trajectory are forced to be 0, which is used to ensure the safety of the UAV. For degree 3 B-splines, just make sure the last three control points are equal.

q _n-2 =q _n-1 =q _n 7)

Control point constraint C _cp : used to fix the specified control point, and keep the corresponding local fast trajectory (L _p → R in Figure 2) unchanged during security optimization.

Table 1 Trajectory optimization strategy

The forms of fast optimization, safety optimization and backup optimization strategies are shown in Table 1, fixed _seg represents the number of fixed segment fast trajectories, when fixed _seg ≠ 0 means from q ₀ to A total of fixed _seg +3 control points remain unchanged and do not participate in optimization.

Including the following steps:

Step 1: For those in a known safe space Perform a convex decomposition of the known safe space to obtain convex polyhedron />

Step 2: Determine whether the number of control points is sufficient. If not, it means that the obtained trajectory is short. At this time, the known safe space around the trajectory is small, and there is no need for quick optimization, so in this case, conservative backup optimization is performed;

Step 3: If the conditions are satisfied, perform fast optimization to obtain a fast trajectory, and then add termination conditions to it. Then, safety optimization is performed on the unfixed control points of the fast trajectory to obtain a safe trajectory with termination conditions.

Finally, a fast and safe final trajectory is output.

The above descriptions are only preferred embodiments of the present invention, and are not intended to limit the present invention in any other form, and any modifications or equivalent changes made according to the technical essence of the present invention still belong to the scope of protection claimed by the present invention.

Claims (4)

1. A fast and intelligent planning method for UAV track in an unknown complex environment, characterized in that it includes two parts: front-end search and back-end optimization: For the front-end search, in order to ensure the C2 continuity of the trajectory and easy calculation, a cubic B-spline curve with p=3 will be used to search and optimize with the control points of the B-spline as the main body, relying on the local sliding map built around the current position L _p of the drone And according to the current state of the UAV, it includes position L _p , velocity L _v , acceleration L _a and performs re-planning; Including the following steps: Step 1: If the final target G _term is not in the local sliding map , then place the final target G _term in /> Directions are projected onto the map /> point G on Step 2: Run the hop search algorithm from L _p to G and only keep it in the known safe space part inside /> Step 3: Calculate the initial B-spline curve control points through the current state of the UAV; Step 4: towards The vertices of are expanded sequentially, and the next control point is calculated according to the existing control points and dynamic constraints V _max and A _max ; Back-end optimization, by expanding the control points based on the direction information reflected in the JPS search results, using the effective information reflected in the search results to optimize the trajectory to generate a smooth, safe and dynamically feasible trajectory, fixing the remaining two elements of the B-spline curve, and only optimizing the control points of the remaining unique elements. The constraints and objective functions of each part will be displayed in functional modules, and the optimization form will be given at the end; Including the following steps: Step 1: For those in a known safe space Perform a convex decomposition of the known safe space to obtain convex polyhedron /> Step 2: Determine whether the number of control points is sufficient. If not, it means that the obtained trajectory is short. At this time, the known safe space around the trajectory is small, and there is no need for quick optimization, so in this case, conservative backup optimization is performed; Step 3: If the conditions are satisfied, perform fast optimization first to obtain a fast trajectory, then add termination conditions to it, and then perform safety optimization on the unfixed control points of the fast trajectory to obtain a safe trajectory with termination conditions; Finally, a fast and safe final trajectory is output through front-end search and back-end optimization. 2. in the unknown complex environment according to claim 1, the fast intelligent planning method for the track of the unmanned aerial vehicle is characterized in that: During the backend optimization process: The objective function is as follows: Smooth term f _jerk : calculated by the third derivative of the trajectory, used to measure the smoothness of the trajectory, where is the i-th basis matrix of the 3rd degree B-spline basis function; Distance item f _goal : represented by the Euclidean distance between the control point and the end point G, used to measure the degree to which the trajectory is close to the end point. The initial state item f _start : calculated from the initial velocity and acceleration of the trajectory and the corresponding state of the UAV. The purpose of this is to ensure the continuity of the re-planned trajectory on the one hand, and on the other hand to give q ₀ , q ₁ , q ₂ the necessary optimization space, where v ₀ , v ₁ , v ₂ are the control points of the velocity curve, and a ₀ , a ₁ are the control points of the acceleration curve; 3. in the unknown complex environment according to claim 1, the fast intelligent planning method for the track of the unmanned aerial vehicle is characterized in that: During the backend optimization process: The constraints of each part are as follows: Dynamics constraint C _dynamics : For velocity constraints, the control point of the velocity curve is converted into the vertex of the minimum simplex to constrain. For the acceleration constraint, because the degree of acceleration curve is 1, the simplex formed by its own control points is already the smallest, so there is no need to convert it. At the same time, a ₀ is determined by the initial acceleration L _a , so there is no need to add constraints to it. is the quadratic curve basis matrix of the minimum volume basis, V _max and A _max are the maximum velocity and acceleration respectively; Collision-free constraint C _colli-sio : According to Convex decomposition of the known safe space around, get A convex polyhedron {(A _p , c _p )}, p=1, 2, ..., P. Then, based on the properties of the B-spline curve, a corresponding convex polyhedron is assigned to each local trajectory according to the stage of the key point, and the control point expanding toward V ₁ is in the first stage; Initial state constraint C _initial : an equality constraint, used to ensure that the initial position of the trajectory is equal to L _p ; The final state constraint C _final : the velocity and acceleration at the end of the mandatory trajectory are 0. For the third degree B-spline curve, only need to ensure that the last three control points are equal; q _n-2 =q _n-1 =q _n 7) Control point constraint C _cp : it is used to fix the specified control point, and keep the corresponding local fast trajectory, that is, L _p → R, unchanged during security optimization; 4. in the unknown complex environment according to claim 1, the fast intelligent planning method for the track of the unmanned aerial vehicle is characterized in that: The optimization strategies in the back-end optimization process include three types of fast optimization, safety optimization and backup optimization; 1) Fast optimization: the objective function is f _jerk +f _goal +f _start , the constraints are C _dynamics , C _{collision-free} , C _initial , and the fixed _seg is 0; 2) Security optimization: the objective function is f _jerk +f _goal , the constraints are C _dynamics , C _{collision-free} , C _initial , C _cp , and the fixed _seg is 1; 3) Backup optimization: the objective function is f _jerk +f _goal +f _start , the constraints are C _dynamics , C _{collision-free} , C _initial , C _final , and the fixed _seg is 0; in Indicates the number of fixed segment fast trajectories, when fixed _seg ≠ 0 means from q ₀ to /> A total of fixed _seg +3 control points remain unchanged and do not participate in optimization.