Trajectory tracking method of outdoor heavy-load AGV under hydraulic high-delay response
By constructing a dynamic motion trend prediction model and neural network model, predicting and adjusting the inertia and delay effects of hydraulic system in advance, the trajectory tracking problem of hydraulic control AGV under high delay is solved, and a more accurate path tracking effect is achieved.
Patent Information
- Application Number
- CN202411824157.6
- Authority / Receiving Office
- CN · China
- Patent Type
- Applications(China)
- Current Assignee / Owner
- Filing Date
- 2024-12-12
- Publication Date
- 2025-07-08
AI Technical Summary
The hydraulically controlled wheel rudder system has high delay and inertia problems in outdoor heavy load AGV, resulting in poor tracking results. The AGV repeatedly sways after approaching the target path, seriously deviating from the expected path.
By constructing a prediction model of AGV dynamic motion trend, combining neural network model and optimal solution method, predict and pre-intervent the inertia and delay effects of hydraulic system in advance, adjust the vehicle body speed and angular velocity to achieve accurate trajectory tracking.
It improves the trajectory tracking effect of outdoor heavy-load AGV under high delay conditions, reduces the deviation between the actual operating trajectory and the target path, and enhances the accuracy and stability of path tracking.
Smart Images
Figure CN120276426A_ABST
Abstract
Description
Technical Field
[0001] The present invention belongs to the technical field of outdoor AGV control, and specifically relates to a trajectory tracking method for an outdoor heavy-duty AGV under high hydraulic delay response. Background Art
[0002] The wheel-steering system of a conventional AGV is an electric control system, which has a fast response speed and high execution accuracy of the wheel-steering. When an angle or speed is given, the wheel-steering system can respond to this operation in a short time. Therefore, the AGV can make timely adjustments according to the results calculated by the trajectory tracking algorithm, enabling the AGV to travel well along the target path.
[0003] However, the hydraulic control wheel-steering system has a very high response delay and certain inertia. When an execution target is given, the wheel-steering system will respond to this operation after a long delay. And after reaching the target, it will continue to execute for a period of time along the previous change trend, and the delay time and the continued change time are not fixed values. This will cause the AGV to continue running in another direction after approaching the target path until it deviates from the path by a certain distance and then advances towards the target path again. In continuous repetition, it will cause the actual running trajectory of the AGV to seriously deviate from the expected target path, swinging back and forth on both sides of the target path. Summary of the Invention
[0004] The invention provides a trajectory tracking method for an outdoor heavy-duty AGV under high delay based on hydraulic control. It analyzes the running trajectory of the heavy-duty AGV under high delay and the reaction of the wheel-steering system under hydraulic control, enabling the outdoor heavy-duty AGV based on hydraulic control to run better along the target path under the condition of a high reaction delay of the wheel-steering system. It is applied to the vehicle body control of the outdoor heavy-duty AGV, enabling the outdoor heavy-duty AGV with a hydraulic control wheel-steering system to travel better along the target path under high delay conditions.
[0005] The technical solution adopted by the present invention to achieve the above object is: a trajectory tracking method for an outdoor heavy-duty AGV under high hydraulic delay response, including the following steps:
[0006] The first stage:
[0007] Obtain the current position information (x, y, θ) of the AGV through laser positioning, and at the same time receive the section information of the target section of the current task from the remote console; based on the current position information and section information, construct a prediction model of the AGV's dynamic motion trend, and correct the target speed V t and the target angular velocity ω t ;
[0008] The second stage:
[0009] By analyzing the distribution of the target vehicle body speed (V t , ω t ) within a set future time period, and combining with the theoretical vehicle body speed (V tori , ω tori ) during this time period, determine the change trend of the target vehicle body speed (V t , ω t ) relative to the theoretical vehicle body speed (V tori , ω tori ). Find the speed change point (V Change , ω Change ) where the speed change direction changes in the speed change trend;
[0010] If there is no speed change point in the speed change trend, directly output the first value of the target vehicle body speed (V t , ω t ) obtained by iterative solution in the first stage, the target vehicle body speed (V0, ω0), as the target vehicle body speed (V next , ω next );
[0011] If there is a speed change point in the speed change trend, then, combining with the execution ability and reaction speed of the vehicle's own wheel-steering system, readjust the target vehicle body speed (V t , ω t ).
[0012] The first stage includes the following steps:
[0013] 1) Obtain the position information (x, y, θ) of the current AGV through laser positioning, and at the same time receive the section information of the target section of the current task from the remote console; Based on the current position information and section information, calculate the projection point (x pp , y pp ) of the AGV on the current section to obtain the current travel progress L progress ;
[0014] 2) Determine the remaining length L progress of the subsequent section according to the current travel progress L left , and determine the maximum speed V max of the AGV;
[0015] 3) Then, according to the current running speed V cur of the AGV and the set acceleration A max , calculate the speed at the next moment that does not exceed the maximum speed V max ; Take the speed at the next moment as the current running speed V cur, repeat step 3) to obtain the running speed V that meets the kinematic constraints within the future set time t , and use it as the theoretical vehicle body speed V tori , and further obtain the theoretical vehicle body angular velocity ω tori ;
[0016] 4) Project the current position information (x, y, θ) of the AGV onto the target road section to obtain the projection point (x pp , y pp ) of the AGV on the current road section, calculate the difference between the current position and the projection point as the offset (dx, dθ), and predict the target positions (x t , y t ) that the AGV can reach within the prediction domain of the subsequent set time period based on the running speed V t and the traveling angle θ of the AGV, and use it as the theoretical position (x tlast , y tlast ) of the vehicle operation;
[0017] 5) Build a prediction model for the dynamic motion trend of the AGV, and substitute the current position information (x, y, θ) of the AGV, the offset (dx, dθ), multiple target speeds V t predicted within the prediction domain, and the target positions (x t , y t ) into the prediction model as the input system state variables to correct the target speed V t and the target angular velocity ω t within the future prediction domain;
[0018] 6) Establish an objective function based on the purpose of minimizing the target error and the angular velocity change; according to the angular velocity values ω tlast at each moment within the prediction domain at the previous moment, the offset (dx tlast , dθ tlast ) and the angular velocity values ω t within the future prediction domain, the offset (dx t , dθ t ), through the method of optimal solution, obtain the optimal offset (dx, dθ) and the angular velocity deviation, and secondarily correct the target speed V t and the target angular velocity ω t to achieve the purpose of minimizing the objective function; among them, the offset (dx t , dθ t ) within the future prediction domain is obtained according to the target speed V t and the target angular velocity ω t within the future prediction domain, as well as the current position information (x, y, θ) of the AGV.
[0019] The section information includes the destination (x target , y target ) and the maximum speed V of the section pathmax .
[0020] The determination of the maximum speed V that the AGV can reach max is as follows:
[0021] Based on the installation position of the wheel rudder (x wheel , y wheel ) and the maximum speed V of the wheel rudder wheelmax infer the maximum speed that the vehicle body center can reach as the maximum speed V of the AGV max .
[0022] The compliance with kinematic constraints includes the installation position of the wheel rudder (x wheel , y wheel ) and the maximum speed V of the wheel rudder wheelmax .
[0023] The prediction model of the dynamic motion trend of the AGV adopts a neural network model; the training of the neural network model is as follows:
[0024] Obtain the historical data of the AGV, including the position information (x, y, θ), offset (dx, dθ), target speed V t , target position (x t , y t ), actual speed V tReal and actual angular velocity W tReal at each moment to construct a sample data set;
[0025] Use the sample data set to train the neural network model.
[0026] The objective function is where W x is the lateral deviation weight, W θ is the angular deviation weight, N is the lateral deviation, i.e., the offset dx, and δ is the angular deviation, i.e., the offset dθ. W ω is the angular velocity difference weight, dω is the angular velocity difference between two adjacent prediction steps, and this value is 0 initially. n is the number of prediction steps. When calculating for the first time, i starts from 0; i represents the i-th prediction step. When the value of i is negative, it takes values from the previous prediction results;
[0027] The method of optimal solution is the least squares method. By solving with the least squares method, the offset (dx, dθ) and angular velocity deviation are obtained, and finally the target angular velocity ω t is calculated.
[0028] If there is a speed change point in the speed change trend, then in combination with the execution ability and reaction speed of the vehicle's own wheel-steering system, the target vehicle body speed (V t , ω t ) is re-adjusted, including:
[0029] Obtain the distance between the target position (x t , y t ) and the speed change point:
[0030] When the distance is greater than the threshold, multiply the target vehicle body speed (V t , ω t ) by the coefficient a as the target vehicle body speed (V next , ω next ) for output, where 1 ≤ a < 2;
[0031] Otherwise, multiply the target vehicle body speed (V t , ω t ) by the coefficient b as the target vehicle body speed (V next , ω next ) for output, where 0.5 ≤ b < 1.
[0032] The present invention has the following beneficial effects and advantages:
[0033] 1. The present invention can make up for some of the impacts brought by the inertia and high latency of the hydraulic control system through advance prediction and pre-intervention, improve the trajectory tracking effect, and enable the outdoor heavy-load AGV to better run along the predetermined trajectory.
[0034] 2. The method used in the present invention can predict the future movement trend in advance according to the road section shape and past speed and angular velocity, and provide the necessary information for pre-intervention.
[0035] 3. The present invention can combine the prediction result with the real-time motion state to adjust the future movement trend in advance, and alleviate the execution anomaly problem brought by the high latency of the hydraulic system to a certain extent. BRIEF DESCRIPTION OF THE DRAWINGS
[0036] Figure 1 The working principle diagram of the present invention;
[0037] Figure 2 It is a comparison diagram of the actual running trajectory and the target path using the present invention and the traditional method.
[0038] Figure 3 It is a comparison diagram of the actual running angle and the target path angle using the present invention and the traditional method. DETAILED DESCRIPTION OF THE INVENTION
[0039] The present invention will be further described in detail below with reference to the accompanying drawings and embodiments.
[0040] Considering that the wheel-steering system of outdoor heavy-load AGV is based on hydraulic control, with a relatively high response delay and certain inertia, it will cause the AGV to deviate from the predetermined trajectory during operation. The present invention provides a trajectory tracking method of early prediction and early execution. Under normal circumstances, according to vehicle body parameters including wheel-steering position (x, y) and maximum speed (v max ), vehicle body position information (x t , y t , θ t ) and destination information (x target , y target ), the linear velocity v t and angular velocity ω t of the AGV at different times are calculated, and the vehicle body speed change point V Change is searched based on the speed change trend. Based on the vehicle body speed change point, the vehicle body speeds V t at various positions within its influence range are changed in advance to achieve the purpose of pre-intervention. The adopted solution includes the following steps:
[0041] Combine the current position information with the road section information to determine the current progress of operation, and combine it with the current running speed V cur of the AGV and the capacity parameters of its own wheel-steering system to calculate the running speed V t that meets the kinematic constraints and the target position (x t , y t ) within a future period of time.
[0042] Combine the speed and position information calculated in the previous step with the target offset to calculate the target vehicle body speed V t that does not exceed the performance constraints of the AGV, and correct the target position (x t , y t ) in combination with the calculation result.
[0043] Search for the vehicle body speed change point V Change through the speed change trend within a future period of time, and adjust the vehicle body speeds at different times within the influence range of the vehicle body speed change point based on this.
[0044] Finally, according to the influence effect of the inertia of the hydraulic system itself, the first vehicle body speed V0' in the predicted vehicle body speed V t ' is secondarily restricted to obtain the target vehicle body speed V next .
[0045] As Figure 1 shown, the present invention divides trajectory tracking into two stages:
[0046] The first stage:
[0047] Obtain the position information (x, y, θ) of the current AGV through laser positioning. At the same time, receive the current task from the remote console to obtain the section information including the destination (x target , y target ) and the maximum speed (V pathmax ) of the section. Calculate the projection point (x pp , y pp ) of the AGV on the current section based on the current position and section information, and obtain the current traveling progress L progress .
[0048] Determine the remaining length L progress of the subsequent section according to the current traveling progress L left . Calculate the maximum speed (V wheel , y wheel ) that the vehicle body center can reach based on the wheel-steering installation position (x wheelmax ) and its maximum speed (V max ), and then calculate the speed at the next moment that does not exceed the maximum speed (V cur ) according to the current running speed (V max ) and acceleration (A max ) of the AGV. Calculate the running speed (V t ) that meets the kinematic constraints within a future period of time in the same way. In this embodiment, the Kinematic Ackermann Model can be used to calculate the vehicle body speed.
[0049] Project the current position (x, y, θ) onto the target section to obtain the projection point (x pp , y pp ) of the AGV on the current section. Calculate the difference between the current position and the projection point as the offset (dx, dθ). Based on the calculated running speed (V t ), predict the target positions (x t , y t ) that the AGV can reach within a subsequent period of time.
[0050] Construct a prediction model describing the dynamic motion trend of the system by combining the vehicle body limit parameters, starting position, and section information. Substitute the current position (x, y, θ), offset (dx, dθ), multiple target speeds (V t ) within a certain period of time obtained by prediction, and target positions (x t , y t ) and other information as the input system state variables into the prediction model to correct the target speed (V t within a future period of time.and the target angular velocity (ω t ).
[0051] Based on the purpose of minimizing the target error and having a stable speed change, an objective function is established. Then, combined with the linear velocity values (v tlast ) at each moment within the prediction domain calculated at the previous moment, the angular velocity values (ω tlast ), and the theoretical position of the vehicle operation (x tlast , y tlast ) and other information, through the method of optimal solution, the target speed (V t ) and the target angular velocity (ω t ) within a future period of time are corrected twice to achieve the purpose of minimizing the objective function.
[0052] The objective function is where W x is the lateral deviation weight, W θ is the angular deviation weight, N is the lateral deviation, that is, the offset dx, δ is the angular deviation, that is, the offset dθ, W ω is the angular velocity difference weight, dω is the angular velocity difference between two adjacent prediction steps, and this value is 0 initially. n is the number of prediction steps. When calculating for the first time, i starts from 0; i represents the i-th prediction step. When the value of i is negative, it takes values from the previous prediction results;
[0053] The method of the optimal solution is the least squares method. Through the least squares method, the offsets (dx, dθ) and the angular velocity deviation are obtained, and finally the target angular velocity ω t is calculated.
[0054] After two corrections, the target position (x t , y t , θ t ) within a future period of time is calculated, and the future state is used as the calculation input variable for the next time. The target vehicle body speed (V t , ω t ) after two corrections is output for the parsing and adjustment in the second stage. At the same time, the predicted values (V t , ω t ) within the prediction interval are saved for use in the next cycle.
[0055] Second stage:
[0056] By analyzing and comparing the distribution of the target vehicle body speed (V t , ω t ) within a future period of time, combined with the theoretical vehicle body speed (V tori , ω tori ) during this period, the target vehicle body speed (V t , ωt )With respect to the change trend of the theoretical vehicle body speed (V tori , ω tori ), find the speed change point (V Change , ω Change ) in the speed change trend where the speed change direction changes.
[0057] If there is no vehicle body speed change point within the prediction range, directly output the first value of the vehicle body speed (V t , ω t ) obtained by iterative solution calculation, i.e., the vehicle body speed (V0, ω0), as the target vehicle body speed (V next , ω next ). If there is a vehicle body speed change point within this period of time, combine the execution ability and reaction speed of the vehicle's own wheel-steering system to determine the influence range of the current vehicle body speed change point, and re-adjust the target vehicle body speed (V t , ω t ) within the range: Obtain the distance between the target position (x t , y t ) and the speed change point; when the distance is greater than the threshold, multiply the target vehicle body speed (V t , ω t ) by the coefficient a to output as the target vehicle body speed (V next , ω next ), where 1 ≤ a < 2; otherwise, multiply the target vehicle body speed (V t , ω t ) by the coefficient b to output as the target vehicle body speed (V next , ω next ), where 0.5 ≤ b < 1.
[0058] For the vehicle body speed that is relatively far from the vehicle body speed change point, based on the response delay of the wheel-steering system and the influence degree on the vehicle body speed change point, make a certain degree of numerical change to the vehicle body speed (V t , ω t ) through predicting the response time and vehicle body performance indicators, and then output it as the target vehicle body speed (V next , ω next ). By updating the target speed in advance, give the wheel-steering system more reaction time.
[0059] For the vehicle body speed that is relatively close to the vehicle body speed change point, considering the response ability and its own inertia of the wheel-steering system, in addition to making numerical changes, it is also necessary to control the direction so that it can change towards the future target value earlier to achieve the result of pre-intervention.
[0060] Due to the inertia of the hydraulic system during control, after adjusting the angular velocity, it is also necessary to impose a certain degree of additional limitation according to the magnitude of the vehicle body speed to reduce the impact caused by overshoot due to inertia.
[0061] Based on the first value V0' in the adjusted vehicle body speed V t ’ after secondary limitation, the final target vehicle body speed V next is obtained.
[0062] The method in the present invention and the traditional method are respectively used to conduct actual tests on a certain outdoor heavy-duty AGV based on hydraulic control, and the results of path tracking control are compared.
[0063] (1) Since the heavy-duty AGV is large in size and fast in speed, the two methods in this test are carried out in the same outdoor environment. The outdoor environment is relatively open and the road surface flatness is worse than that of the indoor environment. A location with obvious surrounding features and relatively flat road surface is selected for testing to ensure the accuracy of the test results;
[0064] (2) The same heavy-duty AGV is used for testing to exclude the influence brought by the differences of different AGVs themselves. The wheel-steering system of the heavy-duty AGV used in the test is based on hydraulic control. The maximum running speed of the AGV is 2 meters per second;
[0065] (3) To ensure the same test conditions, the same road section and task are used to test the two methods. The starting coordinates of the target straight road section are set as (175, 1.435), the ending coordinates are (215, 1.435), the road section length is 40 meters, and the maximum running speed of the road section is set as 2 meters per second;
[0066] Figure 2 Figure shows the comparison between the actual running trajectory of the method introduced in the present invention and the traditional method and the target path. Figure 3 Figure shows the comparison between the actual running angle of the method introduced in the present invention and the traditional method and the angle of the target path. Among them,
[0067] According to Figure 2 and Figure 3 From the comparison between the actual running trajectory of the traditional method and the target path and the comparison between the actual running angle of the traditional method and the angle of the target path, it can be seen that the AGV cannot adjust the running angle in time when tracking the target path. Due to the inertia of the wheel-steering system, the angle between its actual angle and the target road section will further increase. Due to the high delay of the wheel-steering system, the AGV continues to move in the opposite direction after approaching the target path and starts to move towards the target road section after a period of time.
[0068] From the comparison between the actual running trajectory of the method introduced in the present invention and the target path in the figure, and the comparison between the actual running angle of the method introduced in the present invention and the angle of the target path, it can be seen that through the means of advance prediction and pre-intervention, the wheel-steering system of the AGV can react in advance. During the process of approaching the target path, although it will also exceed the target angle, due to the advance control of the AGV to correct its direction, the exceeded angle is much smaller than that of the traditional method. Therefore, the running trajectory is also closer to the target path than the traditional method.
Claims
1. A trajectory tracking method for an outdoor heavy-duty AGV under hydraulic high-delay response, characterized in that Including the following steps: The first stage: Obtain the position information (x, y, θ) of the current AGV through laser positioning, and at the same time receive the section information of the target section of the current task from the remote console; based on the current position information and section information, construct a prediction model for the dynamic movement trend of the AGV, and correct the target speed V in the future prediction domain t and the target angular velocity ω t ; The second stage: By analyzing the distribution of the target vehicle body speed (V t , ω t ) within a set future time period, and combining with the theoretical vehicle body speed (V tori , ω tori ) during this time period, determine the change trend of the target vehicle body speed (V t , ω t ) relative to the theoretical vehicle body speed (V tori , ω tori ). Find the speed change point (V Change , ω Change ) in the speed change trend where the speed change direction changes; If there is no speed change point in the speed change trend, directly output the first value of the target vehicle body speed (V t , ω t ) obtained through iterative solution in the first stage, namely the target vehicle body speed (V0, ω0), as the target vehicle body speed (V next , ω next ); If there is a speed change point in the speed change trend, then, in combination with the execution ability and reaction speed of the vehicle's own wheel-steering system, the target vehicle body speed (V t , ω t ) is readjusted.
2. The trajectory tracking method of an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 1, characterized in that In the first stage, the following steps are included: 1) Obtain the position information (x, y, θ) of the current AGV through laser positioning, and at the same time receive the section information of the target section of the current task from the remote console; based on the current position information and the section information, calculate the projection point (x pp , y pp ) of the AGV on the current section to obtain the current travel progress L progress ; 2) Determine the remaining length L of the subsequent section based on the current travel progress L progress Determine the remaining length L of the subsequent section left , and determine the maximum speed V of the AGV max ; 3) Then, based on the current running speed V of the AGV cur and the set acceleration A max calculate the speed at the next moment that does not exceed the maximum speed V max ; take the speed at the next moment as the current running speed V cur , repeat step 3) to obtain the running speed V that meets the kinematic constraints within the set future time t , and use it as the theoretical vehicle body speed V tori , and further obtain the theoretical vehicle body angular velocity ω tori ; 4) Project the position information (x, y, θ) of the current AGV onto the target road section to obtain the projection point (x pp , y pp ) of the AGV on the current road section. Calculate the difference between the current position and the projection point as the offset (dx, dθ). Based on the running speed V t , and the traveling angle θ of the AGV, predict the target position (x t , y t ) that the AGV can reach within the prediction domain in the subsequent set time period, and use it as the theoretical position (x tlast , y tlast ) of the vehicle operation; 5) Build a prediction model for the dynamic movement trend of the AGV, and substitute the current position information (x, y, θ), offset (dx, dθ), and the velocities V of multiple targets within the predicted domain obtained by prediction, and the target positions (x t , y t , t ) of the targets as the input system state variables into the prediction model to correct the velocities V t of the targets and the target angular velocities ω t within the future predicted domain; 6) Establish an objective function with the aim of minimizing the target error and the change in angular velocity; according to the angular velocity values ω at each moment within the prediction domain at the previous moment tlast , offset (dx tlast , dθ tlast ) and the angular velocity values ω within the future prediction domain t , offset (dx t , dθ t ), through the method of optimal solution, obtain the optimal offset (dx, dθ) and angular velocity deviation, and secondarily correct the target velocity V t and the target angular velocity ω t within the future prediction domain to achieve the purpose of minimizing the objective function; among them, the offset (dx t , dθ t ) within the future prediction domain is obtained according to the target velocity V t and the target angular velocity ω t within the future prediction domain, as well as the position information (x, y, θ) where the current AGV is located.
3. A trajectory tracking method of an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 2, characterized in that, The section information includes the destination (x target , y target ) and the maximum speed V of the section pathmax .
4. A trajectory tracking method of an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 2, characterized in that, The maximum speed V that the AGV can reach is determined as follows: max , as follows: According to the installation position of the wheel-steering unit (x wheel , y wheel ) and the maximum speed V of the wheel-steering unit wheelmax , estimate the maximum speed that the vehicle body center can reach as the maximum speed V of the AGV max .
5. A trajectory tracking method for an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 1, characterized in that, The kinematic constraints include the installation position (x wheel , y wheel ) of the wheel rudder and the maximum speed V wheelmax .
6. A trajectory tracking method for an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 1, characterized in that For the prediction model of the AGV dynamic motion trend, a neural network model is adopted; the training of the neural network model is specifically as follows: Obtain the historical data of the AGV, including the position information (x, y, θ), offset (dx, dθ), target speed V corresponding to each moment t , target position (x t , y t ), actual speed V tReal and actual angular velocity W tReal Construct a sample data set; Use the sample data set to train the neural network model.
7. A trajectory tracking method for an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 1, characterized in that, The objective function is where W x is the lateral deviation weight, W θ is the angular deviation weight, N is the lateral deviation, i.e., the offset dx, and δ is the angular deviation, i.e., the offset dθ. W ω is the angular velocity difference weight, dω is the angular velocity difference between two adjacent prediction steps, and its value is 0 initially. n is the number of prediction steps. When calculating for the first time, i starts from 0; i represents the i-th prediction step. When i is negative, the value is taken from the previous prediction result; The optimal solution method is the least squares method. The offset (dx, dθ) and the angular velocity deviation are obtained by solving with the least squares method, and finally the target angular velocity ω is calculated. t 。 8. A trajectory tracking method for an outdoor heavy-duty AGV under hydraulic high-delay response according to claim 1, characterized in that, If there is a speed change point in the speed change trend, then, in combination with the execution ability and reaction speed of the vehicle's own wheel-steering system, the target vehicle body speed (V t , ω t ) is re-adjusted, including: Obtain the distance between the target position (x t , y t ) and the speed change point: When the distance is greater than the threshold, multiply the target vehicle body speed (V t , ω t ) by a coefficient a and output it as the target vehicle body speed (V next , ω next ), where 1 ≤ a < 2; Otherwise, multiply the target vehicle body speed (V t , ω t ) by the coefficient b, and output it as the target vehicle body speed (V next , ω next ), where 0.5 ≤ b < 1.