Four-rotor unmanned aerial vehicle flight path planning method

Abstract

The invention belongs to the technical field of unmanned aerial vehicle control, and particularly relates to a four-rotor unmanned aerial vehicle flight path planning method. The method comprises the steps of environment modeling, path searching, flight corridor generation and trajectory optimization. Wherein in the path searching process, an initialized path is found by utilizing the rapid exploration capability of an RRT algorithm in a complex environment, unnecessary node expansion is reduced, then the path is accurately optimized in a small range by virtue of a JPS algorithm, a collision-free global path is rapidly searched, the path searching time is greatly shortened, the searching efficiency is improved, and the path searching efficiency is improved. The strict requirement of the unmanned aerial vehicle on the real-time performance of path planning in a complex environment can be met; through innovative algorithm design and optimization strategies, the unmanned aerial vehicle can rapidly plan a safe and smooth flight path in a complex environment, the task execution capability and efficiency of the unmanned aerial vehicle are improved, and wide application of the four-rotor unmanned aerial vehicle in more fields is promoted.

Description

Technical Field

[0001] This invention belongs to the field of unmanned aerial vehicle (UAV) control technology, and particularly relates to a method for planning the trajectory of a quadcopter UAV. Background Technology

[0002] Quadrotor UAV trajectory planning refers to the process of planning a flight path that passes through various waypoints and performs a series of tasks during a flight mission, based on set waypoint coordinates. The goal of trajectory planning algorithm design is to achieve efficient mission execution while ensuring UAV flight safety. Trajectory planning typically considers the UAV's flight environment, dynamic constraints, and mission requirements. Currently, commonly used quadrotor UAV trajectory planning algorithms mainly include front-end path search and back-end local path optimization. The specific process includes: UAV flight environment modeling; path search algorithms generating a global path for the UAV in the flight environment, which must avoid collisions and meet mission requirements. Commonly used path search algorithms include the classic A* algorithm, Dijkstra's algorithm, RRT algorithm, and JPS algorithm. Since the global path obtained by the path search algorithm is a series of discrete points and does not meet the UAV's dynamic constraints, it is impossible to directly control the UAV to fly along this path. The next stage will optimize the global path to obtain a smooth, collision-free local path that meets the UAV's dynamic constraints, along which the UAV will track and fly. Currently, commonly used trajectory optimization methods include numerical optimization methods, geometric optimization methods, and hybrid algorithms.

[0003] Currently, the existing quadcopter UAV trajectory planning technologies closely related to this invention and their shortcomings are as follows: Schemes based on a single traditional path search algorithm, such as simply using the A* algorithm for path search. The A* algorithm is a heuristic search algorithm that selects expansion nodes by calculating the heuristic function values ​​of nodes, thereby finding a path from the starting point to the target point. It can effectively find feasible paths in simple environments, but in complex environments, the computational load increases dramatically due to the need to expand a large number of nodes, resulting in low search efficiency. Moreover, the paths it generates are also discrete points, which do not satisfy the dynamic constraints of the UAV and require further optimization. For example, in indoor environments with numerous obstacles or complex terrain such as mountains, using the A* algorithm to plan a flight path may take several seconds or even tens of seconds, failing to meet the real-time flight requirements of UAVs.

[0004] Schemes based on simple trajectory optimization: Some methods employ only simple interpolation algorithms, such as linear interpolation, during trajectory optimization. While linear interpolation is computationally simple, the generated trajectory merely connects discrete points, resulting in noticeable inflections at these points and poor trajectory smoothness. This fails to meet the requirements for trajectory continuity and stability during UAV flight. In actual flight, this can lead to frequent adjustments in the UAV's flight attitude, increasing the difficulty of flight control, flight risks, and energy consumption.

[0005] Solutions based on traditional combinatorial algorithms: Some trajectory planning methods combine traditional path search algorithms, such as the Path Finder (RRT) algorithm, with simple trajectory optimization methods, such as numerical optimization methods. While the RRT algorithm can quickly find a feasible path in complex environments, it suffers from blindness and low path quality. Numerical optimization methods, on the other hand, may not effectively balance trajectory smoothness and safety when dealing with complex constraints, resulting in poor overall trajectory planning performance. Traditional combinatorial algorithms are often designed for specific environments and tasks, making it difficult to flexibly adjust when flight environments or task requirements change. For example, when switching from indoor environments to complex outdoor weather environments, or from simple point-to-point flight tasks to complex area coverage tasks, traditional combinatorial algorithms may not be able to adapt effectively, requiring redesign and adjustment of algorithm parameters, increasing usage costs and complexity.

[0006] Existing trajectory planning technologies struggle to comprehensively address the constraints of UAV dynamics, flight environment, and mission requirements. Some algorithms only consider collision avoidance safety constraints, neglecting the dynamic limitations of the UAV, resulting in generated trajectories that are unenforceable in actual flight. Other algorithms, while considering some dynamic constraints, cannot guarantee that the trajectory remains within a safe zone in complex environments, posing safety risks.

[0007] Chinese patent document CN110162104A discloses a path planning method for unmanned aerial vehicles (UAVs) based on pilot visual perception. This method sets several pilot visual guidance points in the state space as nodes that the UAV must pass through on its path. These guidance points divide the path from the UAV's starting point to the target point into multiple track segments. For each track segment, an improved RRT algorithm is applied for path planning. This improved RRT algorithm introduces target bias into the traditional RRT algorithm, considers constraints imposed by the UAV's physical performance, and incorporates track distance constraints. The path planning for the UAV is obtained by combining the planning results of each track segment. However, this algorithm relies excessively on the setting of guidance points. When guidance points are set in confined spaces, this can easily lead to excessively long path planning times and low path search efficiency.

