Unmanned aerial vehicle trajectory generation method based on geometric smoothing and multi-dimensional dynamics constraints

Abstract

This invention discloses a UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints. The method first resamples the input discrete waypoints by arc length; secondly, it identifies path corners through corner detection, constructs local transition curves using a Hermite interpolation model, and ensures first-order geometric continuity and suppresses trajectory convexity through tangential vector constraints; subsequently, it performs global fitting using cubic spline functions based on the local transitions to achieve acceleration continuity across the entire trajectory, and introduces a safe corridor mechanism to verify fitting deviations; finally, it generates a time-parameterized velocity profile by combining path curvature, vertical slope, and yaw rate coupling constraints. This invention effectively solves the problems of corner overshoot, splicing discontinuities, and yaw instability in traditional trajectory generation methods, improving the flight accuracy and stability of UAVs in complex environments.

Description

Technical Field

[0001] This invention relates to the field of mobile robot path planning and control technology, and in particular to a method for generating UAV trajectories based on hybrid geometric smoothing and multidimensional dynamic constraints. Background Technology

[0002] With the rapid development of drone technology, its applications in fields such as power line inspection, low-altitude logistics, security monitoring, and aerial film and television photography are becoming increasingly widespread. In these tasks, drones typically need to generate a continuous, smooth, and dynamically feasible flight trajectory based on a pre-given sequence of discrete waypoints. High-quality trajectory generation algorithms can not only improve flight efficiency but also effectively extend motor life and ensure the stability of onboard payloads (such as gimbal cameras).

[0003] Existing UAV trajectory generation methods are mainly divided into two categories: geometric interpolation methods and optimization-based methods. However, they still have the following significant drawbacks in practical applications: Limitations of geometric smoothing: Traditional pure polynomial interpolation (such as cubic splines and B-splines) is prone to the "Runge phenomenon" when dealing with sparse waypoints or sharp turns, causing significant overshoot or oscillations in the trajectory at turning points. This not only deviates from the preset safety corridor but may even lead to collisions. Although Bézier curves have good convex hull properties, their control over specific waypoints is weak, making it difficult to accurately fit the preset route.

[0004] The singularity of dynamic constraints: Existing time parameterization methods typically only consider constraints on linear velocity, linear acceleration, and path curvature. However, for quadcopter UAVs, yaw and translation are highly coupled. In scenarios with high-speed flight and drastic changes in heading, if linear velocity is limited only by curvature while ignoring the physical limit of yaw angular velocity, it often leads to "tail-swinging" during turns or the UAV failing to align its nose with the target due to yaw response lag, severely impacting the quality of inspection and imaging and flight stability.

[0005] The contradiction between computational efficiency and safety: Although optimization-based methods (such as the Minimum Snap algorithm) can generate extremely smooth trajectories, their computational complexity is high, and it is difficult to strictly guarantee that the trajectory always lies within the safe corridor in non-convex environments. They usually require time-consuming iterative solutions, which is difficult to meet the real-time replanning requirements of airborne computers.

[0006] Therefore, there is an urgent need for a lightweight trajectory generation method that can effectively suppress corner overshoot, ensure first-order geometric continuity, and tightly couple yaw dynamics with linear motion, in order to solve the above-mentioned technical bottlenecks. Summary of the Invention

[0007] To overcome the problems of corner overshoot, geometric discontinuity, decoupling of yaw and linear motion constraints, and insufficient computational efficiency in existing technologies, this invention provides a UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints. This method uses corner detection and Hermite transition curve parameterization to analyze path turning features, and achieves the full trajectory through global natural cubic spline fitting. 2 Continuity is achieved by verifying geometric deviations through a safety corridor mechanism and coupling the triple constraints of path curvature, vertical slope, and yaw rate into the velocity profile generation, thereby obtaining a geometrically smooth, dynamically feasible, and safe and controllable time-parameterized flight trajectory.

[0008] The technical solution adopted by this invention to solve its technical problem is: In a first aspect, the present invention provides a method for generating UAV trajectories based on hybrid geometric smoothing and multidimensional dynamic constraints, comprising the following steps: S1. Obtain the discrete waypoint sequence of the UAV and resample it based on arc length to obtain equally spaced pathpoints; S2. Perform hybrid geometric smoothing on the equally spaced path points to generate a reference path; The hybrid geometric smoothing process includes: identifying path corners and constructing local transition curves that meet the tangential continuity requirement at each corner; performing global spline fitting on non-turning road segments in the path, and fusing the local transition curves with the globally spline-fitted path to form a reference path; and performing safety corridor constraint verification on the generated reference path. S3. Perform heading angle processing on the reference path to generate a continuous and smooth heading angle sequence; S4. Based on the geometric features and heading angle sequence of the reference path, a time-parameterized velocity profile is generated in combination with preset multidimensional dynamic coupling constraints, and then a flight trajectory containing position, velocity, acceleration, heading angle and heading angular velocity is generated and output; wherein, the multidimensional dynamic coupling constraints include at least three types of constraints: path curvature, vertical slope and yaw angular velocity, to jointly restrict the linear velocity.

[0009] Preferably, the specific method for identifying path corners and constructing local transition curves that satisfy tangential continuity at the corners in step S2 is as follows: Calculate the angle between the direction vectors of adjacent road segments at each path point in the path, and determine a corner when the angle exceeds a preset corner threshold. The Hermite interpolation model is used to construct a transition curve connecting the road segments before and after a corner. By constraining the tangential vectors at the endpoints of the transition curves to be collinear with the direction vectors of adjacent road segments, the geometric first-order continuity of the path at the corner is achieved and the outward convexity of the trajectory is suppressed.

[0010] Preferably, the specific method for global spline fitting in step S2 is as follows: based on the determination of the local transition curve, the remaining road segments are fitted using a piecewise cubic spline function to ensure the continuity of the reference path acceleration.

[0011] Preferably, the safety corridor constraint verification in step S2 includes the following steps: Construct a pipeline-type safety corridor centered on the original discrete waypoint connection and with a preset safety distance as the radius; Calculate the shortest distance from the reference path to the original discrete waypoints. If the maximum value of this shortest distance exceeds the preset safety distance, iterative refitting is performed by adjusting the resampling interval or local transition length parameter until the path deviation meets the safety corridor constraint.

[0012] Preferably, the heading angle processing in step S3 includes the following steps: Dewinding is performed on the heading angle on the reference path to eliminate angle jumps; Smooth the dewound heading angle sequence; An integral limiter is used to shape the rate of change of heading angle to ensure that the rate of change of the output heading angle with respect to arc length does not exceed the preset maximum yaw rate threshold.

[0013] Preferably, when generating the time-parameterized velocity profile in step S4, the reference path is... The upper limit of the synthesized horizontal velocity at each arc-length sampling point Determined by the following formula:

[0014] In the formula, The global maximum horizontal speed, For maximum vertical velocity, For maximum normal acceleration, Maximum yaw rate; It is a vertical slope. For path curvature, The derivative of the heading angle with respect to the arc length; For small positive numbers that are numerically stable.

