Unmanned aerial vehicle trajectory online optimization method and system based on curvature-driven local enhanced expression

Abstract

This invention discloses an online trajectory optimization method and system for unmanned aerial vehicles (UAVs) based on curvature-driven local enhancement representation. The method includes: discretizing the trajectory according to the geometric features of the mission path; constructing a curvature distribution model based on local curvature; adaptively adjusting obstacle repulsion force, flight speed, and control point distribution density according to the curvature magnitude, establishing a first mapping relationship between curvature and speed, and a second mapping relationship between curvature and control point distribution density; increasing control point density and decreasing flight speed in high-curvature turning regions to improve local representation capability; adjusting the initial trajectory based on the curvature-enhanced obstacle repulsion force to form a safe and smooth turning trajectory; and introducing curvature constraints and dynamic constraints on the B-spline trajectory representation for optimization and solution to generate a feasible trajectory. This invention achieves coupled adjustment of speed and path geometric complexity, enhancing real-time performance and stability in complex environments while ensuring trajectory continuity and flightability, and is suitable for UAV autonomous navigation and dynamic obstacle avoidance tasks.

Description

Technical Field

[0001] This invention relates to the field of unmanned aerial vehicle (UAV) trajectory planning and autonomous navigation technology, and in particular to an online UAV trajectory optimization method and system based on curvature-driven local enhancement expression. Background Technology

[0002] With the rapid development of UAV technology, it has been widely used in fields such as inspection, surveying, search and rescue, and complex environment exploration. In practical applications, UAVs need to complete safe and efficient path planning and trajectory tracking tasks in complex environments. Existing trajectory planning methods mainly include sampling-based methods, graph search-based methods, and optimization-based methods. Among them, optimization-based methods are widely used in UAV trajectory planning because they can generate high-order continuous trajectories.

[0003] For example, Chinese patent application CN121115851A discloses a gradient optimization-driven multi-stage trajectory planning method for real-time obstacle avoidance of unmanned aerial vehicles (UAVs). This method uses an improved A* algorithm to search for collision-free geometric paths on an occupied grid map, parameterizes the trajectory based on B-spline curves, and constructs a total cost function that includes smoothness, collision, and dynamic feasibility constraints. Iterative optimization of control point positions using gradient descent generates an optimized trajectory. This scheme achieves good results in initial path search and trajectory smoothing.

[0004] However, the above methods and other existing trajectory planning methods still have the following shortcomings: First, they do not fully consider the impact of curvature information in trajectory geometry on flight safety. In high-curvature turning areas, the same high speed as in straight sections is still used, resulting in excessive centrifugal force and potential flight safety hazards. Second, the control point distribution density is fixed and cannot be adaptively adjusted according to the local curvature of the trajectory. This causes computational redundancy in smooth areas and reduces trajectory fitting accuracy and limits local expressive power in sharp curves due to insufficient control points. Third, the obstacle repulsion force in existing trajectory optimization methods usually uses fixed parameters and does not adaptively adjust the obstacle avoidance force in combination with the local curvature of the trajectory. This results in insufficient trajectory safety margin for UAVs in complex turning areas, making it difficult to balance obstacle avoidance safety and trajectory smoothness. Fourth, there is a lack of collaborative coupling modeling between curvature and flight speed, trajectory control point distribution density, and obstacle repulsion force, making it difficult to achieve joint adaptive optimization of trajectory smoothness, obstacle avoidance safety, and flight dynamics stability. Summary of the Invention

[0005] In view of this, the purpose of the present invention is to provide an online optimization method and system for UAV trajectory based on curvature-driven local enhancement expression. This method solves the problems of insufficient local expression capability of trajectory in high curvature regions, flight safety hazards and control lag by co-optimizing the speed, control point density and obstacle repulsion force driven by curvature.

[0006] To achieve the above objectives, the present invention provides the following technical solution: The present invention provides an online UAV trajectory optimization method based on curvature-driven local enhancement expression, which includes the following steps: The trajectory is discretized according to the UAV mission path, the curvature information of each discrete point of the trajectory is calculated, and a geometric feature model of the trajectory is constructed based on the curvature distribution. Based on the curvature information, an obstacle avoidance cost term with curvature adaptation is set in the trajectory optimization objective function, so that the trajectory can form an obstacle avoidance effect that maintains a safe distance from obstacles in the high curvature region; Based on the curvature information, before trajectory optimization, the obstacle repulsion force enhancement coefficient, the basic control point distribution density, and the initial flight speed limit are adaptively set; based on B-spline trajectory representation, an objective function containing curvature adaptive smoothness cost term, obstacle avoidance cost term, and dynamic constraint cost term is constructed, and the first round of trajectory optimization is solved. The precise curvature distribution of the optimized B-spline trajectory is calculated analytically. Based on this precise curvature, the distribution density of trajectory control points in the high curvature region is adjusted again, and the expected flight speed of the UAV in the turning section is reduced accordingly. Using the adjusted control point density and speed constraints, a second round of local optimization is performed on the trajectory, or the optimal flight trajectory that satisfies the kinematic constraints of the UAV is directly generated.

[0007] Furthermore, when calculating the curvature information of discrete points on the trajectory, the discrete difference method is used for approximate solution, and the curvature is calculated based on the first and second derivatives between adjacent discrete points. The expression for curvature is: in, Indicates the curvature of the trajectory; It represents the first derivative along the x-axis between discrete points of the trajectory; It represents the first derivative in the y-axis direction between discrete points of the trajectory; It represents the second derivative along the x-axis between discrete points of the trajectory; It represents the second derivative along the y-axis between discrete points on the trajectory; The calculated curvature sequence is then smoothed and filtered; the curvature information is calculated using the discrete difference method for discrete points and the analytical derivative method for B-spline trajectories.

