Speed control method and system for self-adaptive route tracking of differential robot

Abstract

The invention discloses a differential robot adaptive route tracking speed control method and system, and the method comprises the steps: obtaining a robot global path, a robot pose and a cost map, calculating a suggested linear speed, and obtaining a maximum value of the suggested linear speed; if the linear speed is 0, smoothly outputting the angular speed; otherwise, acquiring a corresponding distance set, selecting the maximum distance from the distance set as the distance of the local target point in the global path relative to the starting point of the global path, and calculating the angular velocity; judging whether the generated control track point is in an obstacle area, if so, deleting the corresponding distance, and if the distance set is not null, selecting the maximum distance, and calculating the angular velocity; if yes, reducing the suggested linear velocity, obtaining a corresponding distance set, and calculating a corresponding angular velocity; if not, the angular speed and the linear speed are subjected to speed smoothing and output. Motion performance of multiple scenes can be taken into consideration, so that the state of the robot can be changed smoothly, and jamming is avoided.

Description

Technical Field

[0001] The present invention belongs to the technical field of robot control, and relates to a speed control method and system for differential robot adaptive route tracking. Background Art

[0002] When an autonomous navigation robot executes a task, the robot is equipped with a variety of sensors such as lidar, depth camera, etc. to detect the environment and perform positioning. After obtaining the surrounding environment modeling, safe and efficient routes are required at the starting position and the task end point of the robot. Therefore, being able to autonomously avoid obstacles and efficiently obtain a navigation path is an important part of robot navigation research. Path planning is divided into global path planning and local path planning. In global navigation, the robot needs to plan a path according to a map containing information such as the starting point, the end point, and static obstacles. In local path planning, the surrounding environment of the mobile robot is changeable, and it is necessary to obtain information about surrounding obstacles during operation to plan a path that is convenient for the robot to follow.

[0003] In the field of mobile robots, it is usually required that the robot can smoothly and without jitter track the global robot route. In local route control methods, the dynamic window method is a relatively common method. However, its method of sampling completely based on the evaluation function may cause abnormalities in operations such as turning in place. At the same time, due to too many parameters, it may be impossible to take into account the motion performance of multiple scenarios. For example, it cannot enter naturally in a narrow passage, and there is jitter at the entrance and exit. Summary of the Invention

[0004] The purpose of the present invention is to provide a speed control method and system for differential robot adaptive route tracking, which can take into account the motion performance of multiple scenarios, so that the state of the robot can change smoothly without jitter.

[0005] The technical solution for realizing the purpose of the present invention is as follows:

[0006] A speed control method for differential robot adaptive route tracking includes the following steps:

[0007] S01: Obtain the global path of the robot, the pose of the robot, and the cost map, calculate the recommended linear velocity, and obtain the maximum value of the recommended linear velocity;

[0008] S02: Judge whether the recommended linear velocity is 0. If it is 0, calculate the angular velocity for in-place rotation, smooth the angular velocity and the linear velocity and output; if the recommended linear velocity is not 0, obtain the corresponding distance set, select the maximum distance in the distance set as the distance of the local target point relative to the starting point of the global path in the global path, and calculate the angular velocity;

[0009] S03: Generate a control trajectory according to the recommended linear velocity and angular velocity, and determine whether the generated control trajectory point is in an obstacle area;

[0010] S04: If it is in an obstacle area, delete the corresponding distance in the distance set. If the distance set is not empty, select the maximum distance and calculate the angular velocity. If it is empty, lower the recommended linear velocity, obtain the corresponding distance set, and calculate the corresponding angular velocity. If it is not in an obstacle area, smooth the angular velocity and linear velocity and output them.

[0011] In the preferred technical solution, the method for calculating the maximum linear velocity in step S01 includes:

[0012] Get the virtual trajectory and get the i-th trajectory point p in the virtual trajectory traj(i) ;

[0013] Calculate the distance between the virtual trajectory point and the robot's current position After obtaining the distance between the virtual trajectory point and the robot, it is assumed that the robot stops at the virtual trajectory point, and if the robot has a constant acceleration from the current position to the virtual trajectory point, t , the length d of the deceleration path formed is obtained by the following formula:

[0014]

