Autonomous flight path planning method and system for quad-rotor unmanned aerial vehicle

Abstract

The invention discloses a quadrotor unmanned aerial vehicle autonomous flight path planning method and system, and the method comprises the steps: introducing a global trajectory updating mechanism to update a global trajectory in real time, and carrying out the re-planning of a generated guide trajectory, and obtaining a global trajectory; based on the generated global trajectory, generating a collision trajectory detection and obstacle avoidance path, and when the global trajectory passing through the obstacle is detected, searching a collision-obstacle-free path of which the starting point is a trajectory point entering the obstacle and the end point is a trajectory point passing through the corresponding obstacle by using A *; performing trajectory optimization on the generated collision-obstacle-free path to generate a safe and smooth trajectory, solving an optimization function and performing trajectory optimization based on a MINCO trajectory to generate an optimized trajectory; and taking the optimized trajectory meeting the dynamics feasibility as an expected trajectory for the unmanned aerial vehicle to execute the autonomous flight task. The problems of local path oscillation, high energy consumption and poor dynamic environment adaptability of the unmanned aerial vehicle are solved.

Description

Technical Field

[0001] The present invention belongs to the technical field of trajectory planning for autonomous navigation of a quad-rotor unmanned aerial vehicle (UAV), and in particular relates to a method and system for autonomous flight trajectory planning of a quad-rotor UAV. Background Art

[0002] In recent years, the application scenarios of multi-rotor drones have continued to expand, making significant contributions to agriculture, industry, commerce, the military, and other fields. However, current mainstream drone systems are still in the remote control stage. Their maneuverability and power resource utilization are limited by operator experience, making them difficult to meet the requirements of autonomous operations in complex scenarios. Drone motion planning is a core technology for autonomous flight, directly impacting the safety, efficiency, and mission execution capabilities of drones. Motion planning helps drones find the optimal path from their starting point to their destination, avoid obstacles, reduce flight time and energy consumption, and improve efficiency. The complexity and diversity of missions place higher demands on the motion planning module. The motion planning module determines whether a drone can accurately and safely complete its flight, as well as the speed and energy consumption achieved during autonomous flight. Most of the current cutting-edge motion planning algorithms focus on speed, accuracy, and smoothness, but fail to consider the actual conditions of autonomous flight. In actual flight, the algorithm needs to run in real time with limited computing resources and adapt to various environments. In addition, the energy issue of drone flight is also a very important hot issue. Currently, the battery life of most remote-controlled drones on the market can only reach less than 30 minutes, so the battery life of autonomous drones will be further reduced.

[0003] Autonomous navigation frameworks are currently being deployed on drones. To ensure the drone can operate within limited resources, these cutting-edge research frameworks do not store map information in memory in advance. Instead, they typically employ global and local planning methods to maintain rolling map storage, ensuring real-time autonomous flight. The most representative example is the lightweight and fast EGO-Planner navigation framework, but this framework also has several issues that require improvement. First, its limited field of view and its rolling storage of a local map detected by the sensors can lead to local optimality. If faced with large obstacles beyond its field of view, the drone will wander around in a local area, unable to reach its destination. Second, while this framework can accomplish autonomous flight missions, its performance still needs improvement. The EGO-Planner navigation framework uses B-spline optimization to optimize trajectories, rather than directly optimizing control energy, which can result in high energy consumption. B-spline optimization can underperform in highly dynamic tasks because the dynamic characteristics of the generated trajectory require additional optimization. Summary of the Invention

[0004] The technical problem to be solved by the present invention is to address the shortcomings of the above-mentioned existing technologies and provide a method and system for autonomous flight trajectory planning for quadrotor drones. Global planning introduces the concept of limited field of view and adds a global path point to ensure the safety of the globally planned guided trajectory. Local planning adopts MINCO optimization, taking smoothness, total time, dynamic feasibility, and safety as the main objectives of the optimization function. This provides a faster, smoother, and more feasible flight trajectory for quadrotor drones, and is used to solve the technical problems of local path oscillation, high energy consumption, and poor adaptability to dynamic environments.

[0005] The present invention adopts the following technical solutions:

[0006] A method for autonomous flight trajectory planning of a quadrotor unmanned aerial vehicle comprises the following steps:

[0007] Obtain raster map information and positioning information, initialize map parameters and algorithm parameters, and generate a guidance trajectory based on the target point specified by the user;

[0008] A global trajectory update mechanism is introduced to update the global trajectory in real time, and the generated guidance trajectory is replanned to obtain the global trajectory;

[0009] Based on the generated global trajectory, collision trajectory detection and obstacle avoidance paths are generated. When a global trajectory that passes through an obstacle is detected, A* is used to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle.

[0010] Perform trajectory optimization on the generated collision-free obstacle path to generate a safe and smooth trajectory, solve the optimization function and perform trajectory optimization based on the MINCO trajectory to generate an executable trajectory;

[0011] The optimized trajectory that satisfies dynamic feasibility is taken as the expected trajectory for the UAV to perform autonomous flight missions.

[0012] Preferably, the Minimum Snap optimization method is used to generate the global trajectory, as follows:

[0013] A minimum snap optimization method is used to calculate a time-dependent trajectory function from the starting point to the end point. A timer is then set to calculate the field of view rate at a frequency of 50 Hz and determine whether the field of view rate is less than a set threshold. If the field of view rate is less than or equal to the threshold, the global trajectory update mechanism is activated, and the final target point is urgently moved to a position on the same side of the field of view and in the same direction as the obstacle.

[0014] If the field of view is greater than the threshold, conventional re-planning is initiated, and a global path point is added within the field of view. The drone is guided in a certain direction when performing autonomous flight missions, and is guided to fly to an area with unobstructed field of view.

[0015] Preferably, the effective field of view γ is calculated as follows:

[0016]

[0017] Among them, n obs Represents the number of discretized slices of the fan-shaped area blocked by obstacles, n free Represents the number of discretized slices of the fan-shaped area that is not blocked by obstacles.

[0018] Preferably, generating a safe and smooth trajectory is as follows:

[0019] The MINCO trajectory optimization method based on spatiotemporal joint planning is adopted. First, the parameters of the MINCO trajectory are defined. The spatial shape of the trajectory is determined by the adjacent path points q of any two connected segments. The flight time of the UAV on each trajectory is controlled by the duration T of each segment, thereby realizing planning in the time dimension.

[0020] Preferably, when the global trajectory of the planned trajectory passes through an obstacle, first find the continuous control point Q entering the obstacle i And record it, then search for a collision-free path L around the obstacle through path search;

[0021] By Q i And the collision-free path L finds the {p, v} pair, records it and numbers it, where p is the point on the obstacle surface and v is the unit vector pointing from the control point to p. The new control point Q is found through the {p, v} pair. j ;

[0022] Then detect whether the control point is within the obstacle, push the control point away from the obstacle, and calculate the distance value to determine whether the control point has been pushed out of the obstacle to achieve obstacle avoidance.