[0008] Furthermore, the coupling relationship between curvature and obstacle repulsion force is constructed according to the following formula, and the obstacle repulsion strength is dynamically adjusted by curvature: in, The repulsive force of the obstacle after the curvature is enhanced. Based on the repulsive force of the basic obstacle, To enhance the weighting coefficient of repulsive force, It is a non-linear adjustment index. Let be the curvature at the trajectory point. The curvature enhancement threshold.

[0009] Furthermore, the relationship between curvature and flight speed is used to adaptively adjust the UAV's flight speed based on the trajectory curvature, and is constructed using the following functional form: in, For maximum flight speed, The velocity attenuation coefficient, For curvature.

[0010] Furthermore, the correspondence between curvature and the number of control points is used to adaptively adjust the distribution density of trajectory control points according to the trajectory curvature, and is constructed using the following functional form: in, This represents the number of control points assigned to the i-th trajectory segment. The number of basic control points, To adjust the parameters.

[0011] Furthermore, the objective function for trajectory optimization includes a curvature-adaptive trajectory smoothness cost term, an obstacle avoidance cost term, and a dynamic constraint cost term; The curvature adaptive trajectory smoothness cost term is used to reduce the degree of abrupt changes in the trajectory during changes in position, velocity, and acceleration. The curvature adaptive obstacle avoidance cost term is used to increase the safe distance between the trajectory and the obstacle, thereby improving the obstacle avoidance safety margin. The curvature adaptive dynamics constraint cost term is used to limit the dynamic parameters of speed, acceleration and angular velocity during the flight of the UAV, thereby improving the smoothness of the trajectory in high curvature regions. The formula for calculating the trajectory optimization objective function is as follows: in, For curvature adaptive trajectory smoothness cost term, For curvature adaptive obstacle avoidance cost term, The cost term for curvature adaptive dynamic constraints. Let be the local curvature of the trajectory.

[0012] Furthermore, the trajectory optimization solution is based on B-spline curve representation, and the trajectory is parameterized by a set of control points. The B-spline curve representation is as follows: in, As control points, It is a k-order B-spline basis function.

[0013] Furthermore, in the step of dynamically adjusting the distribution density of trajectory control points based on the local curvature of the smooth turning trajectory, a segmented optimization strategy is adopted: The trajectory is divided into high curvature regions and low curvature regions based on at least one preset or dynamically calculated curvature threshold. In the high curvature region, a first sampling interval is used to increase the number of control points. A second sampling interval is used in the low curvature region to reduce the number of control points. The first sampling interval is smaller than the second sampling interval; the curvature threshold is determined based on the maximum permissible centripetal acceleration, minimum turning radius, or trajectory curvature statistical characteristics of the UAV.

[0014] The present invention provides an online UAV trajectory optimization system based on curvature-driven local enhancement representation, comprising: The map building module is configured to build an occupancy grid map and a Euclidean distance field map of the environment based on sensor data. The path search module is configured to search for a collision-free geometric path from the start point to the end point in the occupied grid map; The curvature calculation module is configured to discretize the geometric path and calculate the curvature information of each discrete point; An obstacle repulsion force adaptive module is configured to adaptively adjust the obstacle repulsion force based on the curvature information and the correspondence between curvature and obstacle repulsion force. The speed adaptive module is configured to adaptively adjust the flight speed of the UAV based on the curvature information and the correspondence between curvature and flight speed. The control point adaptive module is configured to adaptively adjust the distribution density of trajectory control points based on the curvature information and the correspondence between curvature and the distribution density of trajectory control points. The trajectory optimization module is configured to use B-spline curves to parametrically represent the trajectory, construct an objective function consisting of trajectory smoothness cost, obstacle avoidance cost, and dynamic constraint cost based on adaptive adjustment of trajectory local curvature, and use gradient descent method to iteratively optimize the control points to generate the optimal flight trajectory that satisfies the kinematic constraints of the UAV. The control execution module is configured to send the optimal flight trajectory to the UAV flight control system for execution.

[0015] The beneficial effects of this invention are as follows: This invention provides an online trajectory optimization method and system for unmanned aerial vehicles (UAVs) based on curvature-driven local enhancement representation. The method includes: discretizing the trajectory according to the geometric features of the mission path; constructing a curvature distribution model based on the local curvature information of the trajectory; adaptively adjusting the obstacle repulsion force according to the curvature magnitude to form an obstacle avoidance trajectory that maintains a safe distance from obstacles in high curvature regions; increasing the control point density and reducing the flight speed in high curvature turning regions to smooth and dynamically adjust the obstacle avoidance trajectory, improving the local representation capability of the trajectory, the smoothness of the trajectory in turning regions, and the flight stability of the UAV; and introducing a trajectory optimization constraint mechanism based on curvature adaptive adjustment on the basis of B-spline trajectory representation to optimize and solve the trajectory, generating a feasible trajectory that satisfies flight safety and smoothness. This invention achieves coupled adjustment of speed and path geometric complexity, enhancing the real-time performance and stability of trajectory planning in complex environments while ensuring trajectory continuity and flightability, and is suitable for UAV autonomous navigation and dynamic obstacle avoidance tasks. This method employs a phased temporal decoupling design (based on initial path curvature before optimization and on precise B-spline curvature after optimization). This approach leverages curvature information to enhance local expressiveness while avoiding iterative instability caused by the coupling between curvature and control points during online optimization, thus ensuring the algorithm's convergence and computational efficiency. Compared to existing technologies, its advantages can be summarized as follows: 1. Improve the flightability and stability of the trajectory. Existing trajectory planning methods often lack sufficient consideration of kinematic constraints in complex environments, which can easily lead to trajectories that are unflyable or pose safety hazards. This invention introduces a curvature constraint mechanism to effectively limit the geometric characteristics of the trajectory, making the generated trajectory smoother and meeting the dynamic requirements of the aircraft, thereby significantly improving the flightability and stability of the trajectory.

