WO2022237392A1 - Vehicle lateral control method and apparatus, and vehicle - Google Patents

Abstract

A vehicle lateral control method and apparatus, and a vehicle. The method comprises: obtaining the current pose of a vehicle and the pose of a target point; calculating a state matrix according to the deviation between the current pose and the pose of the target point; determining a first model parameter matrix and a second model parameter matrix in a linear quadratic regulator (LQR) algorithm according to a vehicle dynamics model, and selecting a first weight matrix and a second weight matrix in the LQR algorithm; determining an optimal matrix according to the LQR algorithm; calculating a steering control amount on the basis of the state matrix and the optimal matrix; and controlling a steering executor of the vehicle to execute the steering control amount to control the vehicle laterally. Thus, the stability and comfort of vehicle tracking on a complex road having a large curvature change and in a driving scenario having a fast speed change are ensured, and the control accuracy and adaptability of an LQR controller are improved.

Description

Vehicle lateral control method, device and vehicle

This application requires the Chinese patent application filed on May 11, 2021 with the application number 202110510779.1 and the title of the invention as "Lateral control method, device and vehicle for autonomous vehicles", and the application number submitted on March 4, 2022 It is the priority of Chinese patent application 202210210929.1, titled "Lateral Control Method, Device and Vehicle for Autonomous Vehicles", the entire contents of which are incorporated in this application by reference.

technical field

The present application relates to the technical field of vehicles, in particular to a vehicle lateral control method, device and vehicle.

Background technique

Lateral control generally refers to the tracking control method based on the path, curvature and other information output by the upper-level motion planning module to reduce tracking errors and ensure the stability and comfort of the vehicle at the same time; according to whether the lateral control method uses the same vehicle model , which can be divided into two types: (1) model-free lateral control methods; (2) model-based lateral control methods. Among them, the model-based lateral control method can be further divided into: a lateral control method based on a vehicle kinematics model and a lateral control method based on a vehicle dynamics model.

The mainstream PID (Proportion Integral Differential, PID algorithm) control algorithm is a model-free lateral control, which takes the current path tracking deviation of the vehicle as an input, and performs proportional (Proportion), integral (Integration) and differential (Differentiation) on the tracking deviation. ) control to get the steering control amount.

However, because the PID algorithm does not consider the characteristics of the vehicle itself, it is less robust to external disturbances and cannot meet the effective control requirements of the vehicle during high-speed driving.

Contents of the invention

The present application provides a vehicle lateral control method, device and vehicle.

The first aspect of the embodiments of the present application is to provide a lateral control method of a vehicle, including the following steps:

Obtain the actual coordinates and current heading angle of the vehicle, and obtain the current pose and position information of the target point;

Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix; and

Use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and select the first weighting matrix and the second weighting matrix at the same time to determine the optimal model according to the linear quadratic regulator (LQR) algorithm matrix, and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.

Optionally, also include:

Determine the vehicle dynamics model according to the front wheel cornering stiffness, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle .

Optionally, the calculation of the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculating the state matrix include:

Determine the type of the current road;

If the type is a straight type, then the target point is the closest point on the trajectory to the current position;

If the type is a curve type, when the actual vehicle speed of the vehicle is greater than a preset threshold, the target point is a point away from the preview distance, otherwise the distance is determined by the curvature of the road.

Optionally, the formula for calculating the distance determined by the curvature of the road is:

L=kV+lmin,

Wherein, k is the linear change ratio of the speed, V is the actual vehicle speed of the vehicle, and lmin is the minimum setting value of the preview distance.

Optionally, the calculation formula of the state matrix is:

Wherein, e1 is the distance deviation, e2 is the change rate of the distance deviation, e3 is the heading angle deviation, and e4 is the change rate of the angle deviation.

The second aspect of the embodiments of the present application is to provide a lateral control device for a vehicle, including:

The obtaining module is used to obtain the actual coordinates and the current heading angle of the vehicle, and obtain the current pose and position information of the target point;

Calculation module, used to calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix; and

The control module is used to determine the first model parameter matrix and the second model parameter matrix by using the vehicle dynamics model, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.

Optionally, the device also includes:

A determining module, configured to determine the required weight according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle. The vehicle dynamics model described above. Optionally, the calculation module includes:

a judging unit, configured to judge the type of the current road;

The first determination unit is configured to, if the type is a straight type, then the target point is a point on the trajectory closest to the current position;

The second determining unit is configured to: if the type is a curve type, when the actual speed of the vehicle is greater than a preset threshold, the target point is a point that is a distance away from the preview; otherwise, the distance is a distance determined by the curvature of the road point.

Optionally, the formula for calculating the distance determined by the curvature of the road is:

L=kV+lmin,

Wherein, k is the linear change ratio of the speed, V is the actual vehicle speed of the vehicle, and lmin is the minimum setting value of the preview distance.

Optionally, the calculation formula of the state matrix is:

Wherein, e1 is the distance deviation, e2 is the change rate of the distance deviation, e3 is the heading angle deviation, and e4 is the change rate of the angle deviation.

The third aspect of the embodiments of the present application is to provide a lateral control method of a vehicle, including:

Obtaining the current pose of the vehicle and the pose of a target point, the target point being a track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track;

calculating a state matrix according to the deviation between the current pose and the pose of the target point;

Determine the first model parameter matrix and the second model parameter matrix in the linear quadratic regulator LQR algorithm according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm, wherein the first a weighting matrix related to the vehicle information of the vehicle and the road information of the current road;

Determine the optimal matrix according to the LQR algorithm;

A steering control amount is calculated based on the state matrix and the optimal matrix, and a steering actuator of the vehicle is controlled to execute the steering control amount to perform lateral control on the vehicle.

Optionally, the calculation of the state matrix according to the deviation between the current pose and the pose of the target point includes:

calculating the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point;

Taking the distance deviation, the rate of change of the distance deviation, the heading angle deviation and the rate of change of the angle deviation as elements in the state matrix to obtain the state matrix.

Optionally, the first weighting matrix is a diagonal matrix, and the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;

Wherein, the first element is the deviation coefficient of the vehicle, the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, the third element is determined according to the vehicle information and the road information, and the Both the second element and the fourth element are 0.

Optionally, the vehicle information includes the current speed, and the road information includes the curvature radius of the current road;

Determining the third element according to the vehicle information and the road information includes:

When the curvature radius of the current road is less than the first curvature radius threshold, it is determined that the current road is a straight road, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the current road is a small curvature curve, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the current road is a large curvature curve, and the third element is calculated according to the following formula:

Wherein, q ₃ is the third element; q ₁ is the first element; V is the current speed of the vehicle, m/s;

The first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve, and the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.

Optionally, the calculating the steering control amount based on the state matrix and the optimal matrix includes:

Multiplying the state matrix and the optimal matrix to obtain the steering control amount.