[0008] Chinese patent document CN116608877A proposes a biomimetic global path search method based on the original RRT algorithm, incorporating the phototropism of plants. This method optimizes aspects such as poor guidance during random expansion, slow convergence speed, low search efficiency in narrow areas, and tortuous planned paths. The biomimetic design in this invention simulates the phototropism of plants, allowing the random tree to select expansion nodes on a series of concentric circles centered on the target point, thus accelerating the expansion speed of the random tree towards the target point and improving the algorithm's convergence efficiency. To enhance safety during the random tree's expansion towards the target point, obstacles are processed by merging closely spaced obstacles and expanding existing obstacles. Secondly, an adaptive step size is designed, where the random tree adjusts the step size according to the density of obstacles to better adapt to complex environments. After extracting key points from the generated path, curve optimization is performed to smooth the path and meet the driving requirements of intelligent vehicles. Finally, MATLAB simulation analysis is used to verify the effectiveness of the algorithm. However, setting up nodes on concentric circles centered on the target point requires constructing concentric circles and determining the nodes within them, which takes a long time and makes it difficult to guarantee that the trajectory obtained by expanding nodes through concentric circles is optimal.

[0009] In view of this, the present invention relates to a trajectory planning method for a quadcopter unmanned aerial vehicle (UAV) to solve the problems of low path search efficiency, poor trajectory smoothness, insufficient algorithm versatility, and inability to effectively take into account multiple constraints. Summary of the Invention

[0010] This invention proposes a trajectory planning method for quadrotor UAVs, including front-end path search and back-end trajectory optimization. The front-end path search algorithm integrates improved RRT and JPS algorithms, while the back-end adopts a trajectory optimization method that combines flight corridors and Bézier curves to improve the performance of trajectory planning. The specific steps include environment modeling, path search, generation of flight corridors, and trajectory optimization to obtain efficient, accurate, and universal optimal local paths for UAV flight.

[0011] The present invention aims to overcome at least one of the defects of the prior art and provide a method for planning the trajectory of a quadcopter unmanned aerial vehicle.

[0012] The detailed technical solution of this invention is as follows: A method for trajectory planning of a quadcopter unmanned aerial vehicle (UAV), the method comprising: S1. Environment Modeling: Based on the size of the UAV, the dilation parameters are set, and the set dilation parameters are used to process the grid map containing obstacle distribution information to generate a three-dimensional grid map for path search. S2, Path Search: Obtain a collision-free global path from the 3D raster map used for path search established in step S1 by integrating the improved RRT algorithm and JPS algorithm. S3. Generate flight corridor: Use the dilation method to process the global path generated in step S2 to obtain a flight corridor suitable for UAV flight; S4. Trajectory Optimization: The unoptimized local path is obtained by cutting the global path generated in step S2, and the local path is represented by a trajectory based on Bézier curves to obtain a trajectory model. Then, the trajectory is optimized based on the optimization objective and the constraints of the flight corridor generated in step S3 to obtain the optimal local path.

[0013] Preferably, the expansion parameter set in step S1 is greater than the radius of the UAV.

[0014] Preferably, the process of fusing the RRT and JPS algorithms in step S2 is as follows: S21. Initialize a 3D raster map and a growing tree containing the starting point using the RRT algorithm; S22. The sampling strategy based on the RRT algorithm starts from the starting point of the growth tree in the initialized 3D grid map, randomly generates sampling points, and continuously expands the growth tree until the growth tree extends into the neighborhood of the target point, thus obtaining a feasible initialization path. S23. Expand the surrounding neighborhood of the initialization path obtained in step S22 by 2 to 4 times the radius of the UAV, and finally obtain the feasible area for initializing the UAV. S24. Use the JPS algorithm to perform path search in the initial feasible area of ​​the UAV determined in step S23 to find the optimal path and obtain a collision-free global path.

[0015] More preferably, in step S22, the sampling strategy set by the RRT algorithm is used to randomly sample and generate sampling points, continuously expanding the growth tree until the growth tree extends into the neighborhood of the target point, thus obtaining a feasible initialization path, specifically as follows: S221. A sampling strategy based on the RRT algorithm randomly generates sampling points in the map; S222. Based on the sampling points generated in step S221, calculate the expansion step size, and expand the growth tree according to the expansion step size to generate new branches; S223. Check whether the new branch generated in step S222 passes through an obstacle. If it does, delete the new branch. The new branch will not be used as a planned path. However, check whether the new branch passes through the target point area. If it does, connect the other branches except the deleted branch and execute step S23. If it does not pass through an obstacle, continue to check whether the new branch passes through the target point area. If yes, execute step S23. If no, return to step S221 and continue to loop steps S221-S223.

[0016] Preferably, the expansion treatment method in step S3 is as follows: The global path is extended outward until an obstacle is encountered, thus creating a flight corridor consisting of multiple consecutive areas.

