A four-wheel drive four-wheel turning robot chassis control method based on dynamic LQR

Abstract

The application provides a four-wheel drive four-wheel steering robot chassis control method based on a dynamic LQR, and belongs to the technical field of robot chassis motion control. The method comprises a PI regulator 1, a PI regulator 2, an LQR regulator and a robot four-wheel steering mode dynamics model. The robot four-wheel steering mode dynamics model is established, and the dynamics model comprises a robot four-wheel steering mode lateral dynamics model and a robot four-wheel steering mode yaw dynamics model. The lateral and yaw dynamics models are converted into state space expression forms. According to the modern control theory, a control performance index function is established, a real-time feedback adjustment control parameter matrix is adjusted, LQR control performance is optimized, and a full-closed-loop parameter control is formed. The dynamic control of the LQR is realized according to the current chassis motion state, and the control performance and adaptability are further enhanced.

Description

TECHNICAL FIELD

[0001] The application relates to a four-wheel drive four-wheel steering robot chassis control method based on a dynamic LQR, and belongs to the technical field of robot chassis motion control. BACKGROUND

[0002] Mobile robots have a great promoting effect on the rapid development of society, economy and technology, and wheeled robots are currently the most rapidly developed and widely applied type. A four-wheel independent drive (4WID) and four-wheel independent steering (4WIS) wheeled robot chassis can realize more flexible and mobile motion control, such as two-wheel steering, four-wheel steering, front / rear steering, turning in place, front / rear drive, four-wheel drive, time-sharing four-wheel drive, parking, wedge-shaped motion, transverse movement and the like, and can better adapt to narrow and complex road conditions, thereby being suitable for various production, inspection, security and rescue application scenarios. Meanwhile, the flexible and mobile chassis structure also improves the complexity of control, and lateral stability control is particularly important.

[0003] At present, there are many lateral control stability controls for wheeled robot chassis, such as LQR lateral control of a chassis with front-wheel steering, i.e., Ackerman model, but there are relatively few lateral control stability controls for the four-wheel steering mode of a four-wheel independent drive and four-wheel independent steering wheeled robot chassis. The optimal design of an LQR linear quadratic regulator is to design a full-state feedback controller K to satisfy that a quadratic objective function J reaches a minimum value, and K is determined by a weight matrix Q and R, so the selection of the Q and R matrices is particularly important. The traditional selection method is to select fixed values according to experience or simulation conclusions, and the control effect is difficult to guarantee to be optimal, and the adaptability is poor. Therefore, the weight matrix Q and R are adjusted according to specific actual application scenarios and parameters to realize dynamic LQR control of the controller. SUMMARY

[0004] The application aims to provide a four-wheel drive four-wheel steering robot chassis control method based on dynamic LQR, which realizes dynamic control of LQR according to the current chassis motion state and further enhances the control performance and adaptability.

[0005] To achieve the above-mentioned purpose, the following technical solutions are adopted:

[0006] The application comprises a PI regulator 1, a PI regulator 2, an LQR controller and a robot four-wheel steering mode dynamics model.

[0007] Step 1: establishing a robot four-wheel steering mode dynamics model, wherein the dynamics model comprises a robot four-wheel steering mode lateral dynamics model and a robot four-wheel steering mode yaw dynamics model.

[0008] Step 2: convert the lateral and yaw dynamics model into state space expression form, and define the state vector matrix, control vector matrix and output vector matrix; according to modern control theory, establish the control performance index function;

[0009] Step 3: use the PI regulator to adjust the control parameter matrix according to the current motion state of the robot, and optimize the LQR control performance in real time to form a real-time dynamic LQR adjustment robot chassis motion posture.

[0010] Preferably, the lateral dynamics model of the robot four-wheel steering mode is specifically as follows:

[0011]

[0012] Wherein K i (i = 1, 2, 3, 4) is the side slip angle and side slip force coefficient;

[0013] The yaw dynamics model of the robot four-wheel steering mode is specifically as follows:

[0014]