Optionally, before acquiring the pose of the target point, the method further includes:

Determine the type of the current road;

When the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;

When the type is a curve type, determine a track point on the tracking track that is apart from the target distance from the vehicle as the target point, and the target distance is related to the current speed of the vehicle and the current road speed. related to the curvature.

Optionally, the determining as the target point a track point on the tracking track that is at an aiming distance from the vehicle includes:

obtaining the current speed of the vehicle;

The aiming distance is determined according to the current vehicle speed, wherein,

When the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

When the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle , m; a is the comfortable deceleration, m/s ² ;

Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.

Optionally, before determining the first model parameter matrix and the second model parameter matrix in the linear quadratic regulator LQR algorithm according to the vehicle dynamics model, the method further includes:

Determine the vehicle dynamics model according to the front wheel cornering stiffness of the vehicle, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle .

The fourth aspect of the embodiments of the present application provides a lateral control device for a vehicle, including:

An acquisition module, configured to acquire the current pose of the vehicle and the pose of a target point, the target point being the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track ;

A calculation module, configured to calculate a state matrix according to the deviation between the current pose and the pose of the target point;

A determining module, configured to determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm , wherein the first weighting matrix is related to vehicle information of the vehicle and road information of the current road;

The calculation module is also used to determine the optimal matrix according to the LQR algorithm, and calculate the steering control amount based on the state matrix and the optimal matrix;

A control module, configured to control a steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.

Optionally, the calculation module is also used for:

calculating a distance deviation, a rate of change of distance deviation, a heading angle deviation and a rate of change of angle deviation between the vehicle and the target point; and,

Taking the distance deviation, the rate of change of the distance deviation, the heading angle deviation and the rate of change of the angle deviation as elements in the state matrix to obtain the state matrix.

Optionally, the first weighting matrix is a diagonal matrix, and the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;

Wherein, the first element is the deviation coefficient of the vehicle, the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, the third element is determined according to the vehicle information and the road information, and the Both the second element and the fourth element are 0.

Optionally, the vehicle information includes the current speed, and the road information includes the curvature radius of the current road;

The calculation module is also used for:

When the curvature radius of the current road is less than the first curvature radius threshold, it is determined that the current road is a straight road, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the current road is a small curvature curve, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the current road is a large curvature curve, and the third element is calculated according to the following formula:

Wherein, q3 is the third element; q1 is the first element; V is the current speed of the vehicle, m/s;

The first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve, and the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.

Optionally, the calculation module is further configured to multiply the state matrix and the optimal matrix to obtain the steering control amount.

Optionally, the device further includes a judging module, and the judging module is used for:

determine the type of the current road;

When the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;

When the type is a curve type, determine a track point on the tracking track that is apart from the target distance from the vehicle as the target point, and the target distance is related to the current speed of the vehicle and the current road speed. related to the curvature.

Optionally, the obtaining module is also used to obtain the current speed of the vehicle;

The judgment module is also used for:

The aiming distance is determined according to the current vehicle speed, wherein,

When the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

When the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle , m; a is the comfortable deceleration, m/s ² ;

Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.

Optionally, the determining module is further configured to: according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, and the z-axis rotation of the vehicle Inertia and overall vehicle mass determine the vehicle dynamics model.

A fifth aspect of the embodiments of the present application is to provide a vehicle, including a controller configured to execute the vehicle lateral control method described in the first aspect and/or the third aspect of the embodiments of the present application.

A sixth aspect of the embodiments of the present application provides a vehicle, which includes the vehicle lateral control device described in the second aspect and/or the fourth aspect of the embodiments of the present application.

Additional aspects and advantages of the application will be set forth in part in the description which follows, and in part will be obvious from the description, or may be learned by practice of the application.

Description of drawings

The above and/or additional aspects and advantages of the present application will become apparent and easy to understand from the following description of the embodiments in conjunction with the accompanying drawings, wherein:

FIG. 1 is a flowchart of a first vehicle lateral control method provided in an embodiment of the present application;

Fig. 2 is an example diagram of the LQR horizontal and vertical error provided by the embodiment of the present application;

Fig. 3 is the flowchart of the LQR algorithm that the embodiment of the present application provides;

4 is a schematic diagram of the tracking effect of curves at different speeds before optimization provided by the embodiment of the present application;

Fig. 5 is a schematic diagram of the tracking effect of the curve at different speeds after optimization provided by the embodiment of the present application;

Fig. 6 is a schematic block diagram of a lateral control device of a first vehicle provided in an embodiment of the present application;

FIG. 7 is a flow chart of a second vehicle lateral control method provided by an embodiment of the present application;

FIG. 8 is a flow chart of a third vehicle lateral control method provided by an embodiment of the present application;

Fig. 9 is a schematic block diagram of a second vehicle lateral control device provided by an embodiment of the present application.

Detailed ways

Embodiments of the present application are described in detail below, examples of which are shown in the drawings, wherein the same or similar reference numerals denote the same or similar elements or elements having the same or similar functions throughout. The embodiments described below by referring to the figures are exemplary, and are intended to explain the present application, and should not be construed as limiting the present application.

The vehicle lateral control method, device and vehicle according to the embodiments of the present application will be described below with reference to the accompanying drawings.

Before introducing the lateral control method of the vehicle in the embodiment of the present application, the processing methods in the related art are briefly introduced.

In related technologies, the LQR algorithm uses a two-degree-of-freedom dynamic model to design the lateral controller. The advantage of the LQR algorithm is that it can effectively reduce the steady-state tracking error when driving on a part of the curve by effectively combining it with the steering feedforward, so that When the vehicle is traveling on a medium-speed curve, its steady-state error approaches zero, which greatly improves the tracking performance.

However, for some large curvature and high-speed situations, the tracking effect will be significantly reduced, and the dependence on the environment and parameter selection is high, that is, when the environment changes suddenly, it cannot be well adapted to the trajectory tracking under the new state conditions. At the same time, the adjustment of LQR parameters is complicated. It not only needs to obtain the model parameters of the vehicle itself, but also needs to adjust the QR matrix of the LQR objective function (including the first weighting matrix Q and the second weighting matrix R). If the selection of the QR matrix is inaccurate, then the LQR The tracking performance of the algorithm will be greatly reduced, resulting in control failure, and the LQR algorithm in the related art basically uses a fixed QR matrix, resulting in poor system self-adaptability. This problem needs to be solved urgently.

Therefore, the present application provides a lateral control method for an autonomous vehicle. In this method, the actual coordinates and current heading angle of the vehicle can be obtained, the current pose and the position information of the target point can be obtained, and according to the current pose of the vehicle Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment based on the position information of the target point, calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix Model parameter matrix, and select the first weighting matrix and the second weighting matrix at the same time, to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and control the steering actuator of the vehicle to perform the multiplication of the optimal matrix and the state matrix to get Steering control amount, so as to ensure the stability and comfort of vehicle tracking on complex roads with rapid changes in curvature and speed, and improve the control accuracy and adaptability of the LQR controller.

