A tracking control method of a mobile robot

Abstract

The application discloses a tracking control method of a mobile robot, comprising the following steps: S1, dividing a tracking path into track points and putting the track points into a track point set; S2, selecting part of the track points before and after the current position of the mobile robot from the track point set, and performing fitting of a current tracking straight line based on the selected track points; S3, calculating a position deviation between the current pose of the mobile robot and the current tracking straight line Δ l and the angle deviation Δw ; S4, determining a preview adjustment coefficient of the current moment based on the position deviation Δ l and the angle deviation Δw , adjusting a preview point on the tracking straight line based on the preview coefficient of the current moment; S5, adjusting the angular velocity of the mobile robot at the current moment based on the relative relationship between the preview point and the current pose of the mobile robot, and performing the step S2 until the mobile robot drives to the end point of the tracking path. The method is more adaptive to the road curvatures of different expected paths, more accurate in path tracking, and more stable in operation of the vehicle in the tracking process.

Description

Technical Field

[0001] This invention belongs to the field of path tracking technology, and more specifically, this invention relates to a tracking and control method for a mobile robot. Background Technology

[0002] In recent years, mobile robots have become increasingly common in commercial and industrial environments. During the path tracking process of mobile robots, their control precision and quality directly affect the accuracy and safety reliability of mobile robots in motion. It is particularly important to select an appropriate path tracking method to quickly eliminate the tracking deviation between the actual path and the ideal trajectory of the mobile robot during the driving process and achieve accurate path tracking.

[0003] In the path tracking process of mobile robots, since the tracking path may be very long, it is necessary to form a series of pre-aiming points on the tracking path. The mobile robot tracks the entire path by tracking each pre-aiming point. Application publication number: CN 114995436 A, application publication date: 2022.09.02, invention title: A method for calculating lateral deviation of vehicle path tracking based on pre-aiming points. It is based on obtaining the pre-aiming points on the tracking path after fixing the pre-aiming time. Since the pre-aiming points are on the tracking trajectory, when tracking curved road sections, the mobile robot tracks along the inside of the curved road section, which not only results in poor tracking effect, but also easily causes instability in vehicle operation. Summary of the Invention

[0004] This invention provides a tracking and control method for a mobile robot, aiming to improve the above-mentioned problems.

[0005] This invention is implemented as follows: a tracking and control method for a mobile robot, the method comprising the following steps:

[0006] S1. Divide the tracking path into trajectory points and put them into the trajectory point set {list};

[0007] S2. Select some trajectory points before and after the current position of the mobile robot in the trajectory point set {list}, and fit the current tracking line based on the selected trajectory points;

[0008] S3. Calculate the positional deviation Δl and angular deviation Δw between the current pose of the mobile robot and the current tracking line;

[0009] S4. Determine the aiming adjustment coefficient at the current moment based on the position deviation Δl and the angle deviation Δw, and adjust the aiming point on the tracking line based on the aiming coefficient at the current moment.

[0010] S5. Adjust the angular velocity of the mobile robot at the current moment based on the relative relationship between the pre-aiming point and the current pose of the mobile robot, and execute step S2 until the end of the tracking path is reached.

[0011] Furthermore, the specific method for segmenting the tracking path is as follows:

[0012] The tracking path is divided according to the map resolution to form a series of path points, and these path points are placed into a path point set {list}.

[0013] Furthermore, the current method for forming the tracking line is as follows:

[0014] In the trajectory point set {list}, iterate through the trajectory point p0 that is closest to the current position of the mobile robot. Select the trajectory points within the forward distance l1 and backward distance l2 of trajectory point p0 and put them into the trajectory point set {list1}. Use the least squares method to fit the trajectory points in the trajectory point set {list1} to a straight line. The fitted straight line is the current tracking line of the mobile robot.

[0015] Furthermore, the specific formulas for calculating the forward distance l1 and the backward distance l2 are as follows:

[0016] l1=k*v t-1 *T, l2=0.5*k*v t-1 *T

[0017] Where k is the proportionality coefficient, v t-1 T represents the linear velocity of the mobile robot at the previous moment, and T represents the unit of time.

[0018] Furthermore, the specific method for adjusting the aiming point is as follows:

[0019] S41. Input the current position deviation Δl and angle deviation Δw into the fuzzy PID control model. The fuzzy PID control model outputs the aiming control coefficient s under the current position deviation Δl and angle deviation Δw.

[0020] S42. Adjust the distance between the pre-aiming point on the tracking line and the current position based on the pre-aiming control coefficient s, that is, adjust the position of the pre-aiming point.