[0015] In the formula, m is the total mass of the robot, J r is the moment of inertia of the robot around the vertical line passing through the center of mass, F iXr (i = 1, 2, 3, 4) is the force on the wheel along the Xr axis in the robot coordinate system, F iYr is the force on the wheel along the Yr axis in the robot coordinate system, α Yr is the acceleration of the vehicle body along the Yr axis, θ i is the steering angle of each wheel, ω r is the yaw angular velocity of the robot center of mass, a is the wheelbase of the chassis, b is the front and rear axle distance of the chassis, a Xr is the acceleration of the vehicle body along the Xr axis, β represents the side slip angle of the robot center of mass, v Yr is the longitudinal component speed of the vehicle body, is the side slip angle acceleration of the robot center of mass.

[0016] Preferably, the state vector matrix, control vector matrix and output vector matrix are specifically as follows:

[0017]

[0018] Wherein, X = [βw r ] T , Y = [βw r ] T , U = [θ1θ2θ3θ4] T , A is the system matrix, B is the control matrix, C is the output matrix, and E is the deviation matrix.

[0019] Preferably, the left and right wheel track of the chassis is:

[0020]

[0021] The front and rear wheel track of the chassis is:

[0022]

[0023] wherein,

[0024] Preferably, the control performance index function is specifically as follows:

[0025]

[0026] wherein, Q and R are the weighting matrices of the linear quadratic controller.

[0027] The weighting matrices of the linear quadratic controller are specifically as follows:

[0028]

[0029] wherein, q 11 is the weight coefficient of the side slip angle, q 22 is the weight coefficient of the yaw rate, r 11 , r 22 , r 33 , and r 44 are elements of the weighting matrix R.

[0030] Preferably, the specific steps of step 3 are as follows:

[0031] The coefficient q(t) is selected as the dynamic adjustment output parameter of the PI regulator 1 multiplied by Q as the new Q parameter matrix of the LQR controller, and the coefficient r(t) is selected as the dynamic adjustment output parameter of the PI regulator 2 multiplied by R as the new R parameter matrix of the LQR controller; the coefficient q(t) is obtained by the PI controller 1 according to the measured yaw rate dynamic output, and the coefficient r(t) is obtained by the PI controller 2 according to the average value of the deviation between the wheel angle and the expected wheel angle dynamic output;

[0032] The optimal control law is obtained by LQR theory:

[0033] U = -R -1 B T PX

[0034] wherein, P is the solution of the Riccati equation, Q and R are the dynamic change values calculated by the PI parameter regulator according to the real-time vehicle condition parameters, and remain unchanged in a control period.

[0035] Preferably, the coefficient q(t) is specifically formulated as follows:

[0036]

[0037] The coefficient r(t) is specifically formulated as follows:

[0038]

[0039] Wherein, k p1 Indicates the PI regulator 1 proportional coefficient, k i1 Indicates the PI regulator 1 integral coefficient, k p2 Indicates the PI regulator 2 proportional coefficient, k i2 Indicates the PI regulator 2 integral coefficient.

[0040] The application has the advantages that: the application establishes a dynamic model on the basis of four-wheel independent drive, independent steering four-wheel steering motion mode of a four-wheel drive four-turn robot chassis, and adds two PI controllers to realize dynamic adjustment of the weighting matrix Q and R on the basis of a traditional LQR controller; dynamic control of the LQR is realized according to the current chassis motion state, and the control performance and adaptability are further enhanced. BRIEF DESCRIPTION OF DRAWINGS

[0041] The accompanying drawings are included to provide a further understanding of the application, and constitute a part of the specification, illustrate the application, and are used to explain the application together with the embodiments of the application, and do not constitute a limitation on the application.

[0042] Figure 1 It is a four-wheel steering mode dynamics diagram of the four-wheel drive four-turn robot chassis of the application.

[0043] Figure 2 It is a dynamic LQR motion control diagram of the four-wheel drive four-turn robot chassis of the application DETAILED DESCRIPTION

[0044] The technical solutions in the embodiments of the application will be clearly and completely described below with reference to the drawings in the embodiments of the application. Obviously, the described embodiments are only part of the embodiments of the application, not all the embodiments. Based on the embodiments in the application, all other embodiments obtained by those skilled in the art without creative labor are within the protection scope of the application.

[0045] EMBODIMENT

