A method and system for longitudinal and lateral control of an autonomous vehicle

Abstract

The application relates to a kind of automatic driving vehicle transverse and longitudinal control method and system, belong to vehicle automatic driving control technical field.Firstly, feedforward control is established, and the compensation steering wheel angle / acceleration is obtained by using feedforward control calculation, secondly, MPC feedback control is established, and the dynamics error model and prediction model of vehicle in MPC feedback control are updated using KF algorithm, to obtain the predicted steering wheel angle / acceleration, finally, the predicted steering wheel angle / acceleration is added to the compensation steering wheel angle / acceleration as target steering wheel angle / acceleration, and the vehicle is controlled according to target steering wheel angle / acceleration.This method solves the problem that when the vehicle dynamics model is nonlinear, the prediction accuracy of the prediction model used to predict the vehicle action is not accurate, resulting in poor transverse and longitudinal control effect.

Description

Technical Field

[0001] This invention relates to a method and system for lateral and longitudinal control of autonomous vehicles, belonging to the field of vehicle autonomous driving control technology. Background Technology

[0002] Currently, the main lateral and longitudinal control methods for autonomous sanitation vehicles include a combination of lateral LQR and longitudinal PID control, and lateral and longitudinal MPC control. The lateral LQR + longitudinal PID method uses a single-vehicle dynamics model as the research object, but the purpose of the lateral LQR and longitudinal PID controllers is only to find the optimal control solution. For example... Figure 1 As shown, the MPC lateral and longitudinal control receives information such as the target trajectory from the planning module, the current vehicle position from the positioning module, and the vehicle's state. The feedforward control system performs slope compensation calculations based on the acquired information to obtain feedforward acceleration, and simultaneously performs trajectory compensation calculations to obtain the feedforward steering wheel angle. The target steering wheel angle / acceleration when tracking the target trajectory is obtained by summing the compensated steering wheel angle / acceleration calculated by the feedforward control system based on curvature / slope and the steering wheel angle / acceleration calculated by the MPC feedback. The feedback target angle / acceleration calculated by the MPC is mainly based on the vehicle's dynamic error model and the current state variables, which can predict the system output over a period of time (i.e., prediction time domain and control time domain). By solving an optimization problem that satisfies the objective function and various constraints, a control sequence in the control time domain is obtained. Then, the first value in the control time domain is used as the current output value and applied to the controlled object. The above process is repeated in the next cycle, continuously completing a constrained optimization problem, thereby achieving continuous lateral and longitudinal control of the vehicle. The lateral and longitudinal MPC control uses a linear time-invariant (LTI) dynamic model to predict future behavior. If the object is highly nonlinear, or its characteristics change over time, the prediction accuracy of linear time-invariant (LTI) predictions may be severely reduced, to the point that MPC performance becomes unacceptable. The dynamics model of an autonomous sanitation vehicle is nonlinear, and the vehicle's mass changes significantly during cleaning and sweeping operations; therefore, inaccurate prediction accuracy leads to poor control performance. Summary of the Invention

[0003] The purpose of this invention is to provide a method and system for lateral and longitudinal control of autonomous vehicles, in order to solve the problem that the accuracy of predicting vehicle actions using a linear time-invariant dynamic model is inaccurate when the vehicle dynamics model is nonlinear, resulting in poor lateral and longitudinal control performance.

[0004] To achieve the above objectives, the present invention includes:

[0005] The present invention provides a method for lateral and longitudinal control of an autonomous vehicle, comprising the following steps:

[0006] 1) Establish feedforward control and use it to calculate the compensation steering wheel angle / acceleration;

[0007] 2) Establish MPC feedback control, and use the KF algorithm to update the vehicle's dynamic error model and prediction model in MPC feedback control to obtain the predicted steering wheel angle / acceleration;

[0008] 3) The predicted steering wheel angle / acceleration is added to the compensated steering wheel angle / acceleration to form the target steering wheel angle / acceleration, and the vehicle is controlled according to the target steering wheel angle / acceleration.