[0023] Preferably, from the control point Q of the trajectory i The intersection point p of the tangent normal plane and the obstacle at the control point ij The distance field d ij The calculation is as follows:

[0024] d ij =(Q i -p ij )·v ij

[0025] Among them, v ij From the control point Q i Point to pij The unit vector of .

[0026] Preferably, for a quadrotor drone, the dynamic feasibility is ensured by limiting the size of the trajectory derivative, and the dynamic penalty term J of the trajectory speed, acceleration, and jerk is limited. d,v 、J d,a 、J d,j The total dynamic penalty term is the dynamic penalty term J of velocity, acceleration, and jerk d,v 、J d,a 、J d,j The smoothness of the trajectory is ensured by limiting the size of the trajectory integral, and the corresponding penalty term is J s ; Control the drone's flight time by limiting the total time so that the drone can quickly reach the target destination; for collision penalties, push the control point away from the obstacle and use a safe distance s f and punish ij <s f The control point is realized.

[0027] Preferably, when the generated optimized trajectory does not meet the dynamic feasibility, the MINCO trajectory is time-redistributed, the trajectory generation is refined, the trajectory optimization is performed again, and then the optimized trajectory is published at a frequency of 100 Hz.

[0028] Preferably, a 20Hz timer is added to sample the executable trajectory points and check whether a collision occurs at each sampling point. If an emergency collision occurs during the flight of the drone, an emergency braking strategy is adopted; if all checkpoints are safe, the executable trajectory points are released.

[0029] In a second aspect, an embodiment of the present invention provides a quadrotor drone autonomous flight trajectory planning system, comprising:

[0030] The initialization module obtains raster map information and positioning information, initializes map parameters and algorithm parameters, and generates a guidance trajectory based on the target point specified by the user;

[0031] The update module introduces a global trajectory update mechanism to update the global trajectory in real time, replans the generated guidance trajectory, and obtains the global trajectory;

[0032] The path module generates collision trajectory detection and obstacle avoidance paths based on the generated global trajectory. When a global trajectory that passes through an obstacle is detected, it uses A* to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle.

[0033] The trajectory module performs trajectory optimization on the generated collision-free obstacle path to generate a safe and smooth trajectory, solves the optimization function and performs trajectory optimization based on the MINCO trajectory to generate the optimized trajectory;

[0034] The release module takes the optimized trajectory that satisfies dynamic feasibility as the expected trajectory for the UAV to perform autonomous flight missions.

[0035] In a third aspect, a computer device comprises a memory, a processor, and a computer program stored in the memory and executable on the processor, wherein the processor implements the steps of the above-mentioned method for autonomous flight trajectory planning of a quadrotor drone when executing the computer program.

[0036] In a fourth aspect, an embodiment of the present invention provides a computer-readable storage medium, comprising a computer program, which, when executed by a processor, implements the steps of the above-mentioned method for autonomous flight trajectory planning of a quadrotor drone.

[0037] In a fifth aspect, a chip comprises a memory, a processor, and a computer program stored in the memory and executable on the processor. When the processor executes the computer program, the steps of the above-mentioned method for autonomous flight trajectory planning of a quadrotor drone are implemented.

[0038] In a sixth aspect, an embodiment of the present invention provides an electronic device, comprising a computer program, which, when executed by the electronic device, implements the steps of the above-mentioned method for autonomous flight trajectory planning of a quadrotor drone.

[0039] Compared with the prior art, the present invention has at least the following beneficial effects:

[0040] A method for autonomous flight trajectory planning for a quadrotor unmanned aerial vehicle (UAV) uses a closed-loop "global-local" dual planning mechanism (guiding trajectory generation, replanning, obstacle avoidance path search, and trajectory optimization). This method balances real-time performance with global optimality, avoiding the local optimality problem caused by the local field of view in traditional rolling planners. A global trajectory update mechanism is introduced to adjust the trajectory in real time to adapt to environmental changes, improving robustness in complex scenarios. By directly linking control energy, time, and dynamic feasibility (rather than simply trajectory smoothness) through trajectory optimization, the UAV's trajectory performance is improved, power resource waste is reduced, and flight time is extended.

[0041] Furthermore, it generates trajectories with continuous low-order derivatives (e.g., minimizing the Snap value) to reduce high-frequency jitter in the drone's motors, lowering energy consumption and improving flight stability. A 50Hz timer calculates the field of view rate in real time, and based on threshold judgment, switches between "emergency replanning" and "regular replanning" modes, effectively addressing large-scale obstacle occlusion and avoiding local optimal loops. When the field of view rate is too low, the target point is forcibly adjusted to the same side of the field of view, ensuring that the drone always has a viable trajectory direction and enhancing its survivability in extreme environments.

[0042] Furthermore, by discretizing the sector-shaped regions to calculate the ratio of occlusion to free area, we can accurately measure environmental complexity and avoid misjudgments caused by subjective threshold settings. The discrete slice statistics method is computationally efficient and suitable for real-time operation on embedded platforms, reducing resource usage.

[0043] Furthermore, MINCO trajectories achieve spatiotemporal decoupling of trajectory optimization by separating spatial shape parameters (pathway points q) and temporal parameters (segment duration T), improving planning flexibility. By adjusting the temporal parameters, velocity and acceleration curves can be indirectly controlled, reducing computational complexity.

[0044] Furthermore, by recording the obstacle penetration point pairs {p, v}, a collision-free path L is generated. The distance field is then used to dynamically adjust the control points to ensure the trajectory strictly avoids obstacles. Only the control points related to the obstacle are adjusted locally (rather than global replanning), reducing computational overhead and meeting real-time requirements.

[0045] Furthermore, the distance formula quantifies the safe distance between the control point and the obstacle, providing strict mathematical constraints for trajectory optimization and avoiding collision risks. The control point positions are iteratively adjusted through the gradient descent method to accelerate the obstacle avoidance trajectory generation process.

[0046] Furthermore, the derivatives of velocity, acceleration, and jerk are limited to ensure the trajectory complies with the physical limits of the drone, preventing motor overload or instability. Control points are forced to move away from each other at a safe distance to avoid potential collisions caused by sensor noise or planning errors.

[0047] Furthermore, time redistribution is used to refine trajectory segment durations, alleviating dynamic infeasibility issues caused by initial time estimation errors. Trajectories are updated at a 100Hz frequency to meet the drone's rapid response requirements and adapt to highly dynamic environments.

[0048] Furthermore, 20Hz periodic sampling checks the safety of trajectory points, combined with emergency braking strategies, form a "prediction-execution" dual insurance policy, significantly reducing the probability of collision. Even if the optimized trajectory is theoretically feasible, sampling points are used to verify the safety of the actual flight path, improving system reliability.

[0049] It can be understood that the beneficial effects of the second to sixth aspects mentioned above can be found in the relevant description of the first aspect mentioned above, and will not be repeated here.