[0015] Among them, T r represents the translation matrix, v traj(m) represents the velocity obtained in the mth iteration during the virtual trajectory integration process, d traj(m) represents the length of the mth iteration in the virtual trajectory integration process, Δt represents the incremental time of the virtual trajectory, and v traj(0) and d traj(0) The value of is 0;

[0016] when When the robot's linear velocity v1=min(v traj ,v max ), min(·) means taking the smaller value, v max The maximum line speed that the robot is allowed to control;

[0017] By comparing the starting point p of the global path start and the set point p of the global path end The path length d path and the length of the line to determine the global path curvature Linear speed v2 = min(v1, ratio v max );

[0018] Calculate the difference θ between the robot heading angle and the global path angle path, the linear velocity v3 = min(v2, C path ·v max ), C path is the speed adjustment coefficient related to the path;

[0019] Take the linear velocity v3 as the maximum value of the recommended speed.

[0020] In the preferred technical solution, calculate the i-th trajectory point p in the virtual trajectory traj(i) :

[0021] p traj(i) = R(ω·Δt, Z)·R r ·v·n·Δt + T r

[0022] where v is the linear velocity, ω is the angular velocity, R r represents the rotation matrix, T r represents the translation matrix, Δt represents the incremental time of the virtual trajectory, n represents the virtual trajectory detection step size, n ≤ n max , n max represents the maximum number of virtual trajectory detection step sizes, and R(ω·Δt, Z) represents the rotation matrix of rotating ω·Δt around the Z axis.

[0023] In the preferred technical solution, in step S02, according to the difference θ between the robot's heading angle and the global path angle path calculate the target angular velocity in an iterative manner:

[0024]

[0025] ω traj(m) represents the angular velocity at the m-th time in the calculation process, θ traj(m) represents the angle at the m-th time in the calculation process, is the angular acceleration;

[0026] When θ traj(m) > θ path , the robot's angular velocity is θ traj(m) , otherwise the angular velocity is 0.

[0027] In the preferred technical solution, the calculation method of the global path angle includes:

[0028] Among the path points on the global path at distances ΔT, 2·ΔT…N·ΔT from the starting point, where ΔT is the unit step size of translation and N is the set maximum number of translations, starting from the farthest point, connect each point to the initial point of the global path one by one, check whether each point on the connection is in the obstacle area, and select the farthest point where the first connection is not in the obstacle area as the reference point P of the global path angle ref = (x ref , y ref )T The first point of the global path is P0 = (x0, y0). T , x ref , x0 is the x-axis coordinate, y ref , y0 is the y-axis coordinate, and the angle of the global path

[0029] In the preferred technical solution, the method for determining the local target point in step S02 includes:

[0030] Calculate the distance set between each specified proposed linear velocity and an alternative target point and the robot's position

[0031]

[0032] where ΔTrans is the step size, and the number of translation times T < T thres , T thres represents the maximum number of translation times, represents the distance between the alternative target point and the robot's position;

[0033] Select the maximum distance value in the distance set and find the first point in the global path that exceeds this distance value as the local target point, and determine the local target point P local = (x local , y local , 0) T .

[0034] In the preferred technical solution, the method for generating the control trajectory in step S03 includes:

[0035] After obtaining the proposed linear velocity v suggest and the angular velocity set S ω , generate the control trajectory through an iterative method. The end point of the control trajectory is jointly confirmed by the robot's pose, linear velocity, angular velocity, and iterative duration Δt step and the number of iterations count; the calculation is as follows:

[0036]

[0037] where R(·) represents the rotation matrix, t(·) represents the translation matrix, and P (i) is the i-th control trajectory point i, and the set it forms is S ω , and in the initial state, P (0) = P local ; the number of iterations step is the iterative step.

[0038] In the preferred technical solution, the method for smoothing the angular velocity and linear velocity includes:

[0039] Smoothing linear acceleration a smooth and angular acceleration are as follows:

[0040] wherein, a max is the maximum linear acceleration, is the smoothing linear acceleration at the previous moment, is the smoothing angular acceleration at the previous moment, max(a, b) is the larger value of a and b, j linear is the linear jerk, j angular is the angular jerk;

[0041] The smoothed linear velocity v smooth and the smoothed angular velocity ω smooth are as follows:

[0042]

[0043] wherein, v target is the target linear velocity, ω target is the target angular velocity, is the smoothed linear velocity at the previous moment, and is the smoothed angular velocity at the previous moment Δt smooth is the interval time of the velocity smoothing process.

[0044] The present invention also discloses a speed control system for differential robot adaptive route tracking, including a processor, and the processor is built-in with the speed control method for differential robot adaptive route tracking as described above.

[0045] The present invention further discloses a computer storage medium, on which a computer program is stored, and when the computer program is executed, the speed control method for differential robot adaptive route tracking as described above is implemented.

[0046] Compared with the prior art, the remarkable advantages of the present invention are:

[0047] The angle difference between the robot and the global path is comprehensively considered, and the obstacle information is used to smoothly adjust the linear velocity in a way of a set of linear velocity and local target point selection. Since each linear velocity corresponds to a series of local target points, and each sampling from the global path starts from the farthest local target point, the change amount of the angular velocity of the robot is also smoother, making the whole machine have better running effects. Brief Description of the Drawings

[0048] Figure 1 is a flowchart of a speed control method for differential robot adaptive route tracking in a preferred embodiment;

[0049] Figure 2The working flowchart of a speed control system for a differential robot's adaptive route tracking in a preferred embodiment;

[0050] Figure 3 It is a graph showing the attenuation of the cost value with the change of distance. Specific implementation manner

[0051] The principle of the present invention is as follows: comprehensively considering the angular difference between the robot and the global path, as well as obstacle information, a set of linear velocity and local target point selection methods are used to smoothly adjust the linear velocity, which can take into account the motion performance of multiple scenarios, enabling the state of the robot to change smoothly without jamming.

[0052] Embodiment 1:

[0053] As Figure 1 shown, a speed control method for a differential robot's adaptive route tracking includes the following steps:

[0054] S01: Obtain the global path of the robot, the pose of the robot, and the cost map, calculate the recommended linear velocity, and obtain the maximum value of the recommended linear velocity;

[0055] S02: Determine whether the recommended linear velocity is 0. If it is 0, calculate the angular velocity for in-place rotation, perform speed smoothing on the angular velocity and the linear velocity, and output; if the recommended linear velocity is not 0, obtain the corresponding distance set, select the maximum distance in the distance set as the distance of the local target point relative to the starting point of the global path in the global path, and calculate the angular velocity;

[0056] S03: Generate a control trajectory according to the recommended linear velocity and angular velocity, and determine whether the generated control trajectory points are in the obstacle area;

[0057] S04: If it is in the obstacle area, delete the corresponding distance in the distance set. If the distance set is not empty, select the maximum distance and calculate the angular velocity; if it is empty, reduce the recommended linear velocity, obtain the corresponding distance set, and calculate the corresponding angular velocity; if it is not in the obstacle area, perform speed smoothing on the angular velocity and the linear velocity and output.

[0058] In one embodiment, the calculation method of the maximum value of the recommended linear velocity in step S01 includes:

[0059] Obtain the virtual trajectory and get the i-th trajectory point p in the virtual trajectory traj(i) ;

[0060] Calculate the distance between the virtual trajectory point and the current position of the robot After obtaining the distance between the virtual trajectory point and the robot, assume that the robot stops at the virtual trajectory point, and if the robot has a constant acceleration a from the current position to the virtual trajectory point when stopping t, the length d of the formed deceleration path is obtained by the following formula:

[0061]

[0062] where T r represents the translation matrix, v traj(m) represents the velocity obtained at the m-th iteration during the virtual trajectory integration process, d traj(m) represents the length obtained at the m-th iteration during the virtual trajectory integration process, Δt represents the incremental time of the virtual trajectory, v traj(0) and d traj(0) have a value of 0;

[0063] When , the linear velocity v1 of the robot = min(v traj , v max ), min(·) represents taking the smaller value, v max is the maximum linear velocity allowed for the robot to control its operation;

[0064] By comparing the path length d start of the starting point p of the global path and the set point p end of the global path, as well as the straight-line length, to determine the global path curvature path The linear velocity v2 = min(v1, ratio·v max max );