[0017] Preferably, step S4 specifically includes: A hybrid Bézier curve and flight corridor approach is used as the trajectory optimization algorithm to generate smooth and safe flight paths in complex environments in real time. First, the local path length is set, and the global path generated in step S2 is cut based on this length. Unoptimized local paths are obtained from the global paths, and the local paths are represented by Bézier curves. An objective function that minimizes the snap is established. Then, the trajectory optimization problem is transformed into a convex quadratic programming problem based on constraints constructed by the flight corridor. Finally, an optimal local path is obtained through optimization.

[0018] More preferably, the step of representing the local path based on a Bézier curve specifically involves: The local trajectory is represented by a Bézier curve, which is represented by Bernstein polynomials. The basis functions of the Bernstein polynomials are: (1) Where n represents the highest order of curvature, i is the index of the control point of the Bézier curve, and t is time. Let Bernstein's basis function represent the i-th control point. The polynomial composed of Bernstein's basis functions is called a Bézier curve. Therefore, a seventh-order Bézier curve can be written as: (2) in This is the set of control points for this segment of the Bézier curve. Let be the Bernstein basis function for the i-th control point, and t be time.

[0019] More preferably, the objective function for minimizing the snap is: For a certain dimensional variable The minimization objective function can be written as: (3) in, yes The objective function to be minimized is given by T, where T represents the total time and t represents the time variable. Indicates in dimension Above, the position function of the drone at time t, where m represents the number of segments into which the total time T is divided. yes The components within the k-th time interval, and These represent the start and end times of the k-th time interval, respectively. The cost function for the Kth segment of the trajectory can be expressed as: (4) in, The time scaling factor for the Kth segment of the trajectory. Here are the polynomial coefficients, and t is the time variable. For in dimension The cost function of the k-th segment of the trajectory. This represents the starting time of the k-th time interval.

[0020] More preferably, the constraints based on the flight corridor construction include boundary constraints, continuity constraints, safety constraints, and dynamic feasibility constraints, as detailed below: First, using the derivative properties of Bézier curves, we define the control points of each order for the UAV. The dimension is Let the i-th control point of the K-th trajectory be... The dimension is The i-th control point of the K-th trajectory l The first derivative, then It can be represented as: = (5) The dimension is The i-th control point of the K-th trajectory l The order derivative, where n is the order of the Bézier curve. The dimension is The Kth segment of the trajectory i +1 control point l -1st derivative, The dimension is The Kth segment of the trajectory i control points l -1st derivative; Boundary constraints: These restrict the position, velocity, acceleration, and sag of the starting and ending points of each trajectory segment. The derivative constraints for the starting and ending points can be expressed as follows: (6) in, Represents the constraints of each order at the starting point. Constraints of various orders representing the endpoint, Representative dimension is The derivatives of each order of the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the trajectory of the k-th segment. Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory; Continuity Constraints: The smoothness of the trajectory is crucial for the safety and stability of the aircraft. To achieve a smooth trajectory, continuity constraints need to be added between segments to ensure that parameters such as position, velocity, acceleration, and jerk remain consistent between adjacent segments. For the Kth and K+1th segments, we have: (7) in, Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory. Represents a time-related scaling factor. Representative dimension is The derivatives of each order of the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the (k+1)th segment of the trajectory. The dimension is The i-th control point of the K-th trajectory segment; Safety constraints: When generating the trajectory, it is necessary to ensure that the drone flies within a safe corridor, avoiding collisions and entering no-fly zones. Therefore, the Bézier curve needs to be restricted to the feasible region. The position inequality constraint introduced by the optimization problem is as follows: (8) in and For dimension The upper and lower boundaries of the flight corridor corresponding to the Kth trajectory segment, where n is the highest order of the polynomial. It is a dimension of The i-th control point of the K-th trajectory segment. The time-dependent scaling factor representing the trajectory of the k-th segment; Dynamic feasibility constraints: The generated trajectory needs to take into account the dynamic limitations of the UAV to ensure the executability of the trajectory in actual flight. Inequality constraints are introduced to ensure the dynamic feasibility of the trajectory.

[0021] (9) (10) (11) in , These are the minimum and maximum speed limits for the drone. , These are the minimum and maximum acceleration limits, respectively. , These are the minimum and maximum limits for the jump, respectively. It is the order of the Bézier curve. The dimension is The i-th control point of the K-th trajectory segment. The dimension is The (i-1)th control point of the Kth segment of the trajectory. The dimension is The (i-2)th control point of the Kth segment of the trajectory. The dimension is The (i-3)th control point of the Kth segment of the trajectory. Represents the time scaling factor of the Kth segment of the trajectory; Boundary constraints and continuity constraints can be written in the form of equality constraints: (12) in, This represents the coefficient matrix describing the problem, and c represents the control points. Represents the target value or external input.

[0022] Safety constraints and multi-order dynamic feasibility constraints can be written in the form of inequality constraints: (13) in, The coefficient matrix, where c represents the control points. This represents the upper bound vector, which is the maximum allowed value.

[0023] The final trajectory optimization problem for the local path of the UAV can be expressed as: Min: c(14) St: (15) Where c is the polynomial coefficient to be solved, a one-dimensional column vector composed of the polynomial coefficients of k trajectory segments, and also the control point of the UAV trajectory. Q is a positive definite symmetric matrix. According to the design of the flight corridor and the convex hull property of the Bézier curve, the constraint region is also a convex region. Therefore, this is a convex quadratic programming problem. The optimal control point is obtained by using a convex solver.

