Novel track inflection point correcting and smoothing method

Abstract

The invention discloses a novel trajectory inflection point correction and smoothing method, relates to the field of unmanned aerial vehicle autonomous navigation and motion control, and systematically eliminates redundant inflection points by analyzing a path steering angle and verifying barrier-free connectivity between adjacent nodes. According to the local tangent circle smoothing strategy, for the corrected trajectory inflection point, the arc radius is dynamically adjusted according to the bending degree of the inflection point, local arc fitting is only carried out on the two sides of the inflection point, excessive modification of the overall path is avoided, the trajectory smoothness is improved, and meanwhile it is ensured that the deviation between the smoothed path and the original path is controlled within 5%. The strategy is also combined with flight physical constraints such as the minimum turning radius of the unmanned aerial vehicle to automatically adapt to different curvature requirements, so that the trajectory safety is improved, and an innovative solution is provided for efficient optimization and safe flight of the trajectory of the unmanned aerial vehicle in a complex environment.

Description

Technical Field

[0001] The present invention relates to the field of autonomous navigation and motion control of unmanned aerial vehicles (UAVs), and in particular to a novel trajectory inflection point correction and smoothing method. Background Art

[0002] With the rapid development of unmanned aerial vehicle (UAV) technology, the demand for autonomous navigation in complex environments (such as battlefield reconnaissance, logistics distribution, and disaster relief) is becoming increasingly urgent. Path planning, a core task for autonomous UAV flight, requires generating efficient, smooth, and dynamically adaptable flight trajectories while ensuring safety. Existing path planning technologies, including graph search algorithms (such as the A algorithm), sampling algorithms (such as RRT), and optimization algorithms (such as genetic algorithms), have been widely used. However, these technologies still face the following key challenges: Paths generated by traditional path planning algorithms (such as Astar and Dijkstra) often contain redundant turning points, leading to frequent UAV turns and increased energy consumption and flight time. For example, Chinese patent CN118329062A proposes an intelligent navigation system based on the pruning-optimized Astar algorithm. This algorithm introduces a node search depth limit (limited to d) to reduce invalid node traversal, but still suffers from the high number of turning points and uneven trajectories. Existing smoothing techniques struggle to maintain the shortest path while avoiding intersections with obstacles. For example, Bezier curve smoothing may cause the trajectory to invade obstacle areas due to insufficient control points, while local interpolation methods may over-soften the path and deviate from the global optimal solution. Summary of the Invention

[0003] In response to the shortcomings of the existing technology, the purpose of the present invention is to provide a novel trajectory inflection point correction and smoothing method. This method can be directly applied to dynamic path planning scenarios for drones, mobile robots, and autonomous vehicles. It addresses the core problems of traditional trajectory planning algorithms, such as many redundant inflection points, insufficient path smoothness, and low dynamic obstacle avoidance efficiency. Through an adaptive inflection point correction mechanism and a tangential circle smoothing strategy under local geometric constraints, high-safety, low-energy trajectory optimization is achieved.

[0004] To achieve the above objectives, the present invention adopts a technical solution: a new trajectory inflection point correction method, comprising the following steps: Step S101: Represent the original path as a three-dimensional node sequence ,in As a starting point, For the end point, is the node on the path; the obstacle set is recorded as obs ; Step S102: For adjacent nodes and , calculate the direction vector ; Step S103. Calculate adjacent direction vectors and If the angle is greater than the preset threshold , then mark is the initial inflection point, and the initial inflection point set is obtained ; Step S104. Add starting points separately and end point To the initial inflection point set The first and last positions of form a new trajectory set ; Step S105. Create a new optimized trajectory list ,index i =1; Step S106. From the starting point First, check the line segments between the current point and subsequent inflection points in turn Whether to gather with obstacles obs intersect; if , that is, the line segment does not pass through the obstacle, then Move to the next inflection point and recheck whether the new line segment passes through the obstacle. If i=m+ 1, then finally ;like , that is, if the line segment passes through an obstacle, it will return to the previous inflection point. , , recheck whether the next line segment passes through the obstacle, until ; Step S107. Traverse all initial inflection points and finally generate an optimized trajectory list containing only the starting point, valid inflection points and end point. , where each line segment satisfies .

[0005] Based on the above technical solution, in step S101 , the obstacle set is a polygon set or an occupied grid in a three-dimensional space.

[0006] On the basis of the above technical solution, in step S102, the adjacent direction vectors and Angle .

