Omnidirectional motion chassis device and trajectory tracking control method thereof

Abstract

The invention relates to the technical field of automatic driving, in particular to an omnidirectional motion chassis device and a trajectory tracking control method thereof. The control method comprises the following steps: constructing a two-degree-of-freedom chassis dynamical model of the chassis, converting the chassis dynamical model to obtain a chassis trajectory tracking error model, establishing a Riccati equation according to the chassis trajectory tracking error model, and solving the Riccati equation according to the current motion state information of the chassis. Obtaining a tracking error according to the current motion state information of the chassis and the target path, and obtaining an initial rotation angle control quantity of a driving wheel of the chassis according to a coefficient obtained by solving a Riccati equation and the tracking error; the current course error of the chassis is obtained, PID adjustment is conducted on the initial rotation angle control quantity of the driving wheels of the chassis according to the current course error of the chassis, and the adjusted rotation angle control quantity is obtained; and trajectory tracking control is carried out on the chassis according to the adjusted rotation angle control quantity, so that the control precision of the chassis is improved.

Description

Technical Field

[0001] The present application relates to the field of autonomous driving technology, and in particular to an omnidirectional motion chassis device and a trajectory tracking control method thereof. Background Art

[0002] With the development of technologies such as artificial intelligence, research on autonomous driving is also advancing. As an important technology in the future development of society, autonomous driving technology is widely used in transportation logistics, transportation and other aspects.

[0003] The continuous increase in the number of cars in recent years has brought about increasingly serious traffic jams and safety problems. Autonomous driving technology is an effective solution to these problems. At the same time, the autonomous driving industry is also an important node for the current country to improve the level of intelligent development. The research and advancement of autonomous driving technology is in line with the needs of the national development strategy. However, in recent years, traffic accidents caused by autonomous driving have seriously affected the development and implementation speed of autonomous driving. Doubts about whether autonomous driving is safe and reliable are the main obstacles to the implementation of autonomous driving, and autonomous driving testing is an effective means to prove whether autonomous driving is safe and reliable.

[0004] Due to the danger and high cost of testing real human-vehicle pre-collision and vehicle-to-vehicle pre-collision situations, autonomous driving tests have introduced autonomous driving chassis devices. Testers can simulate various real-life movement situations of people and vehicles by placing dummy models and vehicle models on the chassis device and controlling the movement of the chassis through control methods, thereby achieving the purpose and requirements of autonomous driving tests while reducing danger and cost.

[0005] Among the relevant autonomous driving test technologies, the omnidirectional autonomous driving chassis devices are mainly four-wheel drive four-turn, front-wheel drive two-turn and rear-wheel drive two-turn types. Among them, the front-wheel drive two-turn means that the front two wheels are driven by motors and actively steer, and the rear two wheels are driven wheels; the rear-wheel drive two-turn means that the rear two wheels are driven by motors and actively steer, and the front two wheels are driven wheels. Among these chassis, the four-wheel drive four-turn chassis has higher control accuracy but complex control methods; the front-wheel drive two-turn chassis and the rear-wheel drive two-turn chassis have relatively simple control methods but are not conducive to balancing the load and traction of the chassis. Summary of the invention

[0006] In order to solve the technical problems in the related art that the four-wheel drive four-turn chassis control is complex and the front-wheel drive two-turn and rear-wheel drive two-turn chassis control is not conducive to balancing the load and traction of the chassis, the present application provides an omnidirectional motion chassis with two diagonal driving wheels and two driven wheels and a trajectory tracking control method thereof.

[0007] On the one hand, the present application provides a trajectory tracking control method for an omnidirectional motion chassis, the control method comprising:

[0008] Based on the movement of the two driving wheels and two driven wheels on the diagonal line of the chassis, a two-degree-of-freedom chassis dynamics model of the chassis is constructed;

[0009] According to the geometric relationship between the chassis target path and the actual path being tracked, the chassis dynamics model is converted to obtain a chassis trajectory tracking error model, a discrete state equation and a dynamic cost function of LQR are established according to the chassis trajectory tracking error model, and a Riccati equation is established according to the discrete state equation and the dynamic cost function of LQR;

[0010] Acquire current motion state information and a target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain a tracking error according to the current motion state information and the target path of the chassis, and obtain a preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and the tracking error obtained by solving the Riccati equation;

[0011] Acquire a current heading error of the chassis, and use the current heading error of the chassis to perform PID adjustment on a preliminary steering angle control amount of the chassis to obtain an adjusted steering angle control amount;

[0012] The chassis is subjected to trajectory tracking control according to the adjusted turning angle control amount.

[0013] In some possible designs, the two-degree-of-freedom chassis dynamics model of the chassis is:

[0014]

[0015] Among them, v x is the running speed of the chassis, is the lateral displacement acceleration of the chassis, is the angular acceleration of the chassis, m is the total mass of the chassis, I m is the moment of inertia of the chassis around the vertical line of the center of mass, C f is the sum of the cornering stiffness of the two front wheels of the chassis; C r is the sum of the cornering stiffness of the two rear wheels of the chassis, δ f is the front driving wheel turning angle of the chassis; δ r is the rear driving wheel turning angle of the chassis, a is the distance from the midpoint of the center line of the two front wheels of the chassis to the center of mass, b is the distance from the midpoint of the center line of the two rear wheels of the chassis to the center of mass, is the lateral displacement speed of the chassis, is the angular velocity of the chassis.

[0016] In some possible designs, the chassis trajectory tracking error model is:

[0017]

[0018] Among them, e d is the lateral error, is the first-order derivative of the lateral error with respect to time; is the second-order derivative of the lateral error with respect to time; is the heading error, is the first-order derivative of the heading error with respect to time; is the second-order derivative of the heading error with respect to time;

[0019] The chassis trajectory tracking error model is converted into:

[0020] in,

[0021] In some possible designs, the discretized state equation of the omnidirectional motion chassis is: k+1 =A d x k +B d u k ;

[0022] in, B d =Bdt, I is the unit matrix, dt is a small time quantity, k is the discrete step length, x k+1 is the value of x at time k+1, x k is the value of x at time k, u k is the value of u at time k.

[0023] In some possible designs, the Riccati equation is solved according to the current motion state information of the chassis, a tracking error is obtained according to the current motion state information of the chassis and a target path, and a preliminary steering angle control amount of the driving wheel of the chassis is obtained according to the coefficient and the tracking error obtained by solving the Riccati equation, including:

[0024] Set the dynamic cost function of LQR to:

[0025]

[0026] Among them, Q and R are the state weight matrix and dynamic output weight matrix of LQR, and n is the end time;

[0027]

[0028] The coefficients of the Q matrix are fixed and the R matrix is ​​dynamically adjusted, where r 1 and r 2 It can be expressed as:

[0029] r 1 =k 1 vx 2 +k 2 v x +k 3

[0030] r 2 =k 4 v x 2 +k 5 v x +k 6

[0031] Among them, q 1 is the lateral error weighting coefficient, q 2 is the weighting coefficient of the first-order derivative of the lateral error, q 3 is the heading error weighting coefficient, q 4 is the weighted coefficient of the first-order derivative of the heading error, r 1 is the weighted coefficient of the output front driving wheel angle, r 2 k is the weighted coefficient of the output driving wheel angle; 1 ,k 2 ,k 3 ,k 4 ,k 5 ,k 6 is the adjustment coefficient;

[0032] The Riccati equation is established as:

[0033] Solving for tracking error:

[0034] e d =n T d err

[0035] e s =τ T d err

[0036]

[0037] Among them, (x, y) is the current coordinate of the chassis, θ is the current heading angle of the chassis, (x d ,y d ) is the coordinate of the tracking target point, θ d To track the heading angle of the target point,

[0038] is the tangent vector of the target point, is the normal vector of the target point, k is the curvature of the target point, is the chassis heading angle;

[0039] The initial steering angle control amount of the driving wheel of the omnidirectional motion chassis device is: u = -Kx;

[0040] In the above formula, K=(R+B T PB) -1 BPA;

[0041] In the above formula, P = Q + A T P(I+BR -1 B T P) -1 A.

[0042] In some possible designs, the using the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis to obtain the adjusted steering angle control amount includes:

[0043] Get the current heading error of the omnidirectional motion chassis

[0044] The turning angle adjustment Δδ of the omnidirectional motion chassis device is calculated by the following formula:

[0045]

[0046] According to the angle adjustment amount Δδ, the adjusted angle control amounts of the two driving wheels are:

[0047] δ 1 =δ f -Δδ, δ 2 =δ r +Δδ;

[0048] Among them, K p is the proportionality coefficient, K i is the integration coefficient, K d is the differential coefficient, Δδ is the adjustment amount, δ 1 is the front driving wheel angle, δ 2 is the rear driving wheel turning angle.

[0049] On the other hand, the present application also provides an omnidirectional motion chassis device, the omnidirectional motion chassis device comprising:

[0050] A dynamic model building module is used to build a two-degree-of-freedom chassis dynamic model based on the movement modes of the two driving wheels and two driven wheels on the diagonal line of the chassis;

[0051] A trajectory tracking model building module, used for converting the chassis dynamics model according to the geometric relationship between the chassis target path and the actual path being tracked, obtaining a chassis trajectory tracking error model, establishing a discrete state equation and a dynamic cost function of LQR according to the chassis trajectory tracking error model, and establishing a Riccati equation according to the discrete state equation and the dynamic cost function of LQR;

[0052] A steering angle control amount calculation module, used to obtain current motion state information and a target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain a tracking error according to the current motion state information and the target path of the chassis, and obtain a preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and the tracking error obtained by solving the Riccati equation;

[0053] A steering angle control amount adjustment module is used to obtain a current heading error of the chassis, and use the current heading error of the chassis to perform PID adjustment on a preliminary steering angle control amount of the chassis to obtain an adjusted steering angle control amount;

[0054] The tracking control module is used to perform trajectory tracking control on the chassis according to the adjusted turning angle control amount.

[0055] On the other hand, the present application also provides a computer-readable storage medium storing a computer program, which executes the trajectory tracking control method of the omnidirectional motion chassis as described above when the computer program is run on a processor.

[0056] On the other hand, the present application also provides a computer program product, which includes: a computer program code, when the computer program code is run on a computer, the computer executes the trajectory tracking control method of the omnidirectional motion chassis as described above.