[0024] In another aspect of the invention, an electronic device is also provided, comprising: At least one processor; and The memory stores instructions that, when executed by the at least one processor, cause the at least one processor to perform the quadcopter UAV trajectory planning method as described above.

[0025] In another aspect of the invention, a machine-readable storage medium is also provided, which stores executable instructions that, when executed, cause the machine to perform the quadcopter unmanned aerial vehicle (UAV) trajectory planning method as described above.

[0026] Compared with the prior art, the present invention has the following advantages: (1) This invention provides an efficient, accurate and universal quadcopter drone trajectory planning method. Through innovative algorithm design and optimization strategy, the drone can quickly plan a safe and smooth flight trajectory in complex environments, improve the drone's ability and efficiency in performing tasks, and promote the widespread application of quadcopter drones in more fields.

[0027] (2) This invention improves the RRT algorithm to overcome its blindness and efficiency problems caused by fixed step size expansion. It combines the characteristics of fast search and optimal results of the JPS algorithm to quickly search for feasible and better distance paths in a wide range, thereby improving the speed and quality of path search.

[0028] (3) Highly efficient and fast path search: Existing single path search algorithms are inefficient in complex environments. For example, the A* algorithm requires the expansion of a large number of nodes in complex environments, resulting in slow search. This invention integrates the improved RRT and JPS algorithms. It utilizes the ability of the RRT algorithm to quickly explore in complex environments to find the initial path and reduce unnecessary node expansion. Then, it uses the JPS algorithm to accurately optimize the path within a small range and quickly search for a collision-free global path, which greatly reduces the path search time and improves the search efficiency. This can meet the strict requirements of UAVs for real-time path planning in complex environments.

[0029] (4) Smooth, stable and safe trajectory: Traditional simple trajectory optimization methods generate poor trajectory smoothness, which affects the flight stability and safety of UAVs. The trajectory optimization method based on Bézier curves and flight corridors in this invention utilizes the special properties of Bézier curves, such as the requirement to pass boundary points and being restricted to convex polygons of control points, to represent local trajectories with Bézier curves. By minimizing Snap, the trajectory is ensured to be multi-order continuous and smooth. At the same time, by utilizing the characteristics of Bézier curves and flight corridor constraints, the trajectory is always kept in a safe area. The generated trajectory satisfies the dynamic constraints of UAVs while having good smoothness, ensuring the flight stability of UAVs, reducing flight risks, reducing energy consumption, and optimizing quickly.

[0030] (5) Comprehensive consideration of multiple constraints: Existing technologies have shortcomings in handling multiple constraints and cannot effectively take into account the constraints of dynamics, environment and mission requirements. This invention comprehensively considers boundary, continuity, safety and dynamic feasibility constraints in the trajectory optimization process, accurately constructs constraint conditions, and ensures that the generated trajectory not only conforms to the dynamic characteristics of the UAV, but also can perform the mission within a safe area, thereby improving the accuracy and reliability of trajectory planning. Attached Figure Description

[0031] Figure 1 This is a schematic diagram of the method flow of the present invention; Figure 2 This is a flowchart illustrating the fusion of RRT and JPS algorithms in Embodiment 1 of the present invention; Figure 3 This is a rendering of the drone's flight trajectory in an indoor scene according to Embodiment 1 of the present invention; Figure 4 This is a rendering of the drone's flight trajectory in an indoor scene, as shown in Embodiment 1 of the present invention. Detailed Implementation

[0032] Example 1 This invention provides a method for trajectory planning of a quadcopter unmanned aerial vehicle (UAV), such as... Figure 1 As shown, the method includes: S1. Environment Modeling: Based on the size of the UAV, the dilation parameters are set, and the set dilation parameters are used to process the grid map containing obstacle distribution information to generate a three-dimensional grid map for path search. Specifically, to perform fast and effective path planning and generate reliable global paths, this invention first uses a 3D grid map to perform high-precision modeling of the surrounding environment. The grid map is chosen because it has high accuracy and a simple structure that meets the real-time requirements of UAV planning. Each grid map stores obstacle information in the surrounding environment. Then, the obstacle grids are processed according to the set dilation parameters to finally obtain a 3D grid map for path search.

[0033] Specifically, the set expansion parameter is greater than the drone's radius.

[0034] S2, Path Search: Obtain a collision-free global path from the 3D raster map established in step S1 by integrating the improved RRT algorithm and the JPS algorithm; To further improve the reliability of path search and reduce the time consumption of path search, this invention adopts a fusion of improved RRT and JPS algorithms to obtain a collision-free global path. Subsequently, the global path is optimized in the backend to generate a smooth trajectory for UAV tracking.

[0035] The pathfinding algorithm is based on a fusion of the RRT and JPS algorithms: The RRT algorithm, or Fast Random Tree Algorithm, is a sampling-based path search algorithm. Its main idea is to quickly find a path from a starting point to an end point in a map environment. This involves continuously building a random tree within the map by sampling the map and expanding the tree based on the sampling results until the tree reaches the end point, indicating a successful path search. Backtracking through this random tree yields a feasible path. Compared to other algorithms, RRT can quickly find a feasible path in high-dimensional spaces and complex environments. However, RRT performs random sampling across the entire map, which introduces a degree of randomness and can lead to inefficient expansion towards the target point. Furthermore, traditional RRT algorithms use a fixed step size for each expansion, resulting in inefficiency. Therefore, improvements to the RRT algorithm are needed to enhance both its speed and quality.