[0009] Beneficial Effects: The present invention provides a lateral and longitudinal control method for autonomous vehicles, which mainly improves upon MPC feedback control. Specifically, it uses the KF algorithm to update the vehicle's dynamic error model and prediction model in MPC feedback control to obtain the predicted steering wheel angle / acceleration. The predicted steering wheel angle / acceleration is then fused with the compensated steering wheel angle / acceleration calculated by feedforward control to form the target steering wheel angle / acceleration. The vehicle is then controlled according to the target steering wheel angle / acceleration. This method considers the case where the vehicle's dynamic error model is nonlinear. Even when the error model is nonlinear, this method can still accurately predict the vehicle's operating state and future actions, thereby achieving more precise lateral and longitudinal control of the vehicle.

[0010] Furthermore, the state update of the dynamic error model is as follows:

[0011]

[0012] X(k|k)=X(k|k-1)+K g (k)*Z(k)-X(k|k-1))

[0013] P(k|k=(1-K) g (k))*(k|k-1)

[0014] In the formula, R is the observation noise variance at time k, and P(k|k-1) is the error covariance matrix for predicting time k based on time k-1.

[0015] K g (k) is the Kalman gain matrix at time k, and X(k|k-1) is the state matrix predicted from time k-1 at time k.

[0016] e cg This refers to the lateral distance deviation. θ represents the rate of change of lateral distance deviation. e For heading angle deviation, e is the rate of change of heading angle deviation. xThe vertical distance is... Let X(k|k) be the longitudinal distance change rate, X(k|k) be the state matrix at time k, Z(k) be the observed state matrix at time k, and P(k|k) be the error covariance matrix at time k.

[0017] Furthermore, the formula used to update the prediction model is:

[0018] L(k)=A d P(k|k-1) / (P(k|k-1)+R)

[0019] X(k|k)=X(k|k-1)+K g (k)*Z(k)-X(k|k-1))

[0020] In the formula A d =(I-0.5A*T) s ) -1 (I+0.5A* s A is X(k|k) is the state variable at time k, L(k) is the updated Kalman gain matrix at time k, and P(k|k-1) is the error covariance matrix for predicting time k based on time k-1. The l in matrix A... f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x l is the speed of the vehicle's center of gravity. z Let represent the vehicle's moment of inertia.

[0021] Furthermore, the dynamic error model is as follows:

[0022] x(k+1)=A d x(k)+B d u(k)+C d

[0023] In the formula A d =(I-0.5A*T) s ) -1 (I+0.5A* s ), B d =B*T s C d =C*T s T s To control the cycle, x(k) represents the state variables at time k, which include lateral deviation, rate of change of lateral deviation, orientation angle deviation, rate of change of orientation angle deviation, distance error, and velocity error. δ fLet 'a' be the predicted steering wheel angle and 'a' be the predicted acceleration.

[0024] Furthermore, the objective function and constraints established by the MPC feedback control model are as follows:

[0025]

[0026] x min ≤x(k)≤x max

[0027] u min ≤u(k)≤u max

[0028] x(0)=x0

[0029] In the formula, N represents the prediction and control time domains, Q is the state weighting matrix, R1 is the control weighting matrix, and x... r As a reference state, x min x max These are the minimum and maximum constraints of the state matrix, u. min u max These represent the minimum and maximum constraints for lateral and longitudinal control, respectively, and x0 represents the initial state of the controlled vehicle.

[0030] The present invention discloses a lateral and longitudinal control system for an autonomous vehicle, comprising a feedforward control unit and an MPC feedback control unit. The feedforward control unit is used to calculate a compensating steering wheel angle / acceleration based on the road slope and trajectory curvature. The MPC feedback control unit is used to update the vehicle's dynamic error model and prediction model using the KF algorithm to obtain the predicted steering wheel angle / acceleration, and combine it with the compensating steering wheel angle / acceleration to obtain the target steering wheel angle / acceleration, enabling the vehicle to drive according to the target steering wheel angle / acceleration.

