A navigation speed planning method for mobile robots

Abstract

This invention discloses a navigation speed planning method for a mobile robot. The method specifically includes the following steps: S1, dividing the planned path into trajectory points and placing them into a trajectory point set; S2, selecting some trajectory points before and after the current position of the mobile robot from the trajectory point set, and performing straight line fitting based on the selected trajectory points; S3, calculating the positional deviation between the current pose of the mobile robot and the fitted straight line. Δ l and angular deviation Δw S4. Calculate the positional deviation. Δl and angle deviation Δ w The adjustment ratio is then used to obtain the linear velocity and angular velocity of the mobile robot at the next moment. By designing an adjustment ratio model, the positional and angular deviations of the mobile robot relative to the fitted straight line at different moments are considered. Corresponding ratio coefficients are obtained for positional and angular deviations in different quadrants. This enables faster and relatively smoother adjustment of the linear velocity of the mobile robot at the next moment while achieving planned path tracking.

Description

Technical Field

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

[0002] A mobile robot is an intelligent integrated system that combines environmental perception, dynamic decision-making and planning, behavior control and execution. In recent years, with the rapid development of robotics technology, mobile robots have become increasingly powerful, bringing significant benefits to many fields such as logistics and distribution, intelligent manufacturing, and medical services.

[0003] Li Qingzhong, Gu Weikang, Ye Xiuqing, Xiang Zhiyu. Research on Intelligent Pre-aiming Control Method for Path Tracking of Mobile Robots. Robot. 2002. The control of the forward speed of the mobile robot described in this paper is mainly based on two factors: the curvature of the road and the directional deviation of the robot relative to the pre-aiming point on the reference path. The linear velocity of the mobile robot at the current moment is calculated based on the curvature of the road, which has the problems of large fluctuations in linear velocity adjustment and low adjustment efficiency. Summary of the Invention

[0004] This invention provides a navigation speed planning method for mobile robots, aiming to solve the above-mentioned technical problems.

[0005] This invention is implemented as follows: a navigation speed planning method for a mobile robot, the method specifically including the following steps:

[0006] S1. Divide the planned path into trajectory points and put them into the trajectory point set;

[0007] S2. Select some trajectory points before and after the current position of the mobile robot from the trajectory point set, and perform straight line fitting 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 fitted straight line;

[0009] S4. Calculate the adjustment ratio under the position deviation Δl and angle deviation Δw, and then obtain the linear velocity and angular velocity of the mobile robot at the next moment.

[0010] Furthermore, the specific method for segmenting trajectory points on straight road segments is as follows:

[0011] The straight road segment is divided successively based on the set segmentation length. The segmentation point is the trajectory point. The trajectory point and its position coordinates are put into the trajectory point set.

[0012] Furthermore, the specific method for segmenting trajectory points on curved road segments is as follows:

[0013] (1) Obtain the control point p1(x) of the current curved road segment p1 ,y p1 p2(x) p2 ,y p2 );

[0014] (2) The coordinates of each trajectory point are determined based on the starting point, control point, and ending point on the curved road segment. The coordinates of each trajectory point are as follows:

[0015]

[0016] Among them, t k = k·len, where len represents the set segment length, (x k ,y k ) represents the k-th trajectory point p on the curved road segment. k The position coordinates, (x0, y0) represent the starting point coordinates of the curved road segment, (x0, y0) n ,y n ) represents the coordinates of the end point of the curved road segment.

[0017] Furthermore, the method for selecting trajectory points is as follows:

[0018] Traverse the trajectory point set that is closest to the current position of the mobile robot, and find the index of the nearest trajectory point in the trajectory point set. Then, take the index as the center and extract a set number of trajectory points at the front and back ends of the trajectory point set to form a trajectory point subset. Use the least squares method to fit a straight line based on the trajectory points in the trajectory point subset to generate a fitted straight line.

[0019] Furthermore, the process for obtaining the position deviation Δl is as follows:

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

[0021] Furthermore, the process of obtaining the angle deviation Δw is as follows:

[0022] The angle difference between the current pose angle of the mobile robot in the world coordinate system and the fitted straight line is the angle deviation Δw.

[0023] Furthermore, the specific formula for calculating the adjustment ratio I(Δl,Δw) is as follows:

[0024]