[0036] The JPS algorithm is a high-efficiency graph search algorithm derived from the A* algorithm. It aims to accelerate the search process by finding and breaking down symmetric neighbor nodes, thus avoiding the expansion of a large number of useless nodes. The JPS algorithm's fast search is primarily based on its forward exploration rule, jump rule, and priority queue.

[0037] In complex environments with a wide range of areas, using the JPS algorithm alone for pathfinding can present some challenges. It may result in a large exploration range at certain key nodes, thus impacting the algorithm's search speed. To improve the search speed of the pathfinding algorithm while retaining the optimal search result characteristic of the JPS algorithm, this method can quickly find the optimal path in complex environments, further enhancing pathfinding efficiency.

[0038] This invention will use an improved path search algorithm that combines RRT and JPS, which will retain the advantages of both algorithms, to obtain a feasible path over a wide range and obtain the optimal path distance.

[0039] Considering that the RRT algorithm has significant advantages in path search, but as a probability-based path search method, it lacks optimality and is time-consuming, this invention improves the RRT algorithm by combining it with the JPS algorithm, thereby quickly finding a feasible path with a better distance.

[0040] Specifically, such as Figure 2 The process of the fusion and improvement of the RRT and JPS algorithms is as follows: S21. Initialize a 3D raster map and a growing tree containing the starting point using the RRT algorithm; S22. The sampling strategy based on the RRT algorithm starts from the starting point of the growth tree in the initialized 3D grid map, randomly generates sampling points, and continuously expands the growth tree until the growth tree extends into the neighborhood of the target point, thus obtaining a feasible initialization path. However, this path does not have optimality. S23. Expand the surrounding neighborhood of the initial path obtained in step S22. The expansion range is related to the size of the UAV and is 2 to 4 times the radius of the UAV to obtain the initial feasible area. At this time, the initial feasible area restricts the path selection to a fixed range. S24. Use the JPS algorithm to perform path search in the initial feasible area of ​​the UAV determined in step S23 to find the optimal path and obtain a collision-free global path.

[0041] Specifically, step S22 involves randomly generating sampling points using the sampling strategy set by the RRT algorithm, continuously expanding the growth tree until it extends into the neighborhood of the target point, thus obtaining a feasible initialization path. S221. A sampling strategy based on the RRT algorithm randomly generates sampling points in the map; S222. Based on the sampling points generated in step S221, calculate the expansion step size, and expand the growth tree according to the expansion step size to generate new branches; S223. Check whether the new branch generated in step S222 passes through an obstacle. If it does, delete the new branch. The new branch will not be used as a planned path. However, check whether the new branch passes through the target point area. If it does, connect the other branches except the deleted branch and execute step S23. If it does not pass through an obstacle, continue to check whether the new branch passes through the target point area. If yes, execute step S23. If no, return to step S221 and continue to loop steps S221-S223.

[0042] By combining the RRT and JPS algorithms, a safe path can be quickly found in the drone flight environment. However, this path is not suitable for the drone to follow directly. Therefore, a trajectory optimization algorithm is needed to smooth the global path.

[0043] S3. Generate flight corridor: Use the dilation method to process the global path generated in step S2 to obtain a flight corridor suitable for UAV flight; Specifically, the expansion process involves extending the global path outward until an obstacle is encountered, thereby obtaining a flight corridor composed of multiple continuous regions.

[0044] S4. Trajectory Optimization: The unoptimized local path is obtained by cutting the global path generated in step S2, and the local path is represented by a trajectory based on Bézier curves to obtain a trajectory model. Then, the trajectory is optimized based on the optimization objective and the constraints of the flight corridor generated in step S3 to obtain the optimal local path.

[0045] Specifically, a hybrid Bézier curve and flight corridor approach is used as the trajectory optimization algorithm to generate smooth and safe flight paths in real time in complex environments: First, the local path length is set, and the global path generated in step S2 is cut based on this length. Unoptimized local paths are obtained from the global paths, and the local paths are represented by Bézier curves. An objective function that minimizes the snap is established. Then, the trajectory optimization problem is transformed into a convex quadratic programming problem based on constraints constructed by the flight corridor. Finally, an optimal local path is obtained through optimization.

[0046] The trajectory representation based on Bézier curves, for quadcopter UAVs, due to their differential flatness, represents the trajectory model in a flat output space from the high-dimensional state space. The flat output quantity selected in this invention is: (1) in, The position coordinates of the drone in three-dimensional space. Let be the heading angle of the UAV around the vertical z-axis. Specifically, the trajectory is generated using a vector composed of the three-dimensional position and the two-dimensional horizontal rotation space. Since the minimized snap method aims to minimize the square integral of the fourth derivative of the quadrotor UAV trajectory, to achieve this goal while ensuring the trajectory has continuous position, velocity, acceleration, and thrust, a polynomial of at least order seven is needed to represent the trajectory. To achieve a reasonable balance between ensuring trajectory smoothness and computational complexity, this invention uses a seventh-order polynomial to represent the trajectory based on minimized snap. Each segment of the polynomial function can be expressed as: (2) in Here are the polynomial coefficients, and t is time. This invention represents the local trajectory using Bézier curves, which are represented by Bernstein polynomials. The basis functions of the Bernstein polynomials are: (3) Where n represents the highest order of curvature, i is the index of the control point of the Bézier curve, and t is time. Let Bernstein's basis function represent the i-th control point. The polynomial composed of Bernstein's basis functions is called a Bézier curve. Therefore, a seventh-order Bézier curve can be written as: (4) in This is the set of control points for this segment of the Bézier curve. Let be the Bernstein basis function for the i-th control point, and t be time.