[0050] In summary, the present invention avoids the local optimality problem caused by limited local field of view through a multi-level planning architecture, lightweight spatiotemporal optimization, dynamic field of view management, and safety constraints, and ensures flight safety in complex scenarios with high-frequency trajectory updates and collision detection.

[0051] The technical solution of the present invention is further described in detail below through the accompanying drawings and embodiments. BRIEF DESCRIPTION OF THE DRAWINGS

[0052] In order to more clearly illustrate the technical solutions of the embodiments of the present application, the following is a brief introduction to the drawings required for use in the embodiments of the present application. Obviously, the drawings described below are only some embodiments of the present application. For ordinary technicians in this field, other drawings can be obtained based on these drawings without any creative work.

[0053] Figure 1 It is a schematic diagram of the process of the present invention;

[0054] Figure 2 This is the flow chart of the global trajectory planning algorithm;

[0055] Figure 3 Define a schematic diagram for field of view efficiency;

[0056] Figure 4 This is a schematic diagram of triggering emergency global replanning;

[0057] Figure 5 This is a schematic diagram of a conventional global replanning trigger;

[0058] Figure 6 Schematic diagram of the principle of pushing the control point away from obstacles;

[0059] Figure 7 Schematic diagram of the trajectory gradually pushing away from the obstacle;

[0060] Figure 8 To improve the trajectory generation results for large obstacles;

[0061] Figure 9 Results for improved trajectory generation for large obstacles;

[0062] Figure 10 This is the ROS node relationship diagram for the dynamic obstacle simulator;

[0063] Figure 11 Simulation results generated for dynamic obstacle trajectories;

[0064] Figure 12 The obstacle avoidance simulation effect diagram under complex and dense obstacles, among which (a) is the improved front top view, (b) is the improved front side view, (c) is the improved rear top view, and (d) is the improved rear side view;

[0065] Figure 13 A schematic diagram of a computer device provided in accordance with an embodiment of the present invention;

[0066] Figure 14 The present invention is a block diagram of an electronic device according to an embodiment of the present invention.

[0067] Among them, 60. Computer device; 61. Processor; 62. Memory; 63. Computer program; 600. Electronic device; 610. Processing unit; 620. Storage unit; 6201. Random access memory unit; 6202. Cache memory unit; 6203. Read-only memory unit; 6204. Program / Utility; 6205. Program module; 630. Bus; 640. Display unit; 650. Input / output interface; 660. Network adapter; 700. External device. DETAILED DESCRIPTION

[0068] This invention provides a method for autonomous flight trajectory planning for quadrotor drones. This method adds a replanning module to the global planning module, enabling the drone to adapt to a wider range of environments. It also adds MINCO (Minimum Control) trajectories to the local planning module, improving overall planning performance and enabling better completion of autonomous flight missions. This method, combined with the positioning and mapping modules and the control system module for joint debugging, improves the drone's mission decision-making time, reduces system power consumption, and enables the drone to complete collision-free missions from its starting point to its destination.

[0069] Example 1

[0070] See also Figure 1 The present invention provides a method for autonomous flight trajectory planning of a quadrotor unmanned aerial vehicle, comprising the following steps:

[0071] S1. The motion planning module obtains the grid map information and positioning information from the perception positioning and mapping module, initializes the map parameters and algorithm parameters, and determines the target point specified by the user;

[0072] The motion planning module generates the guidance trajectory according to the target point information specified by the user. The present invention uses the Minimum Snap optimization algorithm to generate the global trajectory. At this time, it does not consider whether the trajectory avoids obstacles, but only considers whether the trajectory is executable.

[0073] S2. After the global trajectory is generated, collision trajectory detection and obstacle avoidance path generation are performed. When a global trajectory that passes through an obstacle is detected, A* is used to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the ending point being the trajectory point passing through the obstacle. These path points become the control points for the subsequent MINCO trajectory optimization.

[0074] S3. To prevent the drone from falling into a local optimal solution, the present invention introduces a global trajectory update mechanism, uses a 50Hz timer to update the global trajectory in real time, and uses a three-point method instead of a two-point method to replan the global trajectory;

[0075] See also Figure 2First, the global trajectory is initialized, using the Minimum Snap algorithm to calculate a time-dependent trajectory function from the starting point to the end point. A timer is then set to calculate the field of view at a 50Hz frequency and determine whether the field of view is less than a specific threshold. If the field of view is less than or equal to the threshold, the global trajectory update mechanism is activated, urgently moving the final target point to the same side of the field of view and in the same direction as the obstacle. If the field of view is greater than the threshold, conventional replanning is initiated, adding a global path point within the field of view. This provides the drone with a defined direction when performing autonomous flight missions, guiding it to an area with unobstructed field of view. This avoids frequent U-turns during planning, which wastes energy, while also ensuring the drone's safety during flight.

[0076] This invention introduces the concept of effective field of view to control global replanning under different conditions, preventing the drone from falling into local minimums and also preventing the drone from oscillating around obstacles and causing a crash. Figure 3 As shown, let the visible field radius of the drone be R uav , the visual range of the drone is discretely sliced, and obstacle query is performed for each discrete space, n obs Represents the number of discretized slices of the fan-shaped area blocked by obstacles, n free The number of discretized slices representing the sector area that is not blocked by obstacles can then be used to calculate the visibility of the drone at its current location, which is represented by the effective field of view rate γ. The larger the field of view, the safer the drone is during autonomous flight.

[0077] Then, the calculation formula of the effective field of view rate γ is:

[0078]

[0079] When the effective field of view rate γ of the UAV is less than or equal to a certain threshold, the system will enter the emergency global replanning module.

[0080] Emergency global replanning module such as Figure 4 As shown in the figure, the blue area is the field of view of the drone. In state ①, the green point is selected as the target point, and the drone approaches the target point. At this time, the field of view rate γ is 100%, and the global trajectory is a straight line from the current position of the drone to the target point.

[0081] When reaching state ②, the field rate γ decreases, but the field of view rate has not yet reached the threshold set by the present invention. Therefore, the conventional global replanning module is entered, and a point within the field of view is selected as the third control point for global planning, and the global trajectory becomes a curve. Assuming that the global trajectory generated by the conventional global replanning module does not pull the drone away from the obstacle, the drone reaches state ③, where the field of view rate γ falls below the threshold set by the present invention. At this time, emergency global replanning is triggered, and the target point is pulled to the same side of the field of view and in the same direction as the obstacle, reaching state ④.

[0082] When the UAV’s field of view rate γ is greater than the set threshold, in order to minimize the situation where the field of view rate is low, a global replanning under normal circumstances is also added to the system, such as Figure 5 As shown, the green dot is the target point selected by the user, the gray solid line is the initial global planning trajectory, and a point in the fan-shaped slice area that is not blocked by obstacles and close to the nose is selected as the third global path point, which is the red dot in the figure. This helps the drone obtain better global trajectory guidance and avoid the drone being trapped in a local optimal solution. After the drone has flown for a period of time, a regular global replanning is triggered again, and a red point is added as a global path point, generating a purple curve as a new global trajectory. The design scheme of adopting global trajectory replanning increases the guidance accuracy and reliability of the global path, enabling the drone to cope with a wider variety of unknown environments during autonomous flight, thereby improving the safety of the drone.