[0016] 2. Enhance obstacle avoidance safety in sharp turn areas. To address the problem that traditional obstacle avoidance methods often suffer from insufficient obstacle repulsion force or trajectory sticking to obstacles in sharp turns, this invention proposes an obstacle repulsion force adjustment method based on curvature enhancement. By constructing a nonlinear coupling relationship between curvature and obstacle repulsion force, the obstacle repulsion effect is adaptively enhanced in high curvature regions, enabling the trajectory to actively move away from obstacles in turning areas, thereby forming a safer obstacle avoidance trajectory.

[0017] 3. Achieve unified optimization of security, smoothness, and dynamic stability. Simply increasing the repulsive force of obstacles can easily lead to sudden changes in local curvature of the trajectory and UAV oscillation. This invention proposes a curvature-sensing-based repulsive force-control point-velocity collaborative optimization method, which dynamically adjusts three elements based on the local curvature of the trajectory: In high curvature regions: enhance repulsion to improve safety margins, increase the number of control points to improve local trajectory adjustability, and reduce flight speed to reduce dynamic shocks during turns; In the low curvature region: reduce repulsive force to avoid excessive obstacle avoidance, reduce the number of control points to reduce computational overhead, and increase flight speed to ensure mission efficiency.

[0018] Through the aforementioned collaborative mechanism, a unified optimization of security, smoothness, and dynamic stability is achieved.

[0019] 4. Improve computational efficiency Traditional methods use a fixed distribution of control points, which can easily lead to computational redundancy or insufficient expressive power. This invention employs an adaptive control point distribution strategy, dynamically adjusting the control point density based on the complexity of the turning trajectory. This reduces unnecessary computational overhead while maintaining trajectory accuracy, thereby improving overall computational efficiency.

[0020] 5. Excellent engineering compatibility and scalability The method of this invention is compatible with the existing EGO-Planner framework in design, without requiring significant modifications to the original system structure. It has good engineering feasibility and scalability, and is easy to deploy and apply quickly in actual UAV systems.

[0021] 6. Enhance adaptability to complex and dynamic environments The method of this invention can adapt to complex dynamic environments. By comprehensively considering factors such as trajectory curvature, speed adjustment and control point optimization, it improves the robustness and adaptability of the system in dynamic obstacle environments. It has broad application prospects and is suitable for UAV autonomous navigation and dynamic obstacle avoidance tasks.

[0022] The above and other objects, advantages, and features of the present invention will be more fully set forth and demonstrated through the following detailed description of specific embodiments in conjunction with the accompanying drawings. Those skilled in the art, upon referring to the following detailed description and the accompanying drawings, will be able to better understand and realize the above advantages of the present invention. Other objects, features, and advantages of the present invention will become clearer after being described in detail in the detailed description section in conjunction with the accompanying drawings. Attached Figure Description

[0023] To make the objectives, technical solutions, and beneficial effects of this invention clearer, the following drawings are provided for illustration.

[0024] Figure 1 The flowchart shows the overall process of the curvature-driven online trajectory optimization method for unmanned aerial vehicles. Figure 2 A block diagram of a curvature-driven online trajectory optimization system for unmanned aerial vehicles (UAVs). Figure 3 This is a schematic diagram illustrating the implementation process of an online trajectory optimization method for unmanned aerial vehicles (UAVs) based on curvature-driven principles. Figure 4 A schematic diagram comparing the distribution of control points in high-curvature and low-curvature regions; Figure 5 This is a schematic diagram illustrating the effect of curvature enhancing the repulsive force of obstacles.

[0025] Figure 6 This is a schematic diagram of the key mapping relationship curves in this method; Figure 7 This diagram illustrates the comparison of trajectory effects between our method and traditional methods. Detailed Implementation

[0026] The present invention will be further described below with reference to the accompanying drawings and specific embodiments, so that those skilled in the art can better understand and implement the present invention. However, the embodiments described are not intended to limit the present invention.

[0027] Example 1

[0028] like Figure 1 As shown, Figure 1 This is an overall flowchart of the method of the present invention; the online optimization method for UAV trajectory based on curvature-driven local enhancement expression provided in this embodiment includes the following steps: Before the UAV performs its mission, the trajectory is discretized according to the global path or initial reference trajectory and the UAV mission path. The curvature information of each discrete point on the trajectory is calculated, and a geometric feature model of the trajectory is constructed based on the curvature distribution. Based on the curvature information, the obstacle repulsion force in the adaptive enhancement trajectory optimization is enhanced, so that the trajectory forms an obstacle avoidance trajectory that maintains a safe distance from obstacles in the high curvature region; Based on the obstacle avoidance trajectory, the distribution density of trajectory control points and the flight speed of the UAV are adaptively adjusted according to the curvature information to smooth and dynamically adjust the obstacle avoidance trajectory and improve the local expression capability of the trajectory.

[0029] Based on B-spline trajectory representation, a trajectory optimization constraint mechanism based on curvature adaptive adjustment is introduced. By jointly enhancing the trajectory smoothness constraint, collision constraint, and dynamic feasibility constraint through obstacle repulsion force, trajectory control point density, and flight speed, the trajectory is optimized and solved to generate the optimal flight trajectory that satisfies the kinematic constraints of the UAV, and the executable flight path of the UAV is output.