Specifically, FIG. 1 is a schematic flowchart of a lateral control method for an autonomous vehicle provided in an embodiment of the present application.

As shown in Figure 1, the lateral control method of the self-driving vehicle includes the following steps:

In step S101, the actual coordinates and current heading angle of the vehicle are obtained, and the current pose and position information of the target point are obtained.

It can be understood that the method of obtaining the actual coordinates and current heading angle of the vehicle, and obtaining the current pose and position information of the target point according to the actual coordinates and the current heading angle can adopt the processing methods in related technologies. In order to avoid redundancy, in This will not be described in detail.

In step S102, calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix.

Optionally, in some embodiments, calculating the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculating the state matrix includes: judging the current road type; if the type is a straight road type, the target point is the point on the track closest to the current position; if the type is a curve type, when the actual speed of the vehicle is greater than the preset threshold, the target point is a point away from the preview distance , otherwise a point at a distance determined by the curvature of the road.

Wherein, the preset threshold may be a threshold preset by a user, may be a threshold obtained through a finite number of experiments, or may be a threshold obtained through a finite number of computer simulations. Optionally, the preset threshold is 60km/h.

Exemplarily, as shown in Figure 2, Vx is the longitudinal speed of the vehicle, Vy is the lateral speed of the vehicle, ψ is the current heading angle of the vehicle, ψ _des is the heading angle of the target point, and δ is the deflection angle of the front wheels. The ratio to the front wheel deflection angle is defined as ratio. It can be seen from Figure 2 that the calculation of the horizontal and vertical errors is based on the comparison between the current time point and the target point, so how to obtain the target point will be described in detail below.

Specifically, the type of road can generally include straight road type and curve type. If the type of road is straight road type, then the target point is selected as the point closest to the current position on the trajectory, so as to ensure the accuracy of straight line tracking; if the road If the type of curve is a curve type, it can be determined according to the actual speed of the vehicle. For example, when the actual speed of the vehicle is greater than the preset threshold, the selection of the speed threshold (i.e. the preset threshold) is adjusted according to the maximum speed limit of the track road section, usually selected as the speed limit, and the preview distance L=lmax of the selected target point; , when the actual speed of the vehicle is less than or equal to the preset threshold, the distance can be determined according to the curvature of the road, and the calculation formula can be:

L=kV+lmin;

Among them, k is the linear change ratio of the speed, V is the actual speed of the vehicle, and lmin is the minimum setting value of the preview distance.

It should be noted that lmin is selected as twice the minimum turning radius of the vehicle, lmax=lmin/2+v*v/2a, a is the comfortable deceleration set by the vehicle, usually set to 3, k=α*kp , kp is the curvature of the point closest to the current position on the trajectory, and α is an adjustable coefficient, which can be adjusted according to the actual tracking situation. If the curve is tracked early and the position deviation is large, α can be reduced, and vice versa. Then, according to the above formula, the preview distance is obtained, and the preview is performed along the forward direction of the vehicle to obtain the corresponding target point. When the preview distance is greater than the distance to the end point, the distance to the end point is selected as the preview distance.

Further, in some embodiments, e1 is the displacement from the current moment to the target point, and the state matrix state is calculated by the following formula;

e3=θ-θ1;

e2=Vx*e3+Vy;

get:

Among them, R is the radius of curvature of the target track point, e1 is the distance deviation, e2 is the change rate of the distance deviation, e3 is the heading angle deviation, and e4 is the change rate of the angle deviation.

In step S103, use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.

Wherein, as shown in Figure 3, Figure 3 is a flowchart of the LQR algorithm, which mainly includes the following steps:

S301, sensing environment and vehicle information.

Among them, the perceived environment and vehicle information include: vehicle coordinates and heading angles, and tracking track target point coordinates and heading angles.

S302, data processing.

Wherein, after data processing, the processed data is sent to step S309.

S303, determining the curvature of the straight road and curve, if it is a straight road, execute step S304, and if it is a curve, execute step S305.

S304, the target point is the point on the track closest to the current position, and skip to step S308.

S305, judging whether the actual vehicle speed of the vehicle is greater than a preset threshold, if yes, execute step S306, otherwise, execute step S307.

S306, select the target point as the preview distance L=lmax, and skip to step S308.

S307, the target point is selected as the preview distance L=kV+lmin.

S308, state quantity, and jump to step S310.

Among them, the distance deviation e1 between the vehicle and the target point, the distance deviation change rate e2, the heading deviation e3, and the angle deviation change rate e4 are calculated according to the real-time pose and target point position information, so as to obtain the state matrix state.

S309, a QR weight matrix selector.

That is to say, in the embodiment of the present application, the model parameter matrices A and B can be determined according to the above dynamic model parameters, and the weighting matrices Q and R can be selected at the same time (selection of QR weights).

From this, the state matrix can be obtained, and the state matrix and the QR weight matrix selector are input to the LQR controller.

S310, LQR controller.

S311, the vehicle turns to the actuator, and skips to step S301.

Thus, according to the above determined controller parameters, the steering control amount of the self-driving car is calculated and delivered to the steering actuator for execution.

Optionally, in some embodiments, it also includes: according to the front wheel cornering stiffness of the vehicle, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, and the z-axis moment of inertia of the vehicle Determine the vehicle dynamics model with the vehicle mass.

That is to say, the parameters of the vehicle dynamics model mainly include: the front wheel cornering stiffness Cf, the rear wheel cornering stiffness Cr, the distance lf from the front axle to the center of gravity of the vehicle, the distance lr from the rear axle to the center of gravity of the vehicle, and the z-axis rotation of the vehicle Inertia Iz and vehicle mass m.

It should be noted that the above-mentioned parameters of the vehicle dynamics model can be obtained by querying the basic information of the vehicle, or can be obtained by re-measurement. Specifically, it can be processed by those skilled in the art according to the actual situation, and is not specifically limited here.

Further, the calculation formulas for determining the first model parameter matrix matrix_a_ and the second model parameter matrix matrix_b_ by using the vehicle dynamics model may be as follows:

Among them, Cf and Cr are the front wheel cornering stiffness and rear wheel cornering stiffness, lf and lr are the distances from the front and rear axles to the center of gravity, Iz is the moment of inertia of the vehicle's z-axis, and m is the mass of the vehicle.

Further, select the first weighting matrix Q and the second weighting matrix R, the first weighting matrix Q, select the diagonal matrix matrix_q_=diag[q1, q2, q3, q4], wherein, the four of q1, q2, q3 and q4 The first parameters correspond to the four variables of the state matrix state, and the selection of q1 and q3 is the key to LQR control; the second weighting matrix R selects the unit matrix matrix_r_=[1]; it can be seen from the above flow chart in Figure 3 that the tracking system senses Get environment information and make selections:

(1) First judge the road curvature radius R and R1 and R2, R1 and R2 are the boundary conditions for distinguishing straight roads and curves, small curvature and large curvature respectively;

(2) When R<R1, determine that the tracking curve is a straight line, select q3=kq*q1, kq=0.1*V;

(3) When R1<R<R2, it is judged that the tracking track is a small curvature curve, and q3=kq*q1 is selected, kq=V;

(4) When R>R2, it is judged that the tracking track is a large curvature curve, and q3=kq*q1 is selected, kq=10*V;

(5) According to the above formula, the first weighting matrix Q can be obtained only by determining the initial value of q1 through line tracking of the real vehicle alone.

Further, the optimal matrix matrix_k is determined according to the numerical iterative solution of the Riccati equation, which is obtained by the following formula;

while(num_iteration++<max_num_iteration)

{

matrix_p_next=

matrix_a_T*matrix_p_*matrix_a_-(matrix_a_T*matrix_p_*matrix_b_)*(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)+matrix_q_;

matrix_p_=matrix_p_next;

}

matrix_k＝(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)

Wherein, max_num_iterationa is the maximum number of iterations, exemplarily selected as 150, matrix_a_T and matrix_b_T are the transpose matrices of matrix_a_ and matrix_b_ respectively, matrix_p is the process iteration matrix, and the initial value is the Q matrix.

Further, according to the multiplication of the optimal matrix matrix_k and the state matrix state, the front wheel rotation angle is finally multiplied by ratio and output to the actuator to realize tracking.

As shown in Figure 7, the embodiment of the present application also provides a lateral control method of a vehicle, which can be applied to self-driving cars, including fully automatic driving cars and semi-automatic driving cars, and the lateral control method includes steps S701-S706:

S701. Obtain the current pose of the vehicle and the pose of the target point.

The target point is the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track. Wherein, pose may also be referred to as position information, which may include coordinate information and direction information.

S702. Calculate a state matrix according to the deviation between the current pose and the pose of the target point.

Wherein, the deviation between the current pose and the pose of the target point at least includes the distance deviation between the actual coordinates of the vehicle and the coordinates of the target point, and the heading angle deviation between the current heading angle and the target heading angle. Generally speaking, the distance deviation refers to the absolute distance deviation between the actual coordinates of the vehicle and the coordinates of the target point, but in some embodiments, the distance deviation can also refer to the lateral distance between the actual coordinates of the vehicle and the coordinates of the target point Distance, lateral distance refers to the distance in the width direction of the vehicle body. Exemplarily, the lateral distance can be obtained by orthographically projecting the absolute distance in the vehicle body width direction. In some other embodiments of the present application, the deviation between the current pose and the pose of the target point may also include a distance deviation change rate and an angle deviation change rate.

S703. Determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm.

Wherein, among the first weighting matrix Q and the second weighting matrix R used when using the LQR algorithm, the first weighting matrix Q is a state weight matrix, and the selection condition of this matrix is related to the vehicle information of the vehicle and the road information of the current road; The second weighting matrix R is a control weighting matrix.

In this embodiment of the application, according to the actual situation and needs, the first weighting matrix Q and the second weighting matrix R can be selected after determining the first model parameter matrix and the second model parameter matrix, or after determining the first model parameter matrix Select the first weighting matrix Q and the second weighting matrix R before and the second model parameter matrix, and select the first weighting matrix Q and the second weighting matrix R while determining the first model parameter matrix and the second model parameter matrix.

S704. Determine the optimal matrix according to the LQR algorithm.

Among them, the optimal matrix can be obtained by substituting the first model parameter matrix and the second model parameter matrix into the Riccati equation for iterative solution.

It should be noted that, in this embodiment of the application, steps S701-S702 can be executed first, and then steps S703-S704 can be executed; steps S701-S702 can also be executed first, and then steps S703-S704 can be executed; Steps S701-S702 and steps S703-S704.

S705. Calculate the steering control amount based on the state matrix and the optimal matrix.

S706. Control the steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.

Wherein, step S705 can be executed by the controller of the vehicle or other units with calculation functions. After obtaining the steering control amount, the steering command can be sent to the steering actuator of the vehicle, so that the steering actuator that receives the steering command can follow the steering control Quantitative control of steering to achieve lateral control of the vehicle.

To sum up, in the embodiment of this application, by obtaining the current pose of the vehicle and the pose of the target point, and calculating the state matrix according to the deviation between the current pose of the vehicle and the pose of the target point; Determine the first model parameter matrix and the second model parameter matrix of the LQR algorithm China based on the scientific model, and select the first weight matrix and the second weight matrix of the LQR algorithm; then according to the LQR algorithm, the first model parameter matrix and the second model parameter matrix The optimal matrix is obtained by processing; the steering control amount can be obtained by calculating based on the optimal matrix and the state matrix; by controlling the steering actuator of the vehicle to execute the steering control amount, the lateral control of the vehicle can be realized, thus ensuring The tracking stability and comfort of the vehicle on complex roads with large curvature changes and fast-changing speeds have improved the control accuracy and adaptability of the LQR controller.

Referring to FIG. 8 , an embodiment of the present application also provides a lateral control method for an automatic driving vehicle, which can be applied to automatic driving vehicles, including fully automatic driving vehicles and semi-autonomous driving vehicles. Taking an autonomous vehicle as an example, the lateral control method will be described below. The lateral control method includes steps S801-S808:

S801. Judging the type of the current road and determining the target point;

The self-driving car will plan the driving route in advance or in real time, and then drive on the current road according to the planned driving route. The driving route is the tracking trajectory. When the self-driving car is required to perform lateral control, the target point will be determined according to the type of the current road. Wherein, the target point is the track point corresponding to the vehicle on the tracking track at the current moment.

In the embodiment of the present application, step S801 may further include:

When the type of the current road is a straight road type, the track point closest to the vehicle on the tracking track is determined as the target point.

In some embodiments, the track point on the tracking track closest to the vehicle may be the point closest to the vehicle in the vertical direction, or the point closest to the vehicle in the lateral direction (ie, in the width direction of the vehicle beam).

When the type of the current road is a curve type, the track point on the tracking track that is separated from the vehicle by the aiming distance is determined as the target point, and the aiming distance is related to the current speed of the vehicle and the curvature of the current road.

Wherein, when the current road type is a curve type, the method for determining the target point may further include:

S8011. Obtain the current speed of the vehicle;

S8012. Determine the aiming distance according to the current vehicle speed, wherein,

When the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

When the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle, m; a is the comfort Deceleration, m/s ² ;

S8013. Aim along the traveling direction of the vehicle, and determine a track point on the tracking track that is at an aiming distance from the vehicle as a target point.

After the target point is determined, step S802 is executed.