[0083] S4. After the trajectory under the limited field of view pushes away the obstacle, it is necessary to optimize the generated collision-free path points to generate a safe and smooth trajectory. The optimization function including the costs of smoothness, obstacle avoidance, and dynamic feasibility is solved using nonlinear optimization, and the trajectory is optimized based on the MINCO trajectory.

[0084] Guided by global trajectory planning, this invention rapidly pushes the trajectory away from obstacles through front-end A* search and back-end MINCO trajectory optimization, resulting in a safe, smooth, and dynamically constrained local trajectory. This not only ensures the drone's flight safety but also enhances its mission efficiency and flight performance by collaborating with global planning to adapt to dynamic environments and improve flight stability and accuracy.

[0085] The key factor in determining the generation of the obstacle avoidance curve is the successful generation of the control point Q. During the local replanning process, it is necessary to continuously detect whether the global trajectory passes through an obstacle. If it passes through an obstacle, the control point of the collision segment needs to be pushed away from the obstacle. The principle of pushing away is as follows Figure 6As shown, i is used to index the control point, j is used to index the {p, v} pair, φ is the global trajectory generated by the global trajectory planning, within the limited field of view, this global trajectory passes through the obstacle, and the control point that penetrates the obstacle is Q i , p is a point on the obstacle surface, v is a unit vector pointing from the control point to p, and L is the path initially planned after A* path search. When the global trajectory of the trajectory planning passes through an obstacle, it is first necessary to find the continuous control point Q entering the obstacle. i , these Q i Record it. Then search for a collision-free path L around the obstacle through path search. Then use Q i Find the {p, v} pair with the collision-free path L, record it and number it, and find the new control point Q through the {p, v} pair j , and then continuously detect whether the control point is within the obstacle, and slowly push the control point away from the obstacle to achieve the purpose of obstacle avoidance.

[0086] From Q i Click on the obstacle ij The distance field calculation formula of a point is:

[0087] d ij =(Q i -p ij )·v ij (2)

[0088] By calculating the distance value, we can determine whether the control point has been pushed out of the obstacle, such as Figure 7 The figure shows the process of gradually pushing away the obstacle in the local trajectory. The control point Q is gradually moved until the value of the distance field d at the position of the control point Q is greater than 0, which means that the point Q has been pushed out of the obstacle. The gradient of each push is

[0089]

[0090] After initially pushing the trajectory away from obstacles, it needs to be optimized to meet the autonomous flight mission requirements in terms of obstacle avoidance, smoothness, safety, and time allocation. This invention uses a MINCO trajectory with spatiotemporal joint planning to achieve trajectory optimization. This trajectory decouples the temporal and spatial parameters of the trajectory and, based on this, designs linear complexity operations to facilitate spatiotemporal deformation. First, the parameters of the MINCO segmented trajectory of this invention are defined as follows:

[0091]

[0092] Here, T is the duration of each segment, q is the number of adjacent path points between any two connected segments, and M is the number of segments. In other words, the spatial shape of the trajectory is determined by a series of path points q, and the flight time of the drone along each trajectory segment can be controlled through T, thereby achieving planning in the temporal dimension. The two are independent of each other, making it easier to perform spatiotemporal deformation of the trajectory. For example, to adjust the spatial shape of the trajectory, the path points in q can be modified; to change the flight time, the duration in T can be adjusted. Furthermore, the operational complexity of these adjustments is linear, and the computational effort does not increase significantly with the number of segments M.

[0093] The 3D point at any time t on the MINCO trajectory is defined as p(t). Given a set of path points q and the duration of each segment T, the specific position p(t) of the drone in space can be calculated at different times t, thus determining the entire flight trajectory. The value of p(t) is calculated based on the parameters q and T and the time t. Here, p(t) is represented by an operator:

[0094] p(t)=Ω q,t (t) (5)

[0095] The present invention is based on a 3rd order integral dynamics system, and the MINCO trajectory is assumed to be a 5th order C 2 The polynomial spline curve has a constant boundary and minimum control quantity under the given {q,T}. The control optimization quantity of the third-order system is expressed as follows:

[0096]

[0097] It should be noted that, since the present invention uses a third-order acceleration control system model, the smoothness is maximized by minimizing the control amount.

[0098] MINCO trajectory can also convert the parameters {q,T} into polynomial coefficients c and time curve T with linear complexity O(M) p , the corresponding relationship is:

[0099]

[0100] Here, b(q) is the result of applying a specific mapping to the pathpoint parameters q of the MINCO trajectory, and M is the number of segments in the MINCO segmented trajectory. As the number of segments M increases, the dimensions of the vectors and matrices also increase accordingly. However, due to the linear complexity of the MINCO method, the entire parameter conversion and calculation process remains highly efficient. This conversion ensures that both the recovered trajectory and the propagated gradient have linear complexity, facilitating optimization of the MINCO trajectory and achieving better trajectory planning results.

[0101] S5. After the optimized trajectory is generated, it is necessary to check whether it meets the dynamic feasibility requirements. If so, it can be published as the desired trajectory for the UAV to perform autonomous flight missions. If not, the MINCO trajectory needs to be time-redistributed, the trajectory generation needs to be refined, and step S4 is re-entered. The optimized trajectory is then published at a frequency of 100Hz.

[0102] To ensure mission safety, the present invention also incorporates a 20Hz timer to sample executable trajectory points, checking each sampled point for collisions. If an emergency collision occurs during flight, an emergency braking strategy is immediately implemented. If all checkpoints are safe, the executable trajectory points are successfully released. The present invention's multi-threaded planning module, controlled by a state machine, enables the drone to respond more quickly and efficiently during autonomous flight missions.

[0103] Quadcopter motion planning requires achieving smooth and efficient flight while satisfying dynamic constraints and obstacle avoidance. The constraints for trajectory planning include continuous time constraints, which are composed of an infinite number of inequalities and are difficult to handle directly. Therefore, constraint transformation is required to simplify the problem-solving process. The optimization problem for the time-dependent objective function under the equality constraint H and the inequality constraint G is originally:

[0104]

[0105] First, constraints are imposed based on the integral of the penalty function. The weight of the penalty function must be set large enough. In the obstacle avoidance constraint, if the trajectory approaches an obstacle, the penalty function value will increase. This "penalty" for constraint violations is accumulated through integration, prompting the optimization algorithm to adjust the trajectory to avoid the obstacle. Then formula (8) is transformed into:

[0106]

[0107] Among them, χ H and χ G is a user-defined weight whose value is large enough.

