CN113433942B - Long-axis vehicle path tracking control method based on optimal course angle - Google Patents

Abstract

The invention discloses a long-axis vehicle path tracking control method based on an optimal heading angle. It includes the following steps: select the state quantity and control quantity of the control system according to the kinematic model of the long-axis vehicle, establish the tracking error equation of the reference state point, establish the overall deviation model between the long-axis vehicle and the reference path, and solve it according to the method of coordinate transformation to obtain The maximum value of the overall deviation is used to determine the optimal heading angle of the long-axis vehicle in steering. The MPC path tracking controller is established based on the pose error equation between the heading angle and the actual vehicle, and the long-axis is determined by solving the cost function in the controller. The optimal front wheel angle of the axle vehicle during the steering process is determined, and then the vehicle is controlled to drive along the reference path. The invention realizes the path tracking control of the long-axis vehicle and makes the driving track of the front wheels closer to the reference path, effectively reduces the sweeping area of the front wheels during the steering process of the long-axis vehicle, and improves the passability of the long-axis vehicle in narrow areas .

Description

A Path Tracking Control Method for Long-axis Vehicles Based on Optimal Heading Angle

technical field

The invention belongs to the field of intelligent vehicle path tracking, and in particular relates to a long-axis vehicle path tracking control method based on an optimal heading angle.

Background technique

Intelligent vehicle path tracking control technology is a hot research topic at present, but the research objects are mainly passenger vehicles, and the research on commercial vehicles is insufficient. Realizing the path tracking control of long-axis vehicles is an important part of intelligent transportation. The vehicle parameters of long-axis vehicles are obviously different from those of passenger cars. At the same time, long-axis vehicles are prone to collide with the road environment during steering through narrow areas. At the same time, the commonly used path tracking control technologies such as PID, pure tracking, MPC control algorithms, etc. Both simplify the vehicle to a mass point for control research, but for long-axis vehicles, the above algorithm cannot reflect the steering characteristics of long-axis vehicles, so the above method has shortcomings.

Chinese Invention Patent No. CN202011557293.5, published on December 24, 2020, discloses a path tracking control method based on Lyapunov-MPC, which establishes a tracking error model between the target value and the expected value based on the vehicle dynamics model, based on CLF( Control Lyapunov Function) theory to design an LMPC controller, use the backstepping method to design the auxiliary tracking control law and convert it into the constraints in the MPC optimization process, theoretically guarantee the closed-loop stability of the entire system. However, this method only considers the stability and robustness of the vehicle during path tracking, and cannot guarantee a safe area between the long-axle vehicle and the narrow road environment.

In short, the main problem in the path tracking process of long-axis vehicles is that the characteristics of long-axis vehicles cannot be reflected when the long-axis vehicles are purely regarded as particle control research, and the long-axis vehicles are easy to collide with the road environment in the process of turning in a narrow area.

Contents of the invention

The purpose of the present invention is to provide a path tracking control method for a long-axis vehicle based on an optimal heading angle, which can reduce the overall deviation between the long-axis vehicle and the reference path, so that the long-axis vehicle can safely pass through a narrow area without collision.

The technical solution for realizing the object of the present invention is: a long-axis vehicle path tracking control method based on an optimal heading angle, which is characterized in that it includes the following steps:

Step S1: Establish the kinematics model of the long-axis vehicle according to the speed constraints of the front and rear wheels of the vehicle, then select the state quantity and control quantity of the vehicle, and establish the error equation of the model predictive control MPC algorithm;

Step S2: Establish the overall deviation model between the long-axis vehicle and the reference route according to the given reference route, and obtain the maximum value of the overall deviation model through the method of coordinate transformation to determine the optimal heading between the long-axis vehicle and the reference route horn;