S802. Obtain the current pose of the vehicle and the pose of the target point.

The current pose of the vehicle includes the actual coordinates and the current heading angle of the vehicle at the current moment, and the pose of the target point includes the target coordinates and the target heading angle of the target point. The manner of acquiring the current pose of the vehicle and the pose of the target point can be any acquisition manner in the related art, and to avoid redundancy, details are not repeated here.

S803. Calculate a state matrix according to the deviation between the current pose and the pose of the target point.

Among them, the deviation between the current pose and the pose of the target point includes at least the distance deviation between the actual coordinates of the vehicle and the target coordinates, and the heading angle deviation between the current heading angle and the target heading angle, and may also include the distance The rate of change of deviation and the rate of change of angular deviation of the heading angle.

In some embodiments of the present application, step S803 may further include:

Step S8031, calculating the distance deviation e1, the distance deviation change rate e2, the heading angle deviation e3 and the angle deviation change rate e4 between the vehicle and the target point.

As shown in Figure 2, there is a target point on the tracking track, the distance deviation between the vehicle and the target point (generally refers to the distance deviation between the center of mass of the vehicle and the target point) is e1, the current heading angle of the vehicle is ψ, and the target point The target heading angle of the point is ψ _des , and the heading angle deviation e3=ψ _des -ψ.

Step S8032, taking the distance deviation e1, the distance deviation change rate e2, the heading angle deviation e3 and the angle deviation change rate e4 as elements in the state matrix to obtain the state matrix.

Therefore, the state matrix obtained according to the above elements state(1,0)=e1, state(2,0)=e2, state(3,0)=e3, state (4,0)=e4, namely

Step S804, determining the vehicle dynamics model.

Among them, the vehicle dynamics model can be based on the front wheel cornering stiffness Cf of the vehicle, the rear wheel cornering stiffness Cr, the distance lf from the front axle to the center of gravity of the vehicle, the distance lr from the rear axle to the center of gravity of the vehicle, the z-axis moment of inertia Iz and The vehicle mass m and other parameters are determined. These parameters can be obtained by querying the basic information of the vehicle, or can be obtained by measurement, and specifically can be processed by those skilled in the art according to actual conditions, and are not specifically limited here.

Step S805: Determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix.

In the embodiment of the present application, the calculation formulas of the first model parameter matrix matrix_a_ and the second model parameter matrix matrix_b_ determined according to the vehicle dynamics model may be as follows:

Among them, Cf is the cornering stiffness of the front wheel, Cr is the cornering stiffness of the rear wheel, lf is the distance from the front axle to the center of gravity of the vehicle, lr is the distance from the rear axle to the center of gravity of the vehicle, Iz is the moment of inertia of the z-axis of the vehicle, and m is the integer car quality.

Before using the LQR algorithm for calculation, it is also necessary to select the first weighting matrix (usually called Q matrix) and the second weighting matrix (usually called R matrix), where the first weighting matrix is related to the vehicle information of the vehicle and the current road road information.

In some embodiments of the present application, the first weighting matrix may choose a diagonal matrix, namely

matrix_q_=diag[q1 q2 q3 q4];

The first element q1, the second element q2, the third element q3 and the fourth element q4 located on the main diagonal of the diagonal matrix are respectively related to the distance deviation e1, the distance deviation change rate e2, and the heading angle deviation in the state matrix e3 corresponds to the angular deviation change rate e4.

In the embodiment of the present application, the selection of the first element q1 and the third element q3 is the key to the LQR control algorithm. Wherein, the first element q1 in the first weighting matrix is the deviation coefficient of the vehicle, and the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, which is used to adjust the state weight of the distance deviation e1 in the state matrix. The deviation coefficient of the vehicle can be determined based on the individual straight-line tracking of the real vehicle. For example, before the self-driving car leaves the factory, technicians test the deviation generated when the vehicle drives along the straight-line tracking track for many times, and collect the front wheel deflection data in real time during the test. and the distance deviation data, after processing the collected data, the ratio of the front wheel deflection angle to the distance deviation, namely the deviation coefficient, can be stored in the vehicle in advance. The third element q3 in the first weighting matrix is determined according to vehicle information and road information, and both the second element q2 and the fourth element q4 are 0. In some embodiments of the present application, the above-mentioned "distance deviation" can generally be regarded as a lateral distance deviation, that is, a distance deviation generated in a direction parallel to the width of the vehicle.

Therefore, the first weighting matrix matrix_q_ can be:

In some embodiments of the present application, the vehicle information may include the current speed, and the road information may include the curvature radius of the current road. The method of determining the third element q3 according to vehicle information and road information may be to compare the curvature radius of the current road with the first curvature radius threshold and the second curvature radius threshold, wherein the first curvature radius threshold is used to distinguish straight roads from curved roads The boundary condition of the road, the second curvature radius threshold is the boundary condition used to distinguish small curvature curves from large curvature curves. One of three possible outcomes can occur after the comparison:

(1) When the curvature radius of the current road is smaller than the first curvature radius threshold, it is determined that the tracking track is a straight road, and the third element q ₃ is calculated according to the following formula:

(2) When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the tracking track is a small curvature curve, and the third element q3 is calculated according to the following formula:

(3) When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the tracking trajectory is a large curvature curve, and the third element q3 is calculated according to the following formula:

In the above formula, q1 is the first element; V is the current speed of the vehicle, m/s.

Based on the above method, the first weighting matrix can be obtained.

In some embodiments of the present application, the second weighting matrix may choose an identity matrix, namely

matrix_r_=[1];

Wherein, matrix_r_ is the second weighting matrix.

S806. Determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator.

After selecting the first weighting matrix and the second weighting matrix, the first model parameter matrix and the second model parameter matrix are calculated according to the LQR algorithm of the linear quadratic regulator to obtain the optimal matrix matrix_k, which may specifically include:

According to the following formula, the optimal matrix matrix_k is determined by the numerical iterative solution of the Riccati equation:

while(num_iteration++<max_num_iteration)

{

matrix_p_next = matrix_a_T*matrix_p_*matrix_a_-(matrix_a_T*matrix_p_*matrix_b_)*(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)+matrix_q_;

matrix_p_=matrix_p_next;

}

matrix_k＝(matrix_r_+matrix_b_T*matrix_p_*matrix_b_).inverse()*(matrix_b_T*matrix_p_*matrix_a_)

Wherein, max_num_iterationa is the maximum number of iterations, optionally, max_num_iterationa can be 150; matrix_a_T and matrix_b_T are the transpose matrices of matrix_a_ and matrix_b_ respectively; matrix_p_ is the process iteration matrix, the initial value is the first weighting matrix.

Based on the above iterative operation, the optimal matrix matrix_k can be obtained.

Step S807, calculating the steering control amount based on the state matrix and the optimal matrix.

