Robot path tracking method

Abstract

This invention provides a robot path tracking method that improves the robot's adherence to a published path and addresses the problem of the robot not following along on paths with large curves. The method includes the following steps: obtaining the robot's global path and determining whether it is in straight-line mode or circular-arc mode based on the distance traveled; using a PID control algorithm to control linear and angular velocity in the straight-line mode; interpolating turning points using Bezier curves in the circular-arc mode; calculating the relative curvature of the fitted arc between the robot's current position and the local target point; and continuously determining whether the robot is approaching the current turning point during travel and adjusting its posture to ensure safe arrival at the destination.

Description

Technical Field

[0001] The invention relates to a robot path tracking method and belongs to the technical field of artificial intelligence. Background Art

[0002] With the development of technology, various robots have begun to appear in various places of life, replacing humans in some tasks. Service robots are gradually replacing some manual labor. Currently, robots are widely used in restaurants, hotels, hospitals, museums, cultural centers, government agencies, and other settings, providing services such as delivery, guidance, and explanation. Robots used in these scenarios need to overcome the limitations of their use space and move without tracks. In some application scenarios, robots need to move along user-specified paths and avoid moving into unnecessary areas.

[0003] In the original pure path tracking algorithm, the closest point on the specified path to the current robot (reference point) is first calculated. Then, a forward point (a specified distance L) is calculated from this closest point (the path is directed), and the calculated forward point is used as the target point for movement. This method is fully applicable to straight paths, but is not suitable for paths with large curvatures. When the path has large turns, the turning characteristics are ignored, resulting in the robot being unable to follow the path well around the large turns. Summary of the Invention

[0004] The purpose of the present invention is to provide a robot path tracking method to improve the robot's fit on the published path and solve the problem of the robot's non-fitting on paths with larger curvatures.

[0005] To achieve the above-mentioned purpose, the present invention is implemented through the following technical solutions:

[0006] A robot path tracking method comprises the following steps:

[0007] S1. Obtain the robot's global path and determine whether it is in straight line mode or circular mode based on the distance traveled.

[0008] S2. Linear mode uses the PID control algorithm to control linear and angular velocity;

[0009] S3. Walking in circular mode, using Bezier curves to interpolate turning points;

[0010] S4. Calculate the relative curvature of the arc between the robot's current position and the local target point;

[0011] S5. While walking in a straight line, the robot continuously checks whether it is approaching the current turning point. If it is 2 meters before the current turning point, it decelerates. After reaching the turning point, it stops moving and begins to rotate, directing the robot toward the new local target point set. During this period, the robot continuously checks whether it has reached the turning point. If the conditions are met, it begins to accelerate forward. The robot continuously checks whether it has reached the final target point. If it is m meters before the target point, it decelerates, and finally reaches the final position safely.

[0012] S6. During circular motion, the robot continuously checks whether it has reached its final target location. If it is m meters before the target, it decelerates until it safely reaches the target location and stops. In arc mode, the robot calculates a set of linear and angular velocities based on the curvature d of the current path to drive the robot through the corner.

[0013] The preferred embodiment of the robot path tracking method, step S1, has the following specific process:

[0014] S11. Search 5 meters ahead based on the robot's current position (x, y), and extract all points within this distance for judgment;

[0015] S12. Place the points in this distance and Calculate and save the slope between two points ;

[0016] S13. Traverse the slope between these two points. If the slope of the current and subsequent n k values ​​remains within the same range, then the distance pattern of this section of walking can be judged as a straight line pattern. ( is the slope threshold), the walking distance is in arc mode.

[0017] The preferred embodiment of the robot path tracking method, step S2, has the following specific process:

[0018] S21. Move forward to the next point at the initial speed V at the current position of the robot, obtain the current position of the robot in real time, and judge whether the robot is calculating the two points. and The distance D and the deflection angle β between them are calculated, and the points of the straight line are fitted into a straight line. The specific formula is as follows:

[0019] , and The distance between two poses;

[0020] , for and The slopes of the two poses;

[0021] Fitted straight line: ,in is the current position coordinate, is the position coordinate of the target point;

[0022] S22. Calculate the robot's position while the robot is walking Distance straight line : ;

[0023] S23. When the distance is less than the threshold a, the robot moves at a fixed speed. When the threshold is greater than a, the angular velocity Adjust according to the distance D. , k is the ratio coefficient, correct the angle ω to get closer to the straight line, move forward at speed V, and know the end position .

