Robot trajectory tracking control system and method

Abstract

The invention relates to the technical field of robots, in particular to a robot trajectory tracking control system and method.Control sampling and state sampling are conducted on road points in front of a robot, and control sampling points are a series of control points generated by predicting the position where the robot is located at the next moment according to a robot kinematics model; the state sampling points are a series of state points generated according to the transverse distance and the longitudinal distance of the robot planning path. Through a dynamic planning algorithm, a cost function is designed, a robot rotation angle with the minimum cost is calculated, an optimal control rotation angle is output, and the transverse deviation error and the course deviation error of the automatic driving robot are reduced. The technical problems that in the prior art, a robot trajectory tracking control mode is high in model dependence, insufficient in environment adaptability, large in calculation amount, time-consuming and limited in generalization ability are solved.

Description

Technical Field

[0001] The present invention relates to the field of robot technology, and in particular to a robot trajectory tracking control system and method. Background Art

[0002] Robot autonomous navigation technology is one of the most popular research directions at present. It mainly includes four processing modules: environmental perception, positioning, decision planning, and motion control. Trajectory tracking is the main research content of motion control and one of the core technologies of robot autonomous navigation. The trajectory tracking technology of autonomous robots refers to the control of the robot's steering and speed by calculating the robot's steering wheel angle, throttle, brake, etc. based on the reference path given by the upper-level decision planning system and the robot's real-time position information, so that the robot can smoothly and safely follow the reference trajectory. However, since the robot itself is a complex system composed of multiple parts, it has strong coupling, nonlinearity, and multiple constraints. Therefore, the trajectory tracking problem has become a difficulty and focus in unmanned driving technology.

[0003] At present, commonly used trajectory tracking control algorithms include PID control, preview tracking optimal control, pure tracking control, sliding mode control, fuzzy control, feedforward-feedback control, LQR control and model predictive control. PID control is widely used because of its simplicity and high efficiency. However, facing the complexity and variability of robot systems, it becomes difficult to adjust PID parameters, which makes the control effect easily affected. Preview tracking control simulates the driving process of the driver and adjusts the control parameters according to the road environment, but it mainly depends on the curvature of the road and the lateral error, and does not adequately consider the errors of other control parameters, which limits its online adjustment ability. LQR control, as an optimization control method, has high requirements on model accuracy and does not fully consider the influence of robot dynamic parameters and external road environment, making it difficult to handle multi-constraint problems. Sliding mode control has good robustness, but is prone to chattering, which limits its scope of application. Pure tracking control is suitable for low-speed conditions, but is prone to in-cutting at turns and overshoot at high speeds. Model predictive control (MPC) can effectively handle constraint problems, but the accuracy of the model has a great impact on its performance, and the calculation is large and time-consuming. Different models and cost functions need to be designed for different scenarios, and the generalization is not strong.

[0004] In summary, the existing robot trajectory tracking control methods have the problems of strong model dependence, insufficient environmental adaptability, large and time-consuming calculation, and limited generalization ability. Summary of the invention

[0005] The purpose of the present invention is to provide a robot trajectory tracking control system and method, aiming to solve the technical problems of the robot trajectory tracking control method in the prior art, such as strong model dependence, insufficient environmental adaptability, large and time-consuming calculation, and limited generalization ability.

[0006] To achieve the above object, a robot trajectory tracking control method adopted by the present invention comprises the following steps:

[0007] First, obtain the driving path issued by the planning layer;

[0008] Based on the acquired formal path, the objective function is designed through the LQR algorithm based on the kinematic model to calculate the robot's turning angle;

[0009] Set the step size and range of control sampling;

[0010] Set the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points;

[0011] Design dynamic programming cost function;

[0012] Calculate the cost function value of the control sampling layer;

[0013] Calculate the cost function value of the state sampling layer;

[0014] Calculate the sampling point with the minimum cost function of the final state sampling layer;

[0015] The state-control point inversion obtains the control-state path point, and the starting point of the path contains the optimal controlled robot angle.

[0016] The acquired driving path includes multiple waypoints, each of which includes coordinates, curvature, turning angle, coordinate id, coordinate point path length and heading in the robot coordinate system.

[0017] Among them, based on the obtained formal path, the objective function is designed through the LQR algorithm based on the kinematic model, and the turning angle of the robot is calculated:

[0018] The discrete state equation of the kinematic model is as follows:

[0019]

[0020] in,