[0047] Expanding the Bézier curve can reduce it to an ordinary polynomial, and there exists a mapping matrix M such that P = M·C, so it can be obtained through... The ordinary polynomial is converted into a Bézier curve. For a Bézier curve, control points can be viewed as weights of basis functions, and basis functions can also be viewed as weights of control points. Since the time parameter t in the Bézier curve is on a fixed interval [0,1], for each segment of the trajectory, we need a scaling factor s to map time t to this interval. By using Bézier curves to represent the UAV trajectory, we can ensure that each sub-unit in the flight corridor region is a convex polygon. Each convex polygon corresponds to a Bézier curve, and all control points of each curve are constrained within the corridor of that curve. For the overall trajectory, each trajectory segment needs to be smoothly connected, so the entire trajectory is restricted to the flight corridor. This avoids situations where local trajectories are outside the flight corridor when using the flight corridor to constrain the UAV state, thus eliminating the need for any posterior detection, saving a significant amount of trajectory optimization time and accelerating the trajectory optimization speed.

[0048] Construction of the minimum objective function: To achieve multi-order continuity and smoothness of the trajectory and reduce the energy consumption of the UAV during trajectory tracking, a cost function is typically used to define the integral of the square of the Kth derivative of the UAV trajectory function. A typical approach is to set K to 3, i.e., the Jerk method, which minimizes the jump. However, in this invention, we consider setting K to 4, i.e., the Snap method, which minimizes the fourth reciprocal of the trajectory.

[0049] The Snap minimization method focuses on minimizing the changes in higher-order derivatives of the system trajectory, thereby ensuring better smoothness of the generated track at higher orders. This guarantees that position, velocity, acceleration, and sag on the trajectory are continuous. Furthermore, the Snap minimization method also helps generate trajectories with lower energy consumption. This invention uses a method that sets the objective function to minimize the Snap minimization to generate the trajectory.

[0050] Based on the above analysis, for a certain dimension of variable The minimization objective function can be written as: (5) in, yes The objective function to be minimized is given by T, where T represents the total time and t represents the time variable. Indicates in dimension Above, the position function of the drone at time t, where m represents the number of segments into which the total time T is divided. yes The components within the k-th time interval, and These represent the start and end times of the k-th time interval, respectively.

[0051] The cost function for the Kth segment of the trajectory can be expressed as: (6) in, The time scaling factor for the Kth segment of the trajectory. Here are the polynomial coefficients, and t is the time variable. For in dimension The cost function of the k-th segment of the trajectory. This represents the starting time of the k-th time interval.

[0052] Corridor-based constraint construction: In segmented trajectory generation, the construction of constraints is crucial. To ensure good trajectory smoothness, safety, and dynamic feasibility, various constraints need to be considered during the trajectory optimization stage. Different constraints need to be adjusted for different application scenarios.

[0053] This invention mainly considers the following four types of constraints: boundary constraints, continuity constraints, security constraints, and dynamic feasibility constraints, as detailed below: First, using the derivative properties of Bézier curves, we define the control points of each order for the UAV. The dimension is Let the i-th control point of the K-th trajectory be... The dimension is The i-th control point of the K-th trajectory l The first derivative, then It can be represented as: = (7) The dimension is The i-th control point of the K-th trajectory l The order derivative, where n is the order of the Bézier curve. The dimension is The Kth segment of the trajectory i +1 control point l -1st derivative, The dimension is The Kth segment of the trajectory i control points l -1st derivative.

[0054] Boundary constraints: These restrict the position, velocity, acceleration, and sag of the starting and ending points of each trajectory segment. The derivative constraints for the starting and ending points can be expressed as follows: (8) in, Represents the constraints of each order at the starting point. Constraints of various orders representing the endpoint, Representative dimension is The derivatives of each order of the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the trajectory of the k-th segment. Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory.

[0055] Continuity Constraints: The smoothness of the trajectory is crucial for the safety and stability of the aircraft. To achieve a smooth trajectory, continuity constraints need to be added between segments to ensure that parameters such as position, velocity, acceleration, and jerk remain consistent between adjacent segments. For the Kth and K+1th segments, we have: (9) in, Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory. Represents a time-related scaling factor. Representative dimension is The derivatives of each order of the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the (k+1)th segment of the trajectory. The dimension is The i-th control point of the K-th segment of the trajectory.

[0056] Safety constraints: When generating the trajectory, it is necessary to ensure that the drone flies within a safe corridor, avoiding collisions and entering no-fly zones. Therefore, the Bézier curve needs to be restricted to the feasible region. The position inequality constraint introduced by the optimization problem is as follows: (10) in and For dimension The upper and lower boundaries of the flight corridor corresponding to the Kth trajectory segment, where n is the highest order of the polynomial. It is a dimension of The i-th control point of the K-th trajectory segment. The time-dependent scaling factor represents the trajectory of the k-th segment.

[0057] Dynamic feasibility constraints: The generated trajectory needs to take into account the dynamic limitations of the UAV to ensure the executability of the trajectory in actual flight. Inequality constraints are introduced to ensure the dynamic feasibility of the trajectory.