[0065] Calculate the difference θ path between the robot's heading angle and the global path angle, the linear velocity v3 = min(v2, C path ·v max ), C path is the velocity adjustment coefficient related to the path;

[0066] Take the linear velocity v3 as the maximum value of the recommended velocity.

[0067] In one embodiment, calculate the i-th trajectory point p traj(i) in the virtual trajectory:

[0068] p traj(i) = R(ω·Δt, Z)·R r ·v·n·Δt + T r

[0069] where v is the linear velocity, ω is the angular velocity, R r represents the rotation matrix, T r represents the translation matrix, Δt represents the incremental time of the virtual trajectory, n represents the virtual trajectory detection step size, n ≤ n max , n maxrepresents the maximum number of detection steps of the virtual trajectory, and R(ω·Δt, Z) represents the rotation matrix of rotating ω·Δt around the Z-axis.

[0070] In one embodiment, in step S02, according to the difference θ between the robot's heading angle and the global path angle path calculate the target angular velocity in an iterative manner:

[0071]

[0072] ω traj(m) represents the angular velocity at the m-th time in the calculation process, and θ traj(m) represents the angle at the m-th time in the calculation process, is the angular acceleration;

[0073] When θ traj(m) > θ path , the robot's angular velocity is θ traj(m) , otherwise the angular velocity is 0.

[0074] In one embodiment, the calculation method of the global path angle includes:

[0075] Among the path points on the global path that are at distances of ΔT, 2·ΔT…N·ΔT from the starting point, where ΔT is the unit step of translation and N is the set maximum number of translations, starting from the farthest point, connect each point to the initial point of the global path one by one, check whether each point on the connection line is in the obstacle area, and select the farthest point where the first connection line is not in the obstacle area as the reference point P of the global path angle ref = (x ref , y ref ) T , the first point of the global path is P0 = (x0, y0) T , x ref , x0 are the x-axis coordinates, and y ref , y0 are the y-axis coordinates, and the angle of the global path

[0076] In one embodiment, the method for determining the local target point in step S02 includes:

[0077] Calculate that each specified proposed linear velocity corresponds to a set of distances between an alternative target point and the robot's position

[0078]

[0079] where ΔTrans is the step size, the number of translation times T < T thres , T thres represents the maximum number of translations, represents the distance between the alternative target point and the robot's position;

[0080] In the distance set Select the maximum distance value, and find the first point in the global path that exceeds this distance value as the local target point, and determine the local target point P local =(x local ,y local ,0) T .

[0081] In one embodiment, the method for generating the control trajectory in step S03 includes:

[0082] After obtaining the proposed linear velocity v suggest and the angular velocity set S ω , generate the control trajectory through an iterative method. The end point of the control trajectory is jointly confirmed by the robot pose, linear velocity, angular velocity, iteration duration Δt step and the iteration count; the calculation is:

[0083]

[0084] where R(·) represents the rotation matrix, t(·) represents the translation matrix, and P (i) is the i-th control trajectory point i, and the set it forms is S ω . In the initial state, P (0) =P local ; the iteration count step is the iteration step number.

[0085] In one embodiment, the method for smoothing the angular velocity and linear velocity includes:

[0086] Smooth the linear acceleration a smooth and the angular acceleration as:

[0087] where a max is the maximum linear acceleration, is the smoothed linear acceleration at the previous moment, is the smoothed angular acceleration at the previous moment, max(a, b) is to take the larger value of a and b, j linear is the linear jerk, and j angular is the angular jerk;

[0088] The smoothed linear velocity v smooth and the smoothed angular velocity ω smooth are:

[0089]

[0090] where v target is the target linear velocity, ω target is the target angular velocity, is the smoothed linear velocity at the previous moment, and is the smoothed angular velocity at the previous moment Δt smooth is the interval time for the velocity smoothing process.

[0091] In another embodiment, a computer storage medium stores a computer program, and when the computer program is executed, it implements the above-mentioned speed control method for differential robot adaptive route tracking.

[0092] The speed control method for differential robot adaptive route tracking can adopt any of the above-mentioned speed control methods for differential robot adaptive route tracking, and the specific implementation will not be elaborated here.

[0093] In another embodiment, a speed control system for differential robot adaptive route tracking includes a processor, and the above-mentioned speed control method for differential robot adaptive route tracking is built in the processor.