[0057] According to the trajectory tracking control method of the omnidirectional motion chassis provided by the present application, it is mainly used for the omnidirectional motion chassis with two diagonal driving wheels and two driven wheels. First, based on the movement mode of the two diagonal driving wheels and two driven wheels of the chassis, a two-degree-of-freedom chassis dynamics model of the chassis is constructed. According to the geometric relationship between the chassis target path and the actual path tracked, the chassis dynamics model is converted to obtain a chassis trajectory tracking error model. According to the chassis trajectory tracking error model, a discrete state equation and a dynamic cost function of LQR are established. According to the discrete state equation and the dynamic cost function of LQR, an LQR dynamics model is established. Ricatti equation; obtain the current motion state information and target path of the chassis, solve the Ricatti equation according to the current motion state information of the chassis, obtain the tracking error according to the current motion state information and target path of the chassis, and obtain the preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and tracking error obtained by solving the Ricatti equation; obtain the current heading error of the chassis, use the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis, and obtain the adjusted steering angle control amount; perform trajectory tracking control on the chassis according to the adjusted steering angle control amount, thereby improving the control accuracy of the chassis.

[0058] Based on the implementations provided in the above aspects, this application can also be further combined to provide more implementations. BRIEF DESCRIPTION OF THE DRAWINGS

[0059] Figure 1 An exemplary flow chart of a trajectory tracking control method for an omnidirectional motion chassis provided in an embodiment of the present application;

[0060] Figure 2 A schematic diagram of a two-degree-of-freedom chassis dynamics model provided in an embodiment of the present application;

[0061] Figure 3 A schematic diagram of a trajectory tracking error model provided in an embodiment of the present application;

[0062] Figure 4 The overall structure diagram of the chassis device with two diagonal driving wheels and two driven wheels provided in the embodiment of the present application;

[0063] Figure 5 Schematic diagram of various movement directions of the chassis device with two diagonal driving wheels and two driven wheels provided in the embodiment of the present application;

[0064] Figure 6 A schematic diagram of a simulation test process for a chassis provided in an embodiment of the present application;

[0065] Figure 7 A schematic diagram of the structure of a chassis trajectory tracking control device provided in an embodiment of the present application. DETAILED DESCRIPTION

[0066] The technical solution in this application will be described below in conjunction with the accompanying drawings.

[0067] In the description of the embodiments of the present application, unless otherwise specified, “ / ” means or. For example, A / B can mean A or B. The “and / or” in this article is merely a way to describe the association relationship of associated objects, indicating that three relationships may exist. For example, A and / or B can mean: A exists alone, A and B exist at the same time, and B exists alone.

[0068] In the following, the terms "first" and "second" are used for descriptive purposes only and are not to be understood as indicating or implying relative importance or implicitly indicating the number of the indicated technical features. Thus, a feature defined as "first" or "second" may explicitly or implicitly include one or more of the features. In the description of this embodiment, unless otherwise specified, "more" means two or more.

[0069] It should be noted that the chassis described in the following technical solutions of this application is specifically an omnidirectional motion chassis.

[0070] At present, the chassis devices of omnidirectional sports chassis are mainly four-wheel drive four-turn, front-wheel drive two-turn and rear-wheel drive two-turn types. The front-wheel drive two-turn means that the front two wheels are driven by motors and actively steer, and the rear two wheels are driven wheels; the rear-wheel drive two-turn means that the rear two wheels are driven by motors and actively steer, and the front two wheels are driven wheels. Among these chassis, the four-wheel drive four-turn chassis has higher control accuracy but complex control methods; the front-wheel drive two-turn chassis and the rear-wheel drive two-turn chassis have relatively simple control methods but are not conducive to balancing the load and traction of the chassis.

[0071] In order to overcome the deficiencies in the above-mentioned prior art, the present application provides an omnidirectional motion chassis and a trajectory tracking control method thereof. The chassis adopts a motion mode of two diagonal driving wheels and two driven wheels, which helps to balance the load and traction of the vehicle body, and the two diagonal driving wheels can be independently controlled, thereby improving the control accuracy of the chassis. The control method is mainly used to control an omnidirectional motion chassis with two diagonal driving wheels and two driven wheels, especially to control an omnidirectional motion chassis with two wheels on the left diagonal as driving wheels and two wheels on the other diagonal as driven wheels. First, the two wheels on the left diagonal are driving wheels, and a chassis trajectory tracking error model is established based on the LQR control method. The Ricatti equation is established according to the chassis trajectory tracking error model. The preliminary turning angle control amount of the driving wheel of the chassis is obtained according to the coefficients and tracking errors obtained by solving the Ricatti equation. Finally, the turning angle control amount of the chassis is PID-adjusted using the current heading error of the chassis to obtain the adjusted turning angle control amount, and the chassis is track-tracked according to the adjusted turning angle control amount. The chassis can be accurately controlled by this control method.

[0072] Specifically, in the present application, firstly, a two-degree-of-freedom chassis dynamics model of the chassis is constructed based on the movement modes of the two driving wheels and the two driven wheels on the diagonal of the chassis; according to the geometric relationship between the chassis target path and the actual path tracked, the chassis dynamics model is transformed to obtain a chassis trajectory tracking error model, and a discrete state equation and a dynamic cost function of LQR are established according to the chassis trajectory tracking error model, and a Ricatti equation is established according to the discrete state equation and the dynamic cost function of LQR; the current motion state information and the target path of the chassis are obtained, and the chassis trajectory tracking error model is converted according to the current motion state information of the chassis. The Riccati equation is solved, and the tracking error is obtained according to the current motion state information and the target path of the chassis. The preliminary steering angle control amount of the driving wheel of the chassis is obtained according to the coefficient and the tracking error obtained by solving the Riccati equation; the current heading error of the chassis is obtained, and the preliminary steering angle control amount of the chassis is PID-adjusted using the current heading error of the chassis to obtain the adjusted steering angle control amount; the chassis is tracked and controlled according to the adjusted steering angle control amount to obtain the adjusted final steering angle control amount, and the omnidirectional motion chassis device is tracked and controlled according to the adjusted final steering angle control amount.