[0015] Preferably, the generation of time-parameterized velocity profiles in step S4 further includes: Based on the aforementioned upper limit of the composite horizontal speed With the maximum tangential acceleration constraint, forward acceleration scan and backward deceleration scan are performed on the reference path respectively to obtain the forward scan velocity curve and the backward scan velocity curve; The two obtained velocity curves are processed by taking the minimum value point by point to obtain the final planned velocity of each point on the path; the path is integrated over time based on the final planned velocity to obtain the timestamps corresponding to each point on the trajectory, thus completing the time parameterization of the trajectory.

[0016] Preferably, it also includes an end convergence control step: when the Euclidean distance between the current position of the UAV and the target endpoint is less than a preset distance threshold, the upper limit of the flight speed is dynamically reduced according to the remaining flight distance of the UAV to the target endpoint, and the heading angle of the UAV is locked to the preset heading angle of the target endpoint. This dynamic speed reduction and heading lock control state is maintained until the UAV reaches the target endpoint.

[0017] In a second aspect, the present invention also provides an electronic device including a processor and a memory, the memory being used to store computer program code, the computer program code including computer instructions, wherein when the processor executes the computer instructions, the electronic device performs a method as described in the first aspect above and any possible implementation thereof.

[0018] Thirdly, the present invention also provides a computer-readable storage medium storing a computer program, the computer program including program instructions that, when executed by a processor of an electronic device, cause the processor to perform a method as described in the first aspect above and any possible implementation thereof.

[0019] The positive effects of this invention: By employing a technical solution that combines hybrid geometric smoothing with multidimensional dynamic coupling constraints, this invention achieves a comprehensive improvement in the performance of trajectory generation in terms of geometric smoothness, dynamic feasibility, flight safety, and computational efficiency. Specific positive effects are as follows: (1) Effectively suppresses outward convexity overshoot at path corners and avoids the risk of collisions during turns: This invention first accurately identifies sharp turning points on the path through corner detection, constructs a Hermite transition curve for each turning point, and uses tangential vector direction constraints to ensure that the transition curve is inside the region formed by the incident and outgoing directions, structurally avoiding outward convexity at corners; then, through global natural cubic spline fitting, all resampled points containing the corner transition region are smoothly connected, ensuring that the trajectory transition at the turning point is without abrupt changes. This dual smoothing mechanism significantly reduces the large outward convexity overshoot problem at sharp turns in traditional methods, ensuring that the UAV always stays within the preset safe path range during the turning process, and reducing the risk of collisions with obstacles during turns.

[0020] (2) Achieving high-order geometric continuity of the trajectory significantly reduces aircraft mechanical losses and vibrations: The geometric continuity of the trajectory directly determines the stability of UAV flight. Trajectories generated by traditional methods are prone to sudden acceleration changes, leading to severe mechanical vibrations, accelerated motor wear, and affecting the stability of airborne loads. This invention achieves high-order geometric continuity of the trajectory by independently constructing global natural cubic spline curves for the x, y, and z three-dimensional coordinate components of the path. 2 Level geometric continuity means that the position, first derivative (velocity), and second derivative (acceleration) at all points along the trajectory are continuous and without abrupt changes. When the UAV flies along this trajectory, the power system does not need frequent acceleration, deceleration, or changes of direction, significantly reducing mechanical vibration of the fuselage and wear on transmission components such as motors and bearings. This not only improves flight stability but also extends the service life of the UAV's power system. At the same time, the absence of abrupt acceleration ensures that onboard gimbal cameras, inspection sensors, and other payloads remain in a stable working state, avoiding measurement and shooting errors caused by fuselage vibration.

[0021] (3) By constructing a full-dimensional safety corridor deviation verification system, a closed-loop control of trajectory "generation-verification-decision" is achieved: This invention is based on a safety corridor constraint verification mechanism of point-to-polyline segment distance. A pipeline-type safety corridor is constructed with the original discrete waypoint connection as the center and a preset safety distance as the radius. The fitted reference path is sampled and verified on the whole path: the shortest distance from the sampling point to the original waypoint polyline is calculated point by point, the trajectory geometric deviation is comprehensively measured, and three core statistical quantities, namely maximum deviation, mean deviation, and 95th percentile deviation, are output. At the same time, a corridor convergence flag is generated. The upper-level task planning system can directly determine the safety and availability of the trajectory based on the deviation statistics and convergence flag, realizing a closed-loop control of trajectory from generation, verification to decision, making the safety of trajectory flight quantifiable and measurable, and ensuring that the UAV always flies within a safe range from a technical perspective.

[0022] (4) Deep coupling of yaw motion and linear motion solves the "tail-flicking" problem of traditional methods: This invention constrains the yaw angular velocity A velocity profile generation process is introduced to achieve deep coupling between yaw motion and linear motion, wherein... This represents the upper bound of the velocity under yaw constraints. Yaw angular velocity, This represents the absolute value of the rate of change of yaw angle with respect to arc length; when the UAV flies to a section of road where the heading changes drastically, As the value increases, The automatic reduction forces the drone's linear velocity to decrease synchronously, ensuring that the yaw rate never exceeds the physical limit. This design allows the drone's linear velocity to match its yaw and turning speed in real time, effectively mitigating the "tail-flicking" problem caused by the lag in yaw response in traditional methods, and ensuring that the drone's nose is always precisely aligned with the target during turns and changes of direction.

