Method for Rapid Intelligent Planning of UAV Trajectory in Unknown Complex Environments
By using the B-spline control point method in unknown complex environments, fast and safe drone trajectory planning is solved, and the problem of difficulty in taking into account both flight speed and safety in the prior art is solved, and efficient and safe trajectory generation is achieved.
Patent Information
- Application Number
- CN202310669654.2
- Authority / Receiving Office
- CN · China
- Patent Type
- Patents(China)
- Current Assignee / Owner
- Filing Date
- 2023-06-07
- Publication Date
- 2025-06-24
- Estimated Expiration
- 2043-06-07
AI Technical Summary
In unknown complex environments, prior art is difficult to plan the trajectory of a drone quickly and safely, especially in the absence of prior maps or global trajectory information, which makes it difficult to take into account both flight speed and safety.
Using the B-spline curve control point as the main body, fast and security strategies are optimized in a known safe space, and smooth, secure and dynamically feasible trajectories are generated through a combination of front-end search and back-end optimization.
It realizes rapid and safe planning of drone tracks in unknown complex environments, improves flight speed and safety, reduces calculation overhead, and avoids the use of plastic shaping variables.
Smart Images
Figure CN116466752B_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of autonomous navigation for motion planning, and specifically relates to a method for rapidly and intelligently planning the flight path of an unmanned aerial vehicle (UAV) in an unknown and complex environment. Background Art
[0002] In recent years, many research results have been achieved in the field of UAV autonomous navigation. However, generating a fast and safe trajectory in an unknown and complex environment remains a difficult problem and challenge. In addition to the partial observability of the environment making it challenging to obtain a fast trajectory, relying solely on the limited on-board sensing conditions to plan a safe trajectory in real-time with a rolling time domain also makes this task extremely difficult. As Figure 1 shown, the entire three-dimensional space where the UAV is located can be divided into a known safe space p and a known occupied space in a rolling local map centered on the UAV L and the unknown space outside or not perceived by the local map That is How to consider the safety of the unknown space is crucial for weighing flight safety and flight speed. The trade-off strategies of different planning methods in these two aspects are as Figure 2 shown. In the figure, L p and E are the starting point and the ending point of the planned trajectory respectively, R is a point on the trajectory L p →E, F is the ending point of the safe trajectory in the corresponding method, ■ means the termination condition (the speed and acceleration at the end of the trajectory are zero), and ▲ indicates no termination condition.
[0003] In addition to the above factors, generating a fast trajectory in an unknown environment is also restricted by many conditions. First of all, in an unknown environment, when the UAV only relies on on-board sensors to sense and plan the environment within a limited range around it, due to the lack of prior map or global trajectory information, relying solely on the local map The planner cannot guarantee the success of the plan. Therefore, termination conditions are forced to be added to the trajectory so that the flight safety of the UAV can be ensured even in the case of the failure of the rolling horizon planning in the future. This strategy sacrifices the flight speed to a certain extent. Secondly, different strategies for the time allocation problem involved in trajectory parameterization (how much time is allocated to each interval) will also affect the flight speed. The common strategy of solving the closed-form solution through the motion equation is efficient, but the solution it provides is too conservative. Finally, the incomplete solution space will also limit the generation of fast trajectories, especially the interval allocation problem involved in the hard constraint trajectory optimization method (in which polyhedron each polynomial segment is located). If the local trajectory is restricted within a single convex polyhedron representing the non-convex free space, the solution space will inevitably be lost. To solve this problem, one method is to introduce binary decision variables to allow the optimizer to freely select the convex polyhedron in which the local trajectory is located, but the computational cost of solving the mixed-integer optimization problem is relatively high.
[0004] To balance speed while ensuring trajectory safety, FASTER creatively performs trajectory optimization in known safe spaces and unknown spaces respectively. To overcome the limitations of trajectory time and interval allocation, FASTER introduces more intervals than the number of polyhedra obtained by convex decomposition of the free space, and allocates the same time obtained by the heuristic strategy to all intervals. The trajectory generation is represented as a Mixed Integer Quadratic Program (MIQP) with high spatial degrees of freedom to obtain fast and safe-guaranteed trajectories. Although the idea and effect of FASTER are amazing, there are still many deficiencies. The biggest hidden danger is the efficiency problem and the compromise for efficiency so that FASTER tends to be conservative. Since the solution time of the MIQP problem is too long, in order to improve efficiency, it is necessary to limit the maximum number of polyhedra used for optimization to reduce the integer decision variables and shorten the solution time. In addition, 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 convex decomposition are interpolated to limit the distance between two points. Under the action of these two factors, when the convex polyhedron does not contain the unknown space, FASTER degenerates into Figure 2 the conservative method in Summary of the Invention
[0005] Aiming at the efficiency problem existing in the current advanced method FASTER, the present invention provides an efficient fast and safe trajectory generation method for a quadrotor UAV with B-spline curve control points as the main body. In each planning, trajectory optimization based on fast and safe strategies is only carried out within the known safe space respectively. In the ideal case, through rolling horizon planning, the UAV can always execute a fast and safe-guaranteed trajectory throughout the navigation process( Figure 2 L p →R).
[0006] To achieve the above object, the technical solution adopted by the present invention is as follows:
[0007] A method for fast and intelligent planning of the UAV flight path in an unknown complex environment, characterized in that it includes two parts: front-end search and back-end optimization:
[0008] Front-end search, in order to ensure the C2 continuity of the trajectory and for simple calculation, a cubic B-spline curve with p = 3 will be used, and the search and optimization are carried out with the control points of the B-spline as the main body, relying on the local sliding map constructed with the current position L of the UAV p as the center and according to the current state of the UAV including the position L p , speed L v , acceleration L a and perform replanning;
[0009] It includes the following steps:
[0010] Step 1: If the final target G term is not within the local sliding map , then project the final target G term onto the point G on the map in the direction;
[0011] Step 2: Run the jump point search algorithm from L p to G, and only retain the part within the known safe space ;
[0012] Step 3: Calculate the initial B-spline curve control points through the current state of the UAV;
[0013] Step 4: Expand towards the vertices of , and calculate the next control point according to the existing control points and dynamic constraints V max , A max ;
[0014] Backend optimization: Obtain control points by expanding based on the direction information reflected in the JPS search results, and use the effective information reflected in the search results to optimize the trajectory to generate a smooth, safe, and dynamically feasible trajectory. Fix the other two elements of the B-spline curve and only optimize the remaining unique element, the control point. Modularize each part of the constraints and objective functions by sub-functions and give the optimized form at the end;
[0015] It includes the following steps:
[0016] Step 1: Perform convex decomposition on the known safety space of to obtain convex polyhedra
[0017] Step 2: Determine whether the number of control points is large enough. If not, it means the obtained trajectory is short. At this time, there is less known safety space around the trajectory, and there is no need for rapid optimization. Therefore, in this case, perform conservative backup optimization;
[0018] Step 3: If the condition is satisfied, first perform rapid optimization to obtain a rapid trajectory, then add termination conditions to it, and then perform safety optimization on the control points that are not fixed in the rapid trajectory to obtain a safe trajectory with termination conditions;
[0019] Finally, output a fast and safe final trajectory through front-end search and backend optimization.
[0020] As a further improvement of the present invention, during the backend optimization process:
[0021] The objective function is as follows:
[0022] Smoothing term f jerk : Calculated from the third derivative of the trajectory and used to measure the smoothness of the trajectory. In the formula, is the i-th basis matrix of the cubic B-spline basis function;
[0023]
[0024] Distance term f goal : Represented by the Euclidean distance between the control point and the end point G and used to measure the degree to which the trajectory approaches the end point.
[0025]
[0026] Initial state term f start : Calculated from the initial velocity and acceleration of the trajectory and the corresponding state of the UAV. The purpose of doing this is to ensure the continuity of the replanned trajectory on the one hand and give necessary optimization space to q0, q1, q2 on the other hand. In the formula, v0, v1, v2 are the control points of the velocity curve, and a0, a1 are the control points of the acceleration curve;
[0027]
[0028] As a further improvement of the present invention, during the said backend optimization process:
[0029] The constraints of each part are as follows:
[0030] Dynamic constraint C dynamics : For the velocity constraint, the control points of the velocity curve are transformed into the vertices of the minimum simplex for constraint. For the acceleration constraint, since the degree of the acceleration curve is 1 and the simplex formed by its own control points is already the minimum, no transformation is required. At the same time, a0 is determined by the initial acceleration L a and thus no constraint needs to be added to it. In the formula is the quadratic curve basis matrix of the minimum volume basis, V max and A max are the maximum velocity and acceleration respectively;
[0031]
[0032] Collision-free constraint C collision-free : According to perform convex decomposition on the known surrounding safe space to obtain convex polyhedra {(A p , c p ), p = 1, 2,..., P}. Then, based on the properties of B-spline curves, assign the corresponding convex polyhedron to each local trajectory according to the stage where the key points are located. The control points expanding towards V1 are in the first stage;
[0033]
[0034] Initial state constraint C initial : An equality constraint used to ensure that the initial position of the trajectory is equal to L p ;
[0035]
[0036] Final state constraint C final : Force the velocity and acceleration at the end of the trajectory to be 0. For a cubic B-spline curve, it is only necessary to ensure that the last three control points are equal;
[0037] q n-2 = q n-1 = q n (7)
[0038] Control point constraint C cp : Used to fix the specified control points. During safety optimization, keep the corresponding local fast trajectory, i.e., L p →R unchanged;
[0039]
[0040] As a further improvement of the present invention, the optimization strategies in the backend optimization process include three types, namely, fast optimization, safety optimization, and standby optimization;
[0041] 1) Fast optimization: The objective function is f jerk +f goal +f start , and the constraints are C dynamics , C collision-free , C initial , and fixed seg is 0;
[0042] 2) Safety optimization: The objective function is f jerk +f goal , and the constraints are C dynamics , C collision-free , C initial , C cp , and fixed seg is 1;
[0043] 3) Standby optimization: The objective function is f jerk +f goal +f start , and the constraints are C dynamics , C collision-free , C initial , C final , and fixed seg is 0;
[0044] Among them, fixed seg represents the number of fixed segmented fast trajectories. When fixed seg ≠0, it means that a total of fixed +3 control points from q0 to seg remain unchanged and do not participate in the optimization.
[0045] Beneficial effects: The trajectory is represented by a B-spline curve, and its control points are used as the main body for search and optimization. The trajectory spatio-temporal information obtained through the front-end search that satisfies the dynamic constraints eliminates the binary integer decision variables for interval allocation, simplifying the backend trajectory optimization problem to a quadratic programming problem. In the known safety space, based on different optimization strategies, fast and safe final trajectories can be efficiently generated. Compared with the current advanced technologies, this method can efficiently generate fast and safe quadrotor UAV motion trajectories. Description of the Drawings
[0046] Figure 1 is the space division and symbol representation;
[0047] Figure 2 is the comparison of various planning algorithms;
[0048] Figure 3 is the flowchart of the method disclosed in the present invention;
[0049] Figure 4 is the method schematic diagram. Detailed implementation manners
[0050] The present invention will be further described below in conjunction with specific embodiments.
[0051] The present invention discloses an efficient, fast and safe trajectory generation method for a quadrotor UAV in an unknown complex environment eliminating integer variables, including the following steps:
[0052] 1. Front-end search: To ensure the C2 continuity of the trajectory and for simplicity of calculation, cubic B-spline curves with p = 3 will be used, and the control points of the B-spline will be used as the main body for search and optimization, relying on the local sliding map p constructed centered on the current position L of the UAV and according to the current state of the UAV including the position L p , speed L v , acceleration L a and perform replanning;
[0053] including the following steps:
[0054] Step 1: If the final target G term is not within the local sliding map , then project the final target G term in the direction onto the point G on the map ;
[0055] Step 2: Run the Jump Point Search (JPS) algorithm from L p to G, and only retain the part within the known safe space ;
[0056] Step 3: Calculate the initial B-spline curve control points through the current state of the UAV;
[0057] Step 4: Expand successively towards the vertices, and calculate the next control point according to the existing control points and the dynamic constraints V max , A max .
[0058] 2. Back-end optimization: Through front-end search, the temporal and spatial 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 to optimize the trajectory to generate a smooth, safe, and dynamically feasible high-quality trajectory. The optimization method is also to fix the other two elements of the B-spline curve and only optimize the remaining single element, the control point. In order to obtain the final fast and secure trajectory, it is necessary to perform fast and safe trajectory optimization on the trajectories that meet the conditions in sequence. Otherwise, perform the backup optimization belonging to the conservative method. Their principles are the same, but there are still differences in form. This article will display the constraints and objective functions of each part in a functional modular manner and give their optimized forms at the end.
[0059] It includes the following steps:
[0060] (2-1) Objective function;
[0061] Smoothing term f jerk : Calculated from the third derivative of the trajectory, used to measure the smoothness of the trajectory. In the formula is the i-th basis matrix of the 3rd B-spline basis function.
[0062]
[0063] Distance term 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 approaches the end point.
[0064]
[0065] Initial state term 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 give the necessary optimization space to q0, q1, q2 to improve the success rate of solving the optimization problem. In the formula, v0, v1, v2 are the control points of the velocity curve, and a0, a1 are the control points of the acceleration curve.
[0066]
[0067] (2-2) Constraints;
[0068] Dynamic constraint C dynamics : For the velocity constraint, the control points of the velocity curve are transformed into the vertices of the minimum simplex for constraint, which can effectively improve the completeness of the solution space. For the acceleration constraint, since the degree of the acceleration curve is 1 and the simplex formed by its own control points is already the smallest, there is no need for transformation. At the same time, a0 is determined by the initial acceleration L aDecided, so there is no need to add constraints to it. In the formula is the quadratic curve basis matrix of the minimum volume basis, and V max and A max are the maximum speed and acceleration respectively.
[0069]
[0070] Collision-free constraint C collision-free : According to perform a convex decomposition on the known surrounding safety space to obtain convex polyhedra {(A p , c p ),}, p = 1, 2,..., P. Then, based on the properties of B-spline curves, assign the corresponding convex polyhedra to each local trajectory according to the stage where the key points are located. For example, Figure 4 the control points expanding towards V1 are in the first stage.
[0071]
[0072] Initial state constraint C initial : An equality constraint used to ensure that the initial position of the trajectory is equal to L p .
[0073]
[0074] Final state constraint C final : Force the speed and acceleration at the end of the trajectory to be 0 to ensure the safety of the UAV. For a cubic B-spline curve, it is only necessary to ensure that the last three control points are equal.
[0075] q n-2 = q n-1 = q n (7)
[0076] Control point constraint C cp : Used to fix the specified control points and keep the corresponding local fast trajectory ( Figure 2 in L p →R) unchanged during safety optimization.
[0077]
[0078] Table 1 Trajectory optimization strategy
[0079]
[0080] The forms of fast optimization, safety optimization, and alternative optimization strategies are shown in Table 1. fixed seg represents the number of fixed segmented fast trajectories. When fixed segWhen it is not equal to 0, it means from q0 to A total of fixed seg + 3 control points remain unchanged and do not participate in the optimization.
[0081] It includes the following steps:
[0082] Step 1: For those within the known safety space Perform convex decomposition of the known safety space to obtain convex polyhedra
[0083] Step 2: Determine whether the number of control points is large enough. If not, it means the obtained trajectory is short. At this time, there is less known safety space around the trajectory, and it is not necessary to perform fast optimization. Therefore, in this case, conservative backup optimization is performed;
[0084] Step 3: If the conditions are met, first perform fast optimization to obtain a fast trajectory, and then add termination conditions to it. Then, perform safety optimization on the control points of the fast trajectory that are not fixed to obtain a safe trajectory with termination conditions.
[0085] Finally, output the fast and safe final trajectory.
[0086] The above is only a preferred embodiment of the present invention, and it is not any other form of limitation to the present invention. Any modification or equivalent change made according to the technical essence of the present invention still belongs to the scope protected by the present invention.
Claims
1. A method for rapid and intelligent path planning of unmanned aerial vehicles in unknown complex environments, characterized in that: It includes two parts: front-end search and back-end optimization: In order to ensure the C2 continuity of the trajectory and the simplicity of calculation, a cubic B-spline curve with p=3 is used. The control points of the B-spline are used as the main body for search and optimization, relying on the current position L of the drone. p Local sliding map built for the center And according to the current state of the drone including the position L p , speed L v , acceleration L a and re-planning; It includes the following steps: Step 1: If the final target G term is not within the local sliding map then project the final target G term in the direction onto the point G on the map ; Step 2: Run the jump point search algorithm from L p to G, and only keep the part of it within the known safe space inside Step 3: Calculate the initial B-spline curve control points based on the current state of the UAV; Step 4: Expand successively towards the vertex, and calculate the next control point according to the existing control points and the dynamic constraint V max , A max ; Back-end optimization: Expand to obtain control points through the direction information reflected by the JPS search results, optimize the trajectory using the effective information reflected by the search results to generate a smooth, safe and dynamically feasible trajectory, fix the other two elements of the B-spline curve, only optimize the remaining unique element, the control point, modularize each part of the sub-function to display the constraints and objective functions, and finally give the optimized form; It includes the following steps: Step 1: For the perform a convex decomposition of the known safe space to obtain convex polyhedra Step 2: Judge whether the number of control points is large enough. If not, it means that the obtained trajectory is short. At this time, there is less known safe space around the trajectory, and there is no need to perform rapid optimization. Therefore, in this case, conservative backup optimization is performed; Step 3: If the conditions are met, first perform rapid optimization to obtain a rapid trajectory, then add termination conditions to it, and then perform safety optimization on the unfixed control points of the rapid trajectory to obtain a safe trajectory with termination conditions; Finally, output a fast and safe final trajectory through front-end search and back-end optimization.
2. The method for rapid intelligent planning of UAV flight path in an unknown complex environment according to claim 1, characterized in that: During the back-end optimization process: The objective function is as follows: Smoothing term f jerk : Calculated from the third derivative of the trajectory and used to measure the smoothness of the trajectory. In the formula, is the i-th basis matrix of the cubic B-spline basis function; Distance term 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 approaches the end point; Initial state term 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 to give necessary optimization space to q0, q1, q2. In the formula, v0, v1, v2 are the control points of the velocity curve, and a0, a1 are the control points of the acceleration curve; 3. The method for rapid intelligent planning of UAV flight path in an unknown complex environment according to claim 1, characterized in that: During the back-end optimization process: The constraints of each part are as follows: Dynamic constraint C dynamics : For the velocity constraint, the control points of the velocity curve are transformed into the vertices of the minimum simplex for constraint. For the acceleration constraint, since the degree of the acceleration curve is 1 and the simplex formed by its own control points is already the minimum, there is no need for transformation. At the same time, a0 is determined by the initial acceleration L a , so there is no need to add a constraint to it. In the formula 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 collision-free : According to perform a convex decomposition on the known surrounding safety space to obtain convex polyhedra {(A p , c p ),}, p = 1, 2,..., P. Then, based on the properties of B-spline curves, assign the corresponding convex polyhedra to each local trajectory according to the stage where the key points are located. The control points expanding towards V1 are 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 : Force the velocity and acceleration at the end of the trajectory to be 0. For a cubic B-spline curve, it is only necessary to ensure that 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. During safety optimization, keep the corresponding local fast trajectory, i.e., L p →R unchanged; 4. The method for rapid intelligent planning of UAV flight path in an unknown complex environment according to claim 1, characterized in that: The optimization strategies during the back-end optimization process include three types: rapid 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 , fixed seg is 0; 2) Safety optimization: The objective function is f jerk +f goal , and the constraints are C dynamics , C collision-free , C initial , C cp , and fixed seg is 1; 3) Spare optimization: The objective function is f jerk + f goal + f start , and the constraints are C dynamics , C collision-free , C initial , C final , and fixed seg is 0; where fixed seg represents the number of fixed segmented fast trajectories. When fixed seg ≠ 0, it means that from q0 to a total of fixed seg + 3 control points remain unchanged and do not participate in the optimization.