[0030] In this embodiment, after discretizing the trajectory according to the UAV mission path and calculating the curvature information of each discrete point, the following steps are also included: based on the curvature information, the initial geometric path is adaptively enhanced to improve obstacle repulsion force to obtain an obstacle avoidance trajectory; during the B-spline optimization of the obstacle avoidance trajectory, the control point distribution density and flight speed are dynamically adjusted according to the real-time curvature; or, after optimization, the curvature of the optimized B-spline trajectory is recalculated (using analytical differentiation), and the control point density and flight speed are adjusted according to the new curvature.

[0031] It should be noted that this method employs a staged curvature adaptive strategy to avoid oscillations in control point density and velocity constraints caused by frequent curvature changes during online optimization. Specifically: Based on the discrete curvature of the initial geometric path (such as the path obtained by A* search), the obstacle repulsion force enhancement parameters, the basic distribution density of B-spline control points, and the upper limit of the UAV velocity constraint are calculated to construct the objective function for the first round of trajectory optimization.

[0032] After optimizing the B-spline trajectory, the curvature of each point on the trajectory is accurately calculated using the analytical derivative properties of the B-spline curve. Based on this accurate curvature, the control point distribution density in the high curvature region is redefined (e.g., by densifying the node vectors using non-uniform nodes), and the velocity reference value for the corresponding trajectory segment is lowered.

[0033] Using the adjusted control point density and velocity constraints as fixed parameters, a second round of local optimization is performed on the trajectory (or the adjusted control points are directly applied to regenerate the trajectory), and the final output is the optimal flight trajectory that satisfies all constraints.

[0034] In this embodiment, when calculating the curvature information of discrete points on the trajectory, the discrete difference method is used for approximate solution, and the curvature is calculated based on the first and second derivatives between adjacent discrete points. The expression for curvature is: in, Indicates the curvature of the trajectory; It represents the first derivative along the x-axis between discrete points of the trajectory; It represents the first derivative in the y-axis direction between discrete points of the trajectory; It represents the second derivative along the x-axis between discrete points of the trajectory; It represents the second derivative along the y-axis between discrete points on the trajectory; The calculated curvature sequence is then smoothed and filtered. Simultaneously, the curvature is normalized to facilitate subsequent unified modeling and constraint design.

[0035] It should be noted that when B-splines are used to represent trajectories, curvature can also be calculated using analytical expressions of the first and second derivatives of B-splines to ensure accuracy and continuity; the discrete difference method is at least applicable to curvature estimation of the initial geometric path.

[0036] In this embodiment, a sliding window smoothing filter is used to smooth the curvature sequence.

[0037] This embodiment introduces a curvature-based obstacle repulsion force enhancement mechanism. By dynamically adjusting the curvature coefficient, the obstacle repulsion strength is adjusted to form an obstacle avoidance trajectory with a safety margin. Its expression is: in, This represents the repulsive force of an obstacle after the curvature is increased; The repulsive force of the obstacle, For adjustment coefficients, For curvature, The curvature saturation threshold, It is a non-linear exponent.

[0038] In this embodiment, the mechanism is used to adaptively enhance obstacle repulsion strength in high-curvature turning regions.

[0039] The initial trajectory is adjusted based on the curvature-enhanced obstacle repulsion force, so that the trajectory actively moves away from the obstacle in the turning area, thereby forming an obstacle avoidance trajectory with a safety margin.

[0040] The expression for the adaptive adjustment of the trajectory control point distribution density in this embodiment is: in, Indicates the first The number of control points assigned to the segment trajectory The number of basic control points, To adjust the parameters; Indicates the first The local curvature corresponding to the segment trajectory.

[0041] In this embodiment, the curvature information is based on a one-time calculation after the task path is discretized. Subsequent obstacle repulsion enhancement, control point density adjustment, and speed adaptation are all based on the same curvature distribution model. The correspondence between curvature and control point distribution density is used to adaptively adjust the distribution position of B-spline control points according to the trajectory curvature, so that the spacing between control points in high curvature regions is smaller and the spacing in low curvature regions is larger. This is specifically achieved through non-uniform node vectors, and the density of the node vectors is positively correlated with the curvature.

[0042] The trajectory is sampled discretely at fixed intervals, the first... The length of the segment trajectory is denoted as Number of basic control points According to the basic sampling interval calculate: When the trajectory curvature is small, the number of control points is close to the number of basic control points. To reduce the computational load for trajectory optimization; As the trajectory curvature increases, the number of control points increases synchronously with the curvature, thereby improving the trajectory's local expressive ability in complex turning areas; Increasing the density of control points in high curvature regions can improve the fitting accuracy of B-spline trajectories to local geometry, making obstacle avoidance trajectories smoother.

[0043] In this embodiment, the expression is used to automatically increase the density of control points in high curvature regions to improve the local expressive power of the trajectory, and to automatically reduce the density of control points in low curvature regions to reduce the computational complexity of optimization.

[0044] Furthermore, the trajectory is divided into regions based on the normalized curvature of the smooth turning trajectory, namely, a high curvature region and a low curvature region.

[0045] In this embodiment, the adaptive adjustment of the UAV's flight speed is constructed using the following function: in, This indicates the flight speed corresponding to the current trajectory point of the drone; For maximum flight speed, The velocity attenuation coefficient, For curvature.

[0046] In this embodiment, the expression is used to automatically reduce flight speed in high curvature regions and automatically increase flight speed in low curvature regions. The nonlinear mapping relationship between curvature and flight speed is as follows: Figure 6 As shown, Figure 6 The key mapping curves of this method depict the nonlinear relationships between trajectory curvature and flight speed, control point distribution density, and obstacle repulsion force.