[0073] Among them, among many control methods, linear quadratic regulator (hereinafter referred to as LQR) is one of the efficient optimal control methods. The optimal design of LQR is to design a full-state feedback controller to obtain the feedback coefficient K to satisfy the quadratic objective function J to obtain the minimum value, and K is determined by the weight matrices Q and R, so the selection of Q and R matrices is particularly important. The traditional selection method is to select fixed values ​​based on experience or simulation conclusions. The control effect is difficult to guarantee the optimal and the adaptability is poor. In order to overcome this technical defect, the present application adjusts the weight matrix R according to the specific actual application speed to realize the controller adaptively forming a dynamic LQR control method. The PID controller is a commonly used control method that uses proportion, integration, and differentiation to calculate the control quantity for control based on the system error; based on the dynamic LQR control method, the present application designs a PID angle adjustment method based on the chassis heading deviation error to achieve precise control of the chassis automatic driving.

[0074] The following is a detailed introduction to a trajectory tracking control method of an omnidirectional motion chassis provided in an embodiment of the present application in conjunction with the accompanying drawings. In the following embodiments, a chassis that controls the movement of two diagonal driving wheels and two driven wheels is taken as an example, and the two wheels on the diagonal of the chassis are driving wheels, and the two wheels on the other diagonal are driven wheels. For example, the left front wheel and the right rear wheel of the chassis are driving wheels, and the control method of the present application is further explained.

[0075] Figure 1 For an exemplary flow chart of the trajectory tracking control method of the omnidirectional motion chassis provided in the embodiment of the present application, please refer to Figure 1 As shown, the trajectory tracking control method includes:

[0076] Step S101: construct a two-degree-of-freedom chassis dynamics model of the chassis based on the movement modes of two driving wheels and two driven wheels on the diagonal lines of the chassis.

[0077] Step S102: According to the geometric relationship between the chassis target path and the actual path being tracked, the chassis dynamics model is transformed to obtain a chassis trajectory tracking error model; a discrete state equation and a dynamic cost function of LQR are established according to the chassis trajectory tracking error model; and a Riccati equation is established according to the discrete state equation and the dynamic cost function of LQR.

[0078] Step S103, obtaining the current motion state information and target path of the chassis, solving the Riccati equation according to the current motion state information of the chassis, obtaining the tracking error according to the current motion state information and target path of the chassis, and obtaining the preliminary steering angle control amount of the driving wheel of the chassis according to the coefficients and tracking error obtained by solving the Riccati equation.

[0079] Step S104, obtaining a current heading error of the chassis, and using the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis to obtain an adjusted steering angle control amount.

[0080] Step S105: performing trajectory tracking control on the chassis according to the adjusted turning angle control amount.

[0081] When developing the chassis trajectory tracking error model, it is necessary to first establish the chassis dynamics model. Figure 2 For a schematic diagram of a two-degree-of-freedom chassis dynamics model provided in the embodiment of this application, see Figure 2 As shown, the two-degree-of-freedom chassis dynamics model of this embodiment with the left front wheel and the right rear wheel as the driving wheels can be expressed as:

[0082]

[0083] In formula (1), v x is the running speed of the chassis, is the lateral displacement acceleration of the chassis, is the angular acceleration of the chassis, m is the total mass of the chassis, I m is the moment of inertia of the chassis around the vertical line of the center of mass, C f is the sum of the cornering stiffness of the two front wheels of the chassis; C r is the sum of the cornering stiffness of the two rear wheels of the chassis, δ f is the front driving wheel turning angle of the chassis; δ ris the rear driving wheel turning angle of the chassis, a is the distance from the midpoint of the center line of the two front wheels of the chassis to the center of mass, b is the distance from the midpoint of the center line of the two rear wheels of the chassis to the center of mass, is the lateral displacement speed of the chassis, is the angular velocity of the chassis.

[0084] Through the above chassis dynamics model, we can x By controlling the rotation angle δ of the two driving wheels f and δ r , to achieve the lateral displacement acceleration of the chassis and angular acceleration control.

[0085] Assume that the current coordinates of the chassis are (x, y), the current heading angle is θ, and the coordinates of the tracking target point are (x d ,y d ), the heading angle of the tracking target point is θ d , the tangent vector and normal vector of the target point are:

[0086]

[0087] The error between the current position and the target point position can be expressed as:

[0088]

[0089] The conversion formula can be obtained through geometric relations:

[0090] e d =n T d err (4)

[0091] e s =τ T d err (5)

[0092]

[0093] Among them, v x is the current speed of the chassis, e d is the lateral error of the chassis; is the first-order derivative of the chassis lateral error with respect to time; is the chassis heading error; is the first-order derivative of the heading error with respect to time; k is the curvature of the target point; is the chassis heading angle.

[0094] Figure 3 For a schematic diagram of the trajectory tracking error model provided in the embodiment of the present application, please refer to Figure 3As shown, the above two-degree-of-freedom chassis dynamics model is converted by the above conversion formula to obtain a chassis trajectory tracking error model, which can be expressed as:

[0095]

[0096] in, is the second-order derivative of the lateral error with respect to time; is the second-order derivative of the heading error with respect to time;

[0097] The chassis trajectory tracking error model is transformed into:

[0098]

[0099] in,

[0100] Among them, the discretized state equation of the omnidirectional motion chassis is:

[0101] x k+1 =A d x k +B d u k (12)

[0102] in, B d =Bdt, I is the unit matrix, dt is a small amount of time, k is the discrete step length, x k+1 is the value of x at time k+1, x k is the value of x at time k, u k is the value of u at time k.

[0103] According to the above trajectory tracking error model, the lateral error and its first-order derivative with respect to time, the heading error and its first-order derivative with respect to time can be adjusted by controlling the tire angle, thereby achieving lateral control of the chassis.

[0104] It can be understood that after constructing the above-mentioned chassis trajectory tracking error model, the current motion state information of the chassis is obtained, and the chassis trajectory tracking error model is solved according to the current motion state information of the chassis to obtain the preliminary steering angle control value of the driving wheel of the chassis.

[0105] In this embodiment, the Riccati equation is solved according to the current motion state information of the chassis, the tracking error is obtained according to the current motion state information of the chassis and the target path, and the preliminary steering angle control amount of the driving wheel of the chassis is obtained according to the coefficient and the tracking error obtained by solving the Riccati equation, including:

[0106] Set the dynamic cost function of LQR to:

[0107]

[0108] Wherein, in the above formula (13), Q and R are the state weight matrix and dynamic output weight matrix of LQR, and n is the end time;

[0109] In the above formula, the above cost function satisfies the constraints: x k+1 =A d x k +B d u k ;

[0110]

[0111] Among them, q 1 is the lateral error weighting coefficient, q 2 is the weighting coefficient of the first-order derivative of the lateral error, q 3 is the weighting coefficient of heading error, q 4 is the weighted coefficient of the first-order derivative of the heading error, r 1 is the weighted coefficient of the output front driving wheel angle, r 2 is the weighted coefficient of the output rear driving wheel angle.

[0112] The coefficients of the Q matrix are fixed and the R matrix is ​​dynamically adjusted, where r 1 and r 2 It can be expressed as:

[0113] r 1 =k 1 v x 2 +k 2 v x +k 3 (15)

[0114] r 2 =k 4 vv x 2 +k 5 v x +k 6 (16)

[0115] Among them, k 1 ,k 2 ,k 3 ,k 4 ,k 5 ,k 6 is the adjustment coefficient, v x is the current speed of the chassis.

[0116] The above cost function is minimized by LQR control theory to obtain the following Riccati equation:

[0117]

[0118] The tracking error is obtained from the above formulas (4), (5), (6), (7), (8) and (9).

[0119] Among them, the initial steering angle control amount of the driving wheel of the omnidirectional motion chassis device is: u = -Kx;

[0120] In the above formula,

[0121] Among them, P is the solution of the Riccati equation, Q and R are the state weighted matrix and output weighted matrix of LQR, I is the unit matrix, K is the feedback coefficient, R matrix changes with the real-time speed, and Q matrix and R matrix remain unchanged in one control cycle. u is the two driving wheel angles calculated by the solution.

[0122] In some embodiments, in order to improve the accuracy of the trajectory tracking control of the chassis, after obtaining the steering angle control amount of the driving wheel of the chassis, the current heading error of the chassis is obtained, and the initial steering angle control amount of the chassis is PID-adjusted using the current heading error of the chassis to obtain the adjusted steering angle control amount, which specifically includes:

[0123] Receive and calculate the driving wheel rotation angle δ f and δ r , and the current heading error The final turning angles of the two driving wheels are obtained through PID adjustment. Specifically:

[0124] The chassis angle adjustment Δδ is calculated using the following formula:

[0125]

[0126] According to the angle adjustment amount Δδ, the adjusted angle control amounts of the two driving wheels are:

[0127] δ 1 =δ f -Δδ (18)

[0128] δ 2 =δ r +Δδ (19)

[0129] Among them, K p is the proportionality coefficient, K i is the integration coefficient, K d is the differential coefficient, Δδ is the adjustment amount, δ 1 is the left front wheel turning angle, δ 2is the right rear wheel turning angle.

[0130] It can be seen that the trajectory tracking control method provided by this embodiment realizes the trajectory tracking control of the chassis with two diagonal driving wheels and two driven wheels. Based on the movement of the two diagonal driving wheels and two driven wheels of the chassis, a two-degree-of-freedom chassis dynamics model of the chassis is constructed; according to the geometric relationship between the chassis target path and the actual path tracked, the chassis dynamics model is converted to obtain a chassis trajectory tracking error model; according to the chassis trajectory tracking error model, a discrete state equation and a dynamic cost function of LQR are established; according to the discrete state equation and the dynamic cost function of LQR, a two-degree-of-freedom chassis dynamics model of the chassis is constructed. Riccati equation; obtain the current motion state information and target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain the tracking error according to the current motion state information and target path of the chassis, obtain the preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and tracking error obtained by solving the Riccati equation, finally use the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis to obtain the adjusted steering angle control amount, and perform trajectory tracking control on the chassis according to the adjusted steering angle control amount. Through this control method, the chassis can be accurately controlled.

[0131] It is understandable that before the omnidirectional motion chassis is officially put into use, that is, before it is officially put on the road, the performance of autonomous driving needs to be tested to test the safety and reliability of the autonomous driving method. Due to the danger and high cost of testing real human-vehicle pre-collision and vehicle-vehicle pre-collision situations, the autonomous driving test introduced an omnidirectional motion chassis device. The tester can simulate various real-life human and vehicle movement situations by placing a dummy model and a chassis model on the chassis device and controlling the chassis movement through a control method, thereby achieving the purpose and test requirements of the autonomous driving test while reducing the danger and cost.