Step S3: Establish an MPC path tracking controller based on the error equation in step S1 and the optimal heading angle in step S2, establish a prediction equation expression, and then establish a cost function based on the feedback amount of the actual state of the vehicle and the reference amount in the ideal state , by solving the cost function to obtain the optimal front wheel angle of the long-axis vehicle in the path tracking process;

Step S4: Determine whether the vehicle has completed the given reference path, if not, repeat steps S2 and S3 until all reference points have been completed, and finally obtain the driving trajectory of the front and rear wheels.

Compared with the prior art, the present invention has the following points:

(1) The present invention does not regard the vehicle as a single mass point for research, but considers the overall deviation between the long-axis vehicle and the reference path, and determines the optimal heading angle between the long-axis vehicle and the reference path by solving the overall deviation model, Finally, not only the vehicle path tracking control is realized, but also the optimal heading angle compared with the heading angle under the given reference path can make the front wheel tracking track of the vehicle closer to the given reference path, reducing the sweep of the long-axis vehicle during the steering process area, so that long-axis vehicles can pass through narrow areas without collision.

(2) The MPC path tracking controller designed by this method can determine the optimal control quantity of the vehicle in the path tracking process through the constraint of the control quantity and the rolling optimization of the control time domain, and can ensure that the vehicle can track the reference path smoothly and accurately.

Description of drawings

FIG. 1 is a flow chart of the long-axis vehicle path tracking method of the present invention.

Fig. 2 is a vehicle kinematics model diagram of the present invention.

Fig. 3 is an overall deviation map between the vehicle of the present invention and the reference path.

Fig. 4 is a flow chart of the MPC control algorithm of the present invention.

Fig. 5 Diagram of reference heading angle and optimal heading angle under double-lane shifting conditions.

Fig. 6 Front wheel track diagram under double lane shifting condition.

Detailed ways

The present invention will be described in further detail below in conjunction with the accompanying drawings.

As shown in Figure 1-6, a long-axis vehicle path tracking control method based on the optimal heading angle includes the following steps:

Step S1: Establish the kinematics model of the long-axis vehicle according to the speed constraints of the front and rear wheels of the vehicle, then select the state quantity and control quantity of the vehicle, and establish the error equation of the model predictive control (MPC) algorithm;

Step S2: Establish the overall deviation model between the long-axis vehicle and the reference route according to the given reference route, and obtain the maximum value of the overall deviation model through the method of coordinate transformation to determine the optimal heading between the long-axis vehicle and the reference route horn;

Step S3: Establish an MPC path tracking controller based on the error equation in step S1 and the optimal heading angle in step S2, determine the expression of the prediction equation, and then establish a cost function based on the feedback amount of the actual state of the vehicle and the reference amount in the ideal state , by solving the cost function to obtain the optimal front wheel angle of the long-axis vehicle in the path tracking process;

Step S4: Determine whether the vehicle has completed the given reference path, if not, repeat steps S2 and S3 until all reference points have been completed, and finally obtain the driving trajectory of the front and rear wheels.

Step S5: In order to verify the proposed method, a simulation model is built on the MATLAB/SIMULINK and TRUCKSIM software simulation platforms, and finally the effectiveness of the proposed method is proved by the results.

Specifically, as shown in Figure 2, the steps for solving the speed constraints of the front and rear wheels in step S1 are:

First, the vehicle model is simplified to a single-axis bicycle model, that is, the two front wheels and the two rear wheels are combined into a single wheel, and the vehicle coordinate system is defined. When the x-axis is on the left side is the y-direction, then the speed constraints of the front and rear wheels are:

Among them, X _f and Y _f represent the coordinates of the front wheel position in the earth coordinate system. The solution steps of the vehicle kinematics model include:

First determine the speed at the position of the rear wheel as:

Then according to the position relationship of the front and rear wheels can be obtained:

Finally, according to formulas (1)(2)(3), the vehicle yaw rate ω can be determined as:

Then the final vehicle kinematics model is:

Among them, X _{r and} Y _r are the position coordinates of the rear wheels of the vehicle in the earth coordinate system, v _r is the driving speed of the rear wheels, L is the length of the vehicle axis, θ is the angle between the vehicle axis and the earth coordinate system, which is the heading of the vehicle angle, δ is the front wheel rotation angle, and r represents the position of the rear wheel.

The above expression can be written in general form as:

Among them, the state quantity is ξ=[X _r , Y _r ,θ], and the control quantity is u=[v _r ,δ].

Further, the error equation establishment process in step S1 includes the following steps:

Firstly, a reference path is given, that is, a set of coordinate values obtained by GPS.

Then it can be known from (6) that on a given reference path, the coordinates of any point must satisfy (5), so the expression of the state equation at the reference position is:

where a represents at the reference path position. Doing Taylor expansion transformation of formula (6) at the reference path can get:

Subtracting (8) from (7) yields the error equation in the MPC controller:

The above formula is the continuous equation expression of the error equation. In order to be able to design the MPC controller, it is necessary to use the Euler method to discretize the above formula to obtain the state equation:

T为离散采样时间，k表示离散处的值，取值为1，2…n。in

T is the discrete sampling time, k represents the value at the discrete point, and the value is 1, 2...n.

The control objective of the system is that the position of the rear wheels of the vehicle is as close as possible to the reference position, then the output equation of the system is:

in,

Further, as shown in Figure 3, the optimal heading angle acquisition step in step S2 includes:

First, define the overall deviation between the vehicle and the reference path. When the rear wheels of the vehicle are on the reference path, the position of the front wheels of the vehicle is determined by the axle length and the heading angle of the vehicle. Therefore, different heading angles under the same rear wheel position correspond to different front wheel position, at this time, draw a vertical line from the reference track to the front wheel, then define the vehicle axis length, the area enclosed by the vertical line and the reference track as the overall deviation between the vehicle and the reference track, as shown in the shaded part in Figure 3 . As mentioned above, different heading angles can determine the overall deviation between different long-axis vehicles and the reference path, so there is a heading angle that minimizes the overall deviation between the long-axis vehicle and the reference path, that is, the heading angle definition at this time is the optimal heading angle. According to Figure 3, the expression of the overall deviation can be obtained as:

Where xoy is the vehicle coordinate system, the origin o of the coordinate system is at the rear wheel, p(x) is the expression of the reference path in the vehicle coordinate system, θ _t is the heading angle of the reference path at the current origin position, and the given reference The path is determined, ε is the change value of the heading angle, in order to reduce the calculation amount, ε takes a small positive value, and the optimal heading angle between the long-axis vehicle and the reference trajectory can be determined by finding the minimum value of the overall deviation.

In order to solve the overall deviation function between the long-axis vehicle and the reference path, it is necessary to use the method of converting the vehicle coordinate system to the earth coordinate system to solve the problem, in which the points on the vehicle coordinates can be translated and rotated to obtain the coordinates on the earth coordinate system, Then the transformation form is:

Among them, as shown in Figure 3, XOY is the earth coordinate system, f(X) is the reference path expression in this coordinate system, x _r , y _r are the coordinates of the origin of the vehicle coordinate system in the earth coordinate system, and the final goal The function is expressed in the geodetic coordinate system as:

表示参考路径与长轴车辆之间的最优航向角。此时确定最优航向角之后可以设计MPC路径跟踪控制器。After integrating the above formula, the only parameter in the objective function is θ, and the maximum value of the objective function can be determined according to the value range of the parameter θ, using

Indicates the optimal heading angle between the reference path and the long-axis vehicle. At this time, after determining the optimal heading angle, the MPC path tracking controller can be designed.

Figure 4 shows the design process of the MPC controller. In step S3, the MPC controller is designed according to the error equation and the optimal heading angle. The specific implementation steps are:

First, in order to establish the prediction equation, it is necessary to redefine the new state quantity, and define the deviation state quantity and the control quantity as the new state, expressed as follows:

为当前时刻误差方程的状态量，

当前时刻误差方程中控制量的前一个值。Where γ(k|t) is the new state quantity of the system at the current moment,

is the state quantity of the error equation at the current moment,

The previous value of the control quantity in the error equation at the current moment.

At this time, a new state space expression can be established according to the above state equation,

其中，n为状态量的维度，m为控制量的维度，I_m为单位阵。为计算方便，做如下假设in,

Among them, n is the dimension of the state quantity, m is the dimension of the control quantity, and I _m is the identity matrix. For the convenience of calculation, the following assumptions are made

A _k,t ＝A _t,t ,k=1,2,...t+N-1; B _k,t ＝B _t,t ,k=1,2,...t+N-1; C _k,t =C _t,t ,k=1,2,...t+N-1,

Therefore, a new prediction output expression can be obtained:

Y(t)＝ψt _γ (t|t)+ _Θt ΔU(t) (17)

The symbols are represented as:

Among them, N _p is the prediction time domain, and N _c is the control time domain.

The final establishment of the cost function is as follows:

It can be seen from the cost function that the first term of the formula represents the tracking ability of the system to a given path, Q is the weight matrix, the second term represents the constraint ability of the system on the control increment, and the control amount should be as small as possible, and R is the weight matrix , adding the last ρ weight coefficient and relaxation factor ε is to prevent no solution. Through the QP solver that comes with MATLAB, the above objective function can be obtained to find the minimum value, and the optimal control output of the system can be determined.

In order to obtain a better control effect, it is necessary to constrain the control increment and the control amount. In the process of vehicle path tracking, the actual operation of the vehicle needs to be considered. The constraint form of the controller is:

表示克罗内克积，I_m为单位阵。in

Represents the Kronecker product, and I _m is the identity matrix.

ΔU _min and ΔU _max are the minimum and maximum values of the control increment, respectively, and U _min and U _max are the minimum and maximum sets in the control time domain, respectively. The values of these parameters are determined by the actual control system.

By solving the cost function, the control input increment is obtained as

Then the first element of the series acts on the controlled system as the actual control increment, as shown below:

u(t)=u(t-1)+Δu _t (21)

From formula (21), the optimal front wheel angle δ output by the MPC control system can be obtained, and the above steps can be repeated after entering the next control cycle to realize the tracking control of the vehicle on a given path.

In step S4, it is judged whether the vehicle has completed the given reference path, if not, repeat steps S2 and S3 until all reference points are completed, and finally the driving trajectory of the front and rear wheels is obtained

In step S5, build the MPC controller model in MATLAB/SIMULINK, solve the optimal heading angle model, and use the vehicle model of the long-axis vehicle in TRUCKSIM for joint simulation verification, and judge whether the vehicle has completed path tracking through the actual pose feedback Control, and according to the output of TRUCKSIM, the trajectory of the front and rear wheels of the vehicle is obtained. Figure 5 and Figure 6 show that the trajectory of the front wheels is closer to the given reference path under the optimal heading angle, and the sweep area of the front wheels during driving is smaller, proving that The front and rear wheel trajectories at the optimal heading angle under double-lane-shifting conditions can reduce the deviation between the vehicle as a whole and the reference path better than the results under the reference heading angle, and improve the passability of long-axis vehicles in narrow areas. Valid lines of the mentioned method.

Claims (2)

1. A long-axis vehicle path tracking control method based on an optimal course angle is characterized by comprising the following steps:

step S1: establishing a kinematics model of a long-axis vehicle according to speed constraints of front and rear wheels of the vehicle, then selecting state quantity and control quantity of the vehicle, and establishing an error equation of a model predictive control MPC algorithm;

the step S1 specifically includes the following steps:

step S11: the solving steps of the speed constraint of the front wheel and the rear wheel are as follows:

simplify the vehicle model to unipolar bicycle model, be exactly merge two front wheels and two rear wheels into single wheel, define the vehicle coordinate system, the vehicle rear wheel is the origin of vehicle coordinate system, is the x axle along the vehicle direction of advance, and the left side is the y direction when towards the x axle direction, then the speed constraint of front and rear wheels is:

wherein X _f ，Y _f Coordinates representing the position of the front wheel in a geodetic coordinate system;

step S12: the solving step of the vehicle kinematic model comprises the following steps:

first, the speed at the rear wheel position is determined to be:

then, according to the position relation of the front wheel and the rear wheel, the following can be obtained:

finally, the vehicle yaw rate omega can be determined according to the expressions (1), (2) and (3):

step S13: the final vehicle kinematics model obtained is:

wherein X _r ，Y _r As position coordinates of the rear wheels of the vehicle in the geodetic coordinate system, v _r The driving speed of a rear wheel, L is the axial length of the vehicle, theta is the included angle between the axis of the vehicle and a geodetic coordinate system, namely the heading angle of the vehicle, delta is the rotation angle of a front wheel, and r is the position of the rear wheel;

the above expression is written in the general form:

wherein the state quantity is xi = [ X ] _r ,Y _r ,θ]The controlled variable is u = [ v ] _r ,δ]；

Step S14: establishing an error equation of a model predictive control MPC algorithm:

firstly, a reference path is given, namely a group of coordinate values obtained by a GPS;

then it can be known from (6) that on a given reference path, any point coordinate satisfies (5), and therefore the equation of state at the reference position is expressed as:

where a is at the reference path position; the taylor expansion transform of equation (6) at the reference path can obtain:

subtracting (8) from (7) yields the error equation in the MPC controller:

the above equation is a continuous equation expression of an error equation, and in order to design an MPC controller, a state equation can be obtained by discretizing the above equation by using an euler method:

wherein

T is discrete sampling time, k represents a value at a discrete position, and the value is 1,2 … n;

the control target of the system is that the position of the rear wheel of the vehicle is as close to the reference position as possible, and the output equation of the system is:

wherein,

step S2: establishing an overall deviation model between the long-axis vehicle and the reference path according to the given reference path, and solving by a coordinate transformation method to obtain the most value of the overall deviation model so as to determine the optimal course angle between the long-axis vehicle and the reference path;

the step S2 of establishing an overall deviation model between the long-axis vehicle and the reference path specifically includes:

defining the integral deviation between a vehicle and a reference path, when a rear wheel of the vehicle is on the reference path, determining the position of a front wheel of the vehicle by the axial length and the heading angle of the vehicle, so that different heading angles under the same rear wheel position correspond to different positions of the front wheel, making a vertical line from a reference track to the front wheel at the moment, defining the axial length of the vehicle, the area enclosed by the vertical line and the reference track as the integral deviation between the vehicle and the reference path, and defining a heading angle to minimize the integral deviation between a long-axis vehicle and the reference track, namely defining the heading angle at the moment as an optimal heading angle;

the expression for the overall deviation is:

wherein xoy is a vehicle coordinate system with an origin o at the rear wheel, p (x) is an expression of the reference path in the vehicle coordinate system, and theta _t Determining a course angle of a reference path at the current origin position by a given reference path, wherein epsilon is a change value of the course angle, and a smaller positive value is taken for reducing the calculated quantity epsilon;

the step S2 of solving by a coordinate transformation method to obtain a maximum value of the overall deviation model to determine an optimal heading angle between the long-axis vehicle and the reference path specifically includes:

and (3) solving by using a method of converting the vehicle coordinate system into a geodetic coordinate system, wherein the coordinates on the geodetic coordinate system can be obtained by translating and rotating points on the vehicle coordinate system, and the conversion form is as follows:

wherein XOY is a geodetic coordinate system, f (X) is a reference path expression in the coordinate system, and X _r ，y _r Is the coordinate of the origin of the vehicle coordinate system under the geodetic coordinate system, the final objective function is expressed as:

the parameter in the objective function after the integration of the above formula is only theta, the maximum value of the objective function can be determined according to the value range of the parameter theta, and the maximum value is used

Representing an optimal heading angle between the reference path and the long-axis vehicle;

and step S3: establishing an MPC path tracking controller according to the error equation in the step S1 and the optimal course angle in the step S2, establishing a prediction equation expression, then establishing a cost function according to the feedback quantity of the actual state of the vehicle and the reference quantity in the ideal state, and solving the cost function to obtain the optimal corner of the front wheel of the long-axis vehicle in the path tracking process;

the step S3 specifically comprises the following steps:

to establish the prediction equation, a new state quantity needs to be newly defined, and the deviation state quantity and the controlled variable are defined as a new state, which is expressed as follows:

where γ (k | t) is the new state quantity of the system at the current time,

is the state quantity of the error equation at the current moment,

the previous value of the controlled variable in the error equation at the current moment;

a new state space expression can now be built from the above state equation,

wherein,

where n is the dimension of the state quantity, m is the dimension of the control quantity, I _m Is a unit array; for the convenience of calculation, the following assumptions were made

A _k,t ＝A _t,t ,k＝1,2,…t+N-1；B _k,t ＝B _t,t ,k＝1,2,…t+N-1；C _k,t ＝C _t,t ,k＝1,2,…t+N-1，

A new predicted output expression can thus be derived:

Y(t)＝ψ _t γ(t|t)+Θ _t △U(t) (17)

wherein each symbol is represented as:

wherein N is _p To predict the time domain, N _c Is a control time domain;

the final established cost function is as follows:

according to the cost function, a first term of the formula represents the tracking capacity of the system to a given path, Q is a weight matrix, a second term represents the constraint capacity of the system to a control increment, the control quantity is ensured to be as small as possible, R is the weight matrix, and the final rho weight coefficient and the relaxation factor epsilon are added to prevent the situation of no solution; the minimum value of the objective function can be obtained through a QP solver carried by the MATLAB, and the optimal control output of the system can be determined;

and constraining the control increment and the control quantity, wherein the actual running condition of the vehicle needs to be considered in the vehicle path tracking process, and the constraint form of the controller is as follows:

wherein

Denotes the kronecker product, I _m Is a unit array;

△U _min ，△U _max minimum and maximum values of the control increment, U, respectively _min ，U _max Respectively set as the minimum value and the maximum value in the control time domain, and the values of the parameters are determined by an actual control system;

obtaining a control input increment of

The first element of the control input increment of equation (20) is applied to the controlled system as the actual control increment as follows:

u(t)＝u(t-1)+△u _t (21)

the optimal front wheel steering angle delta output by the MPC control system can be obtained from the formula (21), and the steps are repeated after the next control period is entered, so that the tracking control of the vehicle on the given path can be realized;

and step S4: and judging whether the vehicle finishes walking the given reference path or not, if not, repeating the steps S2 and S3 until all the reference points are finished, and finally obtaining the running tracks of the front wheels and the rear wheels.

2. The method of claim 1, further comprising the steps of:

step S5: the method comprises the steps of building an MPC controller model on MATLAB/SIMULINK, solving a model for an optimal course angle, performing joint simulation verification by using a vehicle model of a long-axis vehicle in TRUCKSIM, judging whether the vehicle completes path tracking control or not through actual pose feedback, obtaining driving tracks of front wheels and rear wheels of the vehicle according to the output of the TRUCKSIM, and finally, proving that the scanning area between the whole vehicle and a reference path can be reduced by the tracks of the front wheels and the rear wheels under the optimal course angle compared with the result under the reference course angle, so that the trafficability of the long-axis vehicle in a narrow area is improved.

Images