CN110598311B - A Trajectory Tracking Method for Autonomous Driving Vehicles - Google Patents

Abstract

The invention relates to a track tracking method, in particular to a track tracking method for an automatic driving vehicle; in particular, it is disclosed to add a virtual representation of the front wheel inclination δ actually acting on an autonomous vehicle to the cost function of a predictive model _true The model is used as a cost-effective amount for model prediction control, so that the wheel slip of the vehicle is prevented and corrected, and the track tracking effect is improved; since the wheel slip phenomenon mainly occurs when the model of the vehicle deviates from the ideal model, that is, the wheel inclination deviates from the driving track of the vehicle, the virtual front wheel inclination δ is determined _true As a cost-effective amount, the problem of wheel slip is effectively solved.

Description

A Trajectory Tracking Method for Autonomous Driving Vehicles

technical field

The invention relates to a trajectory tracking method, in particular to a trajectory tracking method for an automatic driving vehicle.

Background technique

The key technologies of autonomous driving can be divided into environmental perception, behavioral decision-making, path planning and motion control. There are two basic design methods for motion control, one is based on driver simulation, and the other is based on dynamic construction. mode control method. Among the control methods based on dynamic modeling, the vehicle control method that has been widely studied and applied is model predictive control, which can optimize the indicators of punctuality, comfort and energy saving to obtain the optimal control; in addition, compared with The traditional serial loop PID control, preview tracking control, linear model predictive control and linear quadratic LQR control have high calculation accuracy, strong robustness, small overshoot and good tracking effect.

However, most of the current model-based predictive control only relies on its robustness. During the control process, if the autonomous vehicle runs out or spins and deviates from the reference model (that is, there is a side slip), it needs to be The behavioral decision of the autonomous vehicle can reduce the problem of offsetting the set trajectory, but it cannot compensate for the deviation of the model well, so that the phenomenon that the autonomous vehicle is deviating from the reference model cannot be avoided, such as driving out or spinning.

SUMMARY OF THE INVENTION

The purpose of the present invention is to overcome the deficiencies of the prior art, and to provide a method for tracking the trajectory of an automatic driving vehicle. The original model predictive control is optimized, and the model predictive control itself can effectively control the autonomous vehicle to approach the accurate model as much as possible to prevent slippage.

The object of the present invention is achieved through the following technical solutions:

A method for tracking the trajectory of an autonomous vehicle, comprising the following steps:

S1: Establish a ground coordinate system and an airframe coordinate system to model the vehicle dynamics; the vehicle dynamics model is as follows:

Among them, x and y are the x-axis and y-axis coordinates of the center of mass of the autonomous vehicle in the ground coordinate system, respectively, ψ is the heading angle of the autonomous vehicle in the ground coordinate system, v is the forward speed of the autonomous vehicle, and a is the autonomous vehicle. The forward acceleration of , t represents the amount at the current moment, t+1 represents the amount at the next moment, L _f is the wheelbase between the front wheel and the rear wheel; δ _true is a virtual amount, indicating the current real front wheel inclination, which is the vehicle The front wheel inclination angle corresponding to the assumption that the front wheel does not slip under the current driving direction;

S2: Obtain the body's own state vector X _t and the last output vector U _t-1 ; where,

X _t = [x, y, ψ, v] ^T (2)

U _t-1 =[a _t-1 , δ _t-1 ] (3)

δ is the inclination angle of the front wheels of the autonomous vehicle; when it is necessary to calculate the body state vector X _t+1 at the next moment, the X _t at the moment is substituted into the formula (1), and the X _t at the next moment can be calculated. ₊₁ related parameters x _t+1 , y _t+1 , ψ _t+1 and v _t+1 ;

S3: Obtain a virtual quantity, which is the current real front wheel inclination δ _true ;

S4: Obtain the discrete waypoint path P by sampling, and perform interpolation calculation on the waypoint path P, and obtain _xref , _yref , _ψref , _vref with the length l respectively, that is, obtain the continuous trajectory f(P); in,

Substitute into the following formula to obtain a discrete waypoint path P' by setting the reference speed v, where P' contains the states of N points;

P'=[X ₁ , X ₂ , ..., XN] ^T (5)

