Robot path following control method and device

Abstract

The invention relates to the technical field of robots, discloses a path following control method and device for a robot, and aims to solve the problem that the rapidity and accuracy of deviation rectification cannot be considered at the same time in existing path following. The scheme mainly comprises the following steps: uniformly dispersing a global path to be followed by the robot into a plurality of path points; acquiring a set of all path points from the path point closest to the current pose of the robot to the path point at the preset distance, and taking the set as a local path; if the first angle difference value between the direction of the local path and the direction of the robot is larger than a preset difference value, the robot is controlled to stop advancing and rotate to the direction of the local path; detecting a distance error between the pose of the robot and the local path, calculating a target normal velocity component, and calculating a corresponding target linear velocity and a target angular velocity; and driving the robot to follow the path according to the target linear velocity and the target angular velocity. According to the method, the rapidity and accuracy of robot deviation rectification are improved, and the method is suitable for various robots.

Description

Technical Field

[0001] The present application relates to the field of robot technology, and in particular to a path following control method and device for a robot. Background Art

[0002] Robot path following refers to making the robot move along a predetermined path and ensuring that the robot can accurately follow this path. Path following is an important part of robot navigation. It involves controlling the movement of the robot so that it can accurately follow the preset path. The implementation of path following usually requires the following steps: Define the path: First, you need to define a path in the environment where the robot works. This path can be a straight line, a curve, or any other shape, depending on the task requirements. Kinematic model: Establish a kinematic model of the robot. This model describes the robot's motion laws, including parameters such as linear velocity and angular velocity. Controller design: Based on the kinematic model, design a controller to calculate control commands. These commands will tell the robot how to adjust its motion parameters to ensure that it follows the predetermined path.

[0003] Since the robot's angular velocity and linear velocity are coupled to each other during the path following process, and the requirements for the speed and accuracy of the correction are high, it is difficult for the commonly used control methods to take both into account at the same time, and it is easy to have problems such as long correction response time, oscillation during the correction process, large path following error, or even loss of tracking. In the field of cleaning robots, if the correction time is too long or the following error is large during path following, it will not be possible to achieve full coverage cleaning, affecting the cleaning coverage rate of the cleaning robot; in the field of factory robots, if the robot's correction process has a long response time, oscillation during the correction process, large following error, or loss of tracking, it is easy to cause safety accidents such as collisions. Summary of the invention

[0004] The present application aims to solve the problem that the existing path following control cannot take into account both the rapidity and accuracy of deviation correction, and proposes a path following control method and device for a robot.

[0005] The technical solution adopted by this application to solve the above technical problems is:

[0006] In a first aspect, the present application provides a path following control method for a robot, the method comprising:

[0007] After the robot obtains the global path to be followed, the global path is evenly discretized into a plurality of path points;

[0008] Along the global path direction, all path point sets between the path point closest to the robot's current posture and the path point at a preset distance are obtained in real time, and used as the local path that the robot needs to follow;

[0009] If the first angle difference between the direction of the local path and the direction of the robot is greater than a preset difference, the robot is controlled to stop moving forward and rotate to the direction of the local path;

[0010] Decompose the robot's linear velocity into a normal velocity component and a parallel velocity component of the local path;

[0011] Detecting a distance error between the robot's posture and the local path, calculating a target normal velocity component according to the distance error, and calculating a corresponding target linear velocity and a target angular velocity according to the target normal velocity component;

[0012] The robot is driven to follow the path according to the target linear velocity and the target angular velocity.

[0013] Furthermore, if the distance between the path point closest to the current posture of the robot and the last path point of the global path is less than the preset distance, the set of all path points between the path point closest to the current posture of the robot and the last path point of the global path is obtained and used as the local path that the robot needs to follow.

[0014] Furthermore, the method for determining the direction of the local path includes:

[0015] Construct a vector set of the first path point of the local path and other path points, sum all the vectors in the vector set to obtain the sum vector, the angle of the sum vector in the world coordinate system is the direction of the local path, and the direction of the local path changes accordingly with the position of the robot in the local path.

[0016] Further, calculating the target normal velocity component according to the distance error specifically includes:

[0017] The adjustment amount of the normal velocity component is calculated according to the distance error and its change rate, and the adjustment amount is summed with the current normal velocity component of the robot to obtain the target normal velocity component.

[0018] Further, calculating the corresponding target linear velocity according to the target normal velocity component specifically includes:

[0019] The target angle of the target linear velocity is calculated according to the inverse cosine function of the ratio of the target normal velocity component to the current linear velocity of the robot, and the preset velocity value is used as the target size of the target linear velocity. When the angle of the linear velocity of the target size is rotated to the target angle, the corresponding normal velocity component is equal to the target normal velocity component.

[0020] Furthermore, the target angular velocity calculation method specifically includes:

[0021] A second angle difference between the target angle and the angle of the current linear velocity of the robot is calculated, and a target angular velocity is calculated according to the second angle difference and the target adjustment time.

[0022] Furthermore, the method further comprises:

[0023] According to the relationship between the direction of the local path and the direction of the robot, it is judged whether the robot is located on the left or right side of the local path, and the positive or negative value of the distance error is determined according to the judgment result.

[0024] Further, when the distance error is less than a preset distance error, the value of the distance error is set to zero.

[0025] Furthermore, the global path is a straight line segment, a broken line segment or a smooth curve.

[0026] In a second aspect, the present application provides a path following control device for a robot, wherein the device is used to implement the path following control method for a robot as described in the first aspect.

[0027] The beneficial effects of the present application are as follows: the path following control method and device of the robot provided by the present application obtains the global path, discretizes it and intercepts the local path from it, rotates the robot to the direction of the local path, decouples the robot's angular velocity and linear velocity, calculates the distance error between the robot and the global path, calculates the normal velocity component adjustment amount through an adaptive control algorithm, and finally converts the control amount into the chassis motion module control amount to drive the robot to follow the path. Compared with the traditional path tracking control method, the present application can better control the robot to stably follow the predetermined path, respond faster during the control process, and the error value between the robot's driving trajectory and the predetermined path is smaller, which improves the rapidity and accuracy of the deviation correction during path following, and improves the operation efficiency and safety of the robot. BRIEF DESCRIPTION OF THE DRAWINGS

[0028] Figure 1 A schematic flow chart of a path following control method for a robot provided in an embodiment of the present application. DETAILED DESCRIPTION

[0029] In order to enable those skilled in the art to better understand the solution of the present application, the technical solution in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application.

[0030] The technical solution of the embodiments of the present application is applicable to application scenarios where a robot needs to be controlled to travel along a predetermined path, such as a cleaning robot, a factory robot, etc.

[0031] Due to deviations in peripheral devices, mechanical devices, positioning, etc., the robot may deviate from the planned path when driving along the planned path. In order to ensure the normal operation of the robot, it is necessary to correct the robot when it deviates from the path. The current correction method is mainly the Pure Pursuit algorithm, and its main process includes: Determination of the preview point: Draw a circle with the current position of the robot as the center and the preview distance as the radius. The circle has three positional relationships with the planned path: intersection, tangent, or separation. Based on these three positional relationships, the preview point can be determined. Calculate the curvature: Calculate the curvature by connecting the preview point and the current robot position, and then calculate the wheel angle so that the robot can drive along the arc passing through the preview point. Path tracking: According to the calculated front wheel angle, control the robot to drive along the planned path. Since the Pure Pursuit algorithm is very sensitive to the preview distance, it is difficult to balance accuracy and timeliness. Specifically, the setting of the preview distance directly affects the performance of the algorithm. A larger preview distance can make the trajectory tracking smoother, but it may cause the tracking to be inaccurate. Although a smaller preview distance can improve the accuracy of tracking, it is easy to cause oscillation and increase the calculation processing time, making it impossible for the robot to track the path in time.

[0032] Based on this, the technical solution of the present application is proposed. In an embodiment of the present application, after the robot obtains the global path to be followed, the global path is evenly discretized into multiple path points; along the direction of the global path, all path point sets between the path point closest to the current posture of the robot and the path point at a preset distance are obtained in real time, and used as the local path that the robot needs to follow; if the first angle difference between the direction of the local path and the direction of the robot is greater than the preset difference, the robot is controlled to stop moving and rotate to the direction of the local path; the linear velocity of the robot is decomposed into a normal velocity component and a parallel velocity component of the local path; the distance error between the posture of the robot and the local path is detected, the target normal velocity component is calculated according to the distance error, and the corresponding target linear velocity and target angular velocity are calculated according to the target normal velocity component; the robot is driven to follow the path according to the target linear velocity and target angular velocity.