[0023] (5) The core algorithm has a linear / quasi-linear time complexity, high computational efficiency, and can adapt to the online real-time replanning requirements of airborne embedded computers: This invention has a lightweight design for the core algorithm, and the algorithms of all key steps have a linear or quasi-linear time complexity—the Thomas algorithm is adopted. Solving the tridiagonal system of equations for cubic splines has a time complexity of O(n log n). The velocity profile is generated through a double forward-reverse scan, with a time complexity of O(n log n). (in (This indicates that the computational complexity increases linearly with the number of sampling points n). The entire trajectory generation process requires no iterative optimization and does not rely on complex convex programming solvers, resulting in extremely low computational overhead. Trajectory generation and replanning can be quickly completed on an embedded computer on the UAV. Even in complex obstacle environments where waypoints need to be adjusted in real time, this invention can quickly generate new feasible trajectories, adapting to real-world application scenarios of online, real-time, and rapid replanning during UAV flight, thus improving the adaptability and maneuverability of UAVs in complex environments.

[0024] (6) Smooth and controllable terminal convergence ensures that the UAV hovers smoothly at zero speed and accurately aligns its course at the target point, avoiding terminal oscillations: Traditional methods are prone to terminal oscillations, such as sudden speed drops and repeated course adjustments, when the UAV approaches the target endpoint, causing the UAV to be unable to hover smoothly or even deviate from the endpoint position. This invention designs an integrated terminal convergence control strategy of smooth step function + convergence state locking + yaw angle locking: when the UAV approaches the target endpoint, based on the normalized remaining distance Through a smooth step function Achieving speed Level continuous decay ( The continuous representation of the velocity curve's function value and first derivative ensures continuity throughout, preventing fuselage vibration caused by sudden velocity changes. Simultaneously, it locks the convergence state, ensuring that once the velocity enters the decay process, it continuously decreases without reverse fluctuations. Combined with a target point yaw angle locking mechanism, the UAV's heading angle is fixed to the preset heading angle at the endpoint, eliminating repeated adjustments to the heading at the terminal point. These three control strategies work together to ensure the UAV achieves a smooth, zero-speed hover at the target point, with its heading always precisely aligned with the preset target. This avoids oscillations in terminal velocity and yaw, improving the reliability of UAV point-to-point operations and precise landings.

[0025] (7) Possesses complete trajectory quality self-assessment capabilities, providing multi-dimensional and quantitative trajectory credibility decision-making basis for upper-level systems: During the trajectory generation process, this invention can simultaneously output a multi-dimensional and fully quantitative trajectory quality assessment report. The assessment dimensions comprehensively cover all key aspects of trajectory generation, including geometric deviation, curvature quality, velocity profile, yaw constraint ratio, and terminal convergence status. This assessment report can provide upper-level task planning systems with quantitative, traceable, and comparable trajectory credibility decision-making basis. Attached Figure Description

[0026] Figure 1 This is a schematic diagram of the overall process of the trajectory generation method of the present invention; Figure 2 This is a schematic diagram of the overall process of the hybrid geometric smoothing algorithm of the present invention; Figure 3 This is a schematic diagram of the multidimensional dynamics constraint and velocity planning process of this invention; Figure 4 This is a Hermite transition curve diagram at the corner in the embodiment; Figure 5 This is a comparison diagram of the fitted trajectory and the original path in the embodiment; Figure 6 This is a velocity profile diagram from the embodiment; Figure 7 This is a diagram of the terminal convergence rate in the embodiment; Figure 8 This is the yaw angle trajectory diagram in the embodiment. Detailed Implementation

[0027] To enable those skilled in the art to better understand the present invention, the technical solutions of the embodiments of the present invention will be clearly and completely described below with reference to the accompanying drawings. Obviously, the described embodiments are only some embodiments of the present invention, and not all embodiments. Based on the embodiments of the present invention, all other embodiments obtained by those skilled in the art without creative effort are within the scope of protection of the present invention.

[0028] In this document, the term "embodiment" means that a particular feature, structure, or characteristic described in connection with an embodiment may be included in at least one embodiment of the invention. The appearance of this phrase in various places throughout the specification does not necessarily refer to the same embodiment, nor is it a separate or alternative embodiment mutually exclusive with other embodiments. It will be explicitly and implicitly understood by those skilled in the art that the embodiments described herein can be combined with other embodiments.

[0029] Specifically, this embodiment provides a UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints, deployed in a quadcopter UAV flight control system built on ROS (Robot Operating System). This system uses the ENU (East-North-Up) coordinate system to calculate the UAV's spatial position and motion state. The core motion state of the UAV includes three-dimensional position. (Unit: m) and yaw angle (Unit: rad). The independent trajectory generation module in the system receives the discrete waypoint sequence output by the upper-level path planner as input data. After processing by the method described in this invention, it outputs the time-parameterized trajectory state at a fixed frequency (default 20 Hz) for the UAV's position / velocity controller to track and execute. The output trajectory state can be directly mapped to the PositionTarget message format of the MAVROS protocol, realizing seamless data interaction and command compatibility with the UAV flight control system.

[0030] Reference Figure 1 A preferred embodiment of the present invention provides a method for generating UAV trajectories based on hybrid geometric smoothing and multidimensional dynamic constraints, comprising the following steps: S1. Obtain the discrete waypoint sequence of the UAV and resample it based on arc length to obtain equally spaced pathpoints. This includes the following steps: S101. Preprocess the input discrete waypoint sequence. Let the input waypoint sequence be Each waypoint contains a three-dimensional position. Yaw angle and timestamp .

[0031] S1011. Check the monotonically increasing nature of waypoint timestamps. If there are non-monotonic timestamps or the time interval exceeds the preset maximum adjacent waypoint time interval threshold, proceed accordingly. If the time quality is abnormal, a time quality anomaly flag is set and the cause of the anomaly is recorded for reference in subsequent processes. That is: The time interval for checking adjacent waypoints one by one If it exists (Non-monotonous), or If the time interval is abnormal, then the time quality flag will be set to abnormal. (and records the exception reason string). In this embodiment... .

[0032] It should be noted that the timeline of this method is calculated independently by the subsequent velocity profile generation step, without relying on the original timestamps of the input waypoints. The purpose of timestamp verification is to provide diagnostic information on the quality of the input data to the upper-level system.

[0033] S1012. Relative Time Conversion: Based on the First Voyage Point Timestamp Based on this, convert all waypoint timestamps to relative time. In the formula, For the first voyage timestamp, For the first Original timestamps of each waypoint For the first relative timestamps of waypoints For waypoint indexing.

[0034] S1013. Yaw Angle Unwinding: This process unwinds the yaw angles in the waypoint sequence to achieve continuity and eliminate... The abrupt change is discontinuous. Specifically, the difference in yaw angle between adjacent points is calculated point by point. ,when Execution ,when Execution Execute in a loop until This yields a continuous sequence of unwound yaw angles, eliminating the limitations imposed by the range of angle representation. Caused Mutation. In the formula, and The first The and the first Yaw angle of each waypoint This represents the difference between adjacent yaw angles. This represents the boundary of the principal value interval for the yaw angle.

[0035] S1014. Duplicate point removal: Traverse all adjacent waypoint pairs in the corresponding preprocessed discrete waypoint sequence. When adjacent waypoints... and Euclidean distance between Less than the preset minimum point distance threshold If the two waypoints are in a state of flux, then a merging process is performed (to avoid numerical singularities caused by zero-length line segments): the previous waypoint is retained. The three-dimensional position information is obtained using the next waypoint. The yaw angle and timestamp information are used to update the corresponding information of the previous waypoint (updated waypoint data is given priority); special protection is applied to the last waypoint in the waypoint sequence, and the threshold for determining the distance between its adjacent points is adjusted to... To avoid accidental deletion of terminal waypoints due to distance threshold determination, this embodiment presets a minimum point distance threshold. The value is 0.01 m.

[0036] In the formula, and The first The and the first Three-dimensional position vectors of waypoints Describing the Euclidean norm, This is the minimum point distance threshold.

[0037] S1015. Calculate the cumulative arc length for the deduplicated waypoint sequence:

[0038] In the formula, For the first The cumulative arc length at each waypoint This is the initial cumulative arc length. This represents the total number of waypoints after deduplication. This is the total arc length.

[0039] S102. Equal Arc Length Resampling S1021. Resampling Interval Adjustment Configure arc length spacing as (In this embodiment) ), calculate the number of resampling segments:

[0040] in, For the total arc length, This indicates the rounding up operation.

[0041] To ensure precise matching of the beginning and end points, the actual arc length spacing is adjusted to... .

[0042] S1022. Linear interpolation to generate resampling points right Generate target arc length (First point taken) Take the tail point ).in, For the first Target arc length at each resampling point For resampling point index, This represents the number of resampled segments. This is the total arc length.

[0043] The target arc length is located by binary search within the original waypoint cumulative arc length sequence. Calculate interpolation parameters Linear interpolation is performed on position and yaw angle respectively:

[0044]