[0047] The curvature-velocity curve illustrates how the UAV's flight speed adaptively adjusts to changes in the local curvature of its trajectory. When the curvature approaches zero, i.e., when the UAV is on a straight or gentle path, the flight speed remains at the maximum permissible value to ensure mission efficiency. As the curvature gradually increases, the flight speed begins to decrease according to an exponential decay law; when the curvature enters a higher range (sharp turn region), the speed rapidly decreases and tends towards a lower safe saturation value. The physical essence of this design lies in the fact that the smaller the turning radius and the greater the curvature, the greater the centripetal acceleration experienced by the UAV. Active deceleration is necessary to avoid exceeding the flight dynamics limits, thereby ensuring a smooth and safe turning process.

[0048] The curvature-control point density curve depicts the trend of control point distribution density in a B-spline trajectory as curvature increases. In low-curvature regions with straight lines or gentle curves, control points maintain only a basic density to avoid computational redundancy. When the curvature rises to a moderate level, the control point density begins to increase linearly, as more curved trajectories require more control points to accurately fit their geometry. In sharp-turn regions with high curvature, the control point density increases significantly. The dense control points give the B-spline curve sufficient local adjustment capability, enabling the trajectory to smoothly and accurately traverse complex curved paths. This on-demand allocation strategy concentrates computational resources on the most complex segments of the trajectory without significantly increasing the overall number of control points, achieving a balance between representational accuracy and computational efficiency.

[0049] The curvature-obstacle repulsion force relationship curve uses a Sigmoid-type nonlinear function to describe the change in repulsion force. When the curvature is small (straight lines or gentle curves), the obstacle repulsion force remains at a relatively weak level, avoiding unnecessary detours and maintaining a simple and efficient path. When the curvature exceeds a certain threshold and enters a turning area, the repulsion force begins to increase rapidly, forcing the UAV to actively maintain a greater safe distance from obstacles during the turn. When the curvature further increases to a very high level (extreme sharp curves), the repulsion force tends to saturate. This prevents excessive trajectory distortion or optimization oscillations due to excessive repulsion force, while also ensuring numerical stability. This curvature-driven repulsion force enhancement mechanism, in conjunction with the aforementioned deceleration and increased control points, jointly ensures the obstacle avoidance safety and trajectory smoothness of the UAV in complex turning environments. This fundamentally solves the problem of poor performance of traditional fixed-parameter methods in sharp turning areas.

[0050] when →0, → The drone maintains a high flight speed on straight paths or in areas with low curvature; when As the speed increases, it gradually decreases according to an exponential law, thereby improving the stability and safety of the drone in sharp turning areas. parameter Used to adjust the sensitivity of speed to changes in curvature. The larger the value, the faster the speed decays.

[0051] The objective function for trajectory optimization in this embodiment includes: Curvature-adaptive trajectory smoothness cost, obstacle avoidance cost, and dynamic constraint cost; The curvature adaptive trajectory smoothness cost term is used to reduce the degree of abrupt changes in the trajectory during changes in position, velocity, and acceleration. The curvature adaptive obstacle avoidance cost term is used to increase the safe distance between the trajectory and the obstacle, thereby improving the obstacle avoidance safety margin. The curvature adaptive dynamics constraint cost term is used to limit the dynamic parameters of speed, acceleration and angular velocity during the flight of the UAV, thereby improving the smoothness of the trajectory in high curvature regions. The formula for calculating the trajectory optimization objective function is as follows: in, For curvature adaptive trajectory smoothness cost term, For curvature adaptive obstacle avoidance cost term, The cost term for curvature adaptive dynamic constraints. Let be the local curvature of the trajectory.

[0052] Among them, the curvature adaptive trajectory smoothness cost term, obstacle avoidance cost term, and dynamic constraint cost term correspond to the curvature-repulsion force mapping, curvature-velocity mapping, and curvature-control point density mapping, respectively, and are used to achieve collaborative optimization under the same optimization framework, rather than repeatedly applying independent constraints.

[0053] The trajectory smoothness cost term is constructed using the second-order difference of control points to reduce the abrupt changes in trajectory position, velocity, and acceleration. In high curvature regions, the smoothness constraint strength is enhanced by the curvature coefficient, thereby improving trajectory continuity.

[0054] The obstacle avoidance cost is constructed based on the distance between the trajectory point and the obstacle. The cost increases rapidly when the trajectory gets closer to the obstacle. At the same time, the obstacle avoidance cost is further enhanced in high curvature areas to improve the safety margin in turning areas.

[0055] The dynamic constraint cost term is used to limit the speed and acceleration of the UAV during flight. When the trajectory speed or acceleration exceeds the system's allowable range, a penalty term is introduced. In the high curvature region, the speed constraint is further enhanced by the curvature adjustment coefficient, thereby reducing the flight speed in the sharp turn region and improving flight stability.

[0056] In this embodiment, the objective function is solved iteratively using the gradient descent method.

[0057] In this embodiment, the trajectory optimization solution is based on B-spline curve representation. The trajectory is parameterized by a set of control points, and the B-spline curve expression is as follows: in, Indicates parameters The corresponding trajectory location points; Indicates the number of control points; This indicates the control point index; Indicates the order of the B-spline curve; To indicate the first One trajectory control point, To indicate the first indivual B-order spline basis functions.

