Low-speed unmanned vehicle trajectory smoothing method based on arc interpolation

Abstract

The invention relates to the field of low-speed unmanned driving in a dynamic scene, in particular to a low-speed unmanned vehicle track smoothing method based on arc interpolation, which comprises the following steps: acquiring an originally planned path point set path; traversing the original path point set path to generate a smooth trajectory; and outputting a smooth trajectory after traversing all the original path points. Compared with the prior art, the method has the advantages that track curvature continuity is realized, simplicity and adaptability are considered, and driving smoothness and navigation accuracy are improved; the problems that in the prior art, unreasonable oscillation exists, calculation complexity is high, stable running and trajectory tracking of the vehicle are not facilitated, and real-time performance is poor are solved.

Description

Technical Field

[0001] This invention relates to the field of intelligent path planning, and more specifically, to a method for smoothing the trajectory of low-speed autonomous vehicles based on circular arc interpolation. Background Technology

[0002] Existing technologies have limitations: polynomial fitting is prone to oscillation and computationally complex; the improved A* and JPS algorithms produce "jagged" paths, which are not conducive to smooth driving and tracking; the Bézier curve method requires multiple iterations, has poor real-time performance, and poses a collision risk in obstacle-dense scenarios; to address these limitations, a trajectory smoothing method based on circular arc interpolation has emerged. This method achieves continuous trajectory curvature by interpolating adapted and tangent circular arcs between the original path segments, balancing simplicity and adaptability, and improving driving smoothness and navigation accuracy. Summary of the Invention

[0003] The existing technology suffers from problems such as unreasonable oscillations, high computational complexity, which are detrimental to smooth vehicle driving and trajectory tracking, and poor real-time performance.

[0004] To address the above problems, the present invention adopts the following technical solution: a method for smoothing the trajectory of a low-speed unmanned vehicle based on circular arc interpolation, comprising: Obtain the original planned path point set path and the vehicle's minimum turning radius R; Based on the vehicle's minimum turning radius R, the original path point set path is traversed to generate a smooth trajectory with circular arc connections. The output is a smooth trajectory with circular arc connections.

[0005] Preferably, obtaining the original planned path point set `path` and the vehicle's minimum turning radius `R` includes: Get collection path=[ , ... ], obtain each point =( , ), , They are respectively In a planar coordinate system axis, The coordinate values ​​of the axis are used to determine the positional information of the vehicle's original driving path; Obtaining the vehicle's minimum turning radius R, and calculating R based on the Ackermann steering principle, ensures that the subsequent arc radius conforms to the vehicle's dynamic constraints, thus avoiding loss of vehicle control due to an excessively small turning radius.

[0006] Preferably, based on the vehicle's minimum turning radius R, traversing the original path point set path to generate a smooth trajectory with circular arc connections includes: Based on the original planned path point set `path`, calculate the direction and normal vector of the current line segment, and then select the current point. and the next point Calculate the vector between two points. This vector represents a line segment. The direction generates perpendicular to The unit normal vector, Let be the left normal vector, where For vectors exist axis, The directional component on the axis has an absolute value equal to the length of the projection line segment of the X-axis and Y-axis, and its sign is determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative). For the length of the module, Taking into account the bidirectional possibility of vehicle steering, and to ensure the integrity and applicability of the trajectory smoothing scheme, it is also necessary to calculate the right normal vector. (The direction is opposite to the left normal vector); Based on the vehicle's minimum turning radius R, calculate the coordinates of the candidate circle center, using the normal vector and radius. Calculate the left candidate circle center and the center of the candidate circle on the right This takes into account the two-way possibility of vehicle steering, ensuring the integrity and applicability of the trajectory smoothing scheme; from Start traversing subsequent path points Determine if there are any cases that can be connected by circular arcs. Calculate... To the left center distance to the center of the right circle distance .like Then determine It can be connected by the left-side arc, if Then determine It can be connected by the right-side arc; Generate the left arc and replace the original path segment, calculate arrive vector Its angle ,calculate arrive vector Its angle Generate from arrive Uniform angle sequence = , and the original path segment arrive Ensure the number of points is consistent to match the point density of the arc with the original path point density, and calculate the coordinates of each point on the arc. Replace the smooth path with arc points arrive The line segment, that is And index Jump to Skip the points that have already been processed; Generate left and right arcs and replace the original path segments whose arcs are on the same side as the original path. When a right arc endpoint that meets the conditions is found... At that time, the processing logic is the same as that of the left arc.