[0021] Furthermore, the specific formula for calculating the distance L from the pre-aiming point to the current position is as follows:

[0022] L=(1-s)*v t *T

[0023] Among them, v t T represents the linear velocity of the mobile robot at the current moment, and T represents the unit of time.

[0024] Furthermore, the process of obtaining the mobile robot's current angular velocity is as follows:

[0025] Calculate the angle φ between the aiming point and the mobile robot body. The specific calculation formula is as follows:

[0026]

[0027] (P x ,P y (P) represents the current coordinates of the mobile robot on the map. 2·x ,P 2·y () indicates the coordinates of the aiming point on the map;

[0028] Calculate the difference angle α between the included angle φ and the robot's heading direction θ.

[0029] Then, the vehicle's turning radius R is calculated, and the specific formula is as follows:

[0030]

[0031] Then calculate the angular velocity of the mobile robot at the current moment.

[0032] Furthermore, the linear velocity v of the mobile robot at the current moment t The specific method for determining this is as follows:

[0033] Calculate the values ​​of two adjacent trajectory points p in the trajectory point set {list1} in sequence. i p i+1 The slope of the line is obtained and placed into a slope set {list2}. The absolute difference between any two adjacent slopes in the slope set {list2} is calculated. If the sum of all absolute differences is greater than or equal to a preset absolute difference, the robot moves at a set low linear velocity v. l If the sum of all absolute slope differences is less than a preset absolute slope difference, the mobile robot will move at a set higher linear velocity v. h Driving.

[0034] Furthermore, after step S5, the following is also included:

[0035] S6, Based on the current angular velocity ω of the mobile robot t and linear velocity v t The specific formula for calculating the wheel speeds of the left and right wheels is as follows:

[0036]

[0037] V L-t V R-tLet represent the speeds of the left and right wheels at the current time t, respectively, and let C represent the distance between the left and right wheels.

[0038] By tracking the vehicle's linear velocity at the curvature of the path, the mobile robot, based on the vehicle's current motion state and considering the lateral and angular deviations between the current loading point and the desired path, inputs these values ​​into a fuzzy PID control model. This results in adaptive adjustments to the aiming control coefficients, leading to the vehicle's aiming point. Based on the vehicle dynamics model, the robot outputs the control speeds of its left and right wheels, achieving path tracking. This approach offers greater adaptability to different road curvatures, more accurate path tracking, and smoother vehicle operation during the tracking process. Attached Figure Description

[0039] Figure 1 A flowchart of a navigation speed planning method for a mobile robot provided in an embodiment of the present invention;

[0040] Figure 2 This is a schematic diagram of mobile robot tracking and control provided in an embodiment of the present invention. Detailed Implementation

[0041] The specific embodiments of the present invention will be further described in detail below with reference to the accompanying drawings, so as to help those skilled in the art to have a more complete, accurate and in-depth understanding of the inventive concept and technical solution of the present invention.

[0042] Figure 1 This is a flowchart of a mobile robot tracking and control method provided in an embodiment of the present invention. Figure 2 This is a schematic diagram of mobile robot tracking and control, combined with... Figure 2 The tracking and control method for a mobile robot is described, and the method specifically includes the following steps:

[0043] S1. Divide the tracking path into trajectory points and put them into the trajectory point set {list};

[0044] In this embodiment of the invention, the tracking path is divided according to the map resolution to form a series of path points, and the path points are placed into a path point set {list}.

[0045] S2. Select some trajectory points before and after the current position of the mobile robot in the trajectory point set {list}, and fit the current tracking line based on the selected trajectory points;

[0046] In this embodiment of the invention, the method for forming the current tracking line is as follows:

[0047] Iterate through the trajectory point set {list} to find the trajectory point p0 that is closest to the current position of the mobile robot, and then use the index of trajectory point p0 to determine the position of the robot.p0 Select trajectory points within the forward distance l1 and backward distance l2 and put them into the trajectory point set {list1}. Use the least squares method to fit the trajectory points in the trajectory point set {list1} to a straight line. The fitted straight line is the current tracking line of the mobile robot.

[0048] In this embodiment of the invention, the calculation formulas for the forward distance l1 and the backward distance l2 are as follows:

[0049] l1=k*v t-1 *T, l2=0.5*k*v t-1 *T

[0050] Where k is the proportionality coefficient, v t-1 T represents the linear velocity of the mobile robot at the previous moment, and T represents the unit of time.

[0051] In this embodiment of the invention, the current velocity v of the mobile robot is... t The specific method for determining it is as follows:

[0052] In this embodiment of the invention, the method for determining the straightness or curvature of the road segment where the trajectory point set {list1} is located is as follows:

[0053] Calculate the values ​​of two adjacent trajectory points p in the trajectory point set {list1} in sequence. i p i+1 The slope of the line is obtained and placed into a slope set {list2}. The absolute difference between any two adjacent slopes in the slope set {list2} is calculated. If the sum of all absolute differences is greater than or equal to a preset absolute difference, the robot moves at a set low linear velocity v. l Driving, i.e., v t =v l If the sum of all absolute slope differences is less than a preset absolute slope difference, the mobile robot will move at a set higher linear velocity v. h Driving, i.e., v t =v h .

[0054] S3. Calculate the positional deviation Δl and angular deviation Δw between the current pose of the mobile robot and the current tracking line. The specific process for obtaining the positional deviation Δl and angular deviation Δw is as follows:

[0055] After obtaining the current pose of the mobile robot in the world coordinate system, obtain the projection point of the current position coordinates of the mobile robot on the current fitted line, and calculate the distance between the current position of the robot and the projection point, which is the position deviation Δl. Calculate the angle deviation Δw between the current pose angle of the mobile robot and the current fitted line in the world coordinate system.

[0056] S4. Determine the current aiming adjustment coefficient based on the current position deviation Δl and angle deviation Δw. Adjust the aiming point on the tracking line based on the current aiming coefficient (i.e., ...). Figure 2 (P2 in the pre-aiming point).

[0057] In this embodiment of the invention, the method for adjusting the aiming point is as follows:

[0058] S41. Input the current position deviation Δl and angle deviation Δw into the fuzzy PID control model. The fuzzy PID control model outputs the aiming control coefficient s under the current position deviation Δl and angle deviation Δw.

[0059] Fuzzy PID control integrates the knowledge and experience of experts or on-site operators to form a knowledge base, mimicking human thought processes to achieve automatic adjustment of PID parameters. Based on empirical principles, a larger position deviation Δl corresponds to a larger pre-aiming control coefficient s in the fuzzy PID output, resulting in a smaller forward tracking distance for the mobile robot. Conversely, a larger angle deviation Δw corresponds to a larger pre-aiming control coefficient s in the fuzzy PID output, resulting in a smaller forward tracking distance for the mobile robot. The parameters of the fuzzy PID controller in this invention are set according to the actual on-site tracking route, normalizing the pre-aiming control coefficient s to within the range of 0-1.

[0060] S42. Adjust the distance between the pre-aiming point on the current tracking line and the current position of the mobile robot based on the current pre-aiming control coefficient s, that is, adjust the position of the pre-aiming point.

[0061] In this embodiment of the invention, the formula for calculating the distance L from the pre-aiming point to the current position is as follows:

[0062] L=(1-s)*v t *T

[0063] Among them, v t T represents the linear velocity of the mobile robot at the current moment, and T represents the unit of time.

[0064] S5. Determine the current angular velocity of the mobile robot based on the relative relationship between the pre-aiming point and the current pose of the mobile robot, and execute step S2 until the robot travels to the end of the tracking path.

[0065] Determine the position of the aiming point in the direction the robot is moving. If the aiming point is to the right of the direction the robot is moving, the robot turns to the right, and the linear velocity of the right wheel is less than the linear velocity of the left wheel (V). R <V L At this point, R is positive; if the aiming point is located to the left of the direction the car is moving, the car needs to turn to the left, and the linear velocity of the right wheel is greater than that of the left wheel (V). R >V L The radius of rotation R is negative.

[0066] First, calculate the angle φ between the aiming point and the vehicle. The specific formula for this calculation is as follows:

[0067]

[0068] (P x ,P y (P) represents the current position coordinates of the mobile robot on the map. 2·x ,P 2·y ) indicates the coordinates of the aiming point on the map.

[0069] The formula for calculating the difference angle α between the included angle φ and the direction of the vehicle's heading is as follows:

[0070]

[0071] Where θ is the pose angle of the mobile robot. Then, the vehicle's rotation radius R is calculated:

[0072]

[0073] The formula for calculating angular velocity is as follows:

[0074] S6, based on the current angular velocity ω of the mobile robot t and linear velocity v t The specific formula for calculating the wheel speeds of the left and right wheels is as follows:

[0075]

[0076] V L-t V R-t Let represent the speeds of the left and right wheels at the current time t, respectively, and let C represent the distance between the left and right wheels.

[0077] The present invention has been described by way of example. Obviously, the specific implementation of the present invention is not limited to the above-described manner. Any non-substantial improvements made using the inventive concept and technical solution of the present invention, or the direct application of the inventive concept and technical solution of the present invention to other occasions without modification, are all within the protection scope of the present invention.

Claims

1. A tracking and control method for a mobile robot, characterized in that, The method includes the following steps: S1. Divide the tracking path into trajectory points and put them into the trajectory point set {list}; S2. Select some trajectory points before and after the current position of the mobile robot in the trajectory point set {list}, and fit the current tracking line based on the selected trajectory points; S3. Calculate the positional deviation Δl and angular deviation Δw between the current pose of the mobile robot and the current tracking line; S4. Determine the aiming adjustment coefficient at the current moment based on the position deviation Δl and the angle deviation Δw, and adjust the aiming point on the tracking line based on the aiming coefficient at the current moment. S5. Adjust the angular velocity of the mobile robot at the current moment based on the relative relationship between the pre-aiming point and the current pose of the mobile robot, and execute step S2 until the end of the tracking path is reached. The specific method for adjusting the aiming point is as follows: S41. Input the current position deviation Δl and angle deviation Δw into the fuzzy PID control model. The fuzzy PID control model outputs the aiming control coefficient s under the current position deviation Δl and angle deviation Δw. S42. Adjust the distance between the pre-aiming point on the tracking line and the current position based on the pre-aiming control coefficient s, that is, adjust the position of the pre-aiming point; The formula for calculating the distance L from the pre-aiming point to the current position is as follows: L = (1 - s) * v t *T; wherein v t represents the linear velocity of the mobile robot at the current time, and T represents a unit of time.

2. The tracking and control method for a mobile robot as described in claim 1, characterized in that, The specific method for segmenting the tracking path is as follows: The tracking path is divided according to the map resolution to form a series of path points, and these path points are placed into a path point set {list}.

3. The tracking and control method for a mobile robot as described in claim 1, characterized in that, The current method for forming the tracking line is as follows: In the trajectory point set {list}, iterate through the trajectory point p0 that is closest to the current position of the mobile robot. Select the trajectory points within the forward distance l1 and backward distance l2 of trajectory point p0 and put them into the trajectory point set {list1}. Use the least squares method to fit the trajectory points in the trajectory point set {list1} to a straight line. The fitted straight line is the current tracking line of the mobile robot.

4. The tracking and control method for a mobile robot as described in claim 3, characterized in that, The formulas for calculating the forward distance l1 and the backward distance l2 are as follows: l1 = k * v t-1 *T, l2 = 0.5 * k * v t-1 *T Where k is the proportionality coefficient, v t-1 T represents the linear velocity of the mobile robot at the previous moment, and T represents the unit of time.

5. The tracking and control method for a mobile robot as described in claim 1, characterized in that, The process of obtaining the angular velocity of the mobile robot at the current moment is as follows: Calculate the angle φ between the aiming point and the mobile robot body. The specific calculation formula is as follows: (P x ,P y (P) represents the current coordinates of the mobile robot on the map. 2·x ,P 2·y () indicates the coordinates of the aiming point on the map; Calculate the difference angle α between the included angle φ and the robot's heading direction θ. Then, the vehicle's turning radius R is calculated, and the specific formula is as follows: Then calculate the angular velocity of the mobile robot at the current moment.

6. The tracking and control method for a mobile robot as described in claim 5, characterized in that, The linear velocity v of the mobile robot at the current moment t The specific method for determining this is as follows: Calculate the values ​​of two adjacent trajectory points p in the trajectory point set {list1} in sequence. i p i+1 The slope of the line is obtained and placed into a slope set {list2}. The absolute difference between any two adjacent slopes in the slope set {list2} is calculated. If the sum of all absolute differences is greater than or equal to a preset absolute difference, the robot moves at a set low linear velocity v. l If the sum of all absolute slope differences is less than a preset absolute slope difference, the mobile robot will move at a set higher linear velocity v. h Driving.

7. The tracking and control method for a mobile robot as described in claim 1, characterized in that, The process after step S5 also includes: S6, Based on the current angular velocity ω of the mobile robot t and linear velocity v t The specific formula for calculating the wheel speeds of the left and right wheels is as follows: V L-t V R-t Let represent the speeds of the left and right wheels at the current time t, respectively, and let C represent the distance between the left and right wheels.

Images