[0058] like Figure 4 As shown, Figure 4 This is a schematic diagram comparing the distribution of control points in high-curvature and low-curvature regions; in this embodiment, the step of dynamically adjusting the distribution density of trajectory control points based on the local curvature of the smooth turning trajectory employs a segmented optimization strategy: The trajectory is divided into high curvature region and low curvature region based on at least one preset or dynamically calculated curvature threshold (which can be further divided into multiple regions of high, medium and low). In the high curvature region, a first sampling interval is used to increase the number of control points, and in the low curvature region, a second sampling interval is used to reduce the number of control points, wherein the first sampling interval is smaller than the second sampling interval; the curvature threshold is determined based on the maximum permissible centripetal acceleration, minimum turning radius, or trajectory curvature statistical characteristics of the UAV, and can be a fixed value or an online dynamically calculated value.

[0059] like Figure 2 As shown, Figure 2 The diagram shows a module of an online UAV trajectory optimization system based on curvature-driven local enhancement expression. This embodiment provides an online UAV trajectory optimization system based on curvature-driven local enhancement expression, comprising: The map building module is configured to build an occupancy grid map and a Euclidean distance field map of the environment based on sensor data. The path search module is configured to search for a collision-free geometric path from the start point to the end point in the occupied grid map; The curvature calculation module is configured to discretize the geometric path and calculate the curvature information of each discrete point; An obstacle repulsion force adaptive module is configured to adaptively adjust the obstacle repulsion force based on the curvature information and the correspondence between curvature and obstacle repulsion force. The speed adaptive module is configured to adaptively adjust the flight speed of the UAV based on the curvature information and the correspondence between curvature and flight speed. The control point adaptive module is configured to adaptively adjust the distribution density of trajectory control points based on the curvature information and the correspondence between curvature and the distribution density of trajectory control points. The trajectory optimization module is configured to represent the trajectory using B-spline curves, construct an objective function that includes smoothness, obstacle avoidance, dynamics, and curvature constraints, and use the gradient descent method to iteratively optimize the control points to generate the optimal flight trajectory that satisfies the kinematic constraints of the UAV. The control execution module is configured to send the optimal flight trajectory to the UAV flight control system for execution.

[0060] like Figure 5 As shown, Figure 5 This is a schematic diagram illustrating the effect of curvature enhancing the repulsive force of obstacles. When a drone's trajectory approaches an obstacle, the system adaptively enhances the obstacle repulsion force based on the local curvature of the trajectory, thereby actively moving the trajectory away from the obstacle and forming a smooth turning trajectory. The dashed area around the obstacle represents the safety margin range; the solid black line represents the optimized smooth trajectory; and arrows of varying lengths around the trajectory indicate the direction and intensity of the obstacle repulsion force, pointing outwards from the obstacle, with the arrow length representing the magnitude of the repulsion force. In low-curvature areas, the obstacle repulsion force is weak, requiring only slight adjustments to the trajectory; in high-curvature turning areas, the system enhances the obstacle repulsion effect based on curvature information, actively increasing the safe distance between the trajectory and the obstacle while reducing the risk of collisions caused by sharp turns. This curvature-enhanced obstacle repulsion mechanism improves the drone's obstacle avoidance safety and flightability in complex environments while maintaining trajectory continuity and smoothness.

[0061] To verify the effectiveness of this method, its trajectory performance is compared with that of traditional methods that do not employ curvature-driven methods. Figure 7 As shown, Figure 7To compare the trajectory performance of this method with traditional methods, a comparison of trajectory geometry (planar view) and control point distribution (along the trajectory) is presented. The control point distribution of the two methods is compared using trajectory arc length or sampling point number as the x-axis and control point density or control point interval as the y-axis (or by directly marking the position and number of control points on the trajectory). This method automatically increases the control point density in high-curvature areas, making the trajectory closer to the ideal turning path while maintaining a greater safe distance from obstacles. In contrast, traditional methods have sparse control points in sharp turning areas, resulting in poor trajectory smoothness and a tendency to closely follow obstacles. The comparison results show that the method of this invention has significant advantages in trajectory smoothness, obstacle avoidance safety, and the rationality of control point distribution.

[0062] Example 2

[0063] See appendix Figure 3 , Figure 3 This diagram illustrates the implementation process of a curvature-driven online UAV trajectory optimization method. After inputting task requirements and environmental information, the trajectory is first discretized and its local curvature is calculated. Then, based on the curvature magnitude, the trajectory is divided into high, medium, and low regions, and obstacle repulsion force, flight speed, and control point density are adaptively adjusted accordingly. Next, curvature and dynamic constraints are introduced under B-spline representation to construct a multi-objective optimization problem encompassing trajectory length / time, energy consumption, and smoothness. Finally, a feasible trajectory is output, and closed-loop tracking is performed. This method achieves adaptive adjustment of flight speed and control point distribution by introducing trajectory curvature information and completes trajectory generation and online updating within a B-spline optimization framework. The specific implementation process is as follows: First, the system inputs task requirements information, including the starting point, the destination, and environmental information, and generates an initial reference trajectory based on global path planning or prior trajectory.

[0064] The initial trajectory is discretized to obtain a set of discrete trajectory points. Local curvature information is calculated based on the trajectory's geometric characteristics.

[0065] Furthermore, the trajectory is divided into three feature regions based on the magnitude of curvature: When curvature When the region is identified as a high curvature area, the strength of the obstacle repulsion force is increased, the flight speed is reduced, and the density of control point distribution is increased. when When the region is identified as having moderate curvature, maintain a moderate flight speed, and use the basic configuration for obstacle repulsion force and control point density. when When the region is identified as a low curvature region, the control point density is reduced and the flight speed is increased to reduce computational redundancy and improve flight efficiency.

[0066] in, For high curvature threshold, The threshold values ​​are for medium curvature. Both can be preset or dynamically calculated online based on the current state of the UAV (such as the maximum permissible centripetal acceleration) or the trajectory curvature distribution (such as percentiles).