[0024] The preferred embodiment of the robot path tracking method, step S3, has the following specific process:

[0025] S31. The formula for the Serre curve is P = (1-t)² P0 + 2t(1-t)P1 + t²P2. Set the turning distance λ and calculate the control point based on the subscript of the turning point and λ. , The subscript of

[0026] S32. Input global path, turning point and two control points , ;

[0027] S33. Use the segmented reading method to obtain all interpolation points and original path points, excluding the interpolated points, as follows: the total number of segments is L = 2 × i + 1, the number of iterations is L times, when L is an even number, a straight line segment is loaded; when L is an odd number, an arc segment, i.e., an interpolation point, is loaded.

[0028] The preferred embodiment of the robot path tracking method, step S4, has the following specific process:

[0029] S41. Use the pure tracking model to track the preset global path, calculate the forward distance based on the current speed, set the maximum forward distance lookaheadmax = v × t + ζ and the minimum forward distance lookaheadmin = v × t - ζ, where t is the simulation time and ζ is the gain coefficient;

[0030] S42. Based on the forward distance and the current robot position, find the local target point to be tracked at the current moment in the local target point set. Calculate the relative curvature of the arc fitted between the robot's current position and the local target point, i.e., the ratio of the maximum distance of the straight line generated by the distance between the current point and the selected forward point to the distance between the current point and the selected forward point:

[0031] Reference Point , forward point , in the set of points set The maximum value is the maximum distance of the straight line generated by the distance between the current point and the selected forward point in calculating the relative curvature of the arc:

[0032] The expression for a straight line between two points:

[0033] ,

[0034] in The subscript of the max value for the point in the set This is the turning point.

[0035] S43. Calculate the robot's angular velocity ω = t*v*γ at the current moment, where t is the simulation time, v is the velocity threshold, and γ is the gain coefficient.

[0036] The advantages of the present invention are:

[0037] The calculation of the forward point is modified, and the concept of relative curvature is proposed. The position of the forward point is restricted according to the size of the distance, which improves the robot's fit on the published path and solves the problem of the robot not fitting on paths with larger curvatures. BRIEF DESCRIPTION OF THE DRAWINGS

[0038] The accompanying drawings are used to provide further understanding of the present invention and constitute a part of the specification. They are used to explain the present invention together with the embodiments of the present invention and do not constitute a limitation of the present invention.

[0039] Figure 1 Schematic diagram of relative curvature calculation method. DETAILED DESCRIPTION

[0040] The following will clearly and completely describe the technical solutions in the embodiments of the present invention in conjunction with the accompanying drawings. Obviously, the described embodiments are only part of the embodiments of the present invention, not all of the embodiments. Based on the embodiments of the present invention, all other embodiments obtained by ordinary technicians in this field without making creative efforts are within the scope of protection of the present invention.

[0041] As mentioned in the background technology of the present invention: in some application scenarios, the robot needs to move along the path specified by the user and not move into unnecessary areas. On paths with larger curvatures, the robot may not fit properly.

[0042] refer to Figure 1 The concept of relative curvature is introduced to refine the forward point calculation. Assuming the line segment between the reference point and the forward point is s, and the maximum distance between the reference point and the forward point is d, the relative curvature is d / s. In the improved algorithm, a greater relative curvature decreases the distance d between the forward point and the reference point, and vice versa. This allows the robot to better track various curvature radii when following the forward point.

[0043] In addition, in order to allow the robot to consider the final posture while tracking the forward point, the tangent of the path curve at the forward point is used as the expected direction of the forward point.

[0044] The present invention specifically adopts the following technical solutions:

[0045] A robot path tracking method comprises the following steps:

[0046] S1. The robot can walk in either a straight line or circular motion mode. During operation, the robot obtains its global path and determines whether it is in a straight line or circular motion mode based on the distance traveled.

[0047] S2. Linear mode uses the PID control algorithm to control linear and angular velocity;

[0048] S3. Walking in circular mode, using Bezier curves to interpolate turning points;

[0049] S4. Calculate the relative curvature of the arc between the robot's current position and the local target point;