[0045] The positions and yaw angles of the first and last sampling points are then forcibly overwritten with the precise values ​​of the first and last endpoints of the original sequence to eliminate floating-point cumulative errors.

[0046] S1023. Calculation of the rate of change of yaw angle with respect to arc length resampled yaw angle sequence The rate of change of yaw angle with respect to arc length is calculated using the finite difference method:

[0047]

[0048]

[0049] In this formula, the first endpoint uses forward differencing, the internal points use central differencing, and the last endpoint uses backward differencing. , , The first point and the second point are respectively the first point and the second point. Derivatives of yaw angle arc length at each interior point and terminal point , , , , , These are the yaw angles of the corresponding sampling points. For resampling arc length spacing, For interior point index, This is the endpoint index.

[0050] The rate of change It serves as the input for the yaw rate constraint in subsequent velocity profile generation.

[0051] After resampling, the data structure for each sampling point includes: three-dimensional position. Unwinding yaw angle Cumulative arc length curvature (To be filled in later steps) and yaw rate of change .

[0052] S2. Perform hybrid geometric smoothing on the equally spaced path points to generate a reference path; wherein, the hybrid geometric smoothing includes: identifying path corners and constructing local transition curves that meet the tangential continuity requirement at each corner; performing global spline fitting on non-turning road segments in the path, and fusing the local transition curves with the globally spline-fitted path to form a reference path; and performing safety corridor constraint verification on the generated reference path. Specifically, the steps include: S201. Identify path corners (corner detection) Calculate the angle between the direction vectors of adjacent road segments at each path point. When the angle exceeds a preset corner threshold, it is determined to be a corner. Specifically: For each internal resampling point ( ), calculate the unit vector of the incident direction respectively. and the unit vector of the direction of emission :

[0053] In the formula, , , These are the position vectors of three adjacent path points. This represents the Euclidean norm of a vector.

[0054] If the length of any line segment is less than the machine accuracy threshold If the point is not found, skip that point. Calculate the deflection angle (steering angle) of the two direction vectors:

[0055] In the formula, For deflection angle, It is an inverse cosine function. For vector dot product, Indicates will Limited to Interval.

[0056] in Indicates complete collinearity (straight lines). This indicates a complete reversal (U-turn); the larger the yaw angle, the more abrupt the turn. When When the deflection angle is greater than or equal to a preset corner angle threshold, the point is determined to be a corner point (corner). In this embodiment... That is, the deflection angle is greater than or equal to Only sharp turns are judged as corners.

[0057] S202. A transition curve connecting road segments before and after a corner is constructed using the Hermite interpolation model. By constraining the tangential vectors at the endpoints of the transition curves to be collinear with the direction vectors of adjacent road segments, the geometric first-order continuity of the path at the corner is achieved, and trajectory convexity is suppressed. Specifically, this includes: S2021. Determining the Transition Length For the detected corner points, determine the transition length of the Hermite transition curve. To avoid overlapping transition areas at adjacent corners, the actual transition length is taken as the minimum of the following values:

[0058] In the formula, For the transition length, This indicates taking the minimum value. The preset maximum transition length, For safety ratio coefficient, The length of the incident ray segment. The length of the outgoing ray segment. When A short-term warning may be generated at this time.

[0059] In this embodiment, , . Its function is to limit the transition region to not exceed the length of the incident beam segment and the exit beam segment. To avoid overlapping transition sections at adjacent corners. When When necessary, set a short segment warning sign.

[0060] S2022. Determination of Transitional Entry and Exit Control Points Take one transition control point on each side of the corner point along the incident and exit directions:

[0061] In the formula, and These are the transition entrance and exit control points, respectively. Let be the corner point position vector. and These are the unit vectors for the incident and exit directions, respectively. This is the transition length.

[0062] S2023. Tangential Vector Setting Introducing a tangential amplitude scaling factor (Corresponding parameter corner_tangent_factor) Calculate the magnitude of the tangential vector. Inlet tangential vector , exit tangential vector The direction of the tangential vector is consistent with the direction of the incident / exit ray segment, ensuring that the transition curve is tangent to the original straight line segment at the inlet and outlet, thus achieving... Continuous. Scaling factor Control the curvature of the transition curve. To make the curve smoother, Make the curve more compact. At this time, the tangential amplitude is exactly equal to the transition length, providing a "standard" degree of curvature.

[0063] In this embodiment .

[0064] S2024. Construction of cubic Hermite transition curves With parameters Constructing a transition curve:

[0065] In the formula, For parameters The corresponding transition curve points, , , , For Hermite basis functions, and These are the entrance and exit control points, respectively. and These are the inlet and outlet tangential vectors, respectively.

[0066] The Hermite basis functions and their first and second derivatives are shown in the table below:

[0067] The endpoint conditions can be verified by definition: , , , That is, the tangent of the curve at the entrance and exit points is precisely consistent with the direction of the original line segment, achieving... Continuous. Due to the constraint of the tangential vector direction, the transition curve is confined to the area between the incident and exit directions, thus suppressing the outward overshoot at corners in traditional polynomial interpolation in principle. In the formula, and The curves are respectively in the parameters and The first derivative at that point.

[0068] S2025. Transition Curve Arc Length Estimation The arc length of each transition curve segment is estimated using numerical integration. The parameter range is defined. Divided into equal parts The chord length between adjacent sampling points is accumulated.

[0069] In the formula, This is an approximation of the arc length of the transition curve. For sampling index, For the first Each parameter sampling point, Describing the Euclidean norm, This indicates a summation operation.

[0070] In this embodiment, take That is, the parameter range Divide the arc into 20 equal parts for numerical arc length estimation:

[0071] The aforementioned transition curve parameters (control point coordinates, tangential vector, arc length, etc.) and corner detection results (number of corners, transition length, deflection angle of each corner, short segment warning, etc.) are recorded in the geometric fitting quality report for subsequent trajectory quality assessment.

[0072] S203. Global Natural Cubic Spline Fitting and Safe Corridor Verification For all point sequences after arc length resampling, a natural cubic spline function is used for global fitting to achieve the full trajectory. Continuity (position, first derivative, and second derivative continuity) is ensured, and fitting bias is verified through a safety corridor mechanism. The specific process includes: S2031. Construction of Natural Cubic Splines Point sequence after arc length resampling , respectively , , The three coordinate components independently construct a one-dimensional natural cubic spline. For the first Arc length of each sampling point , , The first Three-dimensional coordinate components of each sampling point , , For the corresponding coordinate axis variables. The spline for each component consists of a piecewise cubic polynomial:

[0073] Natural boundary conditions require that the second derivative at the endpoints be zero, i.e. , In the formula, Arc length parameter spline function values ​​below, , , , For the first coefficients of a segmental cubic polynomial For the first Segment splines define intervals.