[0108] However, since continuous-time constrained integrals are difficult to calculate directly, discretization of the integral is required. The integral is sampled at equal intervals along the time axis and approximated using a finite sum. In this way, the originally complex continuous-time constrained optimization problem is transformed into a discrete unconstrained optimization problem, which reduces the difficulty of solving it. Equation (10) is transformed into:

[0109]

[0110] Among them, t i represents a finite number of sampling time points, k+1 is the number of sampling points, ω iThe interval value for integral evaluation. i The calculation formula for sampling is:

[0111]

[0112] By leveraging the linear complexity of the MINCO trajectory representation method, the computational overhead of the optimization problem after constraint conversion is significantly reduced, and the convergence speed is significantly improved. By converting the constraints into part of the objective function, multiple task requirements and constraints can be considered simultaneously during the optimization process. Finally, the feasibility of the optimization results is ensured by checking, effectively solving the complex constraint processing problem in multi-rotor UAV trajectory planning. Here, the constraints are determined according to the task requirements, and Equation (11) is simplified to:

[0113]

[0114] Among them, J x are various penalty terms, i.e., task specifications, λ x is the relative weight. The subscript x = {s, t, d, o} represents the smoothness s, total time t, dynamic feasibility d, obstacle avoidance o, etc.

[0115] In terms of smoothness requirements, the smoothness penalty term will "penalize" the unstable part of the trajectory, making the generated trajectory smoother. Since the MINCO trajectory is represented as a piecewise polynomial, the smoothness penalty term is calculated in an integral form. According to formula (6), the maximum smoothness is expressed as:

[0116]

[0117] In most cases, autonomous flight of UAVs pursues shorter flight time. Therefore, the present invention also minimizes the weighted total flight time, and the total time penalty term is defined as:

[0118] J t =sum(T) (15)

[0119] For differentially flat quadrotors, dynamic feasibility is ensured by limiting the magnitude of trajectory derivatives. Specifically, the trajectory's velocity, acceleration, and jerk are limited to ensure dynamic feasibility. When these parameters exceed the drone's physical limitations, the corresponding penalty term increases, increasing the total penalty term and forcing the trajectory to adjust to the dynamic constraints. This prevents the drone from losing control due to excessive speed or acceleration. Specifically:

[0120]

[0121] Among them, J d,v 、J d,a 、J d,jis the dynamic penalty term of velocity, acceleration and jerk, v m 、a m 、j m are the maximum permissible magnitudes of velocity, acceleration, and jerk, respectively.

[0122] The total dynamic penalty term is the sum of the dynamic penalty terms of velocity, acceleration, and jerk:

[0123] J d =J d,v +J d,a +J d,j (17)

[0124] Regarding the penalty term for obstacle avoidance, the collision penalty pushes the control point away from the obstacle, which requires adopting a safe distance s f and punish ij <s f control points to achieve this.

[0125] First, construct a second-order continuously differentiable penalty function j o :

[0126]

[0127] c ij =s f -d ij (19)

[0128] Among them, j o (i,j) is the pair {p,v} in Q i The cost value generated on each control point Q i The costs on Q are evaluated independently and accumulated by all corresponding {p,v} pairs. Therefore, if a control point finds more obstacles, it will get a higher trajectory deformation weight. Combining all control points Q i The cost on is the total collision penalty:

[0129]

[0130] MINCO trajectory optimization enables concise trajectory representation, compact parameters, and easy processing, reducing data storage and transmission costs and algorithm complexity. It ensures high trajectory smoothness for smoother flight. It also decouples spatial and temporal parameters, allowing users to independently manipulate spatial and temporal parameters, significantly enhancing trajectory planning flexibility and optimization convenience.

[0131] Those skilled in the art will appreciate that various aspects of the present invention may be implemented as systems, methods, or program products. Accordingly, various aspects of the present invention may be implemented in the following forms: entirely in hardware, entirely in software (including firmware, microcode, etc.), or in a combination of hardware and software, collectively referred to herein as "circuits," "modules," or "platforms."

[0132] Example 2

[0133] The present invention provides a quadrotor unmanned aerial vehicle autonomous flight trajectory planning system, which can be used to implement the above-mentioned quadrotor unmanned aerial vehicle autonomous flight trajectory planning method. Specifically, the quadrotor unmanned aerial vehicle autonomous flight trajectory planning system includes an initialization module, an update module, a path module, a trajectory module and a publishing module.

[0134] Among them, the initialization module obtains grid map information and positioning information, initializes map parameters and algorithm parameters, and generates a guidance trajectory according to the target point specified by the user;

[0135] The update module introduces a global trajectory update mechanism to update the global trajectory in real time, replans the generated guidance trajectory, and obtains the global trajectory;

[0136] The path module generates collision trajectory detection and obstacle avoidance paths based on the generated global trajectory. When a global trajectory that passes through an obstacle is detected, it uses A* to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle.

[0137] The trajectory module performs trajectory optimization on the generated collision-free obstacle path to generate a safe and smooth trajectory, solves the optimization function and performs trajectory optimization based on the MINCO trajectory to generate the optimized trajectory;

[0138] The release module takes the optimized trajectory that satisfies dynamic feasibility as the expected trajectory for the UAV to perform autonomous flight missions.

[0139] Example 3

[0140] The present invention provides a terminal device, which includes a processor and a memory, wherein the memory is used to store a computer program, the computer program includes program instructions, and the processor is used to execute the program instructions stored in the computer storage medium. The processor may be a central processing unit (CPU), or other general-purpose processors, graphics processing units (GPU), tensor processing units (TPU), digital signal processors (DSP), application-specific integrated circuits (ASIC), field-programmable gate arrays (FPGA) or other programmable logic devices, discrete gates or transistor logic devices, discrete hardware components, etc. It is the computing core and control core of the terminal, which is suitable for implementing one or more instructions, specifically suitable for loading and executing one or more instructions to implement corresponding method processes or corresponding functions; the processor described in the embodiment of the present invention can be used for the operation of the autonomous flight trajectory planning method of a quad-rotor unmanned aerial vehicle, including:

[0141] Obtain raster map information and positioning information, initialize map parameters and algorithm parameters, and generate a guidance trajectory based on the target point specified by the user;

[0142] A global trajectory update mechanism is introduced to update the global trajectory in real time, and the generated guidance trajectory is replanned to obtain a global trajectory. Based on the generated global trajectory, collision trajectory detection and obstacle avoidance paths are generated. When a global trajectory that crosses an obstacle is detected, an A* search is used to find a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle. The generated collision-free obstacle path is trajectory optimized to generate a safe and smooth trajectory. The optimization function is solved and trajectory optimization is performed based on the MINCO trajectory to generate an optimized trajectory. The optimized trajectory that meets the dynamic feasibility is used as the expected trajectory for the UAV to perform autonomous flight missions.