[0132] The following takes the chassis as an example to verify the trajectory tracking control method provided by this application.

[0133] Figure 4 The overall structure diagram of the chassis device with two diagonal driving wheels and two driven wheels provided in the embodiment of the present application is as follows: Figure 5 For schematic diagrams of various directions of movement of the chassis device with two diagonal driving wheels and two driven wheels provided in the embodiment of the present application, please refer to Figure 4 As shown, there are two slots for fixing the dummy model or chassis model, and a CAN communication interface and a power interface on the side. Figure 5As shown, among the four wheels of the chassis, the two wheels on the left diagonal (left front wheel and right rear wheel) are driving wheels, which receive drive from the engine through the power transmission system, while the two wheels on the right diagonal (right front wheel and left rear wheel) are driven wheels, which are not directly powered by the engine but can follow the rotation of the driving wheels. In this chassis device, the wheels on the left diagonal receive driving force at the same time, which helps to balance the load and traction of the chassis and reduce tire slippage. At the same time, all four wheels support 360° rotation, allowing the chassis to move in all directions within a limited space, which can meet various directional movement situations, such as Figure 5 As shown, (a) is a C-shaped turn, (b) is straight driving, (c) is diagonal driving, and (d) is turning on the spot.

[0134] According to the trajectory tracking control method provided by the above embodiment, a computer program product or a computer-readable storage medium can be generated. The computer program product includes: a computer program code. When the computer program code runs on a computer, the computer executes the trajectory tracking control method of the omnidirectional motion chassis as described above.

[0135] Figure 6 For a schematic diagram of the simulation test process for the chassis provided in the embodiment of the present application, please refer to Figure 6 As shown, for example, the above computer code is set in an industrial computer, the data interface of the industrial computer is connected to the CAN communication interface on the chassis, the industrial computer is installed on the chassis device for program design and data processing and solving, a laser radar or GPS is installed to provide chassis posture data for the control method, a csv file of the target path is recorded through the open source autonomous driving software, and a CAN card is connected to transmit data between the chassis and the industrial computer, and the industrial computer is enabled to receive the speed data of the chassis. The csv files of posture data, speed data and target path are passed into the control program, and the tire angle obtained by the program solution is passed into the chassis through the CAN bus to complete the control, so that the movement trajectory of real pedestrians or chassis can be simulated for autonomous driving testing.

[0136] In addition, the chassis device provided in this embodiment can also be used for autonomous driving simulation tests. By constructing a chassis model in simulation software such as Simulink and CarSim, designing the target path and using the control method provided by the present invention, the trajectory simulation of pedestrians and chassis in autonomous driving simulation tests can also be achieved.

[0137] It can be seen from the above simulation tests and emulation tests that the trajectory tracking control method provided by the present application has the following technical advantages compared with the related art:

[0138] 1. The control method of the present application is applicable to an omnidirectional motion chassis with two diagonal driving wheels and two driven wheels. This motion mode with two diagonal driving wheels and two driven wheels helps to balance the load and traction of the vehicle body and reduce tire slippage. Compared with a four-wheel drive four-turn chassis device, it reduces the complexity of the control method and solves the problem of load and traction that are not conducive to balancing the chassis compared with a front-wheel drive two-turn chassis and a rear-wheel drive two-turn chassis.

[0139] 2. The control method proposed in the present application can solve the problem of chassis heading deviation caused by the lag of the driven wheels in the chassis device with two diagonal driving wheels and two driven wheels, so that it can complete the trajectory tracking of the target path while keeping the chassis heading parallel to the reference path, thereby improving the control accuracy of the chassis.

[0140] Figure 7 This is a schematic diagram of the structure of the chassis LQR trajectory tracking control device provided in an embodiment of the present application. The control device provided below can refer to the above-mentioned control method.

[0141] See also Figure 7 As shown, the control device includes:

[0142] A dynamic model building module 701 is used to build a two-degree-of-freedom chassis dynamic model of the chassis based on the movement modes of two driving wheels and two driven wheels on the diagonal line of the chassis;

[0143] A trajectory tracking model building module 702 is used to convert the chassis dynamics model according to the geometric relationship between the chassis target path and the actual path being tracked to obtain a chassis trajectory tracking error model, establish a discrete state equation and a dynamic cost function of LQR according to the chassis trajectory tracking error model, and establish a Riccati equation according to the discrete state equation and the dynamic cost function of LQR;

[0144] The steering angle control amount calculation module 703 is used to obtain the current motion state information and the target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain the tracking error according to the current motion state information and the target path of the chassis, and obtain the preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and the tracking error obtained by solving the Riccati equation;

[0145] The steering angle control amount adjustment module 704 is used to obtain the current heading error of the chassis, and use the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis to obtain the adjusted steering angle control amount;

[0146] The tracking control module 705 is used to perform trajectory tracking control on the chassis according to the adjusted turning angle control amount.

[0147] An embodiment of the present application also provides a chassis autonomous driving test device, which includes a chassis and the chassis LQR trajectory tracking control device as described above, and a communication interface is provided on the chassis, which is communicatively connected with the chassis LQR trajectory tracking control device.

[0148] An embodiment of the present application also provides an omnidirectional motion chassis, which moves in a diagonal manner with two driving wheels and two driven wheels, the left front wheel and the right rear wheel of the chassis are driving wheels, and the right front wheel and the left rear wheel of the chassis are driven wheels; the chassis includes the LQR trajectory tracking control device as described above.