[0033] Specifically, the embodiment of the present application discretizes the global path into multiple path points, generates a corresponding local path in real time according to the current position and posture of the robot, and decomposes the robot's linear velocity into the normal velocity component and parallel velocity component of the local path; when the distance error between the robot's position and the local path is greater than a certain value, the robot is determined to have deviated from the path, and the target normal velocity component is calculated according to the distance error, and the corresponding target linear velocity and target angular velocity are calculated according to the target normal velocity component, and then the robot is driven to follow the path according to the target linear velocity and target angular velocity, so that the normal velocity component of the robot's linear velocity in the local path is equal to the target normal velocity component, thereby achieving deviation correction of the robot. The embodiment of the present application utilizes a decoupling control algorithm to achieve full-range following, rapid response and small error, thereby improving the rapidity and accuracy of path following.

[0034] The technical solutions in the embodiments of the present application will be clearly and completely described below in conjunction with the drawings in the embodiments of the present application. Obviously, the described embodiments are only part of the embodiments of the present application, rather than all of the embodiments.

[0035] See also Figure 1 The path following control method of the robot provided in the embodiment of the present application comprises the following steps:

[0036] S1. After the robot obtains a global path to be followed, the global path is evenly discretized into a plurality of path points.

[0037] In an embodiment of the present application, the global path can be a straight line segment, a broken line segment, or a smooth curve. It can be a path planned by a path planning algorithm, or it can be a curve drawn by the user through a tool, representing a set of path points from the starting point to the target point.

[0038] In practical applications, the global path can be discretized into multiple path points with a spacing of 0.02m, which are used to form multiple straight line segments with a length of 0.02m to represent the characteristics of the curve.

[0039] S2. Along the global path direction, obtain in real time the set of all path points between the path point closest to the current posture of the robot and the path point at a preset distance, and use it as the local path that the robot needs to follow.

[0040] When the robot is driving along the global path, the robot's position and the path point closest to the robot are determined in real time. Along the global path direction, the set of all path points within a preset distance from the nearest path point is determined. For example, starting from the nearest path point, the path point ends at a distance of 6 meters, where the set of all path points is the local path.

[0041] If the distance between the path point closest to the current posture of the robot and the last path point of the global path is less than the preset distance, then obtain the set of all path points between the path point closest to the current posture of the robot and the last path point of the global path, and use it as the local path that the robot needs to follow.

[0042] S3. If the first angle difference between the direction of the local path and the direction of the robot is greater than a preset difference, the robot is controlled to stop moving forward and rotate to the direction of the local path.

[0043] In practical applications, when the angle difference between the robot direction and the local path direction is greater than a preset difference (for example, 90°), in order to prevent the robot from losing the path due to untimely turning, the robot needs to stop and rotate to the direction of the local path.

[0044] The method for determining the direction of the local path includes:

[0045] Construct a vector set of the first path point of the local path and other path points, sum all the vectors in the vector set to obtain a sum vector, the angle of the sum vector in the world coordinate system is the direction of the local path, and the direction of the local path changes accordingly with the position of the robot in the local path. For example, the local path includes 5 path points, namely path point 1, path point 2, path point 3, path point 4, and path point 5, then construct the vectors of path point 1 and path point 2, path point 1 and path point 3, path point 1 and path point 4, and path point 1 and path point 5 respectively, and then sum these vectors to obtain a sum vector, and the angle of the sum vector in the world coordinate system is the direction of the local path.

[0046] The direction of the robot refers to the angle of the robot's current driving direction in the world coordinate system.

[0047] S4. Decompose the linear velocity of the robot into the normal velocity component and the parallel velocity component of the local path.

[0048] Specifically, by calculating the angle of the robot's posture relative to the direction of the local path, the robot's linear velocity is decomposed into a normal velocity component in the local path direction and a parallel velocity component in the local path direction. The robot's linear velocity is decomposed so that only the normal component is controlled during posture control, avoiding the angular velocity and linear velocity from affecting the robot's posture at the same time.