[0143] See also Figure 13The terminal device is a computer device. The computer device 60 of this embodiment includes: a processor 61, a memory 62, and a computer program 63 stored in the memory 62 and executable by the processor 61. When the computer program 63 is executed by the processor 61, the method for autonomous flight trajectory planning of the quadrotor drone in the embodiment is implemented. To avoid repetition, the details are not described here. Alternatively, when the computer program 63 is executed by the processor 61, the functions of each model / unit in the autonomous flight trajectory planning system of the quadrotor drone in the embodiment are implemented. To avoid repetition, the details are not described here.

[0144] The computer device 60 may be a desktop computer, a notebook computer, a PDA, a cloud server, or other computing devices. The computer device 60 may include, but is not limited to, a processor 61 and a memory 62. It will be understood by those skilled in the art that Figure 13 This is merely an example of the computer device 60 and does not constitute a limitation of the computer device 60 . The computer device 60 may include more or fewer components than shown in the figure, or a combination of certain components, or different components. For example, the computer device may also include input and output devices, network access devices, buses, etc.

[0145] The processor 61 may be a central processing unit (CPU), or other general-purpose processors, a graphics processing unit (GPU), a tensor processing unit (TPU), a digital signal processor (DSP), an application-specific integrated circuit (ASIC), a field-programmable gate array (FPGA), or other programmable logic devices, discrete gate or transistor logic devices, discrete hardware components, etc. The general-purpose processor may be a microprocessor or any conventional processor, etc.

[0146] The memory 62 may be an internal storage unit of the computer device 60, such as a hard disk or memory of the computer device 60. The memory 62 may also be an external storage device of the computer device 60, such as a plug-in hard disk, a smart media card (SMC), a secure digital (SD) card, a flash card, etc. equipped on the computer device 60.

[0147] Furthermore, the memory 62 may include both an internal storage unit of the computer device 60 and an external storage device. The memory 62 is used to store computer programs and other programs and data required by the computer device. The memory 62 may also be used to temporarily store data that has been output or is about to be output.

[0148] See also Figure 14 The terminal device is an electronic device 600, which is implemented as a general-purpose computing device. The components of the electronic device may include, but are not limited to, at least one processing unit 610, at least one storage unit 620, a bus 630 connecting different platform components (including the storage unit 620 and the processing unit 610), and a display unit 640.

[0149] The storage unit stores program codes, which can be executed by the processing unit 610, so that the processing unit 610 performs the steps according to various exemplary embodiments of the present invention described in the above method section of this specification. For example, the processing unit 610 can perform the following steps: Figure 1 Follow the steps shown in .

[0150] The storage unit 620 may include a readable medium in the form of a volatile storage unit, such as a random access memory unit (RAM) 6201 and / or a cache memory unit 6202 , and may further include a read-only memory unit (ROM) 6203 .

[0151] The storage unit 620 may also include a program / utility 6204 having a set (at least one) of program modules 6205, such program modules 6205 including but not limited to: an operating system, one or more application programs, other program modules, and program data, each of which or some combination may include an implementation of a network environment.

[0152] Bus 630 may represent one or more of several types of bus structures, including a memory bus or memory controller, a peripheral bus, an accelerated graphics port, a processing unit, or a local bus using any of a variety of bus architectures.

[0153] The electronic device 600 may also communicate with one or more external devices 700 (e.g., a keyboard, a pointing device, a Bluetooth device, etc.), one or more devices that enable a user to interact with the electronic device 600, and / or any device that enables the electronic device 600 to communicate with one or more other computing devices (e.g., a router, a modem). Such communication may occur via an input / output interface 650. Furthermore, the electronic device 600 may also communicate with one or more networks (e.g., a local area network, a wide area network, and / or a public network, such as the Internet) via a network adapter 660. The network adapter 660 may communicate with other modules of the electronic device 600 via a bus 630. It should be understood that, although not shown in the figures, other hardware and / or software modules may be used in conjunction with the electronic device 600, including but not limited to microcode, device drivers, redundant processing units, external disk drive arrays, RAID systems, tape drives, and data backup storage platforms.

[0154] Example 4

[0155] The present invention also provides a storage medium, specifically a computer-readable storage medium, which is a memory device in a terminal device for storing programs and data. It is understood that the computer-readable storage medium herein may include both built-in storage media in the terminal device and, of course, extended storage media supported by the terminal device. It may be any tangible medium containing or storing a program that can be used by or in conjunction with an instruction execution system, device, or component. The computer-readable storage medium provides storage space that stores the terminal's operating system. Furthermore, the storage space also stores one or more instructions suitable for being loaded and executed by a processor. These instructions may be one or more computer programs (including program code). It should be noted that more specific examples of the computer-readable storage medium herein include: an electrical connection having one or more wires, a portable disk, a hard disk, a random access memory, a read-only memory, an erasable programmable read-only memory, an optical fiber, a portable compact disk read-only memory, an optical storage device, a magnetic storage device, or any suitable combination thereof.

[0156] Computer-readable storage media also include data signals propagated in baseband or as part of a carrier wave, which carry readable program code. Such propagated data signals can take a variety of forms, including but not limited to electromagnetic signals, optical signals, or any suitable combination of the above. The readable storage medium can also be any readable medium other than a readable storage medium, which can send, propagate, or transmit programs for use by or in conjunction with an instruction execution system, device, or device. The program code contained on the readable storage medium can be transmitted using any appropriate medium, including but not limited to wireless, wired, optical cable, radio frequency, etc., or any suitable combination of the above.

[0157] The program code for performing the operations of the present invention may be written in any combination of one or more programming languages, including object-oriented programming languages such as Java, C++, and the like, as well as conventional procedural programming languages such as "C" or similar programming languages. The program code may be executed entirely on the user computing device, partially on the user device, as a stand-alone software package, partially on the user computing device and partially on a remote computing device, or entirely on a remote computing device or server. In the case of a remote computing device, the remote computing device may be connected to the user computing device via any type of network, including a local area network or a wide area network, or may be connected to an external computing device (e.g., via the Internet using an Internet service provider).

[0158] The processor may load and execute one or more instructions stored in a computer-readable storage medium to implement the corresponding steps of the method for autonomous flight trajectory planning of a quadrotor drone in the above embodiment; the processor may load and execute the following steps:

[0159] A global trajectory update mechanism is introduced to update the global trajectory in real time, and the generated guidance trajectory is replanned to obtain a global trajectory. Based on the generated global trajectory, collision trajectory detection and obstacle avoidance paths are generated. When a global trajectory that crosses an obstacle is detected, an A* search is used to find a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle. The generated collision-free obstacle path is trajectory optimized to generate a safe and smooth trajectory. The optimization function is solved and trajectory optimization is performed based on the MINCO trajectory to generate an optimized trajectory. The optimized trajectory that meets the dynamic feasibility is used as the expected trajectory for the UAV to perform autonomous flight missions.

[0160] The databases involved in the various embodiments provided herein may include at least one of a relational database and a non-relational database. Non-relational databases may include, but are not limited to, distributed databases based on blockchains. The processors involved in the various embodiments provided herein may include, but are not limited to, general-purpose processors, central processing units, graphics processing units, digital signal processors, programmable logic units, data processing logic units based on quantum computing, and the like.