In some embodiments of the present application, after obtaining the state matrix and the optimum matrix, the steering control amount can be obtained by multiplying the state matrix and the optimum matrix.

Step S808, controlling the steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.

It should be noted that the steering control amount obtained in step S808 is the steering control amount of the front wheels, and if it is necessary to calculate the steering wheel steering control amount, it needs to be further calculated on this basis. For example, the steering control amount of the steering wheel can be obtained by multiplying the steering control amount of the front wheels by the ratio value. Among them, ratio is the ratio of the steering wheel angle of the vehicle to the front wheel angle, and the ratio can be obtained by consulting the basic information of the vehicle, or by testing the actual vehicle, and can also be pre-stored in the vehicle.

Exemplarily, after adding the disturbance of the initial distance deviation of 0.5m (simulating a sudden change in the environment), the simulation comparison and verification results of the vehicle traveling on the same curve at different speeds according to the tracking trajectory are shown in Figure 4 and Figure 5, where Figure 4 is Schematic diagram of tracking error before optimization, line 1 in Figure 4 is the case of V=20km/h, line 2 is the case of V=30km/h, line 3 is the case of V=40km/h, line 4 is the case of V=50km In the case of /h, line 5 is the case of V=60km/h; Fig. 5 is a schematic diagram of tracking error after optimization, line 6 in Fig. 5 is the case of V=20km/h, and line 7 is the case of V=30km/h situation, line 8 is the situation of V=40km/h, line 9 is the situation of V=50km/h, and line 10 is the situation of V=60km/h. Obviously, when the optimized vehicle travels according to the tracking track, the The error fluctuation generated after being disturbed is smaller, so it can return to a stable state faster, improving the comfort and stability of the vehicle.

Therefore, by using the lateral control method proposed in the embodiment of the present application to optimize the lateral control performance of the vehicle, the stability and comfort of the vehicle's trajectory tracking in the case of rapid speed changes and complex roads with large curvature changes are guaranteed. Simultaneously, the embodiment of the application has summed up the self-adaptive formula to solve the problem that the Q matrix and the R matrix in the LQR algorithm cause larger errors when selecting, and it can be seen from the results of the simulation comparison verification test that when using this After applying the adaptive formula provided by the embodiment, the tracking effect of the vehicle is significantly improved.

Therefore, according to the lateral control method of the vehicle proposed in the embodiment of the present application, the actual coordinates and current heading angle of the vehicle can be obtained, the current pose of the vehicle can be obtained, and the pose of the target point can be obtained; then according to the current pose of the vehicle and the target point Point pose, calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment, and obtain the state matrix; use the vehicle dynamics model to determine the first model parameter matrix in the LQR algorithm and The second model parameter matrix, and select the first weighting matrix and the second weighting matrix in the LQR algorithm to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and finally control the steering actuator of the vehicle to perform the optimal matrix and The steering control amount obtained by multiplying the state matrix can ensure the stability and comfort of the vehicle tracking on complex roads with large curvature changes and fast-changing driving scenes, and realize the control accuracy and adaptability of the LQR controller. improve.

In the following, a lateral control device for an autonomous vehicle proposed according to an embodiment of the present application will be described with reference to the accompanying drawings.

Fig. 6 is a schematic block diagram of a lateral control device for an automatic driving vehicle provided by an embodiment of the present application.

As shown in FIG. 6 , the lateral control device 10 of the self-driving vehicle includes: an acquisition module 100 , a calculation module 200 and a control module 300 .

Wherein, the acquiring module 100 is used to acquire the actual coordinates and the current heading angle of the vehicle, and obtain the position information of the current pose and the target point;

The calculation module 200 is used to calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the current moment and the target point according to the current pose and position information, and calculate the state matrix; and

The control module 300 is used to determine the first model parameter matrix and the second model parameter matrix by using the vehicle dynamics model, and select the first weight matrix and the second weight matrix at the same time, so as to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator , and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix.

Optionally, in some embodiments, the above-mentioned lateral control device 10 of the self-driving vehicle further includes:

The determining module is used to determine vehicle dynamics according to the front wheel cornering stiffness, rear wheel cornering stiffness, distance from the front axle to the center of gravity of the vehicle, distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle Model.

Optionally, in some embodiments, the calculation module 200 includes:

a judging unit, configured to judge the type of the current road;

The first determining unit is used to determine the nearest point on the track from the current position of the vehicle (ie, the vehicle coordinates) as the target point when it is judged that the type of the current road is a straight road type;

The second determination unit is used to determine the track point that is apart from the current position of the vehicle as the target point if the actual vehicle speed of the vehicle is greater than the preset threshold when it is determined that the type of the current road is a curve type, otherwise the A track point that is at a distance determined by the curvature of the road from the current position of the vehicle is determined as a target point.

Optionally, in some embodiments, the calculation formula for determining the distance from the curvature of the road is:

L=kV+lmin,

Among them, k is the linear change ratio of the speed, V is the actual speed of the vehicle, and lmin is the minimum setting value of the preview distance.

Optionally, in some embodiments, the calculation formula of the state matrix is:

Among them, e1 is the distance deviation, e2 is the change rate of the distance deviation, e3 is the heading angle deviation, and e4 is the change rate of the angle deviation.

It should be noted that the foregoing explanations of the embodiment of the lateral control method for an automatic driving vehicle are also applicable to the lateral control device for an automatic driving vehicle of this embodiment, and details will not be repeated here.

According to the lateral control device of the self-driving vehicle proposed in the embodiment of the present application, the actual coordinates and current heading angle of the vehicle can be obtained, the current pose and the position information of the target point can be obtained, and the current moment and target can be calculated according to the current pose and position information. Point distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate, calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and simultaneously select the first weighting matrix and The second weighting matrix is to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix, so as to ensure that the curvature and speed change quickly The stability and comfort of vehicle tracking on complex roads can be achieved, and the control accuracy and adaptability of the LQR controller can be improved.

Fig. 9 is a schematic diagram of another vehicle lateral control device provided by an embodiment of the present application, which can be applied to autonomous driving vehicles, including fully automatic driving vehicles and semi-autonomous driving vehicles. As shown in Figure 9, the lateral control device 90 includes:

The acquisition module 901 is configured to acquire the current pose of the vehicle and the pose of a target point, wherein the target point is the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track.

The calculation module 902 is configured to calculate the state matrix according to the deviation between the current pose and the pose of the target point.

The determining module 903 is configured to determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm , wherein the first weighting matrix is related to the vehicle information of the vehicle and the road information of the current road.

Wherein, the vehicle information may include the current speed, and the road information may include the curvature radius of the current road.

The calculation module 902 is further configured to determine the optimal matrix according to the LQR algorithm, and obtain the steering control amount based on the state matrix and the optimal matrix.

The control module 904 is configured to control the steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.