[0031] Beneficial Effects: The lateral and longitudinal control system for autonomous vehicles of this invention mainly includes a feedforward control unit and an MPC feedback control unit. This system primarily improves the MPC feedback control unit by employing the KF algorithm to update the vehicle's dynamic error model and prediction model within the MPC feedback control unit, thereby obtaining the predicted steering wheel angle / acceleration. The predicted steering wheel angle / acceleration is then fused with the compensated steering wheel angle / acceleration calculated by the feedforward control to form the target steering wheel angle / acceleration. The vehicle is then controlled according to this target steering wheel angle / acceleration. This method considers the case where the vehicle's dynamic error model is nonlinear. Even with a nonlinear error model, this method can still accurately predict the vehicle's operating state and future actions, thus achieving more precise lateral and longitudinal control of the vehicle.

[0032] Furthermore, the state update of the dynamic error model is as follows:

[0033]

[0034] X(k|k)=X(k|k-1)+K g (k)*Z(k)-X(k|k-1))

[0035] P(k|k=(1-K) g (k))*(k|k-1)

[0036] In the formula, R is the observation noise variance at time k, P(k|k-1) is the error covariance matrix for predicting time k based on time k-1, and K g (k) is the Kalman gain matrix at time k, and X(k|k-1) is the state matrix predicted from time k-1 at time k. e cg This refers to the lateral distance deviation. θ represents the rate of change of lateral distance deviation. e For heading angle deviation, e is the rate of change of heading angle deviation. x The vertical distance is... Let X(k|k) be the longitudinal distance change rate, X(k|k) be the state matrix at time k, Z(k) be the observed state matrix at time k, and P(k|k) be the error covariance matrix at time k.

[0037] Furthermore, the formula used to update the prediction model is:

[0038] L(k)=A d P(k|k-1) / (P(k|k-1)+R)

[0039] X(k|k)=X(k|k-1)+K g (k)*Z(k)-X(k|k-1))

[0040] In the formula A d =(I-0.5A*T) s ) -1 (I+0.5A* s A is X(k|k) is the state variable at time k, L(k) is the updated Kalman gain matrix at time k, and P(k|k-1) is the error covariance matrix for predicting time k based on time k-1. The l in matrix A... f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x l is the speed of the vehicle's center of gravity.z Let represent the vehicle's moment of inertia. Attached Figure Description

[0041] Figure 1 This is a schematic diagram of the lateral and longitudinal control principles of autonomous vehicles in existing technologies;

[0042] Figure 2 This is a schematic diagram of the principle of the lateral and longitudinal systems of an autonomous vehicle in an embodiment of the method of the present invention. Detailed Implementation

[0043] The present invention will now be described in further detail with reference to the accompanying drawings.

[0044] Examples of lateral and longitudinal control methods for autonomous vehicles:

[0045] The autonomous driving lateral and longitudinal control method in this embodiment, such as Figure 2 First, feedforward control is established, and the compensated steering wheel angle / acceleration is calculated using feedforward control. Second, MPC feedback control is established, and the vehicle's dynamic error model and prediction model in the MPC feedback control are updated using the KF algorithm to obtain the predicted steering wheel angle / acceleration. Finally, the predicted steering wheel angle / acceleration is added to the compensated steering wheel angle / acceleration to obtain the target steering wheel angle / acceleration, and the vehicle is controlled according to the target steering wheel angle / acceleration. A specific embodiment is as follows:

[0046] 1) Establish feedforward control and use feedforward control to calculate the compensation steering wheel angle / acceleration.

[0047] First, lateral control is calculated based on the road curvature. Lateral control is mainly determined by the steering wheel angle, and the steering wheel angle for feedforward compensation is primarily related to the curvature. The calculation formula is as follows:

[0048]