[0050] S5. During the straight-line walking process, the robot is constantly judging whether it is approaching the current turning point. If it reaches 2 meters before the current turning point, it will decelerate. After reaching the turning point, it stops moving and starts rotating to make the robot face the new local target point set. During this period, the robot is constantly judging whether it has reached the turning point. If the conditions are met, it will start accelerating forward. The robot is constantly judging whether it has reached the final target point. Similarly, if it reaches m meters before the target point, it will decelerate and finally reach the final position safely. In the straight-line walking mode, the robot decelerates before the turning point, rotates after reaching the position, and then starts accelerating again. Finally, the linear velocity and angular velocity are controlled separately, and acceleration and deceleration are added to the rotation process to give the robot a certain buffer, ensuring the safety of the robot when turning.

[0051] S6. During circular travel, the robot continuously checks whether it has reached its final target point. If it is m meters before the target point, it decelerates until it safely reaches the target point and stops. In arc mode, the robot calculates a set of linear and angular velocities based on the curvature d of the current path to drive the robot through the corner. The speed throughout the corner is calculated by the robot and is not a set constant. This ensures that the robot does not over-curve when turning, ensuring safety and reliability. It also ensures that the robot is very stable during the cornering process, with a smooth trajectory and no speed jumps.

[0052] In this embodiment, the specific process of step S1 is as follows:

[0053] S11. Search 5 meters ahead based on the robot's current position (x, y), and extract all points within this distance for judgment;

[0054] S12. Place the points in this distance and Calculate and save the slope between two points ;

[0055] S13. Traverse the slope between these two points. If the slope of the current and subsequent n k values ​​remains within the same range, then the distance pattern of this section of walking can be judged as a straight line pattern. ( is the slope threshold), the walking distance is in arc mode.

[0056] In this embodiment, the specific process of step S2 is as follows:

[0057] S21. Move forward at the initial velocity V from the current position of the robot to the next point. Obtain the current position of the robot in real time. Calculate the distance D and the deflection angle β between the two points and fit the points of the line into a straight line. The specific formula is as follows:

[0058] and The distance between two poses;

[0059] , for and The slopes of the two poses;

[0060] Fitted straight line: ,in is the current position coordinate, is the position coordinate of the target point;

[0061] S22. Calculate the robot's position while the robot is walking Distance straight line : ;

[0062] S23. When the distance is less than the threshold a, the robot moves at a fixed speed. When the threshold is greater than a, the angular velocity Adjust according to the distance D. , k is the ratio coefficient, correct the angle ω to get closer to the straight line, move forward at speed V, and know the end position .

[0063] In this embodiment, the specific process of step S3 is as follows:

[0064] S31. The formula for the Serre curve is P = (1-t)² P0 + 2t(1-t)P1 + t²P2. Set the turning distance λ and calculate the control point based on the subscript of the turning point and λ. , The subscript of

[0065] S32. Input global path, turning point and two control points , ;

[0066] S33. Use the segmented reading method to obtain all interpolation points and original path points, excluding the interpolated points, as follows: the total number of segments is L = 2 × i + 1, the number of iterations is L times, when L is an even number, a straight line segment is loaded; when L is an odd number, an arc segment, i.e., an interpolation point, is loaded.

[0067] In this embodiment, the specific process of step S4 is as follows:

[0068] S41. Calculate the angular deviation between the yaw angle of the robot's current position and the local target point set and average the deviations. Set the linear velocity to 0 to ensure that the robot does not translate during the current turn. Calculate the angular velocity to rotate the robot to the local target point set position, thereby ensuring the robot's safety during the start. Use the pure tracking model to track the preset global path. Calculate the forward distance based on the current velocity. Set the maximum forward distance lookaheadmax = v × t + ζ and the minimum forward distance lookaheadmin = v × t - ζ, where t is the simulation time and ζ is the gain factor.

[0069] S42. Based on the forward distance and the current position of the robot, find the local target point to be tracked at the current moment in the local target point set. The threshold of the target point set is ε = max(L, W) / 2, where L and W represent the length and width of the local cost map, respectively, indicating that the local target point set is always on the local cost map. Calculate the relative curvature of the arc fitted between the robot's current position and the local target point, that is, the ratio of the maximum distance of the straight line generated by the distance between the current point and the selected forward point to the distance between the current point and the selected forward point:

[0070] Reference Point , forward point , in the set of points set The maximum value is the relative curvature of the arc:

[0071] A straight line between two points:

[0072] ,

[0073] in is a point in the set, and the subscript of the max value is the turning point;

[0074] S43. Calculate the robot's angular velocity ω = t*v*γ at the current moment, where t is the simulation time, v is the velocity threshold, and γ is the gain coefficient.