[0149] An embodiment of the present application further provides a computer-readable storage medium storing a computer program, which executes the trajectory tracking control method of the omnidirectional motion chassis as described above when the computer program is executed on a processor.

[0150] An embodiment of the present application further provides a computer program product, which includes: a computer program code, which, when executed on a computer, enables the computer to execute the trajectory tracking control method for the omnidirectional motion chassis as described above.

[0151] The above embodiments can be implemented in whole or in part by software, hardware, firmware or any other combination. When implemented using software, the above embodiments can be implemented in whole or in part in the form of a computer program product. The computer program product includes one or more computer instructions. When the computer program instructions are loaded or executed on a computer, the process or function described in the embodiment of the present application is generated in whole or in part. The computer can be a general-purpose computer, a special-purpose computer, a computer network, or other programmable device. The computer instructions can be stored in a computer-readable storage medium, or transmitted from one computer-readable storage medium to another computer-readable storage medium. For example, the computer instructions can be transmitted from a website site, computer, server or data center to another website site, computer, server or data center by wired (e.g., coaxial cable, optical fiber, digital subscriber line (DSL)) or wireless (e.g., infrared, wireless, microwave, etc.). The computer-readable storage medium can be any available medium that can be accessed by a computer or a data storage device such as a server or data center containing one or more available media sets. The available medium can be a magnetic medium (e.g., a floppy disk, a hard disk, a tape), an optical medium (e.g., a DVD), or a semiconductor medium. The semiconductor medium can be a solid state drive (SSD).

[0152] Those of ordinary skill in the art will appreciate that the units and algorithm steps of each example described in conjunction with the embodiments disclosed herein can be implemented in electronic hardware, or a combination of computer software and electronic hardware. Whether these functions are performed in hardware or software depends on the specific application and design constraints of the technical solution. Professional and technical personnel can use different methods to implement the described functions for each specific application, but such implementation should not be considered to be beyond the scope of the embodiments of the present application.

[0153] Those skilled in the art can clearly understand that, for the convenience and brevity of description, the specific working processes of the systems, devices and units described above can refer to the corresponding processes in the aforementioned method embodiments and will not be repeated here.

[0154] In the several embodiments provided in the embodiments of the present application, it should be understood that the disclosed systems, devices and methods can be implemented in other ways. For example, the device embodiments described above are only schematic. For example, the division of the units is only a logical function division. There may be other division methods in actual implementation, such as multiple units or components can be combined or integrated into another system, or some features can be ignored or not executed. Another point is that the mutual coupling or direct coupling or communication connection shown or discussed can be through some interfaces, indirect coupling or communication connection of devices or units, which can be electrical, mechanical or other forms.

[0155] The units described as separate components may or may not be physically separated, and the components shown as units may or may not be physical units, and may be located in one place or distributed on multiple network units. Some or all of the units may be selected according to actual needs to achieve the purpose of the solution of this embodiment.

[0156] In addition, each functional unit in each embodiment of the present application may be integrated into one processing unit, or each unit may exist physically separately, or two or more units may be integrated into one unit.

[0157] If the function is implemented in the form of a software functional unit and sold or used as an independent product, it can be stored in a computer-readable storage medium. Based on this understanding, the technical solution of the embodiment of the present application is essentially or the part that contributes to the prior art or the part of the technical solution can be embodied in the form of a software product, and the computer software product is stored in a storage medium, including a number of instructions to enable a memory (which can be a personal computer, a server, or a network device, etc.) to perform all or part of the steps of the method described in each embodiment of the embodiment of the present application. The aforementioned storage medium includes: U disk, mobile hard disk, read-only memory (Read-Only Memory, ROM), random access memory (Random Access Memory, RAM), disk or optical disk and other media that can store program code.

[0158] The above is only a specific implementation of the embodiment of the present application, but the protection scope of the embodiment of the present application is not limited thereto. Any technician familiar with the technical field can easily think of changes or replacements within the technical scope disclosed in the embodiment of the present application, which should be included in the protection scope of the embodiment of the present application. Therefore, the protection scope of the embodiment of the present application should be based on the protection scope of the claims.

Claims

1. A trajectory tracking control method for an omnidirectional motion chassis, characterized in that: The control method comprises: Based on the movement of the two driving wheels and two driven wheels on the diagonal line of the chassis, a two-degree-of-freedom chassis dynamics model of the chassis is constructed; According to the geometric relationship between the chassis target path and the actual path being tracked, the chassis dynamics model is converted to obtain a chassis trajectory tracking error model, a discrete state equation and a dynamic cost function of LQR are established according to the chassis trajectory tracking error model, and a Riccati equation is established according to the discrete state equation and the dynamic cost function of LQR; Acquire current motion state information and a target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain a tracking error according to the current motion state information and the target path of the chassis, and obtain a preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and the tracking error obtained by solving the Riccati equation; Acquire a current heading error of the chassis, and use the current heading error of the chassis to perform PID adjustment on a preliminary steering angle control amount of the chassis to obtain an adjusted steering angle control amount; The chassis is subjected to trajectory tracking control according to the adjusted turning angle control amount.