[0007] The present invention also provides a smoothing method based on the novel trajectory inflection point correction method, comprising the following steps: Step S201. The turning point , calculate the front and back direction vector and ; then normalize the vector, , ; Calculate the angle ; Step S202: Inflection point Angle bisector vector + , after normalization, ; Step S203. For a circle whose center is on the angle bisector and whose inflection point is The circle whose two ends are tangent to each other satisfies , for different and , the radius of the tangent circle to the path is different, resulting in different trajectory smoothness. To ensure that one angle corresponds to only one smooth trajectory, define λ is the smoothing coefficient; the coordinates of the circle center are ; Step S204. The arc starting angle is rotated from the inflection point forward vector direction to the backward vector direction with the center of the circle as the origin, covering the angle , parametric equation ,in , The unit vector pointing from the center of the circle to the inflection point The polar angle.

[0008] Based on the above technical solution, in step S204, The calculation method is: ， ， ；

[0009] ， ， ；

[0010] ， .

[0011] The beneficial effects of the present invention are: This invention eliminates redundant turning points in drone path planning in complex three-dimensional environments. Addressing the problem of existing algorithms (such as graph search and optimization algorithms) generating a large number of path turning points and large steering angles, this method uses a trajectory correction method under geometric constraints to systematically eliminate redundant turning points between adjacent obstacle-free nodes, minimizing the number of path turning points and reducing the energy and time costs of drone steering.

[0012] This invention improves trajectory smoothness while maintaining path optimality. To address the path deviation or over-softening problems caused by insufficient control points in traditional smoothing algorithms (such as Bezier curves and spline interpolation), a local tangent circle smoothing strategy is proposed. This adaptive arc fitting is performed only on the path on both sides of the inflection point. This ensures trajectory smoothness while avoiding global path distortion and ensuring that the minimum distance between the smoothed trajectory and obstacles is greater than or equal to a safety threshold.

[0013] The trajectory inflection point correction algorithm of the present invention systematically eliminates redundant inflection points by analyzing the turning angle of the path and verifying the barrier-free connectivity between adjacent nodes: first, it identifies the turning points in the path whose angles exceed the set threshold, and then checks the connection between these points section by section to see if they are safe to pass. If there are no obstacles, the starting point and the target point are directly connected, skipping the redundant inflection points in the middle, reducing the number of trajectory inflection points by more than 50% compared to traditional algorithms, and significantly reducing the total number of turns and energy consumption. The local tangent circle smoothing strategy dynamically adjusts the arc radius according to the curvature of the corrected trajectory inflection point, and only performs local arc fitting on both sides of the inflection point to avoid excessive modification of the overall path, thereby improving the trajectory smoothness and ensuring that the deviation between the smoothed path and the original path is controlled within 5%. This strategy also combines flight physical constraints such as the minimum turning radius of the drone to automatically adapt to different curvature requirements, thereby improving trajectory safety, and providing an innovative solution for efficient optimization of drone trajectories and safe flight in complex environments. BRIEF DESCRIPTION OF THE DRAWINGS

[0014] Figure 1 It is the original planning trajectory map in the present invention; Figure 2 This is the trajectory diagram after the inflection point trajectory is corrected in the present invention; Figure 3 This is a smooth visualization diagram of the tangent circle in the present invention; Figure 4 This is the trajectory diagram of the smoothed tangent circle in the present invention; Figure 5 It is the trajectory after 3D inflection point correction and tangent circle smoothing. DETAILED DESCRIPTION

[0015] Embodiments of the present invention are described in detail below, examples of which are shown in the accompanying drawings, wherein the same or similar reference numerals throughout represent the same or similar elements or elements with the same or similar functions.

[0016] The following description of the embodiments of the present invention is provided in conjunction with the accompanying drawings to further describe the specific embodiments of the present invention so that the technical solutions and beneficial effects of the present invention will be more clearly understood. The following description of the embodiments with reference to the accompanying drawings is illustrative and intended to explain the present invention, but is not to be construed as limiting the present invention.

[0017] See also Figure 1~Figure 2As shown, an embodiment of the present invention provides a novel trajectory inflection point correction method, comprising the following steps: Step S101: Represent the original path as a three-dimensional node sequence ,in As a starting point, For the end point, is the node on the path; the obstacle set is recorded as obs ; Specifically, the obstacle set is a polygon set or an occupied grid in three-dimensional space.

[0018] Step S102: For adjacent nodes and , calculate the direction vector ; Step S103. Calculate adjacent direction vectors and If the angle is greater than the preset threshold , then mark is the initial inflection point, and the initial inflection point set is obtained ; Specifically, adjacent direction vectors and Angle .