[0075] Finally, it should be noted that the above descriptions are merely preferred embodiments of the present invention and are not intended to limit the present invention. Although the present invention has been described in detail with reference to the aforementioned embodiments, those skilled in the art will be able to modify the technical solutions described in the aforementioned embodiments or substitute equivalents for some of the technical features. Any modifications, equivalent substitutions, and improvements made within the spirit and principles of the present invention shall be included within the scope of protection of the present invention.

Claims

1. A robot path tracking method, characterized in that: The following steps are involved: S1. Obtain the robot's global path and determine whether it is in straight line mode or circular mode based on the distance traveled. S2. Linear mode uses the PID control algorithm to control linear and angular velocity; S3. Walking in circular mode, using Bezier curves to interpolate turning points; S4. Calculate the relative curvature of the arc between the robot's current position and the local target point; S5. While walking in a straight line, the robot continuously checks whether it is approaching the current turning point. If it is 2 meters before the current turning point, it decelerates. After reaching the turning point, it stops moving and begins to rotate, directing the robot toward the new local target point set. During this period, the robot continuously checks whether it has reached the turning point. If the conditions are met, it begins to accelerate forward. The robot continuously checks whether it has reached the final target point. If it is m meters before the target point, it decelerates, and finally reaches the final position safely. S6. During circular motion, the robot continuously checks whether it has reached its final destination. If it is m meters before the destination, it decelerates until it safely reaches the destination and stops. In arc mode, the robot calculates a linear velocity and angular velocity based on the curvature d of the current path to drive the robot through the curve. The specific process of step S4 is as follows: S41. Use the pure tracking model to track the preset global path, calculate the forward distance based on the current speed, set the maximum forward distance lookaheadmax = v × t + ζ and the minimum forward distance lookaheadmin = v × t - ζ, where t is the simulation time and ζ is the gain coefficient; S42. Based on the forward distance and the current robot position, find the local target point to be tracked at the current moment in the local target point set. Calculate the relative curvature of the arc fitted between the robot's current position and the local target point, i.e., the ratio of the maximum distance of the straight line generated by the distance between the current point and the selected forward point to the distance between the current point and the selected forward point: Reference Point , forward point , in the set of points set The maximum value is the maximum distance of the straight line generated by the distance between the current point and the selected forward point in calculating the relative curvature of the arc: The expression for a straight line between two points: ， in The subscript of the max value for the point in the set This is the turning point.

2. The robot path tracking method according to claim 1, wherein: The specific process of step S1 is as follows: S11. Search 5 meters ahead based on the robot's current position (x, y), and extract all points within this distance for judgment; S12. Place the points in this distance and Calculate and save the slope between two points ; S13. Traverse the slope between these two points. If the previous and next n k values ​​remain within the same slope range, the distance pattern of this section of walking can be judged as a straight line pattern. After that, the walking distance is in arc mode. is the slope threshold.

3. The robot path tracking method according to claim 1, wherein: The specific process of step S2 is as follows: S21. Move forward to the next point at the initial speed V at the current position of the robot, obtain the current position of the robot in real time, and judge whether the robot is calculating the two points. and The distance D and the deflection angle β between them are calculated, and the points of the straight line are fitted into a straight line. The specific formula is as follows: , and The distance between two poses; , for and The slopes of the two poses; Fitted straight line: ,in is the current position coordinate, is the position coordinate of the target point; S22. Calculate the robot's position while the robot is walking Distance straight line Distance: ; S23. When the distance is less than the threshold a, the robot moves at a fixed speed. When the threshold is greater than a, the angular velocity Adjust according to the distance D. , k is the ratio coefficient, correcting the angular velocity To approach the straight line, move forward at speed V until the end position .

4. The robot path tracking method according to claim 1, wherein: The specific process of step S3 is as follows: S31. Bezier curve formula P = (1-t)² P0 + 2t(1-t)P1 + t²P2. Set the turning distance λ and calculate the control point based on the subscript of the turning point and λ. , The subscript of S32. Input global path, turning point and two control points , ; S33. Use the segmented reading method to obtain all interpolation points and original path points, excluding the interpolated points, as follows: the total number of segments is L = 2 × i + 1, the number of iterations is L times, when L is an even number, a straight line segment is loaded; when L is an odd number, an arc segment, i.e., an interpolation point, is loaded.

Images