[0025] Where c, k1, and k2 are all control parameters, all of which are positive real numbers, where k1 < k2, Δl represents position deviation, and Δw represents angle deviation.

[0026] Furthermore, at the next moment t+1, the robot's linear velocity v t+1 The specific calculation formula is as follows:

[0027] v t+1 =(1-I(Δl) t ,Δw t ))*v t

[0028] Where, Δl t Δw represents the positional deviation at time t. t v represents the angular deviation at time t. t Let t represent the linear velocity of the robot at time t.

[0029] Furthermore, the robot's angular velocity ω at the next moment t+1 t+1 The calculation process is as follows:

[0030] The angle deviation Δw at the current time t t Input into the incremental PID control model to obtain the angle control quantity Δy within a unit time Δt. t The corresponding radius of rotation R t+1 for:

[0031]

[0032] v t+1 Let represent the linear velocity of the robot at time t+1, then the angular velocity ω of the robot at time t+1 is... t+1 The specific calculation formula is as follows:

[0033]

[0034] By designing an adjustment ratio model, the positional and angular deviations of the mobile robot relative to the fitted straight line at different times are combined, and corresponding ratio coefficients are obtained for positional and angular deviations in different quadrants. This enables the mobile robot to adjust its linear velocity more quickly and relatively smoothly in the next moment while achieving path tracking. Attached Figure Description

[0035] Figure 1 A flowchart of a navigation speed planning method for a mobile robot provided in an embodiment of the present invention. Detailed Implementation

[0036] 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.

[0037] Figure 1 This is a flowchart of a navigation speed planning method for a mobile robot provided in an embodiment of the present invention. The method specifically includes the following steps:

[0038] S1. Divide the planned path into trajectory points and put them into the trajectory point set;

[0039] In this embodiment of the invention, the planned path consists of straight road segments and fully curved road segments. For the straight road segments in the planned path, the method for segmenting trajectory points is as follows:

[0040] The straight road segment is divided successively based on the set segmentation length. The segmentation point is the trajectory point. The trajectory point and its position coordinates are put into the trajectory point set.

[0041] For curved sections in the planned path, the method for segmenting trajectory points is as follows:

[0042] (1) Obtain the control point p1(x) of the current curved road segment p1 ,y p1 p2(x) p2 ,y p2 );

[0043] Curved road sections are generated based on the curve's starting point, two control points, and the ending point.

[0044] (2) The coordinates of each trajectory point are determined based on the starting point, control point, and ending point on the curved road segment. The coordinates of each trajectory point are as follows:

[0045]

[0046] Among them, t k = k·len, where len represents the set segment length, (x k ,y k ) represents the k-th trajectory point p on the curved road segment. k The position coordinates, (x0, y0) represent the starting point coordinates of the curved road segment, (x0, y0) n ,y n ) represents the coordinates of the end point of the curved road segment.

[0047] S2. Select some trajectory points before and after the current position of the mobile robot from the trajectory point set, and perform straight line fitting based on the selected trajectory points;

[0048] In this embodiment of the invention, the method for selecting trajectory points is as follows:

[0049] Traverse the trajectory point set that is closest to the current position of the mobile robot, and find the index of the nearest trajectory point in the trajectory point set. Then, take the index as the center and extract a set number of trajectory points at the front and back ends of the trajectory point set to form a trajectory point subset. Use the least squares method to fit a straight line based on the trajectory points in the trajectory point subset to generate a fitted straight line.

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

[0051] In this embodiment of the invention, the process of obtaining the position deviation Δl and the angle deviation Δw is as follows:

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

[0053] S4. Calculate the adjustment ratio under the position deviation Δl and angle deviation Δw, and then obtain the linear velocity and angular velocity of the mobile robot at the next moment.

[0054] In this embodiment of the invention, the adjustment ratio I(Δl,Δw) under the current position deviation Δl and angle deviation Δw is obtained, and the specific formula for calculating the adjustment ratio I(Δl,Δw) is as follows:

[0055]

[0056] After simplification, we have:

[0057]

[0058] Where c, k1, and k2 are all control parameters, all of which are positive real numbers, where k1 < k2, Δl represents position deviation, and Δw represents angle deviation.

[0059] By adjusting the ratio I(Δl,Δw), the robot's linear velocity v at the next time t+1 is obtained. t+1 The specific calculation formula is as follows:

[0060] v t+1 =(1-I(Δl) t ,Δw t ))*v t

[0061] Where, Δl t Δw represents the positional deviation at time t. tv represents the angular deviation at time t. t Let t represent the linear velocity of the robot at time t.

[0062] In this embodiment of the invention, the angular velocity ω of the robot at the next time step t+1 is calculated. t+1 The calculation process is as follows:

[0063] The angle deviation Δw at the current time t t Input into the incremental PID control model to obtain the angle control quantity Δy within a unit time Δt. t The corresponding radius of rotation R t+1 for:

[0064]

[0065] Then, the robot's angular velocity ω at time t+1 t+1 The specific calculation formula is as follows:

[0066]

[0067] 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 navigation speed planning method for a mobile robot, characterized in that, The method specifically includes the following steps: S1. Divide the planned path into trajectory points and put them into the trajectory point set; S2. Select some trajectory points before and after the current position of the mobile robot from the trajectory point set, and perform straight line fitting 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 fitted straight line; S4. Calculate the adjustment ratio under position deviation Δl and angle deviation Δw, and then obtain the linear velocity and angular velocity of the mobile robot at the next moment. The specific formula for calculating the adjustment ratio I(Δl,Δw) is as follows: Where c, k1, and k2 are all control parameters, all of which are positive real numbers, where k1 < k2, Δl represents the position deviation, and Δw represents the angle deviation; The robot's linear velocity v at the next moment t+1 t+1 The specific calculation formula is as follows: v t+1 =(1-I(Δl t ,Δw t ))*v t Where, Δl t Δw represents the positional deviation at time t. t v represents the angular deviation at time t. t Let t represent the linear velocity of the robot at time t; The robot's angular velocity ω at the next moment t+1 t+1 The calculation process is as follows: The angle deviation Δw at the current time t t Input into the incremental PID control model to obtain the angle control quantity Δy within a unit time Δt. t The corresponding radius of rotation R t+1 for: v t+1 Let represent the linear velocity of the robot at time t+1, then the angular velocity ω of the robot at time t+1 is... t+1 The specific calculation formula is as follows:

2. The navigation speed planning method for a mobile robot as described in claim 1, characterized in that, The specific method for segmenting trajectory points on straight road segments is as follows: The straight road segment is divided successively based on the set segmentation length. The segmentation point is the trajectory point. The trajectory point and its position coordinates are put into the trajectory point set.

3. The navigation speed planning method for a mobile robot as described in claim 1, characterized in that, The specific method for segmenting trajectory points on curved road sections is as follows: (1) Obtain the control point p1(x) of the current curved road segment p1 ,y p1 p2(x) p2 ,y p2 ); (2) The coordinates of each trajectory point are determined based on the starting point, control point, and ending point on the curved road segment. The coordinates of each trajectory point are as follows: Among them, t k = k·len, where len represents the set segment length, (x k ,y k ) represents the k-th trajectory point p on the curved road segment. k The position coordinates, (x0, y0) represent the starting point coordinates of the curved road segment, (x0, y0) n ,y n ) represents the coordinates of the end point of the curved road segment.

4. The navigation speed planning method for a mobile robot as described in claim 1, characterized in that, The specific method for selecting trajectory points is as follows: Traverse the trajectory point set that is closest to the current position of the mobile robot, and find the index of the nearest trajectory point in the trajectory point set. Then, take the index as the center and extract a set number of trajectory points at the front and back ends of the trajectory point set to form a trajectory point subset. Use the least squares method to fit a straight line based on the trajectory points in the trajectory point subset to generate a fitted straight line.

5. The navigation speed planning method for a mobile robot as described in claim 1, characterized in that, The specific process for obtaining the position deviation Δl is as follows: After obtaining the current pose of the mobile robot in the world coordinate system, obtain the projection point of the mobile robot's position coordinates on the fitted straight line, and calculate the distance between the robot's current position and the projection point, which is the position deviation Δl.

6. The navigation speed planning method for a mobile robot as described in claim 1, characterized in that, The process of obtaining the angle deviation Δw is as follows: The angle difference between the current pose angle of the mobile robot in the world coordinate system and the fitted straight line is the angle deviation Δw.

Images