[0074] by For example, the components ( , (The components are completely similar), let the data pairs be... Each interval The cubic polynomial on is:

[0075] common Segment polynomial, One undetermined coefficient. Natural boundary conditions require... , .

[0076] When the input has only 2 points, it degenerates into linear interpolation: , , .

[0077] S2032. Thomas Algorithm for Solving Tridiagonal Equations Solving for spline coefficients reduces to solving for the second derivative. The system of tridiagonal equations. First, calculate the distance between the arc segments. and the right-hand item :

[0078] In the formula, For internal node indexes, , , The function values ​​of adjacent nodes. and The length of the adjacent interval.

[0079] Forward elimination using Thomas's algorithm (catch-up method):

[0080]

[0081]

[0082]

[0083] In the formula, , , These are the variables initialized using the Thomas algorithm's forward elimination. For the first Step diagonal elimination coefficients, The upper diagonal normalized coefficients are... For the right-hand side, it is a recursive variable.

[0084] Backward substitution:

[0085] Finally, calculate the spline coefficients for each segment:

[0086]

[0087]

[0088] In the formula, , , The first Segment spline polynomial coefficients, and The function values ​​of adjacent nodes. For the first Segment interval length, and These are the adjacent second derivative coefficients.

[0089] The overall computational complexity of the Thomas algorithm is: This ensures efficient solution for large-scale waypoint sequences, among which This represents the number of resampling points. , , Solve the three components independently, the total complexity is .

[0090] S2033. Spline Evaluation and Derivative Calculation Given arc length First, use binary search ( Locate the segment to which it belongs. ,make Calculate separately: Function value:

[0091] First derivative:

[0092] Second derivative:

[0093] After synthesizing the three components into a 3D spline, the position can be obtained. First derivative and second derivative .

[0094] S2034. Curvature Calculation For the fitted cubic spline curve, the analytical first derivative of the spline is used. and second derivative Calculate curvature:

[0095] In the formula, Position of arc length Curvature, Represents the cross product of vectors. Let represent the Euclidean norm of a vector. Where: ,

[0096] In the formula Indicates the transpose; the derivatives of each component are directly obtained by analytic differentiation of the spline polynomial:

[0097]

[0098] This method utilizes the analytical derivative of splines, eliminating the need for discrete approximations and achieving high curvature accuracy. In the formula, For the first The first derivative of a segment spline function, For the first The second derivative of a segment spline function.

[0099] S2035. Safety Corridor Constraint Verification A pipeline-type safety corridor is constructed with the original discrete waypoint line as the center and a preset safety distance as the radius. The shortest distance from the reference path to the original discrete waypoint line is calculated. If the maximum value of this shortest distance exceeds the preset safety distance, iterative refitting is performed by adjusting the resampling interval or local transition length parameter until the path deviation meets the safety corridor constraint. Specifically: With the preset maximum deviation threshold The radius of the corridor (in this embodiment) For each sampling point on the fitted curve, calculate the shortest distance to the original input polyline segment.

[0100] The distance from a point to a polyline segment is calculated using the line segment projection method: for the point and line segments Calculate the projection parameters:

[0101] Nearest point ,distance Segment index caching is used for acceleration: the index of the nearest line segment to the previous sampling point is used as the starting hint for the search of the next point, and the search proceeds from this index to both sides. Extended search leverages the spatial continuity of paths to reduce the average search complexity to approximately... .

[0102] Statistical deviation index: maximum deviation mean deviation , 95th percentile deviation .

[0103] Corridor constraint determination: Corridor radius (maximum deviation threshold) Compare:

[0104] When the corridor constraint is not satisfied, mark The data is written to the fitting report, but the trajectory generation process is not interrupted; subsequent modules continue to run. The upper-level system can use this flag to decide whether to accept the current trajectory or trigger replanning.

[0105] S2036. Curvature Quality Assessment Peak curvature of the statistical fitted curve Minimum turning radius , median curvature A curvature spike warning (CURVATURE_SPIKE) is triggered when the ratio of peak curvature to median curvature exceeds 10; a curvature noise warning (CURVATURE_NOISY) is triggered when the ratio of standard deviation of curvature to mean exceeds 1.0. The curvature confidence level is calculated based on the warning flags. .

[0106] Right now: The curvature sequence of all sampling points of the fitted curve Calculate the following statistics: Peak curvature:

[0107] Minimum turning radius: (when ) Median curvature: (Take the median value after sorting) Mean curvature:

[0108] Standard deviation curvature:

[0109] The curvature warning sign should be set according to the following rules:

[0110]

[0111] Curvature confidence is determined based on warning flags: when there are no warnings Only when SPIKE warning is issued Other situations .

[0112] S3. The reference path is processed to generate a continuous and smooth heading angle sequence; the resampled yaw angle sequence is smoothed by an optional sliding window, and the yaw angle velocity is limited by a discrete proportional-integral controller to ensure a smooth transition of the output yaw angle trajectory.

[0113] The heading angle processing includes the following steps: Dewinding is performed on the heading angle on the reference path to eliminate angle jumps; Smooth the dewound heading angle sequence; An integral limiter is used to shape the rate of change of heading angle to ensure that the rate of change of the output heading angle with respect to arc length does not exceed the preset maximum yaw rate threshold.

[0114] Yaw angle processing can be performed after terminal convergence to ensure that the time axis used has included the terminal deceleration effect.

[0115] The specific process includes: S301. Smooth yaw angle sliding window With window width A moving average filter is applied to the yaw angle sequence. To prevent excessive smoothing that deviates from the original yaw command, a maximum deviation constraint is introduced. :

[0116] In the formula, The mean yaw angle within the window. For the first Yaw angle at each sampling point This is the index of the current sampling point. For indexes within the window.

[0117]

[0118] In the formula, For the first Smoothed yaw angle at each sampling point The original yaw angle, Indicates will Limit to Interval.

[0119] In this embodiment, when At that time, based on the window width (Ensure the yaw angles are odd) Perform moving mean filtering on the yaw angle sequence. For each point... ,Pick Mean yaw angle within the range (automatically truncated at the boundary) And apply maximum deviation constraints. :

[0120] S302. Yaw rate limiting A discrete-time proportional controller is used to limit the yaw rate. At each time step... Inside: Error calculation:

[0121] In the formula, For the first Yaw angle tracking error For the first Step reference yaw angle, For the first Step output yaw angle, This indicates that the angle is normalized to... .

[0122] Rate command:

[0123] In the formula, The yaw rate command, For proportional gain, Upper limit of angular velocity Indicates will Limit to Interval.

[0124] Points update:

[0125] In the formula, For the first Step output yaw angle, For the first Step output yaw angle.

[0126] in For proportional gain (take) , (for the maximum expected error) This is the upper limit of yaw rate. The function normalizes the angle difference to interval, The function implements saturation limiting. This controller uses proportional error to drive the output yaw angle to progressively track the reference value, while... Hard constraints ensure that the yaw rate does not exceed the physical limit at any given time.

[0127] In this embodiment, the time step is calculated first. ( (Total duration after update), when Time rollback to configuration value .initialization , .right implement:

[0128]

[0129]

[0130]

[0131] in For proportional gain, in this embodiment , Therefore . The function normalizes the angle difference to Interval.

[0132] The controller works as follows: when the yaw angle error is large ( ), quilt Saturation limiting, the controller tracks at the maximum angular rate; when the error is small ( ), Saturation was not triggered, the controller converged proportionally, and there was no overshoot.

[0133] S303. Target point yaw angle lock When the Euclidean distance between the drone and the destination is less than the yaw lock radius At this time, the yaw reference angle is locked as the target yaw angle to eliminate terminal yaw oscillation.

[0134] In this embodiment, for each sampling point, its distance from the endpoint is calculated. When that distance At that time, the yaw reference angle is directly set as the target yaw angle. Eliminate terminal yaw oscillations.

[0135] S304. Target Yaw Handling Statistics Calculate and record: smoothing deviation statistics (maximum value, mean value), maximum yaw rate after rate limiting (for verification). Shaping deviation caused by rate limiting () The maximum and average values), yaw lock trigger flag and duration.

[0136] S4. Based on the geometric features and heading angle sequence of the reference path, a time-parameterized velocity profile is generated in combination with preset multidimensional dynamic coupling constraints, and then a flight trajectory containing position, velocity, acceleration, heading angle and heading angular velocity is generated and output; wherein, the multidimensional dynamic coupling constraints include at least three types of constraints: path curvature, vertical slope and yaw angular velocity, to jointly restrict the linear velocity.

[0137] This step combines path geometry and yaw motion characteristics to generate a time-parameterized velocity profile that satisfies multidimensional dynamic constraints using a forward-reverse double-pass scanning strategy. The time axis of this velocity profile is entirely determined by path geometry and dynamic constraints, independent of the original timestamps of the input waypoints. The specific process includes: S401. Calculation of Upper Bound of Multidimensional Velocity For each arc length sampling point on the fitted curve Simultaneously calculate the upper bound of velocity under three dynamic constraints, and take the minimum value as the comprehensive velocity limit at that point: Curvature constraint velocity upper bound (limiting centripetal acceleration to no more than) ):

[0138] In the formula, The upper bound of the curvature constraint velocity. For the maximum centripetal (normal) acceleration, For the first Point curvature, This is the effective threshold for curvature.

[0139] Vertical slope constraint speed upper limit (limiting vertical speed to no more than ):

[0140] In the formula, The vertical slope constrains the upper limit of the velocity. Vertical velocity, For the first The absolute value of the vertical slope at a point. This is the effective threshold for vertical slope.

[0141] in Central difference estimation is used: , , The height of adjacent points, , It represents the arc length of adjacent points.

[0142] Yaw rate constraint upper limit (limiting yaw rate to not exceed) ):