[0049] S5. Detect the distance error between the robot's posture and the local path, calculate the target normal velocity component according to the distance error, and calculate the corresponding target linear velocity and target angular velocity according to the target normal velocity component.

[0050] In practical applications, the distance error between the robot's current position and the local path is detected in real time. The distance error refers to the vertical distance between the robot's current position and the straight line in the direction of the local path. It is used to indicate the deviation between the robot's current position and the local path. The larger the distance error, the greater the deviation between the robot's current position and the local path, and vice versa.

[0051] In an embodiment of the present application, whether the robot is located on the left or right side of the local path is judged based on the relationship between the direction of the local path and the direction of the robot, and the positive or negative value of the distance error is determined based on the judgment result. When the robot is on the left side of the local path, the distance error is positive, and when the robot is on the left side of the local path, the distance error is positive or negative.

[0052] The relationship between the direction of the local path and the direction of the robot refers to the relationship between the angle of the robot pointing to the local path and the sum vector of the local path. The sum vector from the robot to the local path is obtained by constructing a vector set of the robot's current position and all points on the local path, and then cross-producting it with the sum vector of the local path. If the cross product result is less than 0, the robot is on the right side of the local path; if the cross product result is greater than 0, the robot is on the left side of the local path; if the cross product result is equal to 0, the robot is on the local path.

[0053] In the embodiment of the present application, when the distance error is less than the preset distance error, the value of the distance error is set to zero to prevent repeated oscillations in the control dead zone from affecting the driving smoothness of the robot.

[0054] After obtaining the real-time distance error of the robot, the adjustment amount of the normal velocity component is calculated according to the distance error and its change rate, and the adjustment amount is summed with the current normal velocity component of the robot to obtain the target normal velocity component.

[0055] Calculating the target linear velocity and target angular velocity means that when the robot travels at the target linear velocity, the normal velocity component after decomposition of the linear velocity is equal to the target normal velocity component, that is, the direction of the linear velocity needs to be rotated to the target angle.

[0056] The method for determining the target angle can be to calculate the target angle of the target linear velocity according to the inverse cosine function of the ratio of the target normal velocity component to the current linear velocity of the robot after obtaining the target velocity component, and use the preset velocity value as the target magnitude of the target linear velocity. When the angle of the linear velocity of the target magnitude is rotated to the target angle, the corresponding normal velocity component is equal to the target normal velocity component. In the embodiment of the present application, the target magnitude of the target linear velocity is maintained at a constant speed of 0.25 m / s.

[0057] The method for determining the target angular velocity includes: calculating a second angle difference between the target angle and the angle of the robot's current linear velocity, calculating the target angular velocity based on the second angle difference and the target adjustment time, and dividing the second angle difference by the target adjustment time to obtain the target angular velocity. The target adjustment time can be set according to actual conditions, for example, 1 second.

[0058] S6. Drive the robot to follow the path according to the target linear velocity and target angular velocity.

[0059] After obtaining the target linear velocity and target angular velocity, the target angular velocity and target linear velocity are converted into control quantities of the chassis motion module according to the robot's chassis motion model, and the robot is driven to follow the path; if the local path length is less than a certain threshold, or the robot reaches the target point within a certain threshold range, the path following task is completed.

[0060] In practical applications, if the robot's posture control value exceeds the actual control response range of the chassis, the robot's linear speed can be reduced to meet the angular velocity control value and avoid large errors during the robot's driving process.

[0061] The chassis motion module can be a two-wheel differential chassis or a crawler chassis. In actual applications, if there are less than two path points in the set corresponding to the local path, a line segment cannot be formed. At this time, it can be judged that the robot has reached the vicinity of the target point; when the distance between the robot position and the end point of the path is less than 3cm, it can be judged that the robot has reached the vicinity of the target point. If it is judged that the robot has not reached the vicinity of the target point, the above steps are executed again until the robot reaches the vicinity of the target point and completes this path following task.