[0161] In order to make the purpose, technical solutions and advantages of the embodiments of the present invention clearer, the technical solutions in the embodiments of the present invention will be clearly and completely described below in conjunction with the drawings in the embodiments of the present invention. Obviously, the described embodiments are part of the embodiments of the present invention, not all of the embodiments. Generally, the components of the embodiments of the present invention described and shown in the drawings herein can be arranged and designed in various different configurations. Therefore, the following detailed description of the embodiments of the present invention provided in the drawings is not intended to limit the scope of the claimed invention, but merely represents selected embodiments of the present invention. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative work are within the scope of protection of the present invention.

[0162] The following describes an application example of the method proposed in this invention in a simulated indoor environment. In this simulation experiment, the drone can only obtain information about obstacles within its field of view and is unaware of the global environment. This simulates the obstacle avoidance process of the drone in a real unknown environment.

[0163] The algorithm part of the present invention is completed under the Ubuntu 20.04 operating system and developed based on the Robot Operating System (ROS) C++ language. The simulation of the algorithm of the present invention is completed based on the visualization tool (ROS Visualization Tool, RVIZ) under the ROS operating system, which can display the model, sensor data, motion trajectory and other information of the quadcopter in real time. The model established by the present invention is a planning problem under limited field of view perception. The mapping module uses the point cloud information provided by the LiDAR Mid-360 sensor for grid mapping. The horizontal field of view angle is 360°, the vertical field of view angle is 59°, and the maximum detection range is 0.1~70m. The simulation parameters of the present invention are set according to the established model. The horizontal field of view angle and the vertical field of view angle of the quadcopter in the simulation are consistent with those of the LiDAR. The limited field of view range radius is set to 50m, and the execution domain radius is set to 5m. The maximum speed of the drone is set to 5m / s and the maximum acceleration is set to 3m / s 2 .

[0164] Before the improvement, global planning did not introduce map information, but only provided a direction from the starting point to the end point. When encountering a large obstacle beyond the field of view, local planning would not allow the drone to fly firmly in one direction, but would constantly search for the optimal trajectory, causing the drone to wander back and forth on the edge of the obstacle and unable to reach the end point. Figure 8 As shown in the figure, the simulation simulates a long corridor model. The drone's task is to fly around the corridor autonomously. At (1), the target point is selected as a point outside the corridor. The drone generates a global trajectory from the starting point to the end point as a straight line, which serves as a direction guide for the end point. According to the trajectory, the drone flies to state (2) in the figure and continues to fly close to the wall of the long corridor, that is, state (3) in the figure. At this time, the drone generates a local optimal trajectory to avoid obstacles. At this time, due to the limited field of view, the drone's upward and downward trajectories are both local optimal trajectories. Therefore, at time (4), the drone plans to fly downward. Finally, the drone jumps back and forth between state (5) and state (6) and cannot reach the end point of the mission.

[0165] Through the improvement of the present invention, the global planning module also introduces map information. The trajectory generation result of the improved long corridor model is as follows: Figure 9 As shown in the figure, the target point is also selected at (1), and the goal is to bypass the corridor and reach the end point. When arriving at (2), the long wall is detected within the limited field of view. At this time, a third global path point is added based on the effective field of view to guide the trajectory to the side of the obstacle. The same is true at moment (3). The drone still flies upward firmly without falling into the local optimum. Gradually, after moments (4) and (5), global planning and local planning are combined, and finally it reaches the end point, that is, moment (6).

[0166] Dynamic obstacles are added to the system for simulation testing. The ROS node relationship of the simulation system is as follows: Figure 10 As shown in the figure, the joy_node is used to control the movement of dynamic obstacles and send dynamic obstacle information to the planner_node. The global map is a randomly generated obstacle map for the simulation. Obstacles within the limited field of view of the simulated drone, i.e., the local map, are sent to the planner_node. The MINCO trajectory generated by the planner_node is sent to the execution node Traj_server, which generates the position, velocity, and acceleration (p, v, a) of the desired trajectory. Finally, the simulated drone feeds the simulated flight pose information back to the global map and the planner_node.

[0167] The simulation results for dynamic obstacles are as follows Figure 11As shown, at (1), the handle is used to control the dynamic obstacle to approach the flight trajectory of the drone. The drone recognizes the dynamic obstacle and plans to fly to the left in advance. At (3), it bypasses the trajectory. At this time, the dynamic obstacle suddenly changes direction. The drone replans the trajectory and chooses to fly to the right to bypass the obstacle at (4). At (5), the direction of the dynamic obstacle is changed again. At this time, the distance between the obstacle and the drone is very close. After (6) and (7), the obstacle is successfully avoided and flies towards the target point. Finally, at (8), it is away from the dynamic obstacle. The reason why the present invention continuously controls the dynamic obstacle to approach the flight trajectory of the drone and frequently changes direction is to verify the obstacle avoidance effect of the drone in an emergency. Through simulation, it can be seen that the improved algorithm of the present invention has the function of emergency avoidance of dynamic obstacles.

[0168] In order to verify the performance of the improved algorithm of the present invention, 200 cylindrical obstacles and 200 circular obstacles are randomly set in a map space of 20m×20m×3m, with a field of view radius of 7.5m, a starting point of [-15,0,1], a second target point of [15,0,1], a third target point of [0,11,1], a fourth target point of [0,-11,1], and an end point of [-15,0,1]. The obstacle avoidance simulation effect is as follows: Figure 12 As shown in Figures 2 and 3, (a) and (b) are simulation results of the original EGO-Planner navigation framework before improvement. It can be seen that the trajectory planned by EGO-Planner performs poorly in terms of smoothness and has large inflection points, causing the drone to frequently and sharply adjust its attitude. Such a trajectory is very energy-consuming. (c) and (d) are the effect diagrams after improvement of the present invention. It can be seen that compared with the trajectory before improvement, the trajectory generated by the planning algorithm proposed in the present invention is smoother and has no large inflection points. The drone does not need to adjust its attitude frequently, and thus the trajectory consumes less energy.

[0169] The specific performance comparison is shown in Table 1. Compared with the original EGO-Planner algorithm, the planning algorithm proposed in the present invention has a trajectory length reduced by 12.43%, a trajectory execution time reduced by 31.73%, and a planning success rate increased by 13%. Under the condition of equivalent average re-planning time, the memory occupied is reduced by 4.5 times, it is more lightweight, and the CPU occupancy rate is lower. The algorithm proposed in the present invention can be deployed on an airborne platform with less computing power. In terms of smoothness, the trajectory planned by EGO-Planner performs poorly, while the algorithm of the present invention performs better and is more suitable for real-time flight. In terms of trajectory energy, the smoother the trajectory, the less energy it consumes. The trajectory planned by the algorithm of the present invention is reduced by 37.83% compared to before the improvement. The algorithm also improves the global planning part and adds local field of view information to the global planning, so that the drone will not fall into the global optimal solution, and the obstacle avoidance effect is good in an environment with large obstacles.