[0143] In the formula, The yaw rate is used to constrain the upper limit of the velocity. Yaw angular velocity, For the first The absolute value of the rate of change of point yaw angle. This is the effective threshold for the yaw rate of change.

[0144] This constraint unifies yaw motion and linear motion into the velocity profile: at the rate of change of heading. Larger sections of road The smaller value forces the linear velocity to automatically decrease in order to maintain the yaw rate. At the same time, for Apply The upper bound is truncated to avoid An unreasonably high speed is generated when the value is extremely small. In the formula, This is the actual yaw rate. For linear velocity, Horizontal speed.

[0145] Overall speed limit:

[0146] In the formula, For the first Overall speed limit This indicates that the minimum value is taken among multiple constraints. Indicates lower boundary protection. To prevent extremely low speed thresholds from dividing the value by zero.

[0147] Specifically, in this embodiment, each arc length sampling point on the fitted curve is... ,initialization Three constraints are superimposed in sequence: (a) Curvature constraint: When hour,

[0148] Physical meaning: limiting centripetal acceleration No more than .

[0149] (b) Vertical slope constraint: For internal points ( ), calculate the central difference vertical slope ,when hour,

[0150] Physical meaning: Limiting the vertical component of velocity No more than .

[0151] (c) Yaw rate constraint: When the yaw rate constraint is enabled ( )and hour,

[0152]

[0153]