In some embodiments of the present application, the computing module 902 is also configured to:

Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point; and use the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate as elements in the state matrix.

In some embodiments of the present application, the first weighting matrix is a diagonal matrix, and the diagonal matrix includes a first element, a second element, a third element, and a fourth element located on the main diagonal, and the first element, the The second element, the third element and the fourth element respectively correspond to the distance deviation, the range deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix. Among them, the first element is the deviation coefficient of the vehicle, and the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, the third element is determined according to the vehicle information and road information, and the second element and the fourth element are both 0.

The determining module 903 is further configured to determine a third element according to vehicle information and road information, wherein,

When the curvature radius of the current road is less than the first curvature radius threshold, it is determined that the tracking trajectory is a straight road, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the tracking track is a small curvature curve, and the third element is calculated according to the following formula:

When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the tracking track is a large curvature curve, and the third element is calculated according to the following formula:

Among them, q3 is the third element; q1 is the first element; V is the current speed of the vehicle, m/s; the first curvature radius threshold is the boundary condition for distinguishing straight roads and curves, and the second curvature radius threshold is for Boundary conditions that differentiate between small curvature curves and large curvature curves.

In some embodiments of the present application, the calculation module 902 is further configured to multiply the state matrix and the optimal matrix to obtain the steering control amount.

In some embodiments of the present application, the lateral control device 90 may also include:

Judging module 905, configured to judge the type of the current road;

When the type of the current road is a straight road type, the track point closest to the vehicle on the tracking track is determined as the target point;

When the type of the current road is a curve type, the track point on the tracking track that is separated from the vehicle by the aiming distance is determined as the target point, and the aiming distance is related to the current speed of the vehicle and the curvature of the current road.

In some embodiments of the present application, the obtaining module is also used to obtain the current speed of the vehicle;

Determine the aiming distance according to the current vehicle speed, where,

If the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle, m;

If the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

Among them, a is the comfortable deceleration, m/s ² ;

Aiming is carried out along the driving direction of the vehicle, and the track point on the tracking track which is far from the vehicle's aiming distance is determined as the target point.

In some embodiments of the present application, the determination module 903 is further configured to, according to the vehicle's front wheel cornering stiffness, rear wheel cornering stiffness, distance from the front axle to the center of gravity of the vehicle, distance from the rear axle to the center of gravity of the vehicle, z The moment of inertia of the shaft and the mass of the whole vehicle determine the vehicle dynamics model.

In addition, an embodiment of the present application also provides a vehicle, which includes the vehicle lateral control device mentioned in any of the above embodiments.

The embodiment of the present application also provides another vehicle, the vehicle includes a controller, and the controller is configured to implement the vehicle lateral control method described in any one of the above embodiments. By implementing the above lateral control method, the tracking stability and comfort of the vehicle on complex roads with large curvature changes and fast speed changes are guaranteed, and the control accuracy and adaptability of the LQR controller are improved.

According to the vehicle proposed in the embodiment of the present application, the vehicle's actual coordinates and current heading angle can be obtained through the above-mentioned lateral control device of the vehicle, and the current pose and the pose of the target point can be obtained, and according to the current pose and the pose of the target point Calculate the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point at the current moment, calculate the state matrix, and use the vehicle dynamics model to determine the first model parameter matrix and the second model parameter matrix, and Select the first weighting matrix and the second weighting matrix to determine the optimal matrix according to the LQR algorithm of the linear quadratic regulator, and then control the steering actuator of the vehicle to execute the steering control amount obtained by multiplying the optimal matrix and the state matrix, so that Ensure the stability and comfort of vehicle tracking on complex roads with large curvature changes and fast-changing driving scenarios, and improve the control accuracy and adaptability of the LQR controller.

In the description of this specification, descriptions referring to the terms "one embodiment", "some embodiments", "example", "specific examples", or "some examples" mean that specific features described in connection with the embodiment or example , structure, material or characteristic is included in at least one embodiment or example of the present application. In this specification, the schematic representations of the above terms are not necessarily directed to the same embodiment or example. Moreover, the described specific features, structures, materials or characteristics may be combined in any one or N embodiments or examples in an appropriate manner. In addition, those skilled in the art can combine and combine different embodiments or examples and features of different embodiments or examples described in this specification without conflicting with each other.

In addition, the terms "first" and "second" are used for descriptive purposes only, and cannot be interpreted as indicating or implying relative importance or implicitly specifying the quantity of indicated technical features. Thus, the features defined as "first" and "second" may explicitly or implicitly include at least one of these features. In the description of the present application, "N" means at least two, such as two, three, etc., unless otherwise specifically defined.

Any process or method description in a flowchart or otherwise described herein may be understood to represent a module, segment or portion of code comprising one or more executable instructions for implementing a custom logical function or step of a process , and the scope of preferred embodiments of the present application includes additional implementations in which functions may be performed out of the order shown or discussed, including in substantially simultaneous fashion or in reverse order depending on the functions involved, which shall It should be understood by those skilled in the art to which the embodiments of the present application belong.

It should be understood that each part of the present application may be realized by hardware, software, firmware or a combination thereof. In the above embodiments, the N steps or methods may be implemented by software or firmware stored in memory and executed by a suitable instruction execution system. For example, if implemented in hardware as in another embodiment, it can be implemented by any one or a combination of the following techniques known in the art: a discrete Logic circuits, ASICs with suitable combinational logic gates, Programmable Gate Arrays (PGA), Field Programmable Gate Arrays (FPGA), etc.

Those of ordinary skill in the art can understand that all or part of the steps carried by the methods of the above embodiments can be completed by instructing related hardware through a program, and the program can be stored in a computer-readable storage medium. During execution, one or a combination of the steps of the method embodiments is included.

Claims (18)

1. A lateral control method for a vehicle, comprising the following steps:

   Obtaining the current pose of the vehicle and the pose of a target point, the target point being a track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track;

   calculating a state matrix according to the deviation between the current pose and the pose of the target point;

   Determine the first model parameter matrix and the second model parameter matrix in the linear quadratic regulator LQR algorithm according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm, wherein the first a weighting matrix related to the vehicle information of the vehicle and the road information of the current road;

   Determine the optimal matrix according to the LQR algorithm;

   A steering control amount is calculated based on the state matrix and the optimal matrix, and a steering actuator of the vehicle is controlled to execute the steering control amount to perform lateral control on the vehicle.
2. The method according to claim 1, said calculating a state matrix according to the deviation between the current pose and the pose of the target point, comprising:

   calculating the distance deviation, distance deviation change rate, heading angle deviation and angle deviation change rate between the vehicle and the target point;

   Taking the distance deviation, the rate of change of the distance deviation, the heading angle deviation and the rate of change of the angle deviation as elements in the state matrix to obtain the state matrix.