[0046] The four-wheel drive four-turn 4WID-4WIS robot chassis motion control method of the application is based on the four-wheel drive four-turn 4WID-4WIS robot chassis four-wheel steering mode dynamics model as shown in the figure. Figure 1The dynamic LQR control flow chart of four-wheel steering mode motion of the four-wheel drive four-wheel steering 4WID-4WIS robot chassis is shown in the figure Figure 2

[0047] According to Figure 1 The four-wheel drive four-wheel steering 4WID-4WIS robot chassis dynamics schematic diagram shown in the figure, assuming that the robot chassis mass distribution is uniform, the mass center position and the geometric center of the vehicle body coincide, ignoring the road slope and air resistance, the two degrees of freedom (lateral, yaw) dynamics model of the four-wheel drive four-wheel steering robot 4WID-4WIS chassis four-wheel steering mode is established:

[0048]

[0049]

[0050]

[0051] In the formula, m is the total mass of the robot (Kg), J r is the moment of inertia of the robot around the vertical line passing through the mass center (Kg·m 2 ), E iXr (i=1, 2, 3, 4) is the force on the wheel along the X r axis in the robot coordinate system, F iYr is the force on the wheel along the Y r axis in the robot coordinate system, a Xr is the acceleration of the vehicle body along the X r axis, a Yr is the acceleration of the vehicle body along the Y r axis, θ i is the steering angle of each wheel, ω r is the yaw angular velocity of the mass center of the robot, a is the wheel track of the chassis, and b is the wheelbase of the chassis.

[0052] Among them, the acceleration of the vehicle body along the X r axis In the formula, v Yr is the longitudinal component speed of the vehicle body, β is the side slip angle of the mass center of the robot, is the side slip angle acceleration of the mass center of the robot; the lateral dynamics model is arranged as:

[0053]

[0054] The lateral and yaw dynamics models are arranged as:

[0055]

[0056]

[0057] ​In the formula, since the side slip angle δ of the robot motion is relatively small, the lateral force and the side slip angle are approximately linear, i.e. F iXr = K i δ, for the front wheel For the rear wheel In the motion of the robot, in order to ensure the stability of the vehicle body in the corner, the wheel angle θ i is generally small, and the generated mass center side slip angle is also small. Therefore, cosθ i ≈ 1, and sinθ i ≈ 0.

[0058] After reasonable simplification and further arrangement, the following is obtained:

[0059]

[0060]

[0061] Let be the yaw moment generated by the longitudinal force of the robot.

[0062] The above lateral and yaw dynamics model is written in the form of state space expression, and the state vector, control vector and output vector are defined:

[0063]

[0064] X = [βw r ] T , Y = [βw r ] T , U = [θ1 θ2 θ3 θ4] T

[0065] Then:

[0066]

[0067]

[0068] According to the modern control theory, the control performance index function is established:

[0069]

[0070]

[0071] Wherein Q and R are the weighting matrix of the linear quadratic controller, greatly affect the performance of the controller, and generally set a fixed value according to experience or simulation test results; and the robot motion process and motion environment influence factors are more complex, a single control parameter is difficult to achieve optimal control effect; in order to improve the adaptability and optimal performance of the lateral control, PI closed loop is used to adjust the control parameter matrix according to the current motion parameters in real time feedback, so as to optimize the LQR control performance, form a full closed loop parameter control, realize the dynamic adjustment of LQR control according to the vehicle condition.

[0072] The element q in the weighting parameter matrix 11 is the weight coefficient of the centroid side slip angle, q 22 is the weight coefficient of the yaw rate, the coefficient q is selected as the PI regulator 1 dynamic adjustment output parameter multiplied by Q as the new Q parameter matrix of the LQR controller, and the coefficient r is selected as the PI regulator 2 dynamic adjustment output parameter multiplied by R as the new R parameter matrix of the LQR controller. According to Figure 2 The four-wheel steering mode motion dynamic LQR control flowchart of the four-wheel drive four-wheel steering 4WID-4WIS robot chassis is shown in the figure, the PI controller 1 outputs the coefficient q according to the measured yaw rate, and the PI controller 2 outputs the coefficient r according to the average value of the deviation between the wheel angle and the expected wheel angle.

[0073]

[0074]