[0021] v r represents the reference speed, δ r represents the front wheel turning angle, T represents the control time, The reference heading angle, l represents the wheelbase;

[0022] The objective function represents the weighted sum of the accumulated tracking deviation and the accumulated control input. The objective function is set to a quadratic form as follows:

[0023]

[0024] Q = diag[q 1 ,q 2 ,q 3 ],R=[r 1 , r 2 ], Q, R value settings are linearly interpolated according to speed,

[0025]

[0026] The control quantity u is a linear function of the state variable X:

[0027] u=-[(R+B T PB) -1 B T PA]X = -KX;

[0028] where P is the solution of the Riccati equation:

[0029] P=A T PA-A T PB(R+B T PB) -1 B T PA+Q;

[0030] The angle δ on the path is selected as the feedforward angle, and the final output angle is:

[0031] u=δ+ku 1 .

[0032] Among them, when setting the step size and range of control sampling:

[0033] The step size is set to [1,5] degrees, the value range is set to [-10,10], and finally the control sampling set {δ 0 ',δ 1 ',δ 2 ',δ 3 ',...δ n '}Sampling the robot Ackerman steering model to calculate the steering curvature:

[0034]

[0035] Where l represents the wheelbase of the robot chassis;

[0036] Set the sampling time according to the control frequency. If the control frequency is 20 Hz and the sampling time is 50 ms, a series of control points are obtained. The coordinates of the control points are expressed as:

[0037]

[0038] x 0 ,y 0 , T s Indicates the coordinates and heading of the initial state.

[0039] When setting the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points:

[0040] The longitudinal sampling length is set according to the vehicle feedback speed. The speed coefficient of the longitudinal sampling length is set to [1,2]. To prevent the length from being too long, the upper limit of the longitudinal sampling length is also set to [5,8] meters. The number of longitudinal sampling points is set in the range of [2,5]. The transverse sampling length is set to [0.1,0.5] meters. The number of transverse sampling points is set in the range of [3,10]. The distance obtained by longitudinal sampling matches the nearest path point, and then the state points are calculated in sequence according to the interval and number of transverse sampling points. The transverse sampling points are located in the longitudinal direction of the longitudinal sampling points. A series of state points are calculated by the length of the path and the transverse distance.

[0041] Among them, the state sampling layer is set to multiple layers.

[0042] The designed dynamic programming cost function includes the cost functions of the control sampling layer and the state sampling layer. The cost function includes the lateral offset cost, the lateral offset change rate cost, the control angle change cost, and the control angle change rate cost:

[0043] control cost =k pl *pl+k plr *plr+k cd *cd+k cdr *cdr;

[0044] Where pl, plr, cd, and cdr represent the lateral displacement, the lateral displacement change rate, the control angle change, and the control angle change rate, respectively. pl , k plr , k cd , k cdr is the cost coefficient.

[0045] Among them, the specific method of calculating the cost function value of the control sampling layer is as follows:

[0046] The control sampling layer calculates the lateral deviation based on the control point matching the nearest path point. The lateral deviation change rate is equal to the lateral deviation of the current control point minus the lateral deviation of the initial position. The control angle change is the angle calculated by the control sampling, and the control angle change rate is equal to the angle obtained by sampling minus the angle of the last control of the robot.

[0047] Among them, the specific method of calculating the cost function value of the state sampling layer is as follows:

[0048] The state sampling layer calculates the lateral deviation based on the absolute value of the lateral sampling interval. The lateral position change rate is equal to the lateral deviation of the current layer minus the lateral deviation of the previous layer. The control angle change rate is equal to the angle calculated by the current sampling layer minus the angle of the robot in the previous layer. The control angle change is calculated as follows:

[0049] (x ni -x 0 ) 2 +(y ni -y 0 ) 2 =(x cj -x 0 ) 2 +(y cj -y 0 ) 2 ;

[0050] (x ni ,y ni ) represents the coordinates of the next state layer, (x cj ,y cj )Current state layer coordinates, (x 0 ,y 0 ) The coordinates of the steering center point are calculated according to the coordinates between the state layers, and then the steering angle δ is calculated according to the steering center point:

[0051]

[0052] Among them, the cost function is calculated cumulatively. According to the idea of ​​dynamic programming, if the state point with the minimum cost of the last layer is calculated, then the state point with the minimum cost of the previous layer is calculated, and the state point with the minimum cost of the previous layer is calculated in sequence until it traces back to the control sampling layer. The control sampling point of the minimum cost function contains the optimal control angle.

[0053] The present invention also provides a robot trajectory tracking control system, comprising a processing module for executing the robot trajectory tracking control method as described above.

[0054] A robot trajectory tracking control system and method of the present invention first obtains the driving path issued by the planning layer, and based on the path, uses the LQR algorithm based on the kinematic model to calculate the turning angle of the robot, effectively reducing the dependence on a specific model. By setting reasonable control sampling and state sampling parameters, and designing a dynamic programming cost function, the present invention can comprehensively consider a variety of environmental factors and significantly improve the environmental adaptability of the robot trajectory tracking control. At the same time, by optimizing the calculation process, the amount of calculation and time consumption are reduced, and the real-time performance of the system is improved. In addition, the method of the present invention has a strong generalization ability and can be applied to different application scenarios and road conditions, thereby solving the technical problems of the robot trajectory tracking control method in the prior art, such as strong model dependence, insufficient environmental adaptability, large amount of calculation and time consumption, and limited generalization ability.

[0055] The present invention can be applied to scenarios requiring high control accuracy, such as U-turns on narrow roads, posture adjustment, and side-to-side driving. BRIEF DESCRIPTION OF THE DRAWINGS

[0056] In order to more clearly illustrate the embodiments of the present invention or the technical solutions in the prior art, the drawings required for use in the embodiments or the description of the prior art will be briefly introduced below. Obviously, the drawings described below are only some embodiments of the present invention. For ordinary technicians in this field, other drawings can be obtained based on these drawings without paying creative work.

[0057] Figure 1 It is a control sampling schematic diagram of the robot trajectory tracking control method of the present invention.

[0058] Figure 2 It is a state sampling schematic diagram of the robot trajectory tracking control method of the present invention.

[0059] Figure 3 It is a schematic diagram of optimization control selection of the robot trajectory tracking control method of the present invention. DETAILED DESCRIPTION

[0060] Embodiments of the present invention are described in detail below. Examples of the embodiments are shown in the accompanying drawings. The embodiments described below with reference to the accompanying drawings are exemplary and are intended to be used to explain the present invention, but should not be construed as limiting the present invention.

[0061] See also Figure 1 to Figure 3 ,in Figure 1 It is a control sampling schematic diagram of the robot trajectory tracking control method of the present invention. Figure 2 It is a state sampling schematic diagram of the robot trajectory tracking control method of the present invention. Figure 3 It is a schematic diagram of optimization control selection of the robot trajectory tracking control method of the present invention.

[0062] The present invention provides a robot trajectory tracking control method, comprising the following steps:

[0063] S1. First, obtain the driving path issued by the planning layer;

[0064] For this specific implementation, the acquired driving path includes multiple waypoints, and each waypoint includes coordinates, curvature, turning angle, coordinate ID, coordinate point path length and heading in the robot coordinate system.

[0065] S2. Based on the obtained formal path, the objective function is designed through the LQR algorithm based on the kinematic model to calculate the turning angle of the robot;

[0066] For this specific implementation, the discrete state equation of the kinematic model is as follows:

[0067]

[0068] in,

[0069] v r represents the reference speed, δ r represents the front wheel turning angle, T represents the control time, The reference heading angle, l represents the wheelbase;

[0070] The objective function represents the weighted sum of the accumulated tracking deviation and the accumulated control input. The objective function is set to a quadratic form as follows:

[0071]

[0072] Q = diag[q 1 ,q 2 ,q 3 ],R=[r 1 , r 2 ], Q, R value settings are linearly interpolated according to speed,

[0073]

[0074] The control quantity u is a linear function of the state variable X:

[0075] u=-[(R+B T PB) -1 B T PA]X = -KX;

[0076] where P is the solution of the Riccati equation:

[0077] P=A T PA-A T PB(R+BT PB) -1 B T PA+Q;

[0078] The angle δ on the path is selected as the feedforward angle, and the final output angle is:

[0079] u=δ+ku 1 .

[0080] S3, set the step size and range of control sampling;

[0081] For this specific implementation, when setting the step size and range of control sampling:

[0082] The step size is set to [1,5] degrees, the value range is set to [-10,10], and finally the control sampling set {δ 0 ',δ 1 ',δ 2 ',δ 3 ',...δ n '}Sampling the robot Ackerman steering model to calculate the steering curvature:

[0083]

[0084] Where l represents the wheelbase of the robot chassis;

[0085] Set the sampling time according to the control frequency. If the control frequency is 20 Hz and the sampling time is 50 ms, a series of control points are obtained. The coordinates of the control points are expressed as:

[0086]

[0087] x 0 ,y 0 , T s Indicates the coordinates and heading of the initial state.

[0088] S4, setting the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points;

[0089] For this specific implementation, when setting the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points:

[0090] The longitudinal sampling length is set according to the vehicle feedback speed. The speed coefficient of the longitudinal sampling length is set to [1,2]. To prevent the length from being too long, the upper limit of the longitudinal sampling length is also set to [5,8] meters. The number of longitudinal sampling points is set in the range of [2,5]. The transverse sampling length is set to [0.1,0.5] meters. The number of transverse sampling points is set in the range of [3,10]. The distance obtained by longitudinal sampling matches the nearest path point, and then the state points are calculated in sequence according to the interval and number of transverse sampling points. The transverse sampling points are located in the longitudinal direction of the longitudinal sampling points. A series of state points are calculated by the length of the path and the transverse distance.

[0091] Among them, the state sampling layer is set to multiple layers.

[0092] S5. Design dynamic programming cost function;

[0093] For this specific implementation, the designed dynamic programming cost function includes the cost functions of the control sampling layer and the state sampling layer. The cost function includes the lateral offset cost, the lateral offset change rate cost, the control angle change cost, and the control angle change rate cost:

[0094] control cost =k pl *pl+k plr *plr+k cd *cd+k cdr *cdr;

[0095] Where pl, plr, cd, and cdr represent the lateral displacement, the lateral displacement change rate, the control angle change, and the control angle change rate, respectively. pl , k plr , k cd , k cdr is the cost coefficient.

[0096] S6, calculating the cost function value of the control sampling layer;

[0097] For this specific implementation, the control sampling layer calculates the lateral deviation based on the control point matching the nearest path point. The lateral deviation change rate is equal to the lateral deviation of the current control point minus the lateral deviation of the initial position. The control angle change is the angle obtained by control sampling calculation. The control angle change rate is equal to the angle obtained by sampling minus the angle of the last control of the robot.

[0098] S7, calculating the cost function value of the state sampling layer;

[0099] For this specific implementation, the state sampling layer calculates the lateral deviation according to the absolute value of the lateral sampling interval. The lateral position change rate is equal to the lateral deviation of the current layer minus the lateral deviation of the previous layer. The control angle change rate is equal to the angle calculated by the current sampling layer minus the angle of the robot in the previous layer. The control angle change amount is calculated as follows:

[0100] (x ni -x 0 ) 2 +(y ni -y 0 ) 2 =(x cj -x 0 ) 2 +(y cj -y 0 ) 2 ;

[0101] (x ni ,y ni ) represents the coordinates of the next state layer, (x cj ,y cj )Current state layer coordinates, (x 0 ,y 0 ) The coordinates of the steering center point are calculated according to the coordinates between the state layers, and then the steering angle δ is calculated according to the steering center point:

[0102]

[0103] S8, calculating the sampling point with the minimum cost function of the final state sampling layer;

[0104] For this specific implementation, the cost function is calculated cumulatively. According to the idea of ​​dynamic programming, if the state point with the minimum cost of the last layer is calculated, then the state point with the minimum cost of the previous layer is calculated, and the state point with the minimum cost of the previous layer is calculated in sequence until the control sampling layer is traced back. The control sampling point of the minimum cost function contains the optimal control angle.

[0105] S9, the state-control point is inverted to obtain the control-state path point, and the starting point of the path contains the optimal controlled robot corner.

[0106] Using a robot trajectory tracking control method of the present embodiment, the present invention first obtains the driving path issued by the planning layer, and calculates the turning angle of the robot based on the path using the LQR algorithm based on the kinematic model, which effectively reduces the dependence on a specific model. By setting reasonable control sampling and state sampling parameters, and designing a dynamic programming cost function, the present invention can comprehensively consider a variety of environmental factors and significantly improve the environmental adaptability of the robot trajectory tracking control. At the same time, by optimizing the calculation process, the amount of calculation and time consumption are reduced, and the real-time performance of the system is improved. In addition, the method of the present invention has a strong generalization ability and can be applied to different application scenarios and road conditions, thereby solving the technical problems of strong model dependence, insufficient environmental adaptability, large amount of calculation and time consumption, and limited generalization ability in the robot trajectory tracking control method in the prior art.

[0107] The present invention also provides a robot trajectory tracking control system, comprising a processing module for executing the robot trajectory tracking control method as described above.

[0108] The following is an explanation of a method for constructing a robot trajectory tracking control system:

[0109] Determine the range and accuracy of the robot's rotation angle. The robot's control angle range and control accuracy are determined according to the control protocol. For example, the range is [-30°, 30°], and the control accuracy is [0.05°, 0.1°]. Select the angle deviation value and the number of control sampling points based on the maximum robot rotation angle. The angle deviation value is usually set to [1°, 5°], the number of longitudinal sampling points is set to {2, 3, 4, 5, ...}, the transverse sampling length is set to [0.1m, 0.5m], and the number of transverse sampling points is set to {3, 4, 5, 6, ..., 10}, as shown in the attached figure. Figure 1 As shown in the figure, if the angle deviation value is too large, serious dragon-drawing phenomenon will occur in the robot's driving process. The control sampling angle should be kept symmetrical about the original rotation angle to ensure the symmetry of the control sampling. The control sampling time can be designed according to the sending frequency of the control node. It should be noted that there are positive and negative differences in the control sampling coordinate points when the robot moves forward and backward.

[0110] The path length of all state sampling points should be less than the path length of the control sampling points. The interval and number of state layer points can be set according to the actual robot external parameters, as shown in the attached Figure 2 As shown, for example, a robot with a larger wheelbase can be designed with a larger value, and the interval between state sampling layers can be set according to the speed value.

[0111] The cost function of the dynamic programming algorithm is equal to the lateral distance from the control sampling point or state sampling point to the path + the lateral distance change rate + the angle size + the angle size change rate. According to the algorithm debugging effect, modify the coefficients of various parameters. If the lateral error converges too slowly during the debugging process, the lateral error coefficient and the lateral error change rate coefficient should be increased. If a serious "dragon drawing" phenomenon occurs, the angle size and the angle change rate coefficient should be increased. When setting parameters, the corresponding effects of each parameter should be considered comprehensively. The larger the coefficient, the smaller the corresponding control error will be. For example, if the lateral distance coefficient from the point to the path is designed to be a larger value, the robot will have a smaller lateral error after optimization. The calculated results of the cost function parameters should be of the same order of magnitude, and the difference between them should not be too large, otherwise some parameters will become invalid.

[0112] From the last layer state sampling node, backtrack according to the minimum cost and parent node, find the previous layer node and record the curve between them, backtrack layer by layer and record the curve, such as Figure 3 shown.

[0113] The present invention obtains the robot state of a control cycle through control sampling, obtains the robot state in the future through state sampling points, calculates the cost function between control sampling and state sampling points through the robot kinematic model, and calculates the current optimal control amount of the robot using a dynamic programming algorithm, thereby improving the robot motion control accuracy while meeting real-time requirements.

[0114] What is disclosed above is only a preferred embodiment of the present invention, and it certainly cannot be used to limit the scope of rights of the present invention. Ordinary technicians in this field can understand that all or part of the processes of the above embodiment and equivalent changes made according to the claims of the present invention still fall within the scope of the invention.

Claims

1. A robot trajectory tracking control method, characterized in that: The steps include: First, obtain the driving path issued by the planning layer; Based on the acquired formal path, the objective function is designed through the LQR algorithm based on the kinematic model to calculate the robot's turning angle; Set the step size and range of control sampling; Set the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points; Design dynamic programming cost function; Calculate the cost function value of the control sampling layer; Calculate the cost function value of the state sampling layer; Calculate the sampling point with the minimum cost function of the final state sampling layer; The state-control point inversion obtains the control-state path point, and the starting point of the path contains the optimal controlled robot angle.

2. The robot trajectory tracking control method according to claim 1, characterized in that: The acquired driving path includes multiple waypoints, each of which contains coordinates, curvature, turning angle, coordinate ID, coordinate point path length and heading in the robot coordinate system.

3. The robot trajectory tracking control method according to claim 2, characterized in that: Based on the obtained formal path, the objective function is designed through the LQR algorithm based on the kinematic model to calculate the robot's turning angle: The discrete state equation of the kinematic model is as follows: in, v r represents the reference speed, δ r represents the front wheel turning angle, T represents the control time, Reference heading angle, l represents Wheelbase; The objective function represents the weighted sum of the accumulated tracking deviation and the accumulated control input. The objective function is set to a quadratic form as follows: Q=diag[q1,q2,q3],R=[r1,r2],Q,R values ​​are set based on linear interpolation of speed. The control quantity u is a linear function of the state variable X: u=-[(R+B T PB) -1 B T PA]X=-KX; where P is the solution of the Riccati equation: P=A T PA-A T PB(R+B T PB) -1 B T PA+Q; The angle δ on the path is selected as the feedforward angle, and the final output angle is: u=δ+ku1.

4. The robot trajectory tracking control method according to claim 3, characterized in that: When setting the step size and range of control sampling: The step size is set to [1,5] degrees, the value range is set to [-10,10], and finally the control sampling set {δ0',δ1',δ2',δ3',...δ n '}Sampling the robot Ackerman steering model to calculate the steering curvature: Where l represents the wheelbase of the robot chassis; Set the sampling time according to the control frequency. If the control frequency is 20 Hz and the sampling time is 50 ms, a series of control points are obtained. The coordinates of the control points are expressed as: x0, y0, T s Indicates the coordinates and heading of the initial state.

5. The robot trajectory tracking control method according to claim 4, characterized in that: When setting the longitudinal sampling length, transverse sampling length and number of sampling points of the state sampling layer sampling points: The longitudinal sampling length is set according to the vehicle feedback speed. The speed coefficient of the longitudinal sampling length is set to [1,2]. To prevent the length from being too long, the upper limit of the longitudinal sampling length is also set to [5,8] meters. The number of longitudinal sampling points is set in the range of [2,5]. The transverse sampling length is set to [0.1,0.5] meters. The number of transverse sampling points is set in the range of [3,10]. The distance obtained by longitudinal sampling matches the nearest path point, and then the state points are calculated in sequence according to the interval and number of transverse sampling points. The transverse sampling points are located in the longitudinal direction of the longitudinal sampling points. A series of state points are calculated by the length of the path and the transverse distance. Among them, the state sampling layer is set to multiple layers.

6. The robot trajectory tracking control method according to claim 5, characterized in that: The designed dynamic programming cost function includes the cost functions of the control sampling layer and the state sampling layer. The cost function includes the lateral offset cost, the lateral offset change rate cost, the control angle change cost, and the control angle change rate cost: control cost =k pl *pl+k plr *plr+k cd *cd+k cdr *cdr; Where pl, plr, cd, and cdr represent the lateral displacement, the lateral displacement change rate, the control angle change, and the control angle change rate, respectively. pl , k plr , k cd , k cdr is the cost coefficient.

7. The robot trajectory tracking control method according to claim 6, characterized in that: The specific method for calculating the cost function value of the control sampling layer is as follows: The control sampling layer calculates the lateral deviation based on the control point matching the nearest path point. The lateral deviation change rate is equal to the lateral deviation of the current control point minus the lateral deviation of the initial position. The control angle change is the angle calculated by the control sampling, and the control angle change rate is equal to the angle obtained by sampling minus the angle of the last control of the robot.

8. The robot trajectory tracking control method according to claim 7, characterized in that: The specific method for calculating the cost function value of the state sampling layer is as follows: The state sampling layer calculates the lateral deviation based on the absolute value of the lateral sampling interval. The lateral position change rate is equal to the lateral deviation of the current layer minus the lateral deviation of the previous layer. The control angle change rate is equal to the angle calculated by the current sampling layer minus the angle of the robot in the previous layer. The control angle change is calculated as follows: (x ni -x0) 2 +(y ni -y0) 2 =(x cj -x0) 2 +(y cj -y0) 2 ; (x ni ,y ni ) represents the coordinates of the next state layer, (x cj ,y cj ) The current state layer coordinates, (x0, y0) The turning center point coordinates, the turning center point coordinates are calculated according to the coordinates between the state layers, and then the turning angle δ is calculated according to the turning center point:

9. The robot trajectory tracking control method according to claim 8, characterized in that: The cost function is calculated cumulatively. According to the idea of ​​dynamic programming, if the state point with the minimum cost in the last layer is calculated, then the state point with the minimum cost in the previous layer is calculated, and the state points with the minimum cost in the previous layer are calculated in sequence until the control sampling layer is traced back. The control sampling point of the minimum cost function contains the optimal control angle.

10. A robot trajectory tracking control system, characterized in that: The method comprises a processing module for executing the robot trajectory tracking control method according to any one of claims 1 to 9.

Images