[0094] Specifically, the following takes a preferred embodiment as an example to illustrate as follows:

[0095] As Figure 2 shown, a speed control method for a differential robot model's adaptive route adds the robot's heading and route angle, odometer speed, and obstacle information to the analysis, and combines the method of speed smoothing to output the smoothed linear velocity and angular velocity of the robot. First, input the robot's global path, robot pose, and cost map. Obtain the initial maximum recommended linear velocity according to the global path curvature and the angle between the robot and the global path. Determine whether the linear velocity is 0. If it is 0, directly calculate the angular velocity for in-place rotation, smooth the angular velocity and linear velocity, and then output. If the recommended linear velocity is not 0, obtain the corresponding distance set, and select the maximum distance in the set as the distance of the local target point relative to the starting point of the global path in the global path. Calculate the angular velocity based on the global path, local target point, and recommended linear velocity. It can be judged whether the control trajectory generated by the linear velocity and angular velocity passes through an obstacle by determining whether the control trajectory is in the lethal cost or inscribed cost in the cost map. If there is a lethal cost or inscribed cost, delete the corresponding distance in the distance set. If the distance set is not empty, continue to select the maximum distance and repeat the process of calculating the angular velocity. If it is empty, reduce the maximum recommended linear velocity, obtain the corresponding distance set, and continue to calculate the corresponding angular velocity.

[0096] During the process of trajectory tracking, the robot needs to adjust the issued linear velocity and angular velocity. In the differential robot model adaptive route tracking method, a target linear velocity and angular velocity will be generated, and then these linear velocities and angular velocities are sent to the speed smoothing module. The speed smoothing module runs at a high frequency and sets the robot to run by generating the corresponding linear velocity and angular velocity.

[0097] The speed control method for adaptive route tracking introduces a cost map to preprocess the distance between points in the environment and the nearest obstacle. The data structure stored in the cost map is a two-dimensional array, and the origin of the map is: O m =(x m , y m ) T , assuming the point p=(x, y) T , and its index value within the map is:

[0098]

[0099] where R m represents the map resolution, and ceil(·) is the ceiling function.

[0100] The higher the cost value of an element in the map, the closer it is to the obstacle, and vice versa. If a point reaches the fatal cost value or the inscribed cost value, it means it is within the inflated range of the obstacle. The attenuation of the cost value with distance is as Figure 3 shown.

[0101] The data received by the speed control method includes: the global path, the robot pose, the robot speed feedback, and the cost map. The speed control method first determines the maximum value V max of the target linear velocity. The maximum value of the target linear velocity is related to the following factors: the distance between the robot and the obstacle, the route curvature of the global path, and the angle difference between the robot's heading angle and the global path.

[0102] The distance between the robot and the obstacle can be used to determine whether the virtual trajectory point is in the obstacle area through the cost value of the virtual trajectory point in the map. The robot pose is where R r represents the rotation matrix, and T r represents the translation matrix. Combining the linear velocity v and the angular velocity ω of the robot, each trajectory point p traj(i) in the virtual trajectory can be calculated.

[0103] p traj(i) =R(ω·Δt, Z)·R r ·v·n·Δt + T r

[0104] where Δt represents the incremental time of the virtual trajectory, n represents the virtual trajectory detection step size, n ≤ n max , n max represents the maximum number of virtual trajectory detection step sizes, and R(ω·Δt, Z) represents the rotation matrix of rotating ω·Δt around the Z axis.

[0105] After obtaining the virtual trajectory points, judge each point p traj(j)Whether it is in the lethal cost value or the inscribed cost value of the cost map. If so, it means that this point is in the dangerous area. Calculate the distance between this virtual trajectory point and the current position of the robot

[0106] After obtaining the distance between the virtual trajectory point and the robot, assume that the robot needs to stop at the virtual trajectory point, and the acceleration of the robot is constant at a from the current position to the stop at the virtual trajectory point t , the length d of the deceleration path formed can be obtained by the following integral:

[0107]

[0108] v traj(m) represents the speed obtained in the m-th iteration of the virtual trajectory integration process, and d traj(m) represents the length obtained in the m-th iteration of the virtual trajectory integration process, and v traj(0) and d traj(0) are 0;