[0058] (11) (12) (13) in , These are the minimum and maximum speed limits for the drone. , These are the minimum and maximum acceleration limits, respectively. , These are the minimum and maximum limits for the jump, respectively. It is the order of the Bézier curve. The dimension is The i-th control point of the K-th trajectory segment. The dimension is The (i-1)th control point of the Kth segment of the trajectory. The dimension is The (i-2)th control point of the Kth segment of the trajectory. The dimension is The (i-3)th control point of the Kth segment of the trajectory. This represents the time scaling factor of the Kth segment of the trajectory.

[0059] Boundary constraints and continuity constraints can be written in the form of equality constraints: (14) in, This represents the coefficient matrix describing the problem, and c represents the control points. Represents the target value or external input.

[0060] Safety constraints and multi-order dynamic feasibility constraints can be written in the form of inequality constraints: (15) in, The coefficient matrix, where c represents the control points. This represents the upper bound vector, which is the maximum allowed value.

[0061] The final trajectory optimization problem for the local path of the UAV can be expressed as: Min: c(16) St: (17) Where c is the polynomial coefficient to be solved, a one-dimensional column vector composed of the polynomial coefficients of k trajectory segments, and also the control point of the UAV trajectory. Q is a positive definite symmetric matrix. According to the design of the flight corridor and the convex hull property of the Bézier curve, the constraint region is also a convex region. Therefore, this is a convex quadratic programming problem. The optimal control point is obtained by using a convex solver.

[0062] Figure 3 and Figure 4 To illustrate different flight trajectories generated during flight in an indoor scene using the method of this invention, the drone's radius is 0.22m, and the expansion parameter is set to 0.25m. After searching for an initial path based on the RRT algorithm, the initial path is expanded to its surrounding neighborhood within a range of 0.44m. Then, the JPS algorithm is used to find the optimal path within this expanded range, obtaining a collision-free global path. Based on the global path, a flight corridor is generated to optimize the local path, and finally, the optimized local path of the drone is output. The indoor scene point cloud map is shown below, including objects such as tripods, cylinders, and square tubes, mainly to construct a complex flight environment and verify the real-time trajectory planning effect of the drone.

[0063] Figure 3 and Figure 4 The red trajectory in the middle represents the optimized UAV trajectory, while the other colored dots represent a 3D map built based on the flight environment. The method of this invention was tested, and the generated trajectory was smooth and collision-free, further verifying the path planning capability of the method of this invention in complex scenarios.

[0064] Example 2 This embodiment also provides an electronic device, including: At least one processor; and The memory stores instructions that, when executed by the at least one processor, cause the at least one processor to perform the quadcopter UAV trajectory planning method as described above.

[0065] In this embodiment, the electronic device may include, but is not limited to: personal computer, server computer, workstation, desktop computer, laptop computer, notebook computer, mobile computing device, smartphone, tablet computer, cellular phone, personal digital assistant (PDA), handheld device, messaging device, wearable computing device, consumer electronic device, etc.

[0066] Example 3 This embodiment also provides a machine-readable storage medium storing executable instructions that, when executed, cause the machine to perform the quadcopter UAV trajectory planning method described above.

[0067] Specifically, a system or apparatus equipped with a readable storage medium may be provided, on which software program code implementing the functions of any of the embodiments described above is stored, and the computer or processor of the system or apparatus can read and execute the instructions stored in the readable storage medium.

[0068] In this case, the program code read from the readable medium itself can perform the functions of any of the above embodiments, and therefore the machine-readable code and the readable storage medium storing the machine-readable code constitute a part of this specification.

[0069] Examples of readable storage media include floppy disks, hard disks, magneto-optical disks, optical disks (such as CD-ROM, CD-R, CD-RW, DVD-ROM, DVD-RAM, DVD-RW, DVD-RW), magnetic tapes, non-volatile memory cards, and ROMs. Alternatively, program code can be downloaded from a server computer or the cloud via a communication network.

[0070] Obviously, the above embodiments of the present invention are merely examples for clearly illustrating the technical solutions of the present invention, and are not intended to limit the specific implementation of the present invention. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the claims of the present invention should be included within the protection scope of the claims of the present invention.

Claims

1. A method for trajectory planning of a quadcopter unmanned aerial vehicle (UAV), characterized in that, The method includes: S1. Environment Modeling: Based on the size of the UAV, the dilation parameters are set, and the set dilation parameters are used to process the grid map containing obstacle distribution information to generate a three-dimensional grid map for path search. S2, Path Search: Obtain a collision-free global path from the 3D raster map used for path search established in step S1 by integrating the improved RRT algorithm and JPS algorithm. S3. Generate flight corridor: Use the dilation processing method to process the global path generated in step S2 to obtain a flight corridor suitable for UAV flight; S4. Trajectory Optimization: The unoptimized local path is obtained by cutting the global path generated in step S2, and the local path is represented by a trajectory based on Bézier curves to obtain a trajectory model. Then, the trajectory is optimized based on the optimization objective and the constraints of the flight corridor generated in step S3 to obtain the optimal local path.

2. The quadcopter UAV trajectory planning method according to claim 1, characterized in that, The expansion parameter set in step S1 is greater than the radius of the UAV.