[0007] Preferably, the output having a smooth trajectory with circular arc connections includes: After traversing all waypoints, output the final smooth path. Continuous straight line segments and circular arc segments, with the circular arc segments being tangent to adjacent line segments, ensure the continuity of the overall trajectory curvature.

[0008] Preferably, the minimum turning radius R includes: The minimum turning radius of the vehicle is calculated based on the Ackermann steering principle; Formula for calculating turning radius:

[0009] in, This refers to the vehicle's wheelbase. This is the maximum turning angle of the wheel.

[0010] Preferably, calculating the direction and normal vector of the current line segment includes: According to the previous point and back point Calculate the vector between two points :

[0011] in, Indicates the aforementioned front point and back point Vectors between; This represents the "current path point" in the original planned path point set; This represents the "next path point of the current path point" in the original planned path point set; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; According to the formula, exist Direction vector of the axis = and in Direction vector of the axis = The absolute value equals shaft and The length of the projection line segment of the axis is determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative). Then based on vectors exist axis, Directional components on the axis Its absolute value is equal to shaft and The length of the projection line segment of the axis is determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative). The modulus can be calculated using the formula. :

[0012] in, For the length of the module, For vectors exist axis, Directional components on the axis; Represents line segment The direction generates perpendicular to Unit normal vector:

[0013] in, Represents the left normal vector; For vectors exist axis, Directional components on the axis; This represents the vector between the current path point and the next path point; Considering the bidirectional possibility of vehicle steering, and to ensure the integrity and applicability of the trajectory smoothing scheme, it is also necessary to calculate the right normal vector. (The direction is opposite to the left normal vector).

[0014] Preferably, the calculation of the candidate circle center coordinates includes: Based on the normal vector and radius Calculate the center of the candidate circle on the left:

[0015] in, Indicates the center of the candidate circle on the left; This represents the "current path point" in the original planned path point set; Represents the left normal vector; The radius is ; Calculate the candidate center on the right:

[0016] in, Indicates the center of the candidate circle on the right; This represents the "current path point" in the original planned path point set; Represents the left normal vector; Let be the radius.