[0049] Where, L is the wheelbase of the vehicle (unit: m), k is the curvature of the target trajectory, m is the mass of the vehicle (unit: kg), and l f l is the distance from the front axle to the center of gravity of the vehicle (in meters). r c is the distance from the rear axle to the center of gravity of the vehicle. f c r The lateral stiffness of a single wheel on the front and rear axles of the vehicle (unit: N / rad) v x The speed of the vehicle's center of gravity (unit: m / s) 2 ).

[0050] Secondly, based on the road slope, the vehicle's feedforward longitudinal control calculates the feedforward compensation acceleration. The feedforward compensation acceleration is the sum of the reference acceleration of the target trajectory and the slope compensation acceleration, where the formula for the slope compensation acceleration is:

[0051] a pitch =g*sinθ)

[0052] In the formula, g is the acceleration due to gravity (g=9.8m / s), and θ is the slope (in radians).

[0053] 2) Establish MPC feedback control, update the vehicle's dynamic error model and prediction model in MPC feedback control using the KF algorithm to obtain the predicted steering wheel angle / acceleration, and finally add the compensated steering wheel angle / acceleration to the predicted steering wheel angle / acceleration as the target steering wheel angle / acceleration, and control the vehicle according to the target steering wheel angle / acceleration.

[0054] The vehicle dynamics error model is as follows:

[0055]

[0056] In equation (1), e cg , These are the lateral deviation and the rate of change of the lateral deviation, respectively (where the unit of lateral deviation is meters), θ e , Orientation angle deviation and the rate of change of orientation angle deviation (the unit of orientation angle deviation is radians), e x This represents the distance error (in meters). For velocity error (in m / s), I Z The moment of inertia of the vehicle (unit: kg·m) 2 ).

[0057] The vehicle dynamics error model in equation (1) can be written as a state equation, as shown in the following formula:

[0058]

[0059] In the formula A and B are coefficient matrices, X is the vehicle state variable, and u is the predicted steering wheel angle / acceleration. In matrix A, l... f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x l is the speed of the vehicle's center of gravity. zLet A be the vehicle's moment of inertia. After discretizing matrices A, B, and C using bilinear methods, the formula is as follows:

[0060] A d =(I-0.5A*T) s )- 1 (I+0.5A*T s )

[0061] B d =B*T s

[0062] C d =C*T s

[0063] Among them, T s The control period (in seconds).

[0064] Therefore, the vehicle dynamics error model equation can be written as:

[0065] x(k+1)=A d x(k)+B d u(k)+C d

[0066] Where x(k+1) is the state matrix at time k+1.

[0067] The vehicle's dynamic error model is updated using the Kalman filter (KF) algorithm:

[0068]

[0069] X(k|k)=X(k|k-1)+K g (k)*(Z(k)-X(k|k-1))

[0070] P(k|k)=(1-Kg(k))*P(k|k-1)

[0071] Where R is the observation noise variance at time k, P(k|k-1) is the error covariance matrix for predicting time k based on time k-1, and K g (k) is the Kalman gain matrix at time k, and X(k|k-1) is the state matrix predicted from time k-1 at time k. e cg This refers to the lateral distance deviation. θ represents the rate of change of lateral distance deviation. e For heading angle deviation, e is the rate of change of heading angle deviation. x The vertical distance is... Let X(k|k) be the longitudinal distance change rate, X(k|k) be the state matrix at time k (in this embodiment, X(k|k) and x(k) are the same concept, both representing the state matrix), Z(k) be the observed state matrix at time k, and P(k|k) be the error covariance matrix at time k.

[0072] The vehicle prediction model is also updated using the Kalman filter algorithm:

[0073] L9k)=A d P(k|k-1) / (P(k|k-1)+R)

[0074] X(k|k)=X(k|k-1)+K g (k)*Z(k)-X(k|k-1))