3. The quadcopter UAV trajectory planning method according to claim 1, characterized in that, The process of fusing the RRT and JPS algorithms in step S2 is as follows: S21. Initialize a 3D raster map and a growing tree containing the starting point using the RRT algorithm; S22. The sampling strategy based on the RRT algorithm starts from the starting point of the growth tree in the initialized 3D grid map, randomly generates sampling points, and continuously expands the growth tree until the growth tree extends into the neighborhood of the target point, thus obtaining a feasible initialization path. S23. Expand the surrounding neighborhood of the initialization path obtained in step S22 by 2 to 4 times the radius of the UAV, and finally obtain the feasible area for initializing the UAV. S24. Use the JPS algorithm to perform path search in the initial feasible area of ​​the UAV determined in step S23 to find the optimal path and obtain a collision-free global path.

4. The quadcopter UAV trajectory planning method according to claim 3, characterized in that, Step S22 describes the random sampling of sampling points using the sampling strategy set by the RRT algorithm, continuously expanding the growth tree until it extends into the neighborhood of the target point, thus obtaining a feasible initialization path. Specifically: S221. A sampling strategy based on the RRT algorithm randomly generates sampling points in the map; S222. Based on the sampling points generated in step S221, calculate the expansion step size, and expand the growth tree according to the expansion step size to generate new branches; S223. Check whether the new branch generated in step S222 passes through an obstacle. If it does, delete the new branch. The new branch will not be used as a planned path. However, check whether the new branch passes through the target point area. If it does, connect the other branches except the deleted branch and execute step S23. If it does not pass through an obstacle, continue to check whether the new branch passes through the target point area. If yes, execute step S23. If no, return to step S221 and continue to loop steps S221-S223.

5. The quadcopter UAV trajectory planning method according to claim 1, characterized in that, The expansion treatment method described in step S3 is as follows: The global path is extended outward until an obstacle is encountered, thus creating a flight corridor consisting of multiple consecutive areas.

6. The quadcopter UAV trajectory planning method according to claim 1, characterized in that, Step S4 is as follows: A hybrid Bézier curve and flight corridor approach is used as the trajectory optimization algorithm to generate smooth and safe flight paths in complex environments in real time. First, the local path length is set, and the global path generated in step S2 is cut based on this length. Unoptimized local paths are obtained from the global paths, and the local paths are represented by Bézier curves. An objective function that minimizes the snap is established. Then, the trajectory optimization problem is transformed into a convex quadratic programming problem based on constraints constructed by the flight corridor. Finally, an optimal local path is obtained through optimization.

7. The quadcopter UAV trajectory planning method according to claim 6, characterized in that, The constraints based on the construction of flight corridors include boundary constraints, continuity constraints, safety constraints, and dynamic feasibility constraints.

8. The quadcopter UAV trajectory planning method according to claim 7, characterized in that, The boundary constraints are specifically as follows: The starting and ending points of each trajectory segment are constrained in terms of position, velocity, acceleration, and jump. The derivative constraints for the starting and ending points can be expressed as follows: （1） in, Represents the constraints of each order at the starting point. Represents the constraints of each order at the endpoint. Representative dimension is The derivatives of each order at the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the trajectory of the k-th segment. Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory; Continuity constraint: For the Kth segment and the (K+1)th segment, we have: （2） in, Representative dimension is The derivatives of each order at the endpoint of the Kth segment of the trajectory. Represents a time-related scaling factor. Representative dimension is The derivatives of each order at the starting point of the Kth segment of the trajectory. The time-dependent scaling factor representing the (k+1)th segment of the trajectory. The dimension is The i-th control point of the K-th trajectory segment; Safety constraints: When generating the trajectory, it is necessary to ensure that the drone flies within a safe corridor, avoiding collisions and entering no-fly zones. Therefore, the Bézier curve needs to be restricted to the feasible region. The position inequality constraint introduced by the optimization problem is as follows: （3） in and For dimension The upper and lower boundaries of the flight corridor corresponding to the Kth trajectory segment, where n is the highest order of the polynomial. It is a dimension of The i-th control point of the K-th trajectory segment. The time-dependent scaling factor representing the trajectory of the k-th segment; Dynamic feasibility constraints: The generated trajectory needs to consider the dynamic limitations of the UAV to ensure the executability of the trajectory in actual flight. Inequality constraints are introduced to ensure the dynamic feasibility of the trajectory. （4） （5） （6） in , These are the minimum and maximum speed limits for the drone. , These are the minimum and maximum acceleration limits, respectively. , These are the minimum and maximum limits for the jump, respectively. It is the order of the Bézier curve. The dimension is The i-th control point of the K-th trajectory segment. The dimension is The (i-1)th control point of the Kth segment of the trajectory. The dimension is The (i-2)th control point of the Kth segment of the trajectory. The dimension is The (i-3)th control point of the Kth segment of the trajectory. This represents the time scaling factor of the Kth segment of the trajectory.

9. An electronic device, characterized in that, The electronic device includes: At least one processor; and A memory that stores instructions that, when executed by the at least one processor, cause the at least one processor to perform the quadcopter unmanned aerial vehicle trajectory planning method as described in any one of claims 1 to 8.

10. A machine-readable storage medium, characterized in that, The machine-readable storage medium stores executable instructions that, when executed, cause the machine to perform the quadcopter unmanned aerial vehicle (UAV) trajectory planning method as described in any one of claims 1 to 8.

Images