[0170] Table 1

[0171]

[0172]

[0173] In summary, the method and system for autonomous flight trajectory planning of a quadrotor drone of the present invention have the following advantages:

[0174] (1) Global replanning introduces the concept of limited field of view into the Minimum Snap algorithm. This adds a global path point while ensuring a smooth trajectory, ensuring the safety of the globally planned guidance trajectory and making it more suitable for diverse and complex environments. The smooth trajectory eliminates the need for frequent, large acceleration and deceleration, as well as attitude adjustments, during flight, providing a very good trajectory foundation for subsequent local planning modules. This also means that the operating conditions of devices such as motors are more stable, with lower energy consumption.

[0175] (2) Local replanning uses front-end A* search and back-end MINCO trajectory optimization to quickly push the trajectory away from obstacles, obtaining a safe, smooth, and dynamically constrained local trajectory. Through the MINCO trajectory optimization method, the trajectory representation is concise, the parameters are compact and easy to process, which reduces data storage and transmission costs and algorithm complexity, ensures high trajectory smoothness, and makes flight smoother. In addition, the coordination with global planning increases the adaptability to dynamic environments, and the flight stability and accuracy are improved, thereby improving the mission execution efficiency and flight performance of the UAV.

[0176] The above content is only for explaining the technical idea of the present invention and cannot be used to limit the protection scope of the present invention. Any changes made on the basis of the technical solution in accordance with the technical idea proposed by the present invention shall fall within the protection scope of the claims of the present invention.

Claims

1. A method for autonomous flight trajectory planning of a quadrotor drone, characterized in that: The following steps are involved: Obtain raster map information and positioning information, initialize map parameters and algorithm parameters, and generate a guidance trajectory based on the target point specified by the user; A global trajectory update mechanism is introduced to update the global trajectory in real time, and the generated guidance trajectory is replanned to obtain the global trajectory; Based on the generated global trajectory, collision trajectory detection and obstacle avoidance paths are generated. When a global trajectory that passes through an obstacle is detected, A* is used to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle. Perform trajectory optimization on the generated collision-free obstacle path to generate a safe and smooth trajectory, solve the optimization function and perform trajectory optimization based on the MINCO trajectory to generate an executable trajectory; The optimized trajectory that satisfies dynamic feasibility is taken as the expected trajectory for the UAV to perform autonomous flight missions.

2. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 1, characterized in that: The Minimum Snap optimization method is used to generate the global trajectory, as follows: A minimum snap optimization method is used to calculate a time-dependent trajectory function from the starting point to the end point. A timer is then set to calculate the field of view rate at a frequency of 50 Hz and determine whether the field of view rate is less than a set threshold. If the field of view rate is less than or equal to the threshold, the global trajectory update mechanism is activated, and the final target point is urgently moved to a position on the same side of the field of view and in the same direction as the obstacle. If the field of view is greater than the threshold, conventional re-planning is initiated, and a global path point is added within the field of view. The drone is guided in a certain direction when performing autonomous flight missions, and is guided to fly to an area with unobstructed field of view.

3. The autonomous flight trajectory planning method for a quadrotor drone according to claim 2, characterized in that: The effective field of view γ is calculated as follows: Among them, n obs Represents the number of discretized slices of the fan-shaped area blocked by obstacles, n free Represents the number of discretized slices of the fan-shaped area that is not blocked by obstacles.

4. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 1, wherein: Generate a safe and smooth trajectory as follows: The MINCO trajectory optimization method based on spatiotemporal joint planning is adopted. First, the parameters of the MINCO trajectory are defined. The spatial shape of the trajectory is determined by the adjacent path points q of any two connected segments. The flight time of the UAV on each trajectory is controlled by the duration T of each segment, thereby realizing planning in the time dimension.

5. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 4, characterized in that: When the global trajectory of the planned trajectory passes through an obstacle, first find the continuous control point Q that enters the obstacle i And record it, then search for a collision-free path L around the obstacle through path search; By Q i And the collision-free path L finds the {p, v} pair, records it and numbers it, where p is the point on the obstacle surface and v is the unit vector pointing from the control point to p. The new control point Q is found through the {p, v} pair. j ; Then detect whether the control point is within the obstacle, push the control point away from the obstacle, and calculate the distance value to determine whether the control point has been pushed out of the obstacle to achieve obstacle avoidance.

6. The method for autonomous flight trajectory planning of a quadrotor UAV according to claim 5, characterized in that: From the control point Q of the trajectory i The intersection point p of the tangent normal plane and the obstacle at the control point ij The distance field d ij The calculation is as follows: d ij =(Q i -p ij )·v ij Among them, v ij From the control point Q i Point to p ij The unit vector of .

7. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 1, wherein: For quadrotor drones, the dynamic feasibility is ensured by limiting the size of the trajectory derivative and limiting the dynamic penalty term J of the trajectory speed, acceleration, and jerk. d,v 、J d,a 、J d,j The total dynamic penalty term is the dynamic penalty term J of velocity, acceleration, and jerk d,v 、J d,a 、J d,j The smoothness of the trajectory is ensured by limiting the size of the trajectory integral, and the corresponding penalty term is J s ; Control the drone's flight time by limiting the total time so that the drone can quickly reach the target destination; for collision penalties, push the control point away from the obstacle and use a safe distance s f and punish ij <s f The control point is realized.

8. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 1, wherein: When the generated optimized trajectory does not meet the dynamic feasibility, the MINCO trajectory is time-redistributed, the trajectory generation is refined, the trajectory optimization is performed again, and then the optimized trajectory is published at a frequency of 100 Hz.

9. The method for autonomous flight trajectory planning of a quadrotor drone according to claim 8, characterized in that: Add a 20Hz timer to sample the executable trajectory points and check whether a collision occurs at each sampling point. If an emergency collision occurs during the flight of the drone, an emergency braking strategy is adopted; if all checkpoints are safe, the executable trajectory points are released.

10. A quadrotor UAV autonomous flight trajectory planning system, characterized in that: include: The initialization module obtains raster map information and positioning information, initializes map parameters and algorithm parameters, and generates a guidance trajectory based on the target point specified by the user; The update module introduces a global trajectory update mechanism to update the global trajectory in real time, replans the generated guidance trajectory, and obtains the global trajectory; The path module generates collision trajectory detection and obstacle avoidance paths based on the generated global trajectory. When a global trajectory that passes through an obstacle is detected, it uses A* to search for a collision-free obstacle path with the starting point being the trajectory point entering the obstacle and the end point being the trajectory point passing through the corresponding obstacle. The trajectory module performs trajectory optimization on the generated collision-free obstacle path to generate a safe and smooth trajectory, solves the optimization function and performs trajectory optimization based on the MINCO trajectory to generate the optimized trajectory; The release module takes the optimized trajectory that satisfies dynamic feasibility as the expected trajectory for the UAV to perform autonomous flight missions.

Images