[0019] Step S104. Add starting points separately and end point To the initial inflection point set The first and last positions of form a new trajectory set ; Step S105. Create a new optimized trajectory list ,index i =1; Step S106. From the starting point First, check the line segments between the current point and subsequent inflection points in turn Whether to gather with obstacles obs intersect; if , that is, the line segment does not pass through the obstacle, then Move to the next turning point , recheck whether the new line segment passes through the obstacle, if i=m+ 1, then finally ;like , that is, the line segment passes through the obstacle, then Back to the previous turning point ,and , , recheck whether the next line segment passes through the obstacle, until ; Step S107. Traverse all initial inflection points and finally generate an optimized trajectory list containing only the starting point, valid inflection points and end point. , where each line segment satisfies .

[0020] See also Figures 3 to 5 As shown, the present invention also provides a smoothing method based on the above-mentioned novel trajectory inflection point correction method, comprising the following steps: Step S201. The turning point , calculate the front and back direction vector and ; then normalize the vector, , ; Calculate the angle ; Step S202: Inflection point Angle bisector vector + , after normalization, ; Step S203. For a circle whose center is on the angle bisector and whose inflection point is The circle whose two ends are tangent to each other satisfies , for different and , the radius of the tangent circle to the path is different, resulting in different trajectory smoothness. To ensure that one angle corresponds to only one smooth trajectory, define λ is the smoothing coefficient; the coordinates of the circle center are ; Step S204. The arc starting angle is rotated from the inflection point forward vector direction to the backward vector direction with the center of the circle as the origin, covering the angle , parametric equation ,in , The unit vector pointing from the center of the circle to the inflection point The polar angle is calculated as , , ;

[0021] ， ， ；

[0022] ， .

[0023] In the description of the specification, reference to the terms "one embodiment," "preferably," "example," "specific example," or "some examples" means that the specific features, structures, materials, or characteristics described in conjunction with the embodiment or example are included in at least one embodiment or example of the present invention. The schematic expressions of the above terms in this specification do not necessarily refer to the same embodiment or example. Moreover, the specific features, structures, materials, or characteristics described may be combined in any appropriate manner in any one or more embodiments or examples.

[0024] The present invention is not limited to the above-described embodiments. Persons skilled in the art may make various improvements and modifications without departing from the principles of the present invention, and such improvements and modifications are considered to be within the scope of protection of the present invention. Any matters not described in detail in this specification constitute prior art known to those skilled in the art.

Claims

1. A new trajectory inflection point correction method, characterized in that: The following steps are involved: Step S101: Represent the original path as a three-dimensional node sequence ,in As a starting point, For the end point, is the node on the path; the obstacle set is recorded as obs ; Step S102: For adjacent nodes and , calculate the direction vector ; Step S103. Calculate adjacent direction vectors and If the angle is greater than the preset threshold , then mark is the initial inflection point, and the initial inflection point set is obtained ; Step S104. Add starting points separately and end point To the initial inflection point set The first and last positions of form a new trajectory set ; Step S105. Create a new optimized trajectory list ,index i =1; Step S106. For the new trajectory set , from the starting point First, check the line segments between the current point and subsequent inflection points in turn Whether to gather with obstacles obs intersect; if , that is, the line segment does not pass through the obstacle, then Move to the next inflection point and recheck whether the new line segment passes through the obstacle. If i=m+ 1, then finally ;like , that is, the line segment passes through the obstacle, then Back to the previous turning point ,and , , recheck whether the next line segment passes through the obstacle, until ; Step S107. Traverse all initial inflection points and finally generate an optimized trajectory list containing only the starting point, valid inflection points and end point. , where each line segment satisfies .

2. The novel trajectory inflection point correction method according to claim 1, characterized in that: In step S101 , the obstacle set is a polygon set or an occupied grid in a three-dimensional space.

3. The novel trajectory inflection point correction method according to claim 1, characterized in that: In step S103, adjacent direction vectors and Angle .

4. A smoothing method based on the novel trajectory inflection point correction method according to any one of claims 1 to 3, characterized in that: The following steps are involved: Step S201. The turning point , calculate the front and back direction vector and ; then normalize the vector, , ; Calculate the angle ; Step S202: Inflection point Angle bisector unit vector + , after normalization, ; Step S203. For a circle whose center is on the angle bisector and whose inflection point is The circle with tangent paths at both ends satisfies the radius ,definition λ is the smoothing coefficient; the coordinates of the circle center are ; Step S204. The arc starting angle is rotated from the inflection point forward vector direction to the backward vector direction with the center of the circle as the origin, covering the angle , parametric equation ,in , The unit vector pointing from the center of the circle to the inflection point The polar angle.

5. The smoothing method according to claim 4, wherein: In step S204, The calculation method is: ， ， ； ， ， ； ， 。

Images