[0062] In summary, the path following control method of the robot provided in the embodiment of the present application obtains the global path, discretizes it and intercepts the local path from it, rotates the robot to the direction of the local path, decouples the robot's angular velocity and linear velocity, calculates the distance error between the robot and the global path, calculates the normal velocity component adjustment amount through an adaptive control algorithm, and finally converts the control amount into the chassis motion module control amount, drives the robot to follow the path until the local path length is less than a certain threshold, or the robot reaches the target point within a certain threshold range, and completes the path following task. Compared with the traditional path tracking control method, the present application can better control the robot to stably follow the predetermined path, respond faster during the control process, and the error value between the robot's driving trajectory and the predetermined path is smaller, which improves the rapidity and accuracy of the deviation correction during path following, and improves the operation efficiency and safety of the robot.

[0063] Based on the above technical solution, an embodiment of the present application further proposes a path following control device for a robot, and the device is used to implement the path following control method for a robot as described in the embodiment of the present application.

[0064] It can be understood that since the path following control device of the robot described in the embodiment of the present application is a device for implementing the path following control method of the robot described in the embodiment, for the device disclosed in the embodiment, since it corresponds to the method disclosed in the embodiment, the description is relatively simple, and the relevant parts can be referred to the partial description of the method, which will not be repeated here.

Claims

1. A path following control method for a robot, characterized in that: The method comprises: After the robot obtains the global path to be followed, the global path is evenly discretized into a plurality of path points; Along the global path direction, all path point sets between the path point closest to the robot's current posture and the path point at a preset distance are obtained in real time, and used as the local path that the robot needs to follow; If the first angle difference between the direction of the local path and the direction of the robot is greater than a preset difference, the robot is controlled to stop moving forward and rotate to the direction of the local path; Decompose the robot's linear velocity into the normal velocity component and the parallel velocity component of the local path; Detecting a distance error between the robot's posture and the local path, calculating a target normal velocity component according to the distance error, and calculating a corresponding target linear velocity and a target angular velocity according to the target normal velocity component; The robot is driven to follow the path according to the target linear velocity and the target angular velocity.

2. The path following control method of a robot according to claim 1, characterized in that: If the distance between the path point closest to the current posture of the robot and the last path point of the global path is less than the preset distance, then obtain the set of all path points between the path point closest to the current posture of the robot and the last path point of the global path, and use it as the local path that the robot needs to follow.

3. The path following control method of a robot according to claim 1, characterized in that: The method for determining the direction of the local path includes: Construct a vector set of the first path point of the local path and other path points, sum all the vectors in the vector set to obtain the sum vector, the angle of the sum vector in the world coordinate system is the direction of the local path, and the direction of the local path changes accordingly with the position of the robot in the local path.

4. The path following control method of a robot according to claim 1, characterized in that: Calculating the target normal velocity component according to the distance error specifically includes: The adjustment amount of the normal velocity component is calculated according to the distance error and its change rate, and the adjustment amount is summed with the current normal velocity component of the robot to obtain the target normal velocity component.

5. The path following control method of a robot according to claim 1, characterized in that: Calculating the corresponding target linear velocity according to the target normal velocity component specifically includes: The target angle of the target linear velocity is calculated according to the inverse cosine function of the ratio of the target normal velocity component to the current linear velocity of the robot, and the preset velocity value is used as the target size of the target linear velocity. When the angle of the linear velocity of the target size is rotated to the target angle, the corresponding normal velocity component is equal to the target normal velocity component.

6. The path following control method of a robot according to claim 5, characterized in that: The target angular velocity calculation method specifically includes: A second angle difference between the target angle and the angle of the current linear velocity of the robot is calculated, and a target angular velocity is calculated according to the second angle difference and the target adjustment time.

7. The path following control method of a robot according to claim 1, characterized in that: The method further comprises: According to the relationship between the direction of the local path and the direction of the robot, it is judged whether the robot is located on the left or right side of the local path, and the positive or negative value of the distance error is determined according to the judgment result.

8. The path following control method of a robot according to claim 1, characterized in that: When the distance error is less than a preset distance error, the value of the distance error is set to zero.

9. The path following control method of a robot according to claim 1, characterized in that: The global path is a straight line segment, a broken line segment or a smooth curve.

10. A path following control device for a robot, characterized in that: The device is used to implement the path following control method of the robot as described in any one of claims 1 to 9.

Images