[0109] When , the maximum linear speed v1 that the robot should be at this time = min(v traj , v max ), and min(·) means taking the smaller value. v max is the maximum linear speed allowed for the robot to control its operation.

[0110] The route curvature of the global path determines whether the robot is turning. By comparing the path length d start of the starting point p end of the global path and the set point p path of the global path and the straight-line length to determine the global path curvature Therefore, the maximum recommended linear speed v2 = min(v1, ratio·v max ).

[0111] At the same time, consider the difference θ path between the robot's heading angle and the path angle. The larger the angle, the lower the linear speed. When the included angle reaches the angle threshold θ thres , the linear speed drops to 0. Therefore, the maximum linear speed v3 = min(v2, C path ·v max ), and C path is the speed adjustment coefficient related to the path.

[0112] So far, the recommended linear speed of the distance between the robot and the obstacle, the route curvature of the global path, and the angle difference between the robot's heading angle and the global path is obtained. If the recommended linear speed is 0, directly enter the in-place rotation. By calculating the difference θ path between the robot's heading angle and the global path angle, the target angular velocity can be calculated.

[0113]

[0114] ω traj(m) represents the angular velocity at the m-th time during the calculation process, and θ traj(m) represents the angle at the m-th time during the calculation process. is the angular acceleration;

[0115] When θ traj(m) > θ path , the angular velocity of the robot is θ traj(m) , otherwise the angular velocity is 0.

[0116] The calculation method of the global path angle is as follows: Path points at distances of ΔT, 2·ΔT…N·ΔT from the starting point on the global path. ΔT is the unit step length of translation, and N is the set maximum number of translation times. Starting from the farthest point, connect lines to the initial point of the global path one by one, and check whether each point on the line is in the obstacle area. Finally, select the farthest point where the first connected line is not in the obstacle area as the reference point P of the global path angle ref =(x ref , y ref ) T , the first point of the global path is P0=(x0, y0) T . Therefore, the angle θ g of the global path is

[0117] When the recommended linear velocity is not 0, the corresponding angular velocity needs to be further calculated. First, obtain the set of angular velocities:

[0118] S ω ={ω|ω = -ω thres +n ω ·Δω, -ω thres < ω < ω thres}}.

[0119] ω thres represents the maximum angular velocity, n ω represents the number of angular velocity increment times, and Δω represents the step size.

[0120] After obtaining the recommended linear velocity v suggest and the set of angular velocities S ω , the control trajectory can be generated by an iterative method. The end point of the control trajectory is jointly confirmed by the robot pose, linear velocity, angular velocity, iterative duration Δt step and the number of iterations count. Among them, what needs to be confirmed is the number of iterations. Each specified recommended linear velocity corresponds to a set of distances between an alternative target point and the robot position

[0121]

[0122] Among them, ΔTrans is the step size, and the number of translations is T. <T thres , T thres represents the maximum number of translations, Indicates the distance between the candidate target point and the robot position;

[0123] At distance collection The maximum distance value is selected, and the first point exceeding the distance value is found in the global path as the local target point. The farther the local target point is from the robot position, the smoother the path tracking is, the lower the tracking accuracy is, and the smaller the robot's posture changes. On the contrary, the more drastic the posture adjustment of path tracking is, the higher the tracking accuracy is, and the greater the robot's posture changes. In the process of route tracking, it tends to select farther target points to reduce the impact of large adjustments to the posture and thus repeated adjustments.

[0124] Determine the local target point P local =(x local ,y local ,0) T Then, calculate the number of iterations Therefore, the recommended linear velocity v suggest and ω∈S ω The control trajectory end point P (end) It can be calculated as:

[0125]

[0126] Where R(·) represents the rotation matrix, t(·) represents the translation matrix, and P (i) is the i-th control trajectory point i, whose constituent set is S ω , in the initial state P (0) =P local , step is the number of iteration steps.

[0127] The point set of the global path is S g , calculate the angular velocity set S ω The control trajectory end point P formed by each angular velocity ω and linear velocity (end) , and calculate the shortest distance d to the global path ω(min) =argmin(||P (end) -P g ||),P g ∈S g , the angular velocity corresponding to this distance is ω(min), and the trajectory set is S ω(min) .right If a point reaches the fatal cost value or the inscribed cost value, it is within the expansion range of the obstacle. distDelete the distance value corresponding to the target point.