X _n =[f ₁ (l _n ) f ₂ (l _n ) f ₃ (l _n ) f ₄ (l _n )] (6)

l _n =(n+1)×v×dt (7)

Among them, n is the label of the point taken;

S5: According to the own state X _t and the waypoint path P', the acceleration at the current moment and the front wheel inclination angle δ _t at the current moment are obtained by calculating the minimum value of the model prediction cost function _J , so as to obtain the optimal output vector as the current output vector U _t ;

Among them, the calculation formula of the model prediction cost function J is as follows:

Among them, N is the model predictive control calculation step size, w represents the weight of each indicator, and the weight of each indicator can be assigned according to the actual situation;

S6: The output vector U _t acts on the autonomous vehicle, and repeats S2 to S6.

In a preferred solution of the present invention, in S3, the method for obtaining the current true front wheel inclination angle δ _true is as follows:

First, through the geometric relationship, we can get:

Therefore:

where R is the turning radius when the front wheel inclination is δ _true , L _f is the wheelbase between the front and rear wheels, dψ is the heading angle difference between the two time intervals, dx is the x coordinate difference between the two time intervals, and dy Represents the y coordinate difference between two time intervals, dt represents the time interval, and ω is the angular velocity of the vehicle.

有关；即可得出：Preferably, a large amount of δ _true , X _t and U _t data are obtained by formulas (9) and (10), and an approximate function g _is obtained by multivariate least squares polynomial fitting; x, y, ψ have nothing to do with the output vector U _t , velocity v _t , acceleration a _t and front wheel inclination δ _t at this moment, as well as the output vector U _t-1 , velocity v _t-1 , Acceleration a _t-1 and true front wheel inclination

related; it follows that:

At the same time, the calculation can be simplified to get:

Finally, the function g is obtained by multivariate least squares polynomial fitting.

In a preferred solution of the present invention, in S2, the current state vector X _t of the body itself is obtained through sensor filtering.

In a preferred solution of the present invention, in S4, the waypoint path P is obtained through the path planner.

Compared with the prior art, the present invention has the following beneficial effects:

1. When there is a certain deviation between the model of the vehicle and the ideal model, the phenomenon of wheel slippage mainly occurs, that is, there is a deviation between the wheel inclination and the driving trajectory of the vehicle; at this time, the virtual is used to represent the front wheel that really acts on the autonomous vehicle. The inclination angle δ _true is used as a cost of model predictive control, so as to improve the coincidence of the actual running vehicle model and the preset ideal model, which is beneficial to prevent and correct the wheel slip of the vehicle and improve the trajectory tracking effect.

2. The present invention only adds one more cost value δ _true in the original model predictive control, which has little influence on the calculation amount of the model and has a good effect.

Description of drawings

FIG. 1 is a schematic diagram of the vehicle trajectory processing of the automatic driving vehicle trajectory tracking method of the present invention.

FIG. 2 is a schematic diagram of a vehicle model of the method for tracking the trajectory of an autonomous vehicle according to the present invention.

FIG. 3 is a schematic diagram of a vehicle trajectory tracking system of the method for tracking a trajectory of an autonomous vehicle according to the present invention.

FIG. 4 is a schematic diagram of δ _t and δ _true in the automatic driving vehicle trajectory tracking method of the present invention.

Detailed ways

The present invention will be further described below with reference to the embodiments and accompanying drawings, but the embodiments of the present invention are not limited thereto.

Referring to FIG. 1 to FIG. 4 , the method for tracking the trajectory of an autonomous driving vehicle in this embodiment includes the following steps:

S1: Establish the ground coordinate system (xoy) 1 and the body coordinate system (x'o'y') 2 to model the vehicle dynamics; the vehicle dynamics model is as follows:

Among them, x and y are the x-axis and y-axis coordinates of the center of mass of the autonomous vehicle in the ground coordinate system 1, respectively, ψ is the heading angle of the autonomous vehicle in the ground coordinate system, v is the forward speed of the autonomous vehicle, and a is the autonomous driving. The forward acceleration of the vehicle, t is the amount at the current moment, t+1 is the amount at the next moment, L _f is the wheelbase between the front and rear wheels; δ _true is a virtual amount, indicating the current real front wheel inclination, is In the current driving direction of the vehicle, it is assumed that the corresponding front wheel inclination angle of the front wheel is not slipping.

S2: Obtain the state vector X _t of the body itself through sensor filtering (or filter), and obtain the last output vector U _t-1 through the recorded information of the system; wherein,

X _t = [x, y, ψ, v] ^T (2)

U _t-1 =[a _t-1 , δ _t-1 ] (3)

δ is the inclination angle of the front wheels of the autonomous vehicle; when it is necessary to calculate the body state vector X _t+1 at the next moment, the X _t at the moment is substituted into the formula (1), and the X _t at the next moment can be calculated. ₊₁ related parameters x _t+1 , y _t+1 , ψ _t+1 and v _t+1 .

S3: Obtain a virtual quantity through the observer, which is the current real front wheel inclination δ _true ; the specific model of the observer is as follows:

First, through the geometric relationship, we can get:

Therefore:

where R is the turning radius when the front wheel inclination is δ _true , L _f is the wheelbase between the front and rear wheels, dψ is the heading angle difference between the two time intervals, dx is the x coordinate difference between the two time intervals, and dy Represents the y coordinate difference between two time intervals, dt represents the time interval, and ω is the angular velocity of the vehicle.

有关；即可得出：

In addition, a large number of δ _true , X _t and U _t data are obtained through formulas (9) and (10), and an approximate function g is obtained by multivariate least squares polynomial fitting; where the function g and x in X _t , y, ψ have nothing to do with the output vector U _t , velocity v _t , acceleration at _t and front wheel inclination δ _t at this moment, as well as the output vector U _t-1 , velocity v _t-1 , acceleration at the previous moment a _t-1 and true front wheel inclination

related; it follows that:

At the same time, the calculation can be simplified to get:

Finally, the function g is obtained by multivariate least squares polynomial fitting. In this way, the current real front wheel inclination angle δ _true at each moment can be obtained directly through the function g, which simplifies the amount of calculation and is beneficial to improving the calculation efficiency.

S4: Referring to Figure 2, the discrete waypoint path P3 is obtained by sampling the path planner, and the waypoint path P3 is interpolated to obtain x _ref , y _ref , ψ _ref , v _ref with the length l respectively, that is, to obtain continuous trajectory f(P)4; where,

Substitute into the following formula to obtain a discrete waypoint path P'5 by setting the reference speed v, where P'5 contains the states of N points;

P'=[X ₁ , X ₂ , . . . , X _N ] ^T (5)

X _n =[f ₁ (l _n ) f ₂ (l _n ) f ₃ (l _n ) f ₄ (l _n )] (6)

l _n =(n+1)×v×dt (7)

Among them, n is the label of the point to be taken.

S5: According to the own state X _t and the waypoint path P'5, the acceleration at the current moment and the front wheel inclination angle δ _t at the current moment are obtained by calculating the minimum value of the model prediction cost function _J , so as to obtain the optimal output vector as the current output vector U _t ;

Among them, the calculation formula of the model prediction cost function J is as follows:

Among them, N is the calculation step size of model predictive control, and w represents the weight of each indicator, and the weight of each indicator can be assigned according to the actual situation.

S6: The output vector U _t acts on the autonomous vehicle, and repeats S2 to S6.

包含在代价函数中进行计算，从而在整个模型预测控制中解决了关于车轮打滑的问题，使得车辆轨迹跟踪更加准确。图4中的方框表示车辆前轮。Referring to Figure 4, when the front wheel of the vehicle is in a slipping state, the direction of the front wheel of the vehicle is inconsistent with the direction the wheel should be in, that is, the direction of the front wheel of the vehicle is inconsistent with the driving direction of the vehicle v _car ; The direction can be obtained by calculation or other methods, so at this time, by introducing a virtual quantity δ _true (the current true front wheel inclination angle), it is expressed as the direction where the front wheels of the vehicle should be, and the deviation between the above two value

It is included in the cost function for calculation, thus solving the problem of wheel slip in the whole model predictive control, making the vehicle trajectory tracking more accurate. The boxes in Figure 4 represent the front wheels of the vehicle.

The above is the preferred embodiment of the present invention, but the embodiment of the present invention is not limited by the above-mentioned content, and any other changes, modifications, substitutions, combinations, and simplifications made without departing from the spirit and principle of the present invention, All should be equivalent replacement modes, which are all included in the protection scope of the present invention.

Claims (5)

1. A method for tracking the trajectory of an autonomous vehicle, comprising the steps of:

s1: establishing a ground coordinate system and a body coordinate system, and performing dynamic modeling on the vehicle; the vehicle dynamics model is as follows:

wherein x and y are respectively the x-axis and y-axis coordinates of the center of mass of the autopilot vehicle in the ground coordinate system, psi is the heading angle of the autopilot vehicle in the ground coordinate system, v is the forward speed of the autopilot vehicle, a is the forward acceleration of the autopilot vehicle, t represents the current time quantity, t +1 represents the next time quantity, L _f Is the wheelbase between the front and rear wheels; delta _true The real front wheel inclination angle is a virtual quantity, represents the current real front wheel inclination angle, and is the corresponding front wheel inclination angle of the vehicle under the condition that the front wheel is not in a slipping state in the current driving direction;

s2: obtaining self state vector X of body _t And last output vector U _t-1 (ii) a Wherein,

X _t ＝[x，y，ψ，v] ^T (2)

U _t-1 ＝[a _t-1 ，δ _t-1 ] (3)

delta is the front wheel inclination of the autonomous vehicle; when the self state vector X of the machine body at the next moment needs to be calculated _t+1 Then, the X at that moment is calculated _t Substituting into formula (1) to calculate X at the next time _t+1 Is related to parameter x _t+1 、y _t+1 、ψ _t+1 And v _t+1 ；

S3: obtaining a virtual quantity which is the current real front wheel inclination angle delta _true ；

S4: obtaining discrete waypoint paths P through sampling, carrying out interpolation calculation on the waypoint paths P, and obtaining x by length l respectively _ref ，y _ref ，ψ _ref ，v _ref Obtaining a continuous track f (P); wherein,

substituting the following formula to obtain a discrete waypoint path P 'by setting a reference speed v, wherein P' comprises the state of N points;

P′＝[X ₁ ，X ₂ ，…，X _N ] ^T (5)

X _n ＝[f ₁ (l _n ) f ₂ (l _n ) f ₃ (l _n ) f ₄ (l _n )] (6)

l _n ＝(n+1)×v×dt (7)

wherein n is the number of the point to be taken;

s5: according to self state X _t And a waypoint path P', and obtaining the acceleration a at the current moment by calculating the minimum value of the model prediction cost function J _t And the front wheel inclination angle delta at the present moment _t So as to obtain the optimal output vector as the current output vector U _t ；

The calculation formula of the model prediction cost function J is as follows:

n is a model predictive control calculation step length, w represents the weight of each index, and the weight of each index can be assigned according to the actual condition;

s6: output vector U _t Act on the autonomous vehicle, repeat S2 to S6.

2. The autonomous-vehicle trajectory tracking method of claim 1, wherein in S3, a current true front-wheel inclination δ is obtained _true The method comprises the following steps:

first, by geometric relationships one can obtain:

therefore, the method comprises the following steps:

wherein R is the front wheel inclination angle delta _true Radius of turning in time, L _f D ψ represents a heading angle difference of two time intervals, dx represents an x-coordinate difference of two time intervals, dy represents a y-coordinate difference of two time intervals, dt represents a time interval, and ω is an angular velocity of the vehicle.

3. The autonomous vehicle trajectory tracking method of claim 2, wherein the plurality of δ is obtained by equations (9) and (10) _true 、X _t And U _t Obtaining approximate function g through multivariate least square polynomial fitting; wherein the functions g and X _t X, y, ψ in (b) has no relation with the output vector U at that time _t Velocity v _t Acceleration a _t And front wheel inclination angle delta _t And the output vector U at the previous moment _t-1 Velocity v _t-1 Acceleration a _t-1 And true front wheel inclination

Related to; the following can be obtained:

meanwhile, the calculation can be simplified to obtain:

and finally, obtaining a function g through multivariate least square polynomial fitting.

4. The automatic driven vehicle trajectory tracking method according to claim 1, characterized in that in S2, the current body own state vector X is acquired through sensor filtering _t 。

5. The autonomous vehicle trajectory tracking method of claim 1, wherein in S4, a waypoint path P is obtained by the path planner.

Images