[0075] Further, the optimal control law is obtained according to the LQR theory: U = -R -1 B T PX, wherein P is the solution of the Riccati equation: A T P + PA - PBR -1 BP T + Q = 0, and Q and R are the dynamic values calculated by the PI parameter regulator according to the real-time vehicle condition parameters, and remain unchanged in a control period.

[0076] Finally, it should be noted that: the above only for the preferred embodiments of the present application, and not for the limitation of the present application, although the foregoing embodiments of the present application are described in detail, for those skilled in the art, the technical scheme recorded in the foregoing embodiments can be modified, or some technical features can be replaced. Any modification, equivalent replacement, improvement, etc. within the spirit and principles of the present application shall be included in the protection scope of the present application.

Claims

1. A chassis control method for a four-wheel drive, four-rotor robot based on dynamic LQR, characterized in that, Includes PI controller 1, PI controller 2, LQR controller, and dynamic model of robot four-wheel steering mode; The specific steps are as follows: Step 1: Establish a dynamic model of the robot's four-wheel steering mode, which includes a lateral dynamic model and a yaw dynamic model of the robot's four-wheel steering mode; Step 2: Transform the lateral and yaw dynamics models into state-space expressions, and define the state vector matrix, control vector matrix, and output vector matrix; establish the control performance index function based on modern control theory; Step 3: Using a PI controller, adjust the control parameter matrix in real time based on the robot's current motion state to optimize LQR control performance and form a real-time dynamic LQR adjustment of the robot chassis motion posture. The specific control performance index function is as follows: in, and This is the weighting matrix for the linear quadratic controller; The weighting matrix of the linear quadratic controller is as follows: ， in, The weight coefficient for the centroid sideslip angle. The yaw rate weighting coefficient is... Weighted matrix R Element; The specific steps of step 3 are as follows: Selecting coefficients As the PI controller 1, the dynamically adjusted output parameter is multiplied by As a new LQR controller Parameter matrix, selecting coefficients As the PI regulator 2, the dynamically adjusted output parameter is multiplied by As a new LQR controller Parameter matrix; the coefficients The coefficient is obtained by dynamically outputting the measured yaw rate through PI controller 1. The PI controller 2 dynamically outputs the value based on the average deviation between the wheel angle and the desired wheel angle. The optimal control law is obtained by solving using LQR theory: in, This is a solution to the Riccati equation. and It is a dynamic value calculated by the PI parameter regulator based on real-time vehicle condition parameters, and remains unchanged within a control cycle; The coefficient The specific formula is as follows: The coefficient The specific formula is as follows: in, This represents the proportional coefficient of the PI controller. This represents the integral coefficient of the PI controller. This represents the proportional coefficient of the PI controller. This represents the integral coefficient of the PI controller. For the steering angle of each wheel, Let ω be the yaw rate of the robot's center of mass.

2. The four-wheel drive, four-rotor robot chassis control method based on dynamic LQR according to claim 1, characterized in that, in These are the sideslip angle and sideslip force coefficients. ; The yaw dynamics model of the robot's four-wheel steering mode is as follows: In the formula, m For the total mass of the robot, Let the moment of inertia of the robot be the moment of rotation around the vertical line containing its center of mass. Let X be the force along the Xr axis on the wheel in the robot's coordinate system. Let Y be the force along the Yr axis on the wheel in the robot's coordinate system. Let be the acceleration of the vehicle body along the Yr axis. For the steering angle of each wheel, Let yaw rate be the angular velocity of the robot's center of mass. a The track width is the width between the left and right wheels of the chassis. b This refers to the front and rear wheelbase of the chassis. Let X be the acceleration of the vehicle body along the Xr axis. Indicates the deflection angle of the robot's center of mass. Let the longitudinal component of the velocity be the vehicle body. This is the lateral acceleration of the robot's center of mass.

3. The four-wheel drive, four-rotor robot chassis control method based on dynamic LQR according to claim 1, characterized in that, The state vector matrix, control vector matrix, and output vector matrix are as follows: in, , , , A For the system matrix, B For the control matrix, C For the output matrix, E This is the deviation matrix.

4. The four-wheel drive, four-rotor robot chassis control method based on dynamic LQR according to claim 3, characterized in that, The left and right wheelbase of the chassis is: The front and rear wheelbase of the chassis is: ， ， in, .

Images