2. The trajectory tracking control method of the omnidirectional motion chassis according to claim 1, characterized in that: The two-degree-of-freedom chassis dynamics model of the chassis is: Among them, v x is the running speed of the chassis, is the lateral displacement acceleration of the chassis, is the angular acceleration of the chassis, m is the total mass of the chassis, I m is the moment of inertia of the chassis around the vertical line of the center of mass, C f is the sum of the cornering stiffness of the two front wheels of the chassis; C r is the sum of the cornering stiffness of the two rear wheels of the chassis, δ f is the front driving wheel turning angle of the chassis; δ r is the rear driving wheel turning angle of the chassis, a is the distance from the midpoint of the connecting line of the two front wheels to the center of mass, b is the distance from the midpoint of the connecting line of the two rear wheels to the center of mass, is the lateral displacement speed of the chassis, is the angular velocity of the chassis.

3. The trajectory tracking control method of the omnidirectional motion chassis according to claim 2, characterized in that: The chassis trajectory tracking error model is: Among them, e d is the lateral error, is the first-order derivative of the lateral error with respect to time; is the second-order derivative of the lateral error with respect to time; is the heading error, is the first-order derivative of the heading error with respect to time; is the second-order derivative of the heading error with respect to time; The chassis trajectory tracking error model is converted into: in, 4. The trajectory tracking control method of the omnidirectional motion chassis according to claim 3, characterized in that: The discretized state equation of the omnidirectional motion chassis is: k+1 =A d x k +B d u k ; in, B d =Bdt, I is the unit matrix, dt is a small time quantity, k is the discrete step length, x k+1 is the value of x at time k+1, x k is the value of x at time k, u k is the value of u at time k.

5. The trajectory tracking control method of the omnidirectional motion chassis according to claim 4, characterized in that: The Riccati equation is solved according to the current motion state information of the chassis, a tracking error is obtained according to the current motion state information of the chassis and a target path, and a preliminary steering angle control amount of the driving wheel of the chassis is obtained according to the coefficient and the tracking error obtained by solving the Riccati equation, including: Set the dynamic cost function of LQR to: Among them, Q and R are the state weight matrix and dynamic output weight matrix of LQR, and n is the end time; The coefficients of the Q matrix are fixed and the R matrix is ​​dynamically adjusted, where r1 and r2 can be expressed as: r1=k1v x 2 +k2v x +k3 r2=k4v x 2 +k5v x +k6 Among them, q1 is the weighted coefficient of the lateral error, q2 is the weighted coefficient of the first-order derivative of the lateral error with respect to time, q3 is the weighted coefficient of the heading error, q4 is the weighted coefficient of the first-order derivative of the heading error with respect to time, r1 is the weighted coefficient of the driving wheel angle before output, r2 is the weighted coefficient of the driving wheel angle after output, k1, k2, k3, k4, k5, k6 are adjustment coefficients; The Riccati equation is established as: Solving for tracking error: e d =n T d err e s =t T d err Among them, (x, y) is the current coordinate of the chassis, θ is the current heading angle of the chassis, (x d ,y d ) is the coordinate of the tracking target point, θ d To track the heading angle of the target point; is the tangent vector of the target point, is the normal vector of the target point, k is the curvature of the target point, is the chassis heading angle; The initial steering angle control amount of the driving wheel of the omnidirectional motion chassis device is: u = -Kx; In the above formula, 6. The trajectory tracking control method of the omnidirectional motion chassis according to claim 5, characterized in that: The method of using the current heading error of the chassis to perform PID adjustment on the preliminary steering angle control amount of the chassis to obtain the adjusted steering angle control amount includes: Get the current heading error of the omnidirectional motion chassis The turning angle adjustment Δδ of the omnidirectional motion chassis device is calculated by the following formula: According to the angle adjustment amount Δδ, the adjusted angle control amounts of the two driving wheels are: δ1=δ f -Δδ,δ2=δ r +Ds; Among them, K p is the proportionality coefficient, K i is the integration coefficient, K d is the differential coefficient, Δδ is the adjustment amount, δ1 is the front driving wheel angle, and δ2 is the rear driving wheel angle.

7. An omnidirectional motion chassis device, characterized in that: The omnidirectional motion chassis device comprises: A dynamic model building module is used to build a two-degree-of-freedom chassis dynamic model based on the movement modes of the two driving wheels and two driven wheels on the diagonal line of the chassis; A trajectory tracking model building module, used for converting the chassis dynamics model according to the geometric relationship between the chassis target path and the actual path being tracked, obtaining a chassis trajectory tracking error model, establishing a discrete state equation and a dynamic cost function of LQR according to the chassis trajectory tracking error model, and establishing a Riccati equation according to the discrete state equation and the dynamic cost function of LQR; A steering angle control amount calculation module, used to obtain current motion state information and a target path of the chassis, solve the Riccati equation according to the current motion state information of the chassis, obtain a tracking error according to the current motion state information and the target path of the chassis, and obtain a preliminary steering angle control amount of the driving wheel of the chassis according to the coefficient and the tracking error obtained by solving the Riccati equation; A steering angle control amount adjustment module is used to obtain a current heading error of the chassis, and use the current heading error of the chassis to perform PID adjustment on a preliminary steering angle control amount of the chassis to obtain an adjusted steering angle control amount; The tracking control module is used to perform trajectory tracking control on the chassis according to the adjusted turning angle control amount.

8. A computer-readable storage medium, characterized in that: The computer program is stored therein, and when the computer program is run on a processor, the trajectory tracking control method of the omnidirectional motion chassis according to any one of claims 1 to 5 is executed.

9. A computer program product, characterized in that The computer program product comprises: a computer program code, and when the computer program code is run on a computer, the computer is enabled to execute the trajectory tracking control method for an omnidirectional motion chassis according to any one of claims 1 to 5.

Images