[0067] Furthermore, the trajectory curvature is approximated using the discrete difference method, and the calculation process is as follows: in, This represents the coordinate position of the i-th discrete point on the trajectory.

[0068] Indicates the first Local curvature values ​​at discrete points on the trajectory; Indicates the first +1 coordinate positions of discrete points on the trajectory; After calculating the trajectory curvature sequence, in order to avoid curvature abrupt changes caused by path discrete errors or sensor noise, the curvature sequence is subjected to sliding window filtering. The curvature value of each discrete point is taken as the weighted average of multiple points in its neighborhood, thus obtaining the smoothed curvature distribution curve.

[0069] Furthermore, a curvature-based obstacle repulsion force enhancement mechanism is introduced. The obstacle repulsion strength is dynamically adjusted through the curvature coefficient. The coupling relationship between curvature and obstacle repulsion force is constructed according to the following formula, and the obstacle repulsion strength is dynamically adjusted through curvature: in, The repulsive force of the obstacle after the curvature is enhanced. Based on the repulsive force of the basic obstacle, To enhance the weighting coefficient of repulsive force, It is a non-linear adjustment index. The curvature enhancement threshold.

[0070] Furthermore, the mapping relationship between curvature and flight speed is constructed using the following functional form: in, For maximum flight speed, The velocity attenuation coefficient, For curvature.

[0071] When the trajectory curvature is small, the drone speed is close to its maximum speed; when the curvature increases, the speed decreases accordingly, thereby avoiding excessive centrifugal force in the drone in sharp turning areas and improving flight safety.

[0072] Furthermore, regarding the adaptive distribution of control points, the mapping relationship between the number of control points and curvature is established as follows: in, This represents the number of control points assigned to the i-th trajectory segment. The number of basic control points, To adjust the parameters.

[0073] Based on the above relationship, more control points are allocated in areas with greater trajectory curvature to improve the accuracy of trajectory representation; the number of control points is reduced in areas with less curvature, thereby reducing the computational complexity of optimization and improving the overall planning efficiency.

[0074] Furthermore, during trajectory segmentation, based on the curvature threshold... The trajectory is partitioned into high-curvature regions and low-curvature regions: when At that time, it was determined to be a high curvature region; when At that time, it was determined to be a low curvature region.

[0075] Curvature threshold The curvature threshold can be determined based on the UAV's minimum turning radius, its own dynamic constraints, or the statistical characteristics of its trajectory curvature. Preferably, the curvature threshold satisfies: in, This is the minimum allowed turning radius for the drone. When the trajectory curvature exceeds this threshold, it is identified as a high curvature region, and the control point density is increased to improve trajectory representation accuracy.

[0076] In areas with high curvature, a denser sampling method is adopted, and a local refinement strategy is introduced to interpolate and supplement the trajectory. Sparse sampling is used in low curvature regions to achieve adaptive representation of the overall trajectory.

[0077] Furthermore, B-spline curves are used to model the trajectory, and its expression is as follows: in, For the control point sequence, It is a k-order B-spline basis function.

[0078] Based on B-spline representation, the trajectory has good continuity and local controllability, making it suitable for UAV trajectory optimization problems.

[0079] Furthermore, the objective function for trajectory optimization is constructed as follows: in, For curvature adaptive trajectory smoothness cost term, For curvature adaptive obstacle avoidance cost term, The cost term for curvature adaptive dynamic constraints. Let be the local curvature of the trajectory.

[0080] Furthermore, the curvature constraint is defined as: Indicates curvature constraint; Indicates the index of discrete points on the trajectory; Indicates the maximum curvature threshold that can be executed; This constraint ensures that the trajectory curvature does not exceed the maximum curvature threshold that the UAV can execute. To avoid generating non-flying trajectories.

[0081] Furthermore, dynamic constraints include velocity constraints and acceleration constraints, which take the following forms: , , ; Indicates flight speed; Indicates flight acceleration; Indicates angular velocity of flight; Indicates maximum speed; Indicates the maximum acceleration; Represents the maximum angular velocity During the optimization process, when the trajectory violates the constraints, the objective function is adjusted through a penalty term to make the optimization result satisfy the constraints.

[0082] Furthermore, during the optimization process, the gradient descent method is used to iteratively update the control points, and the update formula is as follows: in, The learning rate parameter controls the update magnitude in each iteration.

[0083] Indicates the first After the first iteration update One control point; Indicates the first The first iteration One control point; This represents the objective function for trajectory optimization. Indicates the first One trajectory control point variable; In practical engineering implementation, quadratic programming can also be used to solve the objective function to improve convergence speed and stability.

[0084] Furthermore, in dynamic environments, when obstacles or environmental changes are detected, the local trajectory can be replanned in real time, and only the affected area is locally optimized, thereby ensuring the real-time performance of the algorithm.

[0085] Furthermore, the method of the present invention can be integrated with existing trajectory planning frameworks, such as combining with search-based methods or optimization-based methods, to improve the overall performance of the system.

[0086] Finally, the above steps generate a UAV flight trajectory that satisfies continuity, smoothness, and dynamic constraints, and output it to the flight control system for execution.

[0087] The above-described embodiments are merely preferred embodiments provided to fully illustrate the present invention, and the scope of protection of the present invention is not limited thereto. Equivalent substitutions or modifications made by those skilled in the art based on the present invention are all within the scope of protection of the present invention. The scope of protection of the present invention is defined by the claims.

Claims