[0075] In the formula A d =(I-0.5A*T) s ) -1 (I+0.5A* s A is X(k|k) is the state variable at time k, L(k) is the updated Kalman gain matrix at time k, and P(k|k-1) is the error covariance matrix for predicting time k based on time k-1.

[0076] At time k, based on the state error model and prediction model of the autonomous vehicle, predict the system output for step N. Based on the constraints of the constraint function formula, design the objective function and calculate the optimal control solution u. * (t), namely the target steering wheel and acceleration, are input into the controlled vehicle, thereby causing it to move under the current control quantity.

[0077] The constraint function is:

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

[0079] x min ≤x(k)≤x max

[0080] u min ≤u(k)≤u max

[0081] x(0)=x0

[0082] In the formula, x min x max These are the minimum constraint and the maximum constraint of the state matrix, respectively. min The minimum constraint condition for control, u maxThe maximum constraint for control is given by x0, which represents the initial state.

[0083] The optimization objective function is set as follows:

[0084]

[0085] In the formula, N represents the prediction and control time domain (in seconds), Q is the state weighting matrix, R1 is the control weighting matrix, and x... r This is the reference state (the initial state is 0 by default).

[0086] The model predictive control (MPC) feedback control operation is repeated at time k+1, thus continuously implementing the constrained optimization problem and achieving continuous control of the controlled object.

[0087] This embodiment mainly improves upon MPC feedback control by employing the KF algorithm to update the vehicle's dynamic error model and prediction model in MPC feedback control, obtaining the predicted steering wheel angle / acceleration. This predicted steering wheel angle / acceleration is then fused with the compensated steering wheel angle / acceleration calculated by feedforward control to form the target steering wheel angle / acceleration. The vehicle is then controlled according to this target steering wheel angle / acceleration. This method considers the case where the vehicle's dynamic error model is nonlinear. Even with a nonlinear error model, this method can still accurately predict the vehicle's operating state and future actions, thereby achieving more precise lateral and longitudinal control of the vehicle.

[0088] Example of a lateral and longitudinal control system for autonomous vehicles:

[0089] The autonomous driving lateral and longitudinal control system in this embodiment mainly includes a feedforward control unit and an MPC (model predictive control) feedback control unit.

[0090] The feedforward control unit primarily performs feedforward calculations, which involve lateral and longitudinal control calculations based on the acquired vehicle position and parameter information. Specifically, it performs curvature compensation calculations based on the feedforward amount caused by road curvature to obtain the target steering wheel angle for feedforward compensation, thereby enabling lateral control of the vehicle. It also performs slope compensation calculations based on the road gradient to obtain slope compensation acceleration. This slope compensation acceleration is then added to the reference acceleration of the target trajectory to obtain the feedforward compensation acceleration, completing the feedforward calculation.

[0091] The MPC feedback control unit updates the vehicle's dynamic error model and prediction model using the KF algorithm to obtain the predicted steering wheel angle / acceleration. The predicted steering wheel angle / acceleration is then fused with the compensated steering wheel angle / acceleration calculated from the feedforward to obtain the target steering wheel angle / acceleration. Based on the fused and updated error model and prediction model, constraints are set, and an optimization objective function is set according to these constraints. The optimal solution is calculated to obtain the target steering wheel angle and acceleration, which are then input to the controlled vehicle. The controlled vehicle moves according to the input control quantities. The specific principle has been described in detail in the embodiments of the autonomous driving lateral and longitudinal control method, and will not be repeated here.

Claims

1. A method for controlling a vehicle in both lateral and longitudinal directions autonomously, characterized by, Includes the following steps: 1) Establish feedforward control and use it to calculate the compensation steering wheel angle / acceleration; 2) Establish MPC feedback control, and use the KF algorithm to update the vehicle's dynamic error model and prediction model in MPC feedback control to obtain the predicted steering wheel angle / acceleration; 3) The predicted steering wheel angle / acceleration is added to the compensated steering wheel angle / acceleration to obtain the target steering wheel angle / acceleration, and the vehicle is controlled according to the target steering wheel angle / acceleration; The state update of the dynamic error model is as follows: In the formula, R is the observation noise variance at time k. To predict the error covariance matrix at time k based on time k-1, Let K be the Kalman gain matrix at time k. To predict the state matrix at time k based on time k-1 , This refers to the lateral distance deviation. This represents the rate of change of lateral distance deviation. For heading angle deviation, The rate of change of heading angle deviation. The vertical distance is... The rate of change of longitudinal distance. Let k be the state matrix at time k. Let k be the state matrix observed at time k. Let be the error covariance matrix at time k.