[0128] When the distance set is empty, reduce the proposed linear velocity v suggest , and find the corresponding distance set Repeat the previous process. When the linear velocity v suggest is reduced to 0, it means the robot needs to stop in place and calculate the angular velocity.

[0129] So far, the target linear velocity v target and angular velocity ω target of the adaptive route tracking method are calculated and put into the velocity smoother for smoothing. Calculate the smoothed linear velocity v smooth and the smoothed angular velocity ω smooth so that the state of the robot can change smoothly without jamming. The smoothed linear velocity v smooth and the smoothed angular velocity are related to the following factors: the target linear velocity v target and angular velocity ω target , the maximum linear acceleration a max , the maximum angular acceleration the linear jerk j linear , the angular jerk j angular , the smoothed linear velocity at the previous moment and the smoothed angular velocity

[0130] The smoothed linear acceleration a smooth and angular acceleration are:

[0131]

[0132] is the smoothed linear acceleration at the previous moment, is the smoothed angular acceleration at the previous moment, and max(a, b) is the larger value of a and b.

[0133] The smoothed linear velocity v smooth and the smoothed angular velocity ω smooth are:

[0134]

[0135] where Δt smooth is the interval time of the velocity smoothing process. So far, the smoothed linear velocity and angular velocity are obtained.

[0136] The above embodiments are the preferred embodiments of the present invention. However, the embodiments of the present invention are not limited to the above embodiments. Any other changes, modifications, substitutions, combinations, or simplifications made without departing from the spirit and principle of the present invention shall be equivalent replacement methods and are all included in the protection scope of the present invention.

Claims

1. A speed control method for the adaptive route tracking of a differential robot, characterized in that, It includes the following steps: S01: Obtain the global path of the robot, the robot pose, and the cost map, calculate the proposed linear velocity, and obtain the maximum value of the proposed linear velocity; S02: Determine whether the proposed linear velocity is 0. If it is 0, calculate the angular velocity for in-place rotation, perform velocity smoothing on the angular velocity and the linear velocity, and output them; if the proposed linear velocity is not 0, obtain the corresponding distance set, select the maximum distance in the distance set as the distance of the local target point relative to the starting point of the global path in the global path, and calculate the angular velocity; S03: Generate a control trajectory according to the proposed linear velocity and angular velocity, and determine whether the generated control trajectory points are in the obstacle area; S04: If it is in the obstacle area, delete the corresponding distance in the distance set. If the distance set is not empty, select the maximum distance and calculate the angular velocity; if it is empty, reduce the proposed linear velocity, obtain the corresponding distance set, and calculate the corresponding angular velocity; If it is not in the obstacle area, perform velocity smoothing on the angular velocity and the linear velocity and output them.

2. The speed control method for the differential robot to adaptively track the route according to claim 1, characterized in that, The calculation method of the maximum value of the proposed linear velocity in step S01 includes: Obtain a virtual trajectory and get the i-th trajectory point p in the virtual trajectory traj(i) ; Calculate the distance between the virtual trajectory point and the current position of the robot After obtaining the distance between the virtual trajectory point and the robot, assume that the robot stops at the virtual trajectory point, and the acceleration of the robot is constant at a from the current position to the virtual trajectory point t , the length d of the deceleration path formed is obtained by the following formula: Among them, T r represents the translation matrix, v traj(m) represents the velocity obtained in the m-th iteration during the virtual trajectory integration process, d traj(m) represents the length obtained in the m-th iteration during the virtual trajectory integration process, Δt represents the incremental time of the virtual trajectory, v traj(0) and d traj(0) have a value of 0; When , the linear velocity v1 of the robot = min(v traj , v max ), min(·) represents taking the smaller value, and v max is the maximum linear velocity allowed for the robot to control its operation; By comparing the starting point p of the global path start and the set point p of the global path end to determine the path length d path and the straight-line length to determine the global path curvature The linear velocity v2 = min(v1, ratio·v max ) The difference θ between the heading angle of the computer robot and the global path angle path , the linear velocity v3 = min(v2, C path ·v max ), C path is the speed adjustment coefficient related to the path; Taking the linear velocity v3 as the maximum value of the proposed velocity.