1. An online optimization method for UAV trajectory based on curvature-driven local enhancement representation, characterized in that, Includes the following steps: The trajectory is discretized according to the UAV mission path, the curvature information of each discrete point of the trajectory is calculated, and a geometric feature model of the trajectory is constructed based on the curvature distribution. Based on the curvature information, an obstacle avoidance cost term with curvature adaptation is set in the trajectory optimization objective function, so that the trajectory can form an obstacle avoidance effect that maintains a safe distance from obstacles in the high curvature region; Before trajectory optimization, the obstacle repulsion force enhancement coefficient, the basic control point distribution density, and the initial flight speed limit are adaptively set. Based on the B-spline trajectory representation, an objective function is constructed that includes a curvature adaptive smoothness cost term, an obstacle avoidance cost term, and a dynamic constraint cost term, and the first round of trajectory optimization is solved. The precise curvature distribution of the optimized B-spline trajectory is calculated analytically. Based on this precise curvature, the distribution density of trajectory control points in the high curvature region is adjusted again, and the expected flight speed of the UAV in the turning section is reduced accordingly. Using the adjusted control point density and speed constraints, a second round of local optimization is performed on the trajectory, or the optimal flight trajectory that satisfies the kinematic constraints of the UAV is directly generated.

2. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, When calculating the curvature information of discrete points on the trajectory, the discrete difference method is used for approximate solution, and the curvature is calculated based on the first and second derivatives between adjacent discrete points. The expression for curvature is: in, Indicates the curvature of the trajectory; It represents the first derivative along the x-axis between discrete points of the trajectory; It represents the first derivative in the y-axis direction between discrete points of the trajectory; It represents the second derivative along the x-axis between discrete points of the trajectory; It represents the second derivative along the y-axis between discrete points on the trajectory; The calculated curvature sequence is then smoothed and filtered; the curvature information is calculated using the discrete difference method for discrete points and the analytical derivative method for B-spline trajectories.

3. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, The coupling relationship between curvature and obstacle repulsion force is constructed according to the following formula, and the obstacle repulsion strength is dynamically adjusted by curvature: in, The repulsive force of the obstacle after the curvature is enhanced. Based on the repulsive force of the basic obstacle, To enhance the weighting coefficient of repulsive force, It is a non-linear adjustment index. Let be the curvature at the trajectory point. The curvature enhancement threshold.

4. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, The relationship between curvature and flight speed is used to adaptively adjust the UAV's flight speed based on the trajectory curvature, and is constructed using the following function: in, For maximum flight speed, The velocity attenuation coefficient, For curvature.

5. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, The correspondence between curvature and the number of control points is used to adaptively adjust the distribution density of trajectory control points according to the trajectory curvature, and is constructed using the following function form: in, This represents the number of control points assigned to the i-th trajectory segment. The number of basic control points, To adjust the parameters.

6. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, The objective function for trajectory optimization includes a curvature-adaptive trajectory smoothness cost term, an obstacle avoidance cost term, and a dynamic constraint cost term. The curvature adaptive trajectory smoothness cost term is used to reduce the degree of abrupt changes in the trajectory during changes in position, velocity, and acceleration. The curvature adaptive obstacle avoidance cost term is used to increase the safe distance between the trajectory and the obstacle, thereby improving the obstacle avoidance safety margin. The curvature adaptive dynamics constraint cost term is used to limit the dynamic parameters of speed, acceleration and angular velocity during the flight of the UAV, thereby improving the smoothness of the trajectory in high curvature regions. The formula for calculating the trajectory optimization objective function is as follows: in, For curvature adaptive trajectory smoothness cost term, For curvature adaptive obstacle avoidance cost term, The cost term for curvature adaptive dynamic constraints. Let be the local curvature of the trajectory.

7. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 6, characterized in that, The trajectory optimization solution is based on B-spline curve representation. The trajectory is parameterized by a set of control points, and the B-spline curve representation is as follows: in, As control points, It is a k-order B-spline basis function.

8. The online optimization method for UAV trajectory based on curvature-driven local enhancement expression according to claim 1, characterized in that, In the step of dynamically adjusting the distribution density of trajectory control points based on the local curvature of the smooth turning trajectory, a segmented optimization strategy is adopted: The trajectory is divided into high curvature regions and low curvature regions based on at least one preset or dynamically calculated curvature threshold. In the high curvature region, a first sampling interval is used to increase the number of control points. A second sampling interval is used in the low curvature region to reduce the number of control points. The first sampling interval is smaller than the second sampling interval; the curvature threshold is determined based on the maximum permissible centripetal acceleration, minimum turning radius, or trajectory curvature statistical characteristics of the UAV.

9. An online trajectory optimization system for unmanned aerial vehicles (UAVs) based on curvature-driven local enhancement representation, characterized in that, include: The map building module is configured to build an occupancy grid map and a Euclidean distance field map of the environment based on sensor data. The path search module is configured to search for a collision-free geometric path from the start point to the end point in the occupied grid map; The curvature calculation module is configured to discretize the geometric path and calculate the curvature information of each discrete point; An obstacle repulsion force adaptive module is configured to adaptively adjust the obstacle repulsion force based on the curvature information and the correspondence between curvature and obstacle repulsion force. The speed adaptive module is configured to adaptively adjust the flight speed of the UAV based on the curvature information and the correspondence between curvature and flight speed. The control point adaptive module is configured to adaptively adjust the distribution density of trajectory control points based on the curvature information and the correspondence between curvature and the distribution density of trajectory control points. The trajectory optimization module is configured to use B-spline curves to parametrically represent the trajectory, construct an objective function consisting of trajectory smoothness cost, obstacle avoidance cost, and dynamic constraint cost based on adaptive adjustment of trajectory local curvature, and use gradient descent method to iteratively optimize the control points to generate the optimal flight trajectory that satisfies the kinematic constraints of the UAV. The control execution module is configured to send the optimal flight trajectory to the UAV flight control system for execution.

Images