[0017] Preferably, the smooth trajectory with arc connections includes: Based on the calculated candidate circle center coordinates, the bidirectional possibility of vehicle turning is considered to ensure the integrity and adaptability of the trajectory smoothing scheme. from Start traversing subsequent path points ( from arrive Determine if there are any cases that can be connected by an arc; If they exist, generate left and right arcs and replace the original path segment with the arc located on the same side as the original path.

[0018] Compared with existing technologies, this invention provides a low-speed autonomous vehicle trajectory smoothing method based on circular arc interpolation, which has the following beneficial effects: The present invention provides a low-speed autonomous vehicle trajectory smoothing method based on circular arc interpolation. By optimizing the vehicle trajectory, it significantly improves the smoothness of the low-speed autonomous vehicle trajectory. On the one hand, by replacing straight segments with circular arcs, the curvature of the path is ensured to avoid sharp turns, which improves driving comfort and makes the smoothed path conform to vehicle dynamics constraints to reduce the risk of loss of control. At the same time, the tangency of the circular arc with the original path ensures that the trajectory deviates little from the original path, which is suitable for the precise navigation needs of low-speed vehicles. On the other hand, the engineering implementation is simple and efficient. Traversing all original path points only requires adjusting one main parameter, the radius of the circular arc, which reduces redundant calculations and processor load. The method can automatically identify smoothable segments and maintain good performance in complex paths. In addition, the radius of the circular arc can be dynamically adjusted according to vehicle performance (such as minimum turning radius) and scenario requirements, making it widely applicable and well-suited for low-speed autonomous vehicles operating in dynamic scenarios such as sweepers, roaming vehicles, and sightseeing vehicles. Attached Figure Description

[0019] Figure 1 This is a flowchart illustrating a low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to the present invention. Figure 2 This is a schematic diagram illustrating the process of generating a smooth trajectory for a low-speed unmanned vehicle based on circular interpolation according to the present invention. Detailed Implementation

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

[0021] Please see Figure 1 A method for smoothing the trajectory of low-speed autonomous vehicles based on circular arc interpolation, comprising: S100. First, obtain the original planned path point set `path`, and divide the planned path into multiple points. This set of points is called `path=`. , where each point = , representing that point The x-axis and y-axis coordinates in the planar coordinate system are used to determine the position information of the vehicle's original driving path; simultaneously, the vehicle's minimum turning radius R is obtained, which is calculated based on the Ackermann steering principle, using the following formula:

[0022] Where R represents the minimum turning radius of the vehicle, and L is the wheelbase of the vehicle (an inherent parameter of the vehicle). This is the maximum turning angle of the wheel (the physical limit of the vehicle's steering system).

[0023] Calculating R using this formula ensures that the subsequent radius of curvature conforms to vehicle dynamics constraints, preventing loss of vehicle control due to an excessively small turning radius.

[0024] S200. Based on the vehicle's minimum turning radius R, traverse the original path point set path to generate a smooth trajectory with circular arc connections, and take the current path point from the original path point set. and the next waypoint Calculate the vector between two points. The vector It directly reflects the direction of the line segment, providing a benchmark for subsequent normal vector calculations; further calculations... exist Directional components of the axis = , The directional component of the axis = , whose absolute value corresponds to the length of the projection line segment, and the positive and negative are determined by the angle between the vector and the coordinate axis (positive for acute angles and negative for obtuse angles).

[0025] Then calculate through the formula The modulus length of :

[0026] Used to unitize the normal vector; based on and generate a left normal vector perpendicular to :

[0027] At the same time, calculate the right normal vector: <00​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​​ vector Its angle Generate from arrive Uniform angle sequence = , and the original path segment arrive Ensure the number of points is consistent to match the point density of the arc with the original path point density, and calculate the coordinates of each point on the arc. Replace the smooth path with arc points arrive The line segment, that is And index Jump to Skip the points that have already been processed.

[0032] S300 After traversing all path points in the path, output the final smooth trajectory path with circular arc connections. This path contains continuous straight line segments and circular arc segments, and the circular arc segments are tangent to adjacent straight line segments. The tangent design makes the trajectory curvature smoothly transition from straight line segments to circular arc segments without abrupt changes, avoiding sudden turns of the roaming vehicle that may cause passenger discomfort or loss of control. At the same time, the circular arcs deviate little from the original path, ensuring that the roaming vehicle can accurately travel along the planned route of the park, meeting the precise navigation needs of low-speed vehicles.

[0033] S101. Generate the original path point set path=[ for park navigation using the global path planning algorithm of the roaming vehicle (such as the improved A* algorithm). , ... ], where each path point = , , The coordinate system is a plane coordinate system (with the park entrance as the origin and the eastward direction as the coordinate system). The positive direction of the axis is north. The coordinates of the axis in the positive direction.

[0034] S102. Because the rovers must conform to their own dynamic constraints, if the turning radius is too small, the vehicle's steering system may fail or become uncontrollable. Therefore, the minimum turning radius is calculated based on the Ackermann steering principle. . L, Substitute into the formula

[0035] in, This refers to the vehicle's wheelbase. This is the maximum turning angle of the wheel.

[0036] Calculated Considering obstacles and driving safety in the park setting (a value less than the calculated value can further reduce the risk of loss of control, while still meeting the vehicle's steering capability).

[0037] Please see Figure 2 Based on the vehicle's minimum turning radius R, the original path point set path is traversed to generate a smooth trajectory with circular arc connections, including: S201, from the original planned set of path points path= Select the current path point. and the next waypoint This constitutes the current line segment to be calculated. .

[0038] Each waypoint The coordinates are ( , ),in , These are coordinate values ​​in a planar coordinate system, directly reflecting the spatial position of a point; The reason for selecting two adjacent points is that the original path is composed of continuous line segments. The line segments formed by adjacent points are the basic processing units for trajectory smoothing and conform to the actual motion logic of the vehicle "driving segment by segment".

[0039] The direction vector is a line segment The mathematical expression for the direction is calculated using the difference in coordinates between two points, and the formula is:

[0040] in, Indicates the aforementioned front point and back point Vectors between; This represents the "current path point" in the original planned path point set; This represents the "next path point of the current path point" in the original planned path point set; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; vector Two components = (x-axis component) = (y-axis component), its physical meaning is: Absolute value: corresponding line segment The projected lengths on the x and y axes reflect the distance the line segment extends in the two coordinate axis directions; Positive and negative sign: determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative), for example >0 indicates that the line segment extends along the positive x-axis (e.g., eastward). <0 indicates that the line extends along the negative y-axis (such as southward), directly reflecting the spatial direction of the line segment.

[0041] Then based on vectors exist axis, Directional components on the axis Its absolute value is equal to shaft and The length of the projection line segment of the axis is determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative). The modulus can be calculated using the formula. :

[0042] in, For the length of the module, For vectors exist axis, Directional components on the axis; Represents line segment The direction generates perpendicular to Unit normal vector:

[0043] in, Represents the left normal vector; For vectors exist axis, Directional components on the axis; This represents the vector between the current path point and the next path point; Considering the bidirectional possibility of vehicle steering, and to ensure the integrity and applicability of the trajectory smoothing scheme, it is also necessary to calculate the right normal vector. (The direction is opposite to the left normal vector).

[0044] S202. The essence of calculating the coordinates of the candidate circle center is "using the current path point as a reference, offsetting the vehicle's minimum turning radius R along the left / right normal vector direction to determine the center of the arc". The core logic revolves around "direction reference (normal vector) + distance constraint (R)". First, it distinguishes whether the normal vector obtained in the previous step is the left or right normal vector, and substitutes it into different calculation methods according to the type of normal vector to calculate the left candidate circle center:

[0045] in, Indicates the center of the candidate circle on the left; This represents the "current path point" in the original planned path point set; Represents the left normal vector; The radius is ; Calculate the candidate center on the right:

[0046] in, Indicates the center of the candidate circle on the right; This represents the "current path point" in the original planned path point set; Represents the left normal vector; Let be the radius.

[0047] S203, from Start traversing subsequent path points (j from i+1 to n) Determining whether an arc can connect the two circles requires considering the previously calculated left / right candidate center points. , The minimum turning radius R of the vehicle is achieved by first using the formula:

[0048] in, express To the left center The distance; Indicates the subsequent path points during the traversal; Indicates the center of the left circle;

[0049] in, express To the center of the right circle The distance; Indicates the subsequent path points during the traversal; Indicates the center of the right circle; Calculate pⱼ to pⱼ respectively , The distance is then used to determine the relationship between the distance and R. <R indicates that pⱼ is within a circle centered at with a radius of R, and it is determined that pⱼ can be connected by the left arc; if < R, then pⱼ is within a circle centered at with a radius of R, and it is determined that pⱼ can be connected by the right arc. Specifically in implementation, taking the park rover as an example, in the vehicle-mounted controller program, first read the previously stored 、 coordinates and R, then loop to read the subsequent pⱼ coordinates, call the mathematical library function to calculate 、 , and output the determination result by comparing with R and with R. At the same time, verify the calculation accuracy to ensure accurate determination, and once the first pⱼ that does not satisfy < R or < R is found during the traversal, stop the current traversal to reduce redundant calculations and adapt to the real-time trajectory update requirements of low-speed vehicles.

[0050] S204. Generate the left arc and replace the original path segment, calculate the vector from to , its angle , calculate the vector from to , its angle . Generate a uniform angle sequence from to = , which is the same as the number of points of the original path segment from to ensure that the arc point density matches the original path point density, calculate the coordinates of each point on the arc​​​​​​​​​​​​​​​​S301. After traversing all path points, output the final smooth path with arc connections. First, complete the arc interpolation processing of all path points: During the traversal, for each original path segment that meets the conditions (the segment from pᵢ to pⱼ that can be connected by left / right arcs), it has been replaced with the generated tangent arcs. Path segments that do not meet the arc connection conditions retain the original straight line segments, and finally form a smooth path set smoothpath composed of "straight line segments + tangent arc segments".

[0052] In practical implementation, taking low-speed vehicles such as park sweepers as an example, the on-board controller program first caches the path points processed at each step through an array (the arc segment points replace the original pᵢ to pⱼ segment points, and the unreplaced segments retain the original points). After traversing all n original path points in the path, the cached smoothpath set is checked for curvature continuity (ensuring that the arc segment is tangent to the adjacent straight line segment and there is no curvature change) and coordinate validity (excluding abnormal points that exceed the scene boundary or are close to obstacles). After the verification is passed, the smoothpath is output to the vehicle motion control module in the form of a coordinate sequence to provide the vehicle with a real-time driving trajectory reference, which is suitable for the smooth driving and accurate navigation requirements of low-speed unmanned vehicles. In addition, the path point density is kept consistent with the original path during the output process to avoid the impact of uneven point density on trajectory tracking accuracy.

[0053] The implementation of S302, "continuous straight line segments and arc segments with the arc segment tangent to the adjacent line segment", is based on the geometric characteristics of arc interpolation and path processing logic. The arc segment is generated based on the left / right candidate center calculated in the previous order (along the direction of the normal vector of the current line segment and the distance from the current point is the minimum turning radius R). Its starting point is the current path point pᵢ, and pᵢ is also the endpoint of the previous straight line segment (or arc segment), ensuring that the line segment and the arc segment are continuous at pᵢ. Moreover, the direction of the arc radius (the vector from the center to pᵢ) is perpendicular to the direction vector of the current line segment (because the center is generated along the normal vector of the line segment), which naturally satisfies that the arc segment and the current line segment are tangent at pᵢ. Similarly, the endpoint pⱼ of the arc segment is also tangent to the next straight line segment (or arc segment) through the same logic. Path segments that do not meet the arc connection conditions retain the original straight line segments, and finally form a continuous combination of straight line segments and arc segments.

[0054] In practical implementation, taking the park's roaming vehicle as an example, in the vehicle controller program, after generating the arc, it is first verified by vector dot product (at the starting point of the arc, the dot product of the line segment direction vector and the vector from the center of the circle to the starting point is close to 0) to ensure tangency. Then, the arc segment points and the reserved straight line segment points are stored in the smoothpath array in order. After traversing all path points, the consistency of the coordinates of the connection points between adjacent line segments and arc segments is further verified. After confirming that there are no breaks and curvature abrupt changes, the output is performed to adapt to the requirements of smooth steering and accurate tracking of low-speed vehicles.

[0055] Those skilled in the art will understand that embodiments of this application can be provided as methods, systems, computer program products, or electronic devices. Therefore, this application can take the form of a completely hardware embodiment, a completely software embodiment, or an embodiment combining software and hardware aspects. Furthermore, this application can take the form of a computer program product implemented on one or more computer-usable storage media (including but not limited to disk storage, CD-ROM, optical storage, etc.) containing computer-usable program code.

Claims

1. A method for smoothing the trajectory of a low-speed autonomous vehicle based on circular arc interpolation, characterized in that, include: Obtain the original planned path point set path and the vehicle's minimum turning radius R; Based on the vehicle's minimum turning radius R, the original path point set path is traversed to generate a smooth trajectory with circular arc connections. The output is a smooth trajectory with circular arc connections.

2. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 1, characterized in that, The process of obtaining the original planned path point set (path) and the vehicle's minimum turning radius (R) includes: Get collection path=[ , ... ] and each point =( , ); Obtain the vehicle's minimum turning radius R.

3. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 2, characterized in that, The step of traversing the original path point set path based on the vehicle's minimum turning radius R includes generating a smooth trajectory with circular arc connections: Based on the original planned path point set path, calculate the direction and normal vector of the current line segment; Calculate the coordinates of the candidate circle center based on the vehicle's minimum turning radius R; from Start traversing subsequent path points Determine if there are any cases that can be connected by an arc; Generate left and right arcs, and replace the original path segment with the arc located on the same side as the original path.

4. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 3, characterized in that, The output includes a smooth trajectory with circular arc connections, including: A continuous line segment and a circular arc segment, wherein the circular arc segment is tangent to the adjacent line segment; The final output is a smooth path with circular connections.

5. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 3, characterized in that, The minimum turning radius R includes: The minimum turning radius of the vehicle is calculated based on the Ackermann steering principle; Formula for calculating turning radius: ; in, This refers to the vehicle's wheelbase. This is the maximum turning angle of the wheel.

6. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 3, characterized in that, The calculation of the direction and normal vector of the current line segment includes: According to the previous point and back point Calculate the vector between two points : ； in, Indicates the aforementioned front point and back point Vectors between; This represents the "current path point" in the original planned path point set; This represents the "next path point of the current path point" in the original planned path point set; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the current path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; This indicates that the next path point is in the planar coordinate system. The coordinate values ​​of the axis; According to the formula, exist Direction vector of the axis = and in Direction vector of the axis = The absolute value equals shaft and The length of the projection line segment of the axis is determined by the angle between the vector and the coordinate axis (acute angle is positive, obtuse angle is negative). Then based on vectors exist axis, Directional components on the axis Calculate the modulus using the formula. : ； in, For the length of the module, For vectors exist axis, Directional components on the axis; Represents line segment The direction generates perpendicular to Unit normal vector: ； in, Represents the left normal vector; For vectors exist axis, Directional components on the axis; This represents the vector between the current path point and the next path point; Taking into account the two-way possibility of vehicle steering, the right normal vector also needs to be calculated. (The direction is opposite to the left normal vector).

7. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 5, characterized in that, The calculation of the candidate circle center coordinates includes: Calculate the left candidate circle center based on the normal vector and the radius R: ； in, Indicates the center of the candidate circle on the left; This represents the "current path point" in the original planned path point set; Represents the left normal vector; The radius is ; Calculate the candidate center on the right: ； in, Indicates the center of the candidate circle on the left; This represents the "current path point" in the original planned path point set; Represents the left normal vector; Let be the radius.

8. The low-speed unmanned vehicle trajectory smoothing method based on circular arc interpolation according to claim 4, characterized in that, The smooth trajectory with circular arc connections includes: Based on the calculated candidate circle center coordinates, the bidirectional possibility of vehicle turning is considered to ensure the integrity and adaptability of the trajectory smoothing scheme. from Start traversing subsequent path points ( from arrive Determine if there are any cases that can be connected by an arc; If they exist, generate left and right arcs and replace the original path segment with the arc located on the same side as the original path.

Images