[0154] Physical meaning: Limiting yaw rate No more than Upper bound truncation avoid Extremely low speeds result in unreasonably high speeds. The core innovation of this constraint lies in incorporating yaw motion characteristics ( The yaw constraint is mapped to the upper limit of linear velocity, allowing the yaw constraint to be naturally integrated into the velocity profile without the need for additional yaw control loop correction.

[0155] Final guarantee .

[0156] S402. Based on the upper limit of the synthesized horizontal velocity and the maximum tangential acceleration constraint, perform forward acceleration scanning and backward deceleration scanning on the reference path respectively to obtain the forward scanning velocity curve and the backward scanning velocity curve; perform point-by-point minimum value processing on the two obtained velocity curves to obtain the final planned velocity of each point on the path; perform time integration on the path based on the final planned velocity to obtain the timestamp corresponding to each point of the trajectory, and complete the time parameterization of the trajectory. Specifically, the steps include: S4021. Forward acceleration scan Starting from the initial point, based on the maximum tangential acceleration Point-by-point calculation speedup:

[0157] In the formula, The starting point is the forward scanning speed. The starting speed limit, This is a constraint on the initial speed.

[0158]

[0159] In the formula, For the first Point forward scan speed, The forward scan speed of the previous point, The step size is the arc length. This scan is based on the formula for uniformly accelerated motion. Ensure that the tangential acceleration between adjacent points does not exceed the physical limit. The final velocity, The initial velocity is... This is the tangential acceleration.

[0160] Specifically, in this embodiment, the scanning proceeds point by point from the starting point, based on the relationship of uniformly accelerated motion. Calculation acceleration achievable speed:

[0161]

[0162] The complexity of this scan is... .

[0163] S4022. Reverse deceleration scan Starting from the endpoint and scanning backwards towards the starting direction, calculate the achievable speed with deceleration:

[0164] In the formula, This is the initial value for the reverse scan at the end point. The forward scanning speed at the endpoint. The endpoint speed is constrained.

[0165]

[0166] After a double scan in both forward and reverse directions, the final speed Simultaneously satisfying: (a) not exceeding the upper limit of multidimensional velocity; (b) acceleration not exceeding the positive acceleration limit; (c) being able to decelerate to below the limit speed in time before the constraint point ahead.

[0167] Specifically, in this embodiment, the scan proceeds point by point backward from the endpoint to ensure timely deceleration before the preceding constraint point:

[0168]

[0169] Final speed Simultaneously satisfying three conditions: (i) not exceeding the upper bound of the multidimensional velocity, (ii) positive acceleration not exceeding the limit, and (iii) negative deceleration not exceeding the limit. The complexity of this scan pass is... .

[0170] S4023. Time Integration Based on the final velocity profile, time integration is performed using the trapezoidal rule:

[0171] By obtaining the time corresponding to each arc-length sampling point, the parameterization from the arc-length domain to the time domain is completed. In the formula, For the first Click the timestamp, At the initial moment, The average velocity of adjacent points. and For the velocities of adjacent points, The step size is the arc length. This is the minimum speed threshold.

[0172] Specifically, in this embodiment:

[0173] This completes the mapping from the arc length domain to the time domain.

[0174] S4024. Velocity Profile Statistics Statistical maximum speed mean speed Maximum tangential acceleration (approximated by the velocity difference between adjacent points), maximum centripetal acceleration Yaw constraint ratio ( (the proportion of points to the total number of points) and the minimum speed under yaw constraints .when Or the yaw constraint ratio exceeds At that time, a yaw speed limit warning was triggered.

[0175] The dynamic constraint parameters in this embodiment are shown in the table below:

[0176] S5. Terminal convergence control (terminal convergence smoothing) When the drone approaches the target point, the terminal convergence mechanism is activated, and a smooth step function is used to gradually reduce the speed, ensuring that the drone hovers at zero speed at the end point and that there are no sudden acceleration changes during the process.

[0177] Specifically, when the Euclidean distance between the drone's current position and the target endpoint is less than a preset distance threshold, the drone's maximum flight speed is dynamically reduced based on the remaining flight distance to the target endpoint. Simultaneously, the drone's heading angle is locked to the preset heading angle of the target endpoint. This state (dynamic speed reduction and heading lock control state) continues until the drone reaches the target endpoint. This includes the following steps: (1) Convergence triggering condition: Terminal convergence is triggered when any of the following conditions are met:

[0178] In the formula, For the first A vector of sampling point locations, Let the target point's position vector be... Distance trigger threshold, For the total arc length, Given the current arc length, The remaining arc length threshold, This represents the Euclidean norm. Once triggered, the convergence state is locked and will not exit.

[0179] Specifically, in this embodiment, the convergence triggering conditions are checked point by point starting from the path origin. For each sampling point... Calculate the Euclidean distance from the endpoint. and remaining arc length Convergence is triggered when any of the following conditions are met:

[0180] In this embodiment , Record the earliest triggered index. .

[0181] (2) Smooth velocity decay: Calculate the normalized residual distance And a velocity scaling factor is generated using a smooth step function:

[0182] In the formula, The remaining arc length, To be the convergent arc length threshold, Indicates will Limit to Interval.

[0183]

[0184] In the formula, This is the scaling factor for the smooth step velocity.

[0185]

[0186] Smooth step function satisfy , and , To ensure the velocity decay curve Continuity is maintained to avoid abrupt changes in acceleration at the end point. Forced acceleration at the endpoint. .in Continuity means that the function value and the first derivative are continuous. This represents the velocity of the end sampling point.

[0187] Specifically, in this embodiment, from arrive Calculate the velocity scaling factor for each point:

[0188]

[0189]

[0190] in This represents the maximum permissible speed during the convergence segment.

[0191] When the lock mode is enabled ( When enabled by default, an additional monotonically decreasing constraint is applied within the convergence segment:

[0192] Ensure that once convergence is achieved, the velocity only decreases and never increases. Endpoint forced. .

[0193] (3) Time re-integration: Re-execute the time integration in step S4023 with the velocity sequence after terminal convergence adjustment to obtain the updated time axis. .

[0194] S6. Trajectory Assembly and Sampling Output Includes the following steps: S601. Trajectory Object Construction Encapsulate the following data into a trajectory object: Fitted 3D natural cubic spline (position interpolator) Velocity profile sequence (arc length-velocity-time mapping table) Yaw angle trajectory and yaw rate trajectory Configuration parameters, total duration Total arc length

[0195] Finish line and the final yaw angle

[0196] S602. Locus Evaluation For any time The trajectory is calculated using the following steps: (a) Time-to-arc length mapping: Time series in velocity profile Using binary search to locate the time period, linear interpolation is used to obtain the corresponding arc length. .

[0197] (b) Arc length → position: Valued using three-dimensional natural cubic splines. If the spline is unavailable, backtrack to linear interpolation of the resampled points.

[0198] (c) Arc length → velocity magnitude: Linear interpolation in the arc length-velocity mapping table of the velocity profile .

[0199] (d) Velocity direction: The unit tangent vector is obtained using splines. velocity vector .

[0200] (e) Acceleration (optional): Calculate tangential acceleration when acceleration output is enabled. ( (estimated by the center difference of the velocity profile) and normal acceleration Synthetic three-dimensional acceleration ,in The main normal direction.

[0201] (f) Yaw angle and yaw rate: linearly interpolate the yaw angle in the time series. and yaw rate Execute before yaw angle output. Normalization to .

[0202] S603. Sampling at equal time intervals By time step (Corresponding to 20 Hz) Sampling, generating There are trajectory state points, each state containing:

[0203] in Optional acceleration. Final point time clamping is... If the last sampling point and The gap exceeds Then, a precise endpoint state is added.

[0204] S7. Status Codes and Exception Handling Once the trajectory is generated, a status code is returned based on the execution status of each stage:

[0205] When a timestamp is abnormal, the status code is set to TIME_INVALID_FALLBACK instead of failure, because the timeline of this method is generated independently of the input timestamp. When the corridor constraint is not met ( The status code remains SUCCESS (or TIME_INVALID_FALLBACK), and uncertain information is provided in the report. The transmission of signals is decided by the upper-level system.

[0206] S8. Quality Assessment Report Each trajectory generation synchronously outputs a multi-dimensional quality assessment report, including: S801. Geometric Fit Report Includes: Number of waypoints Points after preprocessing Number of resampling points Number of control points (equal to) (Natural cubic spline with all resampled points as nodes) and number of path segments Number of detected corners Deviation statistics ( , , Curvature statistics () , , , ); Curvature warning sign; Corner continuity level ( Short line segment warning sign; corridor convergence sign and the number of fitting iterations (currently fixed at 1); height statistics ( , , ).

[0207] S802. Time Quality Report Includes: time processing mode (USE_STAMP / FALLBACK_MVP), timestamp monotonicity flag, and maximum time interval. , Time anomaly rollback flag and anomaly reason string.

[0208] S803. Velocity Profile Report Includes: Total duration Total path length Maximum speed Mean speed Maximum tangential acceleration, maximum centripetal acceleration, maximum vertical velocity, maximum vertical acceleration; terminal convergence trigger flag and convergence duration; yaw constraint ratio. Minimum speed under yaw constraint Yaw speed limit warning sign.

[0209] S804. Yaw Handling Report Includes: smoothing deviation (maximum value, mean value); maximum yaw rate and rate compliance mark ( ); Rate limiting and shaping deviation (maximum value, average value); Yaw lock trigger flag and lock duration.

[0210] To verify the effectiveness of the method described in this invention, a three-dimensional waypoint sequence containing sharp turns is used as input to demonstrate the complete trajectory generation process and results.

[0211] To verify the effectiveness of the method described in this invention, a three-dimensional waypoint sequence containing sharp turns is used as input to demonstrate the complete trajectory generation process and results.

[0212] (1) Input data

[0213] Total number of waypoints: 1107.

[0214] (2) Preprocessing results Number of waypoints after deduplication: 1107. Total arc length 54.9924m. Timestamp verification result: Monotonic.

[0215] (3) Resampling results by Resampling, number of segments 367, actual spacing 0.149843 m, number of resampling points 368.

[0216] (4) Corner detection results

[0217] (4) Corner detection results Detected There are 3 corners, and the corner information is shown in the table:

[0218] like Figure 4 As shown, the transition curve is aligned with the original line segment direction at both the inlet and outlet. (Continuous), the curve is located inside the region formed by the incident and exit directions.

[0219] (5) Spline fitting and corridor verification results Fitting results: Maximum deviation 1.776e-15 m, mean deviation 7.626e-17 m, 95th percentile deviation 4.441e-16 m, corridor constraints ( Satisfaction status: Satisfied. Curvature statistics: 472.596 rad / m 0.002116 m.

[0220] like Figure 5 As shown, the fitted curve (solid line) smoothly passes through the original broken line segment (dashed line), and the maximum deviation is controlled within the corridor radius.

[0221] (6) Velocity profile results like Figure 6 As shown, the horizontal axis represents the arc length. (m), with velocity on the vertical axis. (m / s). The figure also shows: the upper bound of the multidimensional velocity. (Upper bound of gray area), curvature constraint (Blue curve), yaw constraint (Red curve) Forward scan results (Green curve) and final velocity (Solid black line).

[0222] Key Metric: Total Duration 1099.848 s, maximum speed 0.500 m / s, average velocity 0.4754 m / s, yaw constraint percentage 0.

[0223] As can be seen, in the arc segment where the heading changes drastically ( (larger area) Significantly lower than Final speed The yaw rate will automatically decrease during this segment to ensure it does not exceed [a certain value]. .

[0224] (7) Terminal convergence results Convergence trigger position: 54.9924 m from the endpoint (arc length / distance trigger), convergence segment velocity is determined by... It decays smoothly to 0 using the smoothstep function. For example... Figure 7 As shown.

[0225] (8) Yaw angle processing results like Figure 8 As shown, the horizontal axis represents time. (s), with the vertical axis representing the yaw angle. (rad). The figure also shows: the original reference yaw angle. (Gray dashed line), smoothed yaw angle (Blue curve) and output yaw angle after rate limiting (Solid red line). Maximum yaw rate 0.6 rad / s rad / s, which satisfies the constraints.

[0226] (9) Output trajectory status (excerpt)

[0227] (10) Summary of Quality Report

[0228] The above results demonstrate that the method described in this invention achieves global cubic spline fitting when processing three-dimensional waypoint sequences containing sharp turns. The continuous smooth trajectory and corridor constraints ensure controllable geometric deviations. The coupling of yaw rate constraints and linear velocity enables the UAV to automatically reduce speed when the heading changes drastically. The terminal smoothstep convergence ensures a smooth landing, verifying the effectiveness and practicality of the method of this invention.

[0229] The above are preferred embodiments of the present invention. For those skilled in the art, various improvements and modifications can be made without departing from the principle of the present invention, and these improvements and modifications should also be considered within the scope of protection of the present invention. For example: arc length resampling interval. It can adaptively adjust according to path complexity; corner angle threshold. The dynamic constraint parameters can be adjusted according to the aircraft's maneuverability; the dynamic constraint parameters can be automatically loaded from the aircraft model parameter table; the proportional gain of the yaw rate limiter controller... It can be calibrated according to the bandwidth of the yaw servo system.

Claims

1. A method for generating UAV trajectories based on hybrid geometric smoothing and multidimensional dynamic constraints, characterized in that, Includes the following steps: S1. Obtain the discrete waypoint sequence of the UAV and resample it based on arc length to obtain equally spaced pathpoints; S2. Perform hybrid geometric smoothing on the equally spaced path points to generate a reference path; The hybrid geometric smoothing process includes: identifying path corners and constructing local transition curves that meet the tangential continuity requirement at each corner; performing global spline fitting on non-turning road segments in the path, and fusing the local transition curves with the globally spline-fitted path to form a reference path; and performing safety corridor constraint verification on the generated reference path. S3. Perform heading angle processing on the reference path to generate a continuous and smooth heading angle sequence; S4. Based on the geometric features and heading angle sequence of the reference path, a time-parameterized velocity profile is generated in combination with preset multidimensional dynamic coupling constraints, and then a flight trajectory containing position, velocity, acceleration, heading angle and heading angular velocity is generated and output; wherein, the multidimensional dynamic coupling constraints include at least three types of constraints: path curvature, vertical slope and yaw angular velocity, to jointly restrict the linear velocity.

2. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, The specific method for identifying path corners and constructing local transition curves that satisfy tangential continuity at the corners in step S2 is as follows: Calculate the angle between the direction vectors of adjacent road segments at each path point in the path, and determine a corner when the angle exceeds a preset corner threshold. The Hermite interpolation model is used to construct a transition curve connecting the road segments before and after a corner. By constraining the tangential vectors at the endpoints of the transition curves to be collinear with the direction vectors of adjacent road segments, the geometric first-order continuity of the path at the corner is achieved and the outward convexity of the trajectory is suppressed.

3. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, The specific method for global spline fitting in step S2 is as follows: based on the determination of the local transition curve, the remaining road segments are fitted using a piecewise cubic spline function to ensure the continuity of the reference path acceleration.

4. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, The safety corridor constraint verification in step S2 includes the following steps: Construct a pipeline-type safety corridor centered on the original discrete waypoint connection and with a preset safety distance as the radius; Calculate the shortest distance from the reference path to the original discrete waypoints. If the maximum value of this shortest distance exceeds the preset safety distance, iterative refitting is performed by adjusting the resampling interval or local transition length parameter until the path deviation meets the safety corridor constraint.

5. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, The heading angle processing in step S3 includes the following steps: Dewinding is performed on the heading angle on the reference path to eliminate angle jumps; Smooth the dewound heading angle sequence; An integral limiter is used to shape the rate of change of heading angle to ensure that the rate of change of the output heading angle with respect to arc length does not exceed the preset maximum yaw rate threshold.

6. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, In step S4, when generating the time-parameterized velocity profile, in the reference path... The upper limit of the synthesized horizontal velocity at each arc-length sampling point Determined by the following formula: In the formula, The global maximum horizontal speed, For maximum vertical velocity, For maximum normal acceleration, Maximum yaw rate; It is a vertical slope. For path curvature, The derivative of the heading angle with respect to the arc length; For small positive numbers that are numerically stable.

7. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 6, characterized in that, The generation of time-parameterized velocity profiles in step S4 further includes: Based on the aforementioned upper limit of the composite horizontal speed With the maximum tangential acceleration constraint, forward acceleration scan and backward deceleration scan are performed on the reference path respectively to obtain the forward scan velocity curve and the backward scan velocity curve; The two obtained velocity curves are processed by taking the minimum value point by point to obtain the final planned velocity of each point on the path; the path is integrated over time based on the final planned velocity to obtain the timestamps corresponding to each point on the trajectory, thus completing the time parameterization of the trajectory.

8. The UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in claim 1, characterized in that, It also includes an end-point convergence control step: when the Euclidean distance between the current position of the UAV and the target endpoint is less than a preset distance threshold, the upper limit of the flight speed is dynamically reduced according to the remaining flight distance of the UAV to the target endpoint, and the heading angle of the UAV is locked to the preset heading angle of the target endpoint. This dynamic speed reduction and heading lock control state is maintained until the UAV reaches the target endpoint.

9. An electronic device, characterized in that, include: A processor and a memory, the memory being used to store computer program code, the computer program code including computer instructions, wherein when the processor executes the computer instructions, the electronic device executes the UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in any one of claims 1-8.

10. A computer-readable storage medium, characterized in that, The computer-readable storage medium stores a computer program, which includes program instructions that, when executed by a processor of an electronic device, cause the processor to perform the UAV trajectory generation method based on hybrid geometric smoothing and multidimensional dynamic constraints as described in any one of claims 1-8.

Images