3. The speed control method for the differential robot's adaptive route tracking according to claim 2, characterized in that, Calculate the i-th trajectory point p in the virtual trajectory traj(i) : p traj(i) = R(ωΔt, Z)·R r ·v·n·Δt + T r where v is the linear velocity, ω is the angular velocity, and R r represents the rotation matrix, T r represents the translation matrix, Δt represents the incremental time of the virtual trajectory, n represents the virtual trajectory detection step size, and n ≤ n max , n max represents the maximum number of virtual trajectory detection step sizes, and R(ωΔt, Z) represents the rotation matrix of rotating ω·Δt around the Z axis.

4. The speed control method for the differential robot's adaptive route tracking according to claim 1, wherein, In step S02, the target angular velocity is calculated iteratively based on the difference θ between the heading angle of the robot and the global path angle path as follows: ω traj(m) represents the angular velocity at the m-th time during the calculation process, and θ traj(m) represents the angle at the m-th time during the calculation process, is the angular acceleration; When θ traj(m) > θ path , the angular velocity of the robot is θ traj(m) , otherwise the angular velocity is 0.

5. The speed control method for the differential robot to adaptively track the route according to claim 2, characterized in that, The calculation method of the global path angle includes: Among the path points on the global path at distances of ΔT, 2·ΔT…N·ΔT from the starting point, where ΔT is the unit step of translation and N is the set maximum number of translation times, starting from the farthest point, connect lines to the initial point of the global path one by one, check whether the points on the connecting lines are in the obstacle area, and select the farthest point where the first connecting line is not in the obstacle area as the reference point P for the global path angle ref =(x ref ,y ref ) T , the first point of the global path is P0 = (x0, y0) T , x ref , x0 are the x-axis coordinates, y ref , y0 are the y-axis coordinates, and the angle of the global path 6. The speed control method for differential robot adaptive route tracking according to claim 1, characterized in that The determination method of the local target point in step S02 includes: Calculate that for each specified proposed linear velocity, there is a set of distances between the alternative target point and the robot position where ΔTrans is the step size, and the number of translation times T < T thres , T thres represents the maximum number of translation times, and represents the distance between the alternative target point and the robot's position; Select the maximum distance value from the distance set and find the first point in the global path that exceeds this distance value as the local target point, determining the local target point P local =(x local , y local , 0) T .

7. The speed control method for differential robot adaptive route tracking according to claim 1, characterized in that, The method of generating a control trajectory in step S03 includes: Obtain the recommended linear velocity v suggest and the angular velocity set S ω After that, generate the control trajectory in an iterative manner. The end point of the control trajectory is jointly confirmed by the robot pose, linear velocity, angular velocity, and the iteration duration Δt step and the iteration count count; the calculation is as follows: where, R(·) represents the rotation matrix, t(·) represents the translation matrix, and P (i) is the i-th control trajectory point i, and the set it forms is S ω , and in the initial state, P (0) = P local ; the number of iterations step is the iteration step number.

8. The speed control method for differential robot adaptive route tracking according to claim 1, characterized in that The method of performing velocity smoothing on the angular velocity and the linear velocity includes: Smoothing linear acceleration a smooth and angular acceleration are as follows: Among them, a max is the maximum linear acceleration, is the smoothed linear acceleration at the previous moment, is the smoothed angular acceleration at the previous moment, max(a, b) is the larger value of a and b, j linear is the linear jerk, j angular is the angular jerk; Smoothed linear velocity v smooth and smoothed angular velocity ω smooth are as follows: where, v target is the target linear velocity, ω target is the target angular velocity, is the smoothed linear velocity at the previous moment, and is the smoothed angular velocity at the previous moment Δt smooth is the interval time of the velocity smoothing process.

9. A speed control system for the adaptive route tracking of a differential robot, characterized in that, It includes a processor, and the processor is built-in with the speed control method for differential robot adaptive route tracking according to any one of claims 1-8.

10. A computer storage medium, on which a computer program is stored, characterized in that, When the computer program is executed, it implements the speed control method for differential robot adaptive route tracking according to any one of claims 1-8.

Images