3. The method according to claim 2, the first weighting matrix is a diagonal matrix, and the diagonal matrix includes a first element, a second element, a third element and a fourth element located on the main diagonal, so The first element, the second element, the third element and the fourth element are respectively corresponding to the distance deviation, the distance deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;

   Wherein, the first element is the deviation coefficient of the vehicle, the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, the third element is determined according to the vehicle information and the road information, and the Both the second element and the fourth element are 0.
4. The method according to claim 3, wherein the vehicle information includes the current speed, and the road information includes the curvature radius of the current road;

   Determining the third element according to the vehicle information and the road information includes:

   When the curvature radius of the current road is less than the first curvature radius threshold, it is determined that the current road is a straight road, and the third element is calculated according to the following formula:

   

   When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the current road is a small curvature curve, and the third element is calculated according to the following formula:

   

   When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the current road is a large curvature curve, and the third element is calculated according to the following formula:

   

   Wherein, q3 is the third element; q1 is the first element; V is the current speed of the vehicle, m/s;

   The first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve, and the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.
5. The method according to claim 1, said calculating steering control amount based on said state matrix and said optimal matrix, comprising:

   Multiplying the state matrix and the optimal matrix to obtain the steering control amount.
6. According to the method according to claim 1, before obtaining the pose of the target point, the method also includes:

   determine the type of the current road;

   When the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;

   When the type is a curve type, determine a track point on the tracking track that is apart from the target distance from the vehicle as the target point, and the target distance is related to the current speed of the vehicle and the current road speed. related to the curvature.
7. According to the method according to claim 6, said determining a track point on said tracking track which is apart from said vehicle at an aiming distance as said target point comprises:

   obtaining the current speed of the vehicle;

   The aiming distance is determined according to the current vehicle speed, wherein,

   When the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

   

   When the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

   

   Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle , m; a is the comfortable deceleration, m/s ² ;

   Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.
8. The method according to claim 1, before the first model parameter matrix and the second model parameter matrix in the linear quadratic regulator LQR algorithm are determined according to the vehicle dynamics model, the method also includes:

   Determine the vehicle dynamics model according to the front wheel cornering stiffness of the vehicle, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the moment of inertia of the z-axis of the vehicle, and the mass of the vehicle .
9. A lateral control device for a vehicle, comprising:

   An acquisition module, configured to acquire the current pose of the vehicle and the pose of a target point, the target point being the track point corresponding to the vehicle on the tracking track at the current moment, and the vehicle is traveling on the current road according to the tracking track ;

   A calculation module, configured to calculate a state matrix according to the deviation between the current pose and the pose of the target point;

   A determining module, configured to determine the first model parameter matrix and the second model parameter matrix in the LQR algorithm of the linear quadratic regulator according to the vehicle dynamics model, and select the first weighting matrix and the second weighting matrix in the LQR algorithm , wherein the first weighting matrix is related to vehicle information of the vehicle and road information of the current road;

   The calculation module is also used to determine the optimal matrix according to the LQR algorithm, and calculate the steering control amount based on the state matrix and the optimal matrix;

   A control module, configured to control a steering actuator of the vehicle to execute the steering control amount, so as to perform lateral control on the vehicle.
10. The device according to claim 9, the computing module is also used for:

    calculating a distance deviation, a rate of change of distance deviation, a heading angle deviation and a rate of change of angle deviation between the vehicle and the target point; and,

    Taking the distance deviation, the rate of change of the distance deviation, the heading angle deviation and the rate of change of the angle deviation as elements in the state matrix to obtain the state matrix.
11. The device according to claim 10, the first weighting matrix is a diagonal matrix, and the diagonal matrix includes a first element, a second element, a third element and a fourth element located on the main diagonal, so The first element, the second element, the third element and the fourth element are respectively corresponding to the distance deviation, the distance deviation change rate, the heading angle deviation and the angle deviation change rate in the state matrix;

    Wherein, the first element is the deviation coefficient of the vehicle, the deviation coefficient is the ratio of the front wheel slip angle to the distance deviation, the third element is determined according to the vehicle information and the road information, and the Both the second element and the fourth element are 0.
12. The device according to claim 11, wherein the vehicle information includes a current speed, and the road information includes a curvature radius of the current road;

    The calculation module is further configured to determine a third element according to vehicle information and road information, wherein,

    When the curvature radius of the current road is less than the first curvature radius threshold, it is determined that the current road is a straight road, and the third element is calculated according to the following formula:

    

    When the curvature radius of the current road is greater than the first curvature radius threshold and less than the second curvature radius threshold, it is determined that the current road is a small curvature curve, and the third element is calculated according to the following formula:

    

    When the curvature radius of the current road is greater than the second curvature radius threshold, it is determined that the current road is a large curvature curve, and the third element is calculated according to the following formula:

    

    Wherein, q3 is the third element; q1 is the first element; V is the current speed of the vehicle, m/s;

    The first radius of curvature threshold is a boundary condition for distinguishing a straight road from a curve, and the second radius of curvature threshold is a boundary condition for distinguishing a curve with a small curvature from a curve with a large curvature.
13. According to the device according to claim 9, the calculation module is further configured to multiply the state matrix and the optimal matrix to obtain the steering control amount.
14. The device according to claim 9, further comprising a judging module, the judging module being used for:

    determine the type of the current road;

    When the type is a straight track type, determining the track point closest to the vehicle on the tracking track as the target point;

    When the type is a curve type, determine a track point on the tracking track that is apart from the target distance from the vehicle as the target point, and the target distance is related to the current speed of the vehicle and the current road speed. related to the curvature.
15. The device according to claim 14, the acquiring module is further configured to acquire the current speed of the vehicle;

    The judgment module is also used for:

    The aiming distance is determined according to the current vehicle speed, wherein,

    When the current vehicle speed is less than or equal to the speed threshold, the aiming distance is calculated according to the following formula:

    

    When the current vehicle speed is greater than the speed threshold, the aiming distance is calculated according to the following formula:

    

    Among them, L is the aiming distance, m; V is the current vehicle speed, m/s; α is the adjustment coefficient; kp is the curvature of the track point closest to the vehicle on the tracking track; r is the minimum turning radius of the vehicle , m; a is the comfortable deceleration, m/s ² ;

    Aiming is performed along the traveling direction of the vehicle, and a track point on the tracking track that is apart from the vehicle by the aiming distance is determined as the target point.
16. According to the device according to claim 9, the determination module is further configured to, according to the front wheel cornering stiffness, the rear wheel cornering stiffness, the distance from the front axle to the center of gravity of the vehicle, the distance from the rear axle to the center of gravity of the vehicle, the vehicle The z-axis moment of inertia and vehicle mass determine the vehicle dynamics model.
17. A vehicle, comprising a controller for executing the vehicle lateral control method according to any one of claims 1-8.
18. A vehicle, comprising the vehicle lateral control device according to any one of claims 9-16.

Images