2. The method for lateral and longitudinal control of an autonomous vehicle according to claim 1, characterized in that, The feedforward compensation for steering wheel angle and curvature is related, and its calculation formula is as follows: Where, in the formula For feedforward compensation of steering wheel angle, L is the vehicle's wheelbase, k is the curvature of the target trajectory, m is the vehicle's mass, and l f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x The speed of the vehicle's center of gravity.

3. The automatic driving vehicle longitudinal and lateral control method according to claim 1, characterized in that, The formula used to update the prediction model is: In the formula A is , Let k be the state variable at time k. The Kalman gain matrix is ​​updated at time k. To predict the error covariance matrix at time k based on time k-1, the l in matrix A... f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x l is the speed of the vehicle's center of gravity. z Let represent the vehicle's moment of inertia.

4. The automatic driving vehicle longitudinal and lateral control method according to claim 1, characterized by, The dynamic error model is as follows: In the formula , , , To control the cycle, B = C= , Let k be the state matrix. = Let 'a' be the predicted steering wheel angle and 'a' be the predicted acceleration.

5. The automatic driving vehicle longitudinal and lateral control method according to claim 4, characterized in that, The objective function and constraints established by the MPC feedback control model are as follows: where N is the prediction and control horizon, Q is the state weighting matrix, R1 is the control weighting matrix, x r is the reference state, are the minimum and maximum constraints of the state matrix, respectively, are the minimum and maximum constraints of the lateral and longitudinal control, respectively, is the initial state of the vehicle at the initial time.

6. A longitudinal and lateral control system for an autonomous vehicle, comprising a feedforward control unit for calculating a compensating steering wheel angle / acceleration based on a slope and a trajectory curvature of a road, and an MPC feedback control unit, characterized in that, The MPC feedback control unit is used to update the vehicle's dynamic error model and prediction model using the KF algorithm to obtain the predicted steering wheel angle / acceleration, and combine it with the compensated steering wheel angle / acceleration to obtain the target steering wheel angle / acceleration, so that the vehicle can drive according to the target steering wheel angle / acceleration. The state update of the dynamic error model is as follows: R is the variance of the observation noise at time k. To predict the error covariance matrix at time k based on time k-1, Let K be the Kalman gain matrix at time k. To predict the state matrix at time k based on time k-1 , This refers to the lateral distance deviation. This represents the rate of change of lateral distance deviation. For heading angle deviation, The rate of change of heading angle deviation. The vertical distance is... The rate of change of longitudinal distance. Let k be the state matrix at time k. Let k be the state matrix observed at time k. Let be the error covariance matrix at time k.

7. The lateral and longitudinal control system for autonomous vehicles according to claim 6, characterized in that, The feedforward compensation for steering wheel angle and curvature is related, and its calculation formula is as follows: Where, in the formula For feedforward compensation of steering wheel angle, L is the vehicle's wheelbase, k is the curvature of the target trajectory, m is the vehicle's mass, and l f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x The speed of the vehicle's center of gravity.

8. The automatic driving vehicle longitudinal and lateral control system according to claim 6, characterized in that, The formula used to update the prediction model is: In the formula A is , Let k be the state variable at time k. The Kalman gain matrix is ​​updated at time k. To predict the error covariance matrix at time k based on time k-1, the l in matrix A... f l is the distance from the front axle to the center of gravity of the vehicle. r c is the distance from the rear axle to the center of gravity of the vehicle. f c r These are the lateral stiffness of the front and rear axles of the vehicle, respectively, for each wheel on one side. x l is the speed of the vehicle's center of gravity. z Let represent the vehicle's moment of inertia.

Images