CN104977933A - Regional path tracking control method for autonomous land vehicle - Google Patents

Abstract

The invention discloses a regional path tracking control method for an autonomous land vehicle. A problem that collision of a vehicle with a road boundary or surround obstacle occurs because shapes and sizes of the vehicle and the road are not considered by the existing tracking control method can be solved. The method comprises the following steps: establishing a two-dimensional road model; establishing a mathematical model associated with a vehicle path tracking problem; calculating a roadable region boundary line within a certain distance in front of the vehicle; establishing a vehicle system model; carrying out a design of a regional path tracking control model and selecting a controlled quantity being an optimum front wheel steering angle at current time; according to the optimum front wheel steering angle, controlling a steering execution mechanism to make motion to enable the controlled vehicle to be driven in a roadable region, provided by a vehicle sensing system, within a certain distance in front of the vehicle. When the two-dimensional road model is established, the shapes and sizes of the vehicle and roads are taken into consideration, thereby reducing the possibility of collision of the vechile with the road boundary and thus improving safety of the autonomous land vehicle.

Description

A kind of domain type path tracking control method of autonomous land vehicle

Technical field

The invention belongs to the control method of autonomous driving technical field, relate to a kind of path tracking control method of autonomous land vehicle.

Background technology

The wide application prospect of autonomous driving technology makes it day by day receive the concern of people.Typical autonomous driving system comprises sensory perceptual system and the large functional module of Ride Control System two, and sensory perceptual system is in order to obtain the running state information of vehicle-periphery information and vehicle self, and Ride Control System is then replace driver to control vehicle to travel.For autonomous driving technology, path following control is the most key control problem that Ride Control System needs to solve.The path following control of existing autonomous land vehicle is mostly the motion control method that have references to robot; first the Traffic Information scanned according to sensory perceptual system cooks up a feasible track or path-line, then controls vehicle tracking this feasible track or path-line.These control methods are mostly the road vehicle modellings based on dotted-line style, and the typical control method of some of them has pure point tracking method, takes aim at PID and Stanly method etc. in advance.They can obtain good path following control effect, but due to the shape that neglected vehicle and road in the design process and size, so can not ensure that vehicle can not collide with road boundary or the barrier around it.

Summary of the invention

The problem to be solved in the present invention overcomes the shape not considering vehicle and road and size that exist in the path tracking control method of existing autonomous land vehicle and then can not ensure the problem that vehicle does not collide with road boundary or its peripheral obstacle, provides a kind of domain type path tracking control method of autonomous land vehicle.

The domain type path tracking control method of a kind of autonomous land vehicle that the present invention proposes adopts following technical scheme to realize:

A kind of domain type path tracking control method of autonomous land vehicle, Ride Control System in autonomous land vehicle is first according to the front wheel angle of running state information optimization current time the best of the connecting way area information provided after sensory perceptual system scan process and vehicle self, then according to this front wheel angle control vehicle turn to topworks's action, to make vehicle travel in feasible road area, it is characterized in that step is as follows:

Step one, set up two-dimentional road vehicle model:

Set up two-dimentional road vehicle model, if rigid rod RF is auto model, it crosses the barycenter o of vehicle, and length equals length of wagon l, and expected path is then by expectation road area left side boundary line f ' _lboundary line f ' on the right of (x), expectation road area _rx expectation road area that () and expectation road area center line f (x) form represents, and meets:

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

In formula, f _lx () is the left margin in connecting way region in front one segment distance that obtained by the sensory perceptual system scan process in autonomous land vehicle; f _rx () is the right margin in connecting way region in front one segment distance that obtained by the sensory perceptual system scan process in autonomous land vehicle; F ' _lx () is for expecting boundary line, the road area left side, f _r' (x) for expecting boundary line on the right of road area, w is vehicle width, unit, m;

Step 2, set up the mathematical model of the domain type path trace problem of vehicle:

Based on the two-dimentional road vehicle model that step one is set up, ensure that rigid rod RF is in all the time by expectation road area left side boundary line f ' _lboundary line f on the right of (x), expectation road area _r' (x) and expect that in road area center line f (x) the expectation road area that forms, in conjunction with geometry and the physical characteristics of rigid rod, the mathematical model setting up the domain type path trace problem of vehicle is as follows:

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

In formula, y _ofor the lateral position of vehicle centroid o, unit, m; l _ffor vehicle centroid o is to the distance of vehicle front point F, unit, m; l _rfor vehicle centroid o is to the distance of rear vehicle end point R, unit, m; ψ is Vehicular yaw angle, unit, rad; β is vehicle centroid side drift angle, unit, rad;

Connecting way regional edge boundary line f in step 3, calculating vehicle front one segment distance _l(x) and f _r(x):

Assuming that the sensory perceptual system of autonomous land vehicle can the point sequence information (x of connecting way zone boundary of Real-time Obtaining du vehicule _r, y _r, x _l, y _l), the boundary line function adopting three Lagrange's interpolation formulas to obtain connecting way region in vehicle front one segment distance based on binary search algorithm is:

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

In formula, (x _r(i), y _r(i), x _l(i), y _l(i)), i=j, n, m, k are the point sequence (x of connecting way zone boundary _r, y _r, x _l, y _l) in four groups of coordinate points;

Step 4, set up autonomous land vehicle system model and arranged as state space form:

$\left\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}\right.---\left(19\right)$

In formula,

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

In formula, x is system state vector, and x=[y _oψ β r] ^t; δ _ffor vehicle front wheel angle, be also system control amount, unit, rad; y _ofor system exports; A is system matrix; B is input matrix; C is output matrix; V is the speed at vehicle centroid place, unit, m/s; R is the yaw velocity of vehicle, unit, rad/s; C _ffor the cornering stiffness of vehicle front tyre, unit, N/rad; C _rfor the cornering stiffness of vehicle rear wheel tire, unit, N/rad; M is the quality of vehicle, unit, kg; I _zfor vehicle is around the moment of inertia of z-axis, unit, kgm ²; A is the distance of vehicle centroid o to automobile front-axle, unit, m; B is the distance of vehicle centroid o to vehicle rear axle, unit, m;

The domain type path following control model of step 5, employing model predictive control method design vehicle is:

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min }J\\ J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)\end{array}\right.---\left(32\right)$

Meet: $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x(k+i)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

In formula:

$Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right],R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right],U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right];$

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx _d(k+i)＝v(k)·T _s；

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1)；

Δδ _f(k+i)＝δ _f(k+i)-δ _f(k+i-1)；

C _ψ＝[0 1 0 0],C _β＝[0 0 1 0]；

And choose the front wheel angle of controlled quentity controlled variable and current time the best for:

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right)$

δ in formula _f(k+i) be the system control amount in k+i moment, be the front wheel angle of vehicle, unit, rad;

The system state vector that x (k+i) is the k+i moment;

Y _o(k+i) for the system in k+i moment exports, i.e. the lateral position of vehicle centroid, unit, m;

P is prediction time domain, and N is for controlling time domain;

Γ _yand Γ _ufor weighting matrix;

Γ _d,ifor weight factor;

Y _r(k+i), i=1 ..., P is the discrete magnitude expecting road area center line f (x), and discrete interval is v (k) T _s, unit, m;

Δ x _d(k+i) be the length travel that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

Δ y _d(k+i) be the lateral shift that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

Δ δ _f(k+i) be the controlling increment in k+i moment, unit, rad;

δ _fsatfor turning to the maximum front wheel angle achieved by topworks, unit, rad;

Δ δ _fsatturn to the maximum front wheel angle increment achieved by topworks, unit, rad;

F ' _l(k+i) for expecting road area left side boundary line f ' _l(x) in the sampled value of moment k+i, unit, m;

F _r' (k+i) then for expecting boundary line f on the right of road area _r' (x) in the sampled value of moment k+i, unit, m;

β _rolloverfor the critical quantity of rollover may occur vehicle, unit, rad;

T _sfor the sampling time, unit s;

U ^*for the optimal control sequence obtained by Optimization Solution;

Step 6, front wheel angle according to the current time the best provided in step 5 control turns to topworks's action, makes the front wheel angle of controlled vehicle equal the front wheel angle of current time the best thus controlled vehicle is travelled in connecting way region in vehicle front one segment distance that vehicle sensory perceptual system provides, the control objectives of feasible region path trace.

Further technical scheme is:

The detailed process of step 2 is:

For ensureing that rigid rod RF is in all the time by expectation road area left side boundary line f ' _lboundary line f on the right of (x), expectation road area _r' (x) and expect in road area center line f (x) the expectation road area that forms, need ensure that the relation described in following formula (2) is set up:

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

In formula, y _ffor the lateral position of rigid rod RF forward terminal F, unit, m; y _rfor the lateral position of rigid rod RF aft terminal R, unit, m;

There is following geometric relationship in the forward terminal F of rigid rod RF and aft terminal R and barycenter o:

$\left\{\begin{array}{c}{y}_F=y_o+l_f\sin (\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

In formula, y _ofor the lateral position of vehicle centroid o, unit, m; l _ffor vehicle centroid o is to the distance of vehicle front point F, unit, m; l _rfor vehicle centroid o is to the distance of rear vehicle end point R, unit, m; ψ is Vehicular yaw angle, unit, radian (rad); β is vehicle centroid side drift angle, unit, rad;

Consider that the distance that the sensory perceptual system of autonomous land vehicle can observe at every turn is approximately 50m, and the curvature of road is also mostly smaller, so yaw angle ψ when thinking that vehicle travels in this section of region is very little, consider that again the side slip angle β of vehicle is very little, the present invention adopts following approximation relation:

$\left\{\begin{array}{c}sin(\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ cos(\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

And then formula (3) can be reduced to:

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

Formula (5) is updated in formula (2), arranges the mathematical modulo pattern (6) that can obtain the domain type path trace problem of the vehicle described in step 2.

The detailed process of step 3 is:

In formula (7) in step 3, (x _r(i), y _r(i), x _l(i), y _l(i)), i=j, n, m, k are road point sequence (x _r, y _r, x _l, y _l) in four groups of coordinate points, choosing of these four groups of coordinate points, carries out based on binary search algorithm, and the object of binary search is the starting point in order to obtain connecting way region in vehicle front one segment distance with terminal the concrete derivation of binary search algorithm is as follows:

When not considering reversing, suppose that the position coordinates at the current place of controlled vehicle is (x _o, y _o), get as the starting point of search, then point horizontal ordinate x _rand point (0) horizontal ordinate x _l(0) must be negative value, the object of this time search finds to be positioned at vehicle centroid o rear and one group of point that range points o is nearest in the X-axis direction then point horizontal ordinate x _r(j) and point horizontal ordinate x _lj () must meet formula (8):

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

In formula, x _r(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate;

Search out the one group of point meeting formula (8) after, their information is stored, then when carrying out second time and searching for, starting point using them as search, consider the control signal be applied on Autonomous Vehicles, its useful effect phase is generally about 1s, only consider that vehicle front length is one section of road area of v, in formula, v is the speed at vehicle centroid place, therefore, and the impact point of this search horizontal ordinate must meet inequality relation in formula (9):

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

In formula, x _r(k) for point horizontal ordinate; x _l(k) for point horizontal ordinate; x _r(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate;

By the point searched with as two groups of interpolation points of formula (7), selected point simultaneously with as other two groups of interpolation points of formula (7), j, k, n and m is made to represent above-mentioned four groups of interpolation points respectively at known road point sequence (x _r, y _r, x _l, y _l) position, the relation between j, k, n and m is as follows:

The detailed process of step 4 is:

(1) vehicle kinematics model is set up

Assuming that vehicle is a rigid body, wherein install the wheel that four deformation can not occur, and using vehicle front-wheel as deflecting roller, obtain the kinematics model of vehicle according to kinematical equation and geometric relationship as shown in formula (11):

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

In formula, x _ofor the lengthwise position of vehicle centroid o, unit, m; y _ofor the lateral position of vehicle centroid o, unit, m; V is the speed at vehicle centroid place, unit, m/s; R is the yaw velocity of vehicle, unit, rad/s, formula (4) is updated in formula (11), then the vehicle kinematics model that can be simplified, shown in (12):

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2) vehicle dynamic model is set up

If vehicle centroid o is true origin in vehicle dynamic model, along the positive dirction that vehicle body direction is forward transverse axis x, perpendicular to the positive dirction that the direction on X direction is longitudinal axis y, this method is that the front wheel angle by controlling vehicle carrys out the object of realizing route tracking, so ignore the longitudinal dynamics of vehicle, the lateral dynamics of consideration vehicle and the dynamics of yaw direction, according to Newton second law and equalising torque relation, can obtain such as formula the vehicle dynamic model shown in (13):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

In formula, v _xfor the longitudinal velocity at vehicle centroid place, unit, m/s; F _yffor vehicle front-wheel side force, unit, N; F _yrfor vehicle rear wheel side force, unit, N; M is the quality of vehicle, unit, kg; I _zfor vehicle is around the moment of inertia of z-axis, unit, kgm ²; A is the distance of vehicle centroid o to automobile front-axle, unit, m; B is the distance of vehicle centroid o to vehicle rear axle, unit, m; δ _ffor vehicle front wheel angle, unit, rad, the front wheel angle δ of vehicle _fvery little, formula (13) can be simplified, the vehicle dynamic model after simplification is such as formula shown in (14):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

Assuming that vehicle side does not reach capacity to tire force, now side force F _ysubstantially linear with slip angle of tire α, shown in (15):

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

In formula, C _ffor the tire cornering stiffness of vehicle front-wheel, unit, Nrad; C _rfor the tire cornering stiffness of vehicle rear wheel, unit, Nrad; α _ffor the slip angle of tire of vehicle front-wheel, unit, rad; α _rfor the slip angle of tire of vehicle rear wheel, unit, rad, according to the regulation of coordinate system, the slip angle of tire α of front-wheel _fwith the slip angle of tire α of trailing wheel _rbe respectively:

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_r=\mathrm{\β}-\frac{br}{v_x}\end{array}\right.---\left(16\right)$

Convolution (14), (15) and (16), arrangement can obtain binary vehicle dynamic model, shown in (17):

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3) state-space model of Vehicular system is set up

Convolution (12) and formula (17), consider simultaneously then can obtain Vehicular system motion and dynamic (dynamical) differential equation, specifically such as formula shown in (18):

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

By controlling the front wheel angle of vehicle and then ensureing that the lateral position of vehicle meets the inequality constrain in formula (6), choose the lateral position y of vehicle centroid o _oexport as system, choose front wheel angle δ simultaneously _fas system control amount, system state vector is chosen for x=[y _oψ β r], Vehicular system model can be described as the state-space model shown in step 4 Chinese style (19).

The detailed process of step 5 is:

Suppose that autonomous land vehicle keeps constant speed drive in a prediction time domain, the Differential Model that formula (19) in step 4 is Vehicular system, in order to the design of the domain type path following control model for vehicle, need formula (19) discretize, obtain the Vehicular system model of discrete time, shown in (20):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

In formula, $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ T in formula _sfor the sampling time;

Assuming that prediction time domain is P, control time domain is N, and meets N≤P, and the controlled quentity controlled variable that supposition controls outside time domain simultaneously remains unchanged, i.e. δ _f(k+N-1)=δ _f(k+N)=...=δ _f(k+P-1), then the status predication equation of P step is gone out based on the Vehicular system model inference of discrete time in formula (20), concrete Ru shown in (21):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

The prediction simultaneously deriving P step exports, shown in (22):

$\left\{\begin{array}{c}{y}_o(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_o(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_o(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(22\right)$

Definition control inputs sequence U (k) and control forecasting output sequence Y (k+1|k) are respectively:

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

In order to make autonomous land vehicle as far as possible along expecting that the center line of road area travels, define such as formula reference input sequence R (k) shown in (24):

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

In formula, y _r(k+i), i=1 ..., P is the discrete magnitude expecting road area center line f (x), and discrete interval is v (k) T _s, travelling along expectation road area center line as far as possible to control vehicle, realizing by the objective function minimized in formula (25):

J ₁＝‖Y(k+1|k)-R(k)‖ ²(25)

In order to the function making domain type path following control model have the controlled route or travel by vehicle of the shortizationest, when adopting the domain type path following control model of model predictive control method design vehicle, realize, shown in (26) by minimizing the objective function be made up of the displacement of vehicle traveling:

$J_2=\underset{i=1}{\overset{P}{\Σ}}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(26\right)$

In formula,

Δx _d(k+i)＝v(k)·T _s,i＝1,…,P

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1),i＝1,…,P；

In formula, Δ x _d(k+i) be the length travel that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m; Δ y _d(k+i) be the lateral shift that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

In order to be controlled the control action of controller, make it can not be excessive, turn to ride comfort with what ensure controlled vehicle, the formula (27) that will be made up of control inputs sequence U (k) be as an optimization aim of Controlling model:

J ₃＝‖U(k)‖ ²(27)

Introduce weight coefficient to J ₁, J ₂and J ₃the demand of three optimization aim carries out balance process, and the optimization aim of the domain type path following control model based on Model Predictive Control of design is:

$J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(28\right)$

In formula, Γ _yand Γ _ufor weighting matrix; Γ _d,ifor weight factor;

In order to ensure that controlled vehicle travels all the time in connecting way region, when adopting the domain type path following control model of model predictive control method design vehicle, control system is exported and use restraint, inequality relation in integrating step two Chinese style (6), this output constraint can be written to such as formula the form shown in (29):

$\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\end{array},i=1,...,P---\left(29\right)$

In formula, ψ (k+i)=C _ψx (k+i), and C _ψ=[0 10 0]; β (k+i)=C _βx (k+i), and C _β=[0 00 1]; F ' _l(k+i) for expecting road area left side boundary line f ' _l(x) in the sampled value of moment k+i, unit, m; f _r' (k+i) then for expecting boundary line f on the right of road area _r' (x) in the sampled value of moment k+i, unit, m.

For making vehicle front wheel angle and rate of change thereof not higher than the saturation value of steering mechanism, the control constraints that consideration is shown below when adopting the domain type path following control model of model predictive control method design vehicle and controlling increment constraint:

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

In formula, δ _f(k+i) be the vehicle front wheel angle in k+i moment, unit, rad; δ _fsatfor turning to the maximum front wheel angle achieved by topworks, unit, rad; Δ δ _f(k+i)=δ _f(k+i)-δ _f(k+i-1) be the front wheel angle increment in k+i moment, unit, rad; Δ δ _fsatturn to the maximum front wheel angle increment achieved by topworks, unit, rad;

For improving the lateral stability of vehicle, reduce the risk that rollover occurs for it, the domain type path following control model of the vehicle of design should make vehicle centroid side drift angle β be not more than the critical value β of vehicle generation rollover as far as possible _rollover, therefore, consider following state constraint constantly adopting the domain type path following control model of model predictive control method design vehicle:

|β(k+i)|≤β _rollover,i＝1,…,P (31)

By arranging formula (25) ~ (31), obtain the formula (32) of the zone routing tracing control model in step 5, the optimization problem solved in formula (32) can obtain an optimum control sequence U ^*, the ultimate principle of combination model PREDICTIVE CONTROL, chooses the front wheel angle of controlled quentity controlled variable and current time the best be the formula (33) described in step 5:

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right).$

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

1. the present invention set up two-dimentional road vehicle model time, consider shape and the size of vehicle and road, reduce the possibility that vehicle and road boundary collide, improve the security of autonomous land vehicle.

2. Controlling model of the present invention is based on dynamics of vehicle and kinematic relation design, has good path trace performance when low speed and high speed equally.

3. the present invention is when the domain type path following control model of design vehicle, intact stability and energy consumption of vehicles problem are also taken into account, while guarantee autonomous land vehicle path trace performance, guarantees the riding stability of vehicle, reduce its oil consumption, improve the economy of Autonomous Vehicles.

Accompanying drawing explanation

Fig. 1 is the process flow diagram of the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 2 is the schematic diagram of the two-dimentional road vehicle model set up in the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 3 is the geometric relationship figure of end points and vehicle centroid before and after the vehicle body in the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 4 is the principle schematic in connecting way regional edge boundary line in acquisition vehicle front one segment distance in the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 5 is the vehicle kinematics model schematic in the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 6 is the vehicle dynamic model schematic diagram in the domain type path tracking control method of a kind of autonomous land vehicle of the present invention;

Fig. 7 is the domain type path following control system chart of autonomous land vehicle in embodiment of the present invention;

Fig. 8 is the road condition figure of off-line simulation experiment in embodiment of the present invention;

Fig. 9 a to Fig. 9 d is the simulation result of first group of off-line simulation experiment in embodiment of the present invention, wherein Fig. 9 a is the driving path of controlled vehicle, and Fig. 9 b is the controlled quentity controlled variable that controller optimization goes out, i.e. the front wheel angle of vehicle, Fig. 9 c is the side slip angle of vehicle, and Fig. 9 d is the yaw velocity of vehicle;

Figure 10 a to Figure 10 d is the simulation result of second group of off-line simulation experiment in embodiment of the present invention, wherein Figure 10 a is the driving path of controlled vehicle, Figure 10 b is the controlled quentity controlled variable that controller optimization goes out, the i.e. front wheel angle of vehicle, Figure 10 c is the side slip angle of vehicle, and Figure 10 d is the yaw velocity of vehicle.

Embodiment

Below in conjunction with accompanying drawing, the present invention is described in detail:

The present invention proposes a kind of domain type path tracking control method of autonomous land vehicle, and its concrete implementation step is as follows:

Step one, set up two-dimentional road vehicle model:

The present invention sets up a kind of novel two-dimentional road vehicle model, as shown in Figure 2.Ignore the width on left and right vehicle wheel both sides, with the rigid rod RF of vehicle centroid o represent vehicle, the length of rigid rod RF equals length of wagon l.Expected path is then by expectation road area left side boundary line f ' _lboundary line f on the right of (x), expectation road area _r' (x) and expect that road area center line f (x) the expectation road area that forms represents, wherein expect road area left side boundary line f ' _lboundary line f on the right of (x) and expectation road area _r' (x) can be calculated by following formula:

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

Wherein, f _lx (), for be scanned by sensory perceptual system, then processes the left margin in connecting way region in front one segment distance that obtains; f _rx (), for be scanned by sensory perceptual system, then processes the right margin in connecting way region in front one segment distance that obtains; W is vehicle width, unit, m.

Step 2, set up the mathematical model of the domain type path following control problem of vehicle:

The target of path trace is that autonomous driving automobile is travelled along expected path.Based on the two-dimentional road vehicle model that the present invention sets up, in known the present invention, the main target of path trace ensures that rigid rod RF is in all the time by expectation road area left side boundary line f ' _lboundary line f on the right of (x), expectation road area _r' (x) and expect in road area center line f (x) the expectation road area that forms, so path trace problem of the present invention has another name called for domain type path trace.

Rigid rod RF has the characteristic that size and shape is constant under motion and stressing conditions, as long as so ensure that the forward terminal F of rigid rod RF and aft terminal R is in given expectation road area, whole rigid rod RF is just in this expectation road area.Therefore, domain type path trace problem proposed by the invention, its main target is that the relation described in guarantee formula (2) is set up:

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

Wherein, y _ffor the lateral position of rigid rod RF forward terminal F, unit, m; y _rfor the lateral position of rigid rod RF aft terminal R, unit, m.

As shown in Figure 3, there is following geometric relationship in the forward terminal F of rigid rod RF and aft terminal R and barycenter o:

$\left\{\begin{array}{c}{y}_F=y_o+l_f\sin (\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

Wherein, y _ofor the lateral position of vehicle centroid o, unit, m; l _ffor vehicle centroid o is to the distance of vehicle front point F, unit, m; l _rfor vehicle centroid o is to the distance of rear vehicle end point R, unit, m; ψ is Vehicular yaw angle, unit, radian (rad); β is vehicle centroid side drift angle, unit, rad.

Consider the sensory perceptual system of autonomous land vehicle, its distance that at every turn can observe is approximately 50m, and the curvature of road is also mostly smaller, so yaw angle ψ when thinking that vehicle travels in this section of region is very little.Consider that again the side slip angle β of vehicle is very little, the present invention adopts following approximation relation:

$\left\{\begin{array}{c}sin(\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ cos(\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

And then formula (3) can be reduced to:

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

Formula (5) is updated in formula (2), arranges the mathematical model of the domain type path trace problem that can obtain the vehicle that the present invention proposes, be shown below:

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

Connecting way zone boundary line function f in step 3, calculating front one segment distance _l(x) and f _r(x)

The present invention supposes that vehicle sensory perceptual system can the point sequence (x of connecting way zone boundary, vehicle periphery road under Real-time Obtaining vehicle body coordinate system _r, y _r, x _l, y _l).Based on this, adopt three Lagrange's interpolation formulas to the road point sequence (x obtained _r, y _r, x _l, y _l) carry out interpolation processing, obtain the left side boundary line f in connecting way region in vehicle front one segment distance _l(x) and the right boundary line f _r(x), specifically such as formula shown in (7):

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

In formula, (x _r(i), y _r(i), x _l(i), y _l(i)), i=j, n, m, k are the point sequence (x of connecting way zone boundary _r, y _r, x _l, y _l) in four groups of coordinate points.Choosing of these four groups of coordinate points, carries out based on binary search algorithm, specifically as shown in Figure 4.The object of binary search is the starting point in order to obtain connecting way region in vehicle front one segment distance with terminal the concrete derivation of binary search algorithm is as follows:

When not considering reversing, suppose that the position coordinates at the current place of controlled vehicle is (x _o, y _o), get as the starting point of search, then point horizontal ordinate x _rand point (0) horizontal ordinate x _l(0) must be negative value.This time the object of search finds to be positioned at vehicle centroid o rear and one group of point that range points o is nearest in the X-axis direction then point horizontal ordinate x _r(j) and point horizontal ordinate x _lj () must meet formula (8):

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

Wherein, x _r(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate.

Search out the one group of point meeting formula (8) after, their information is stored, then when carrying out second time and searching for, the starting point using them as search.Consider the control signal be applied on Autonomous Vehicles, its useful effect phase is generally about 1s, so the present invention only considers that vehicle front length is one section of road area of v, wherein v is the speed at vehicle centroid place.Therefore, the impact point of this search horizontal ordinate must meet inequality relation in formula (9):

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

Wherein, x _r(k) for point horizontal ordinate; x _l(k) for point horizontal ordinate; x _r(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate.

The point that the present invention will search with as two groups of interpolation points of formula (7), selected point simultaneously with as other two groups of interpolation points.J, k, n and m is made to represent above-mentioned four groups of interpolation points respectively at known road point sequence (x _r, y _r, x _l, y _l) position, then there is following relation:

Step 4, set up Vehicular system model and be organized into state space form:

Consider that kinematics and the kinetics relation of vehicle have great impact to low speed and autonomous land vehicle when running at high speed respectively, the kinematics of vehicle and kinetics relation, when setting up Vehicular system model, are taken into account by the present invention simultaneously.

(1) vehicle kinematics model is set up

The schematic diagram of vehicle kinematics model as shown in Figure 5, supposes that vehicle is a rigid body, wherein installs the wheel that four deformation can not occur here, and took turns as deflecting roller in the past.The kinematics model of vehicle can be obtained such as formula shown in (11) according to the geometric relationship shown in kinematical equation and accompanying drawing 5:

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

In formula, x _ofor the lengthwise position of vehicle centroid o, unit, m; y _ofor the lateral position of vehicle centroid o, unit, m; V is the speed at vehicle centroid place, unit, m/s; R is the yaw velocity of vehicle, unit, rad/s.Formula (4) is updated in formula (11), then the vehicle kinematics model that can be simplified, shown in (12):

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2) vehicle dynamic model is set up

As shown in Figure 6, wherein vehicle centroid o is true origin to the schematic diagram of vehicle dynamic model, and being the positive dirction of transverse axis x along vehicle body direction forward, is the positive dirction of longitudinal axis y perpendicular to the direction on X direction.Because the present invention carrys out the object of realizing route tracking by the front wheel angle of control vehicle, so ignore the longitudinal dynamics of vehicle, and only consider the lateral dynamics of vehicle and the dynamics of yaw direction.According to Newton second law and equalising torque relation, can obtain such as formula the vehicle dynamic model shown in (13):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

In formula, v _xfor the longitudinal velocity at vehicle centroid place, unit, m/s; F _yffor vehicle front-wheel side force, unit, N; F _yrfor vehicle rear wheel side force, unit, N; M is the quality of vehicle, unit, kg; I _zfor vehicle is around the moment of inertia of z-axis, unit, kgm ²; A is the distance of vehicle centroid o to automobile front-axle, unit, m; B is the distance of vehicle centroid o to vehicle rear axle, unit, m; δ _ffor vehicle front wheel angle, unit, rad.The front wheel angle δ of vehicle _fvery little, so formula (13) can be simplified, the vehicle dynamic model after simplification is such as formula shown in (14):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

Assuming that vehicle side does not reach capacity to tire force, now side force F _ysubstantially linear with slip angle of tire α, shown in (15):

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

In formula, C _fthe tire cornering stiffness of vehicle front-wheel, unit, Nrad; C _rfor the tire cornering stiffness of vehicle rear wheel, unit, Nrad; α _ffor the slip angle of tire of vehicle front-wheel, unit, rad; α _rfor the slip angle of tire of vehicle rear wheel, unit, rad.According to the regulation of coordinate system, the slip angle of tire of front and rear wheel is respectively:

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_r=\mathrm{\β}-\frac{br}{v_x}\end{array}\right.---\left(16\right)$

Convolution (14), (15) and (16), arrangement can obtain binary vehicle dynamic model, shown in (17):

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3) state-space model of Vehicular system is set up

Convolution (12) and formula (17), consider simultaneously then can obtain Vehicular system motion and dynamic (dynamical) differential equation, specifically such as formula shown in (18):

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

The domain type path trace problem of the vehicle that the present invention proposes, its main target is the inequality constrain met by the front wheel angle of control vehicle and then the lateral position of guarantee vehicle in formula (6).So, choose the lateral position y of vehicle centroid o _oexport as system, choose front wheel angle δ simultaneously _fas system control amount, selecting system state vector x=[y _oψ β r] ^t.Based on this, Vehicular system model can be described the state-space model shown in an accepted way of doing sth (19):

$\left\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}\right.---\left(19\right)$

In formula,

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

Wherein, A is system matrix; B is input matrix; C is output matrix.

Step 5, employing model predictive control method carry out the design of domain type path following control model, obtain the front wheel angle of current time the best

The system chart of the domain type path following control system of the present invention's design as shown in Figure 7, characterizes the position (x of the vehicle centroid of travel condition of vehicle _o, y _o), the side slip angle β of the speed v of Vehicular yaw angle ψ, vehicle, the yaw velocity r of vehicle and vehicle obtains by the high-precision GPS sensor RT3002 measurement of device in sensory perceptual system.Consider the superiority of model predictive control method in process constraint, the present invention is based on the design that model predictive control method carries out domain type path following control model.

First, according to the mathematical model of the domain type path trace problem set up above, must retrain the lateral position of vehicle centroid when the domain type path following control model of design vehicle, make it meet inequality relation in formula (6).Secondly, consider that security is the major issue that vehicle must be paid close attention in the process of moving, and for carrying out the Autonomous Vehicles of domain type path trace, it is obviously safest traveling scheme that center line along its connecting way region, front travels, so the domain type path following control model of vehicle of design must can ensure that Autonomous Vehicles travels in the zone on heart line as much as possible.Vehicle generation lateral turnover also drastically influence the driving safety of vehicle.By experience, when vehicle centroid side drift angle β is greater than a certain amount, (we are called rollover critical point β _rollover) time, vehicle just has the danger that rollover occurs, so must use restraint to vehicle centroid side drift angle when the domain type path following control model of design vehicle.Again, consider now extensively by the oil consumption problem paid close attention to both at home and abroad, the present invention proposes to be reduced energy consumption by the travel route of the shortizationest vehicle and then realized reducing the object of oil consumption.Finally, consider the ride performance of Autonomous Vehicles, also need to limit the size of control action, to avoid occurring excessive control action.The steering mechanism of vehicle is a mechanical system, the problem that machinery is saturated can be there is, so to ensure that the controlled quentity controlled variable that controller exports can play control action effectively, also must take into account turning to the saturation problem of topworks when the domain type path following control model of design vehicle.To sum up analyze, the domain type path following control model of the vehicle of the present invention's design needs to realize following targets:

Target 1) make the lateral position of vehicle centroid meet constraint in formula (6);

Target 2) make vehicle travel as much as possible on the center line expecting road area, to reduce the danger that vehicle and road edge or barrier collide;

Target 3) make vehicle centroid side drift angle β be not more than the critical value β of vehicle generation rollover _rollover;

Target 4) vehicle is travelled path short as far as possible, to reduce the oil consumption of vehicle;

Target 5) ensure that the front wheel angle of the controlled quentity controlled variable that controller exports and vehicle is steady all the time, avoid occurring excessive control action.

Target 6) make front wheel angle and rate of change thereof not higher than the saturation value of steering mechanism;

According to above-mentioned control objectives, adopt model predictive control method to carry out the design of the domain type path following control model of vehicle, detailed process is as follows:

Known, the speed of a motor vehicle is a continuous quantity slowly changed, so the present invention makes following hypothesis: suppose that autonomous land vehicle keeps constant speed drive in a prediction time domain.

The Differential Model that formula (19) is Vehicular system is the design of the domain type path following control model for vehicle, needs, by formula (19) discretize, to obtain the Vehicular system model of discrete time, shown in (20):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

In formula, $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ Wherein T _sfor the sampling time.

Assuming that prediction time domain is P, control time domain is N, and meets N≤P.The controlled quentity controlled variable that supposition controls outside time domain simultaneously remains unchanged, i.e. δ _f(k+N-1)=δ _f(k+N)=...=δ _f(k+P-1), then based on the Vehicular system model of discrete time in formula (20), the status predication equation of P step can be derived, concrete Ru shown in (21):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

The prediction simultaneously deriving P step exports, shown in (22):

$\left\{\begin{array}{c}{y}_o(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_o(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_o(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(22\right)$

Definition control inputs sequence U (k) and control forecasting output sequence Y (k+1|k) are respectively:

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_o(k+1)\\ y_o(k+2)\\ .\\ .\\ .\\ y_o(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

According to the above-mentioned analysis about control objectives, one of them control objectives known makes autonomous land vehicle as far as possible along expecting that the center line of road area travels, so invention defines such as formula reference input sequence R (k) shown in (24):

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

In formula, y _r(k+i), i=1 ..., P is the discrete magnitude expecting road area center line f (x), and discrete interval is v (k) T _s.So, vehicle is controlled as far as possible along expecting that this control objectives that road area center line travels realizes by the objective function minimized in formula (25):

J ₁＝‖Y(k+1|k)-R(k)‖ ²(25)

Target 4) to point out: domain type path following control model should have the function of the controlled route or travel by vehicle of the shortizationest, realizes the object of energy-saving and emission-reduction with this.

When adopting the domain type path following control model of model predictive control method design vehicle, this demand for control realizes, shown in (26) by minimizing the objective function be made up of the displacement of vehicle traveling:

$J_2=\underset{i=1}{\overset{P}{\Σ}}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(26\right)$

In formula,

Δx _d(k+i)＝v(k)·T _s,i＝1,…,P

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1),i＝1,…,P；

Wherein, Δ x _d(k+i) be the length travel that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m; Δ y _d(k+i) be the lateral shift that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m.Simultaneously target 5) to point out: the size of the controlled quentity controlled variable that reply controller exports and vehicle front wheel angle is controlled, and makes it can not be excessive, turns to ride comfort with what ensure controlled vehicle.Therefore, the present invention's formula (27) that will be made up of control inputs sequence U (k) is as an optimization aim of Controlling model:

J ₃＝‖U(k)‖ ²(27)

For such one, there is J ₁, J ₂and J ₃the optimization problem of three optimization aim, need introduce the demand conflict of weight coefficient to each optimization aim and weigh and process, to obtain a most suitable optimum results.Therefore, the optimization aim of the domain type path following control model based on Model Predictive Control of the present invention's design is:

$J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)---\left(28\right)$

In formula, Γ _yand Γ _ufor weighting matrix; Γ _d,ifor weight factor.

Know again, domain type path following control needs the most important control objectives realized to be ensure that controlled vehicle travels all the time in connecting way region.For this reason, the present invention exports system when adopting the domain type path following control model of model predictive control method design vehicle and uses restraint.Inequality relation in convolution (6), this output constraint can be written to such as formula the form shown in (29):

$\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\≤y_o\left(k+i\right)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i\left)\right)\end{array},i=1,...,P---\left(29\right)$

In formula, ψ (k+i)=C _ψx (k+i), and C _ψ=[0 10 0]; β (k+i)=C _βx (k+i), and C _β=[0 00 1]; F ' _l(k+i) for expecting road area left side boundary line f ' _l(x) in the sampled value of moment k+i, unit, m; f _r' (k+i) then for expecting boundary line f on the right of road area _r' (x) in the sampled value of moment k+i, unit, m.

There is the saturated situation of machinery in the topworks that turns to of vehicle, excessive or too fast controlled quentity controlled variable can not be applied on controlled vehicle effectively.Invalid controlled quentity controlled variable is provided for avoiding controller, the present invention proposes target 6): make front wheel angle and rate of change thereof not higher than the saturation value of steering mechanism, this then needs the control constraints considering when adopting the domain type path following control model of model predictive control method design vehicle to be shown below and controlling increment constraint:

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

In formula, δ _f(k+i) be k+i moment vehicle front wheel angle, unit, rad; δ _fsatfor turning to the maximum front wheel angle achieved by topworks, unit, rad; Δ δ _f(k+i)=δ _f(k+i)-δ _f(k+i-1) be the front wheel angle increment in k+i moment, unit, rad; Δ δ _fsatturn to the maximum front wheel angle increment achieved by topworks, unit, rad.Be the lateral stability improving vehicle simultaneously, reduce the risk that rollover occurs for it, the domain type path following control model that the present invention proposes to design should make vehicle centroid side drift angle β be not more than the critical value β of vehicle generation rollover as far as possible _rollover.Therefore, following state constraint is considered when adopting the domain type path following control model of model predictive control method design vehicle:

|β(k+i)|≤β _rollover,i＝1,…,P (31)

In sum, the domain type path following control model of the present invention's vehicle of adopting model predictive control method to design can be organized into following form:

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min }J\\ J={||{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)||}^2+{||{\mathrm{\Γ}}_{u}U\left(k\right)||}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}({||{\mathrm{\Δx}}_d\left(k+i\right)||}^2+{||{\mathrm{\Δy}}_d\left(k+i\right)||}^2)\end{array}\right.---\left(32\right)$

Meet: $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\Δ\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x(k+i)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

In formula:

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx _d(k+i)＝v·T _s；

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1)；

Δδ _f(k+i)＝δ _f(k+i)-δ _f(k+i-1)；

C _ψ＝[0 1 0 0],C _β＝[0 0 1 0]；

The optimization problem solved in formula (32) can obtain an optimum control sequence U ^*, the ultimate principle of combination model PREDICTIVE CONTROL, the present invention chooses the front wheel angle of controlled quentity controlled variable and current time the best for:

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right)$

Namely optimal control sequence U is chosen ^*first amount be applied on controlled vehicle as controlled quentity controlled variable.To subsequent time, the domain type path following control model based on Model Predictive Control will recalculate an optimum control amount according to current vehicle condition, reciprocal with this, namely achieve rolling optimization and control.

Step 6, front wheel angle according to the current time the best provided in step 5 control turns to topworks's action, makes the front wheel angle of controlled vehicle equal the front wheel angle of current time the best thus controlled vehicle is travelled in connecting way region in a certain segment distance of vehicle front that vehicle sensory perceptual system provides, the control objectives of feasible region path trace.

For verifying the validity of designed path following control model, the domain type path following control model based on Model Predictive Control is built under Matlab/Simulink environment, adopt high-precision vehicle dynamics simulation software veDYNA as controlled device, carry out off-line simulation and analyze simulation result.

Table 1HQ430 auto model parameter

The Vehicular system model adopted in emulation experiment is red flag HQ430 model, and its major parameter is as shown in table 1.Experiment road condition as shown in Figure 8, is asphalt roads.For fully verifying the performance of designed path following control model, the present invention has carried out two groups of emulation experiments altogether;

One group is carry out under the bituminous pavement operating mode of drying, and the coefficientoffrictionμ of road surface and tire is taken as 0.9.In experimentation, first vehicle carries out Acceleration of starting, then carries out constant speed drive with the longitudinal velocity of 60km/h and 85km/h respectively.

Another group is then carry out under the bituminous pavement operating mode of humidity, and the coefficientoffrictionμ of road surface and tire is taken as 0.6.In experimentation, first vehicle carries out Acceleration of starting, then carries out constant speed drive with the longitudinal velocity of 60km/h and 82km/h respectively.

Two groups of simulation results are respectively as shown in accompanying drawing 9 and accompanying drawing 10: accompanying drawing 9 is the simulation result of first group of emulation, namely the simulation result under dry asphalt roads operating mode (μ=0.9), wherein Fig. 9 a is the driving path of controlled vehicle, Fig. 9 b is the controlled quentity controlled variable that controller optimization goes out, the i.e. front wheel angle of vehicle, Fig. 9 c is the side slip angle of vehicle, and Fig. 9 d is the yaw velocity of vehicle; Accompanying drawing 10 is the simulation result under moist bituminous pavement operating mode (μ=0.6) of second group, wherein Figure 10 a is the driving path of controlled vehicle, Figure 10 b is the controlled quentity controlled variable that controller optimization goes out, the i.e. front wheel angle of vehicle, Figure 10 c is the side slip angle of vehicle, and Figure 10 d is the yaw velocity of vehicle.

Through off-line simulation, can find out that the domain type path following control model based on Model Predictive Control that the present invention designs can control controlled vehicle and travel all the time in given road area, the front wheel angle of optimization is lower than the saturation value of steering mechanism, can ensure driving safety and the lateral stability of controlled vehicle, and the change of road pavement friction factor has certain robustness simultaneously.

Claims (5)

1. the domain type path tracking control method of an autonomous land vehicle, Ride Control System in autonomous land vehicle is first according to the front wheel angle of running state information optimization current time the best of the connecting way area information provided after sensory perceptual system scan process and vehicle self, then according to this front wheel angle control vehicle turn to topworks's action, to make vehicle travel in feasible road area, it is characterized in that step is as follows:

Step one, set up two-dimentional road vehicle model:

Set up two-dimentional road vehicle model, if rigid rod RF is auto model, it crosses the barycenter o of vehicle, and length equals length of wagon l, expects road area then by expectation road area left side boundary line f _l' (x), expect boundary line f on the right of road area _r' (x) and expect that road area center line f (x) the expectation road area that forms represents, and meet:

$\left\{\begin{array}{c}{f}_l^{\′}\left(x\right)=f_l\left(x\right)-\frac{w}{2}\\ f_r^{\′}\left(x\right)=f_r\left(x\right)+\frac{w}{2}\\ f\left(x\right)=\frac{f_l\left(x\right)+f_r\left(x\right)}{2}\end{array}\right.---\left(1\right)$

In formula, f _lx () is the left margin in connecting way region in front one segment distance that obtained by the sensory perceptual system scan process in autonomous land vehicle; f _rx () is the right margin in connecting way region in front one segment distance that obtained by the sensory perceptual system scan process in autonomous land vehicle; f _l' (x) for expect boundary line, the road area left side, f _r' (x) for expecting boundary line on the right of road area, w is vehicle width, unit, m;

Step 2, set up the mathematical model of the domain type path trace problem of vehicle:

Based on the two-dimentional road vehicle model that step one is set up, ensure that rigid rod RF is in all the time by expectation road area left side boundary line f _l' (x), expect boundary line f on the right of road area _r' (x) and expect that in road area center line f (x) the expectation road area that forms, in conjunction with geometry and the physical characteristics of rigid rod, the mathematical model setting up the domain type path trace problem of vehicle is as follows:

$\{\begin{array}{c}{f}_r^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)-l_f(\mathrm{\ψ}+\mathrm{\β})\\ f_r^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\≤y_o\≤f_l^{\′}\left(x\right)+l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}---\left(6\right)$

In formula, y _ofor the lateral position of vehicle centroid o, unit, m; l _ffor vehicle centroid o is to the distance of vehicle front point F, unit, m; l _rfor vehicle centroid o is to the distance of rear vehicle end point R, unit, m; ψ is Vehicular yaw angle, unit, rad; β is vehicle centroid side drift angle, unit, rad;

Connecting way regional edge boundary line f in step 3, calculating vehicle front one segment distance _l(x) and f _r(x):

Assuming that the sensory perceptual system of autonomous land vehicle can the point sequence information (x of connecting way zone boundary of Real-time Obtaining du vehicule _r, y _r, x _l, y _l), the boundary line function adopting three Lagrange's interpolation formulas to obtain connecting way region in vehicle front one segment distance based on binary search algorithm is:

$\left\{\begin{array}{c}{f}_r\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_r(i\left)\right)}{\left(x_r\right(p)-x_r(i\left)\right)}{y}_r\left(p\right)\\ f_l\left(x\right)=\Σ\underset{i\≠p}{\Π}\frac{(x-x_l(i\left)\right)}{\left(x_l\right(p)-x_l(i\left)\right)}{y}_l\left(p\right)\end{array}\right.,\begin{array}{c}p=j,n,m,k;\\ i=j,n,m,k;\end{array}---\left(7\right)$

In formula, (x _r(i), y _r(i), x _l(i), y _l(i)), i=j, n, m, k are the point sequence (x of connecting way zone boundary _r, y _r, x _l, y _l) in four groups of coordinate points;

Step 4, set up autonomous land vehicle system model and arranged as state space form:

$\{\begin{array}{c}\stackrel{\·}{x}=Ax+{\mathrm{B\δ}}_f\\ y_o=Cx\end{array}---\left(19\right)$

In formula,

$A=\left[\begin{array}{cccc}0& v& v& 0\\ 0& 0& 0& 1\\ 0& 0& \frac{2(C_f+C_r)}{mv}& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1\\ 0& 0& \frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}& \frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}\end{array}\right],B=\left[\begin{array}{c}0\\ 0\\ -\frac{2C_f}{mv}\\ -\frac{2{\mathrm{aC}}_f}{I_z}\end{array}\right],C=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right];$

In formula, x is system state vector, and x=[y _oψ β r] ^t; δ _ffor vehicle front wheel angle, be also system control amount, unit, rad; y _ofor system exports; A is system matrix; B is input matrix; C is output matrix; V is the speed at vehicle centroid place, unit, m/s; R is the yaw velocity of vehicle, unit, rad/s; C _ffor the cornering stiffness of vehicle front tyre, unit, N/rad; C _rbe respectively the cornering stiffness of vehicle rear wheel tire, unit, N/rad; M is the quality of vehicle, unit, kg; I _zfor vehicle is around the moment of inertia of z-axis, unit, kgm ²; A is the distance of vehicle centroid o to automobile front-axle, unit, m; B is the distance of vehicle centroid o to vehicle rear axle, unit, m;

The domain type path following control model of step 5, employing model predictive control method design vehicle is:

$\left\{\begin{array}{c}\underset{{\mathrm{\δ}}_f(k+i)}{\min J}\\ J=\left|\right|{\mathrm{\Γ}}_y\left(Y\right(k+1|k)-R(k\left)\right)|{|}^2+|\left|{\mathrm{\Γ}}_{u}U\left(k\right)\right|{|}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i\left)\right|{|}^2+\left|\right|{\mathrm{\Δy}}_d\left(k+i\right)\left|{|}^2\right)\end{array}\right.---\left(32\right)$

Meet: $\left\{\begin{array}{c}x(k+i+1)=A_{c}x(k+i)+B_c{\mathrm{\δ}}_f(k+i)\\ y_o(k+i)=C_{c}x(k+i)\\ \left|\mathrm{\Δ}{\mathrm{\δ}}_f\right(k+i\left)\right|\≤\mathrm{\Δ}{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ f_r^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ f_r^{\′}(k+i)+l_r(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_r(C_{\mathrm{\ψ}}+C_{\mathrm{\β}})x(k+i)\\ \left|C_{\mathrm{\β}}x\right(k+i\left)\right|\≤\left|{\mathrm{\β}}_{rollover}\right|\end{array}\right.$

In formula:

$Y(k+1|k)=\left[\begin{array}{c}{y}_0(k+1)\\ y_0(k+2)\\ .\\ .\\ .\\ y_0(k+P)\end{array}\right],R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right],U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right];$

$A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C;$

Δx _d(k+i)＝v(k)·T _s；

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1)；

Δδ _f(k+i)＝δ _f(k+i)-δ _f(k+i-1)；

C _ψ＝[0 1 0 0],C _β＝[0 0 1 0]；

And choose the front wheel angle of controlled quentity controlled variable and current time the best for:

${\mathrm{\δ}}_f^{*}=U*\left(1\right)---\left(33\right)$

δ in formula _f(k+i) be the system control amount in k+i moment, be the front wheel angle of vehicle, unit, rad;

The system state vector that x (k+i) is the k+i moment;

Y _o(k+i) for the system in k+i moment exports, i.e. the lateral position of vehicle centroid, unit, m;

P is prediction time domain, and N is for controlling time domain;

Γ _yand Γ _ufor weighting matrix;

Γ _d,ifor weight factor;

Y _r(k+i), i=1 ..., P is the discrete magnitude expecting road area center line f (x), and discrete interval is v (k) T _s, unit, m;

Δ x _d(k+i) be the length travel that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

Δ y _d(k+i) be the lateral shift that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

Δ δ _f(k+i) be the controlling increment in k+i moment, unit, rad;

δ _fsatfor turning to the maximum front wheel angle achieved by topworks, unit, rad;

Δ δ _fsatturn to the maximum front wheel angle increment achieved by topworks, unit, rad;

F _l' (k+i) for expect road area left side boundary line f _l' (x) in the sampled value of moment k+i, unit, m;

F _r' (k+i) then for expecting boundary line f on the right of road area _r' (x) in the sampled value of moment k+i, unit, m;

β _rolloverfor the critical quantity of rollover may occur vehicle, unit, rad;

T _sfor the sampling time, unit s;

U ^*for the optimal control sequence obtained by Optimization Solution;

Step 6, front wheel angle according to the current time the best provided in step 5 control turns to topworks's action, makes the front wheel angle of controlled vehicle equal the front wheel angle of current time the best thus controlled vehicle is travelled in connecting way region in vehicle front one segment distance that vehicle sensory perceptual system provides, the control objectives of feasible region path trace.

2. according to the domain type path tracking control method of a kind of autonomous land vehicle according to claim 1, it is characterized in that, the detailed process of step 2 is:

For ensureing that rigid rod RF is in all the time by expectation road area left side boundary line f _l' (x), expect boundary line f on the right of road area _r' (x) and expect in road area center line f (x) the expectation road area that forms, need ensure that the relation described in following formula (2) is set up:

$\left\{\begin{array}{c}{f}_r^{\′}\left(x\right)\≤y_F\≤f_l^{\′}\left(x\right)\\ f_r^{\′}\left(x\right)\≤y_R\≤f_l^{\′}\left(x\right)\end{array}\right.---\left(2\right)$

In formula, y _ffor the lateral position of rigid rod RF forward terminal F, unit, m; y _rfor the lateral position of rigid rod RF aft terminal R, unit, m;

There is following geometric relationship in the forward terminal F of rigid rod RF and aft terminal R and barycenter o:

$\left\{\begin{array}{c}{y}_F=y_o+l_{f}sin(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r\sin (\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(3\right)$

In formula, y _ofor the lateral position of vehicle centroid o, unit, m; l _ffor vehicle centroid o is to the distance of vehicle front point F, unit, m; l _rfor vehicle centroid o is to the distance of rear vehicle end point R, unit, m; ψ is Vehicular yaw angle, unit, radian (rad); β is vehicle centroid side drift angle, unit, rad;

Consider that the distance that the sensory perceptual system of autonomous land vehicle can observe at every turn is approximately 50m, and the curvature of road is also mostly smaller, so yaw angle ψ when thinking that vehicle travels in this section of region is very little, consider that again the side slip angle β of vehicle is very little, the present invention adopts following approximation relation:

$\left\{\begin{array}{c}\sin (\mathrm{\ψ}+\mathrm{\β})\≈\mathrm{\ψ}+\mathrm{\β}\\ \cos (\mathrm{\ψ}+\mathrm{\β})\≈1\end{array}\right.---\left(4\right)$

And then formula (3) can be reduced to:

$\left\{\begin{array}{c}{y}_F=y_o+l_f(\mathrm{\ψ}+\mathrm{\β})\\ y_R=y_o-l_r(\mathrm{\ψ}+\mathrm{\β})\end{array}\right.---\left(5\right)$

Formula (5) is updated in formula (2), arranges the mathematical modulo pattern (6) that can obtain the domain type path trace problem of the vehicle described in step 2.

3. according to the domain type path tracking control method of a kind of autonomous land vehicle according to claim 1, it is characterized in that, the detailed process of step 3 is:

In formula (7) in step 3, (x _r(i), y _r(i), x _l(i), y _l(i)), i=j, n, m, k are the point sequence (x of connecting way zone boundary _r, y _r, x _l, y _l) in four groups of coordinate points, choosing of these four groups of coordinate points, carries out based on binary search algorithm, and the object of binary search is the starting point in order to obtain the connecting way region in vehicle front one segment distance with terminal the concrete derivation of binary search algorithm is as follows:

When not considering reversing, suppose that the position coordinates at the current place of controlled vehicle is (x _o, y _o), get as the starting point of search, then point horizontal ordinate x _rand point (0) horizontal ordinate x _l(0) must be negative value, the object of this time search finds to be positioned at vehicle centroid o rear and one group of point that range points o is nearest in the X-axis direction then point horizontal ordinate x _r(j) and point horizontal ordinate x _lj () must meet formula (8):

$\left\{\begin{array}{c}{x}_r\left(j\right)\·x_r(j+1)\≤0\\ x_l\left(j\right)\·x_l(j+1)\≤0\end{array}\right.---\left(8\right)$

In formula, x _r(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(j+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate;

Search out the one group of point meeting formula (8) after, their information is stored, then when carrying out second time and searching for, starting point using them as search, consider the control signal be applied on Autonomous Vehicles, its useful effect phase is generally about 1s, only consider that vehicle front length is one section of road area of v, in formula, v is the speed at vehicle centroid place, therefore, and the impact point of this search horizontal ordinate must meet inequality relation in formula (9):

$\left\{\begin{array}{c}\left(x_r\right(k)-v)\·\left(x_r\right(k+1)-v)\≤0\\ \left(x_l\right(k)-v)\·\left(x_l\right(k+1)-v)\≤0\end{array}\right.---\left(9\right)$

In formula, x _r(k) for point horizontal ordinate; x _l(k) for point horizontal ordinate; x _r(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate; x _l(k+1) for being positioned at a little front and the point nearest apart from it horizontal ordinate;

By the point searched with as two groups of interpolation points of formula (7), selected point simultaneously with as other two groups of interpolation points of formula (7), j, k, n and m is made to represent above-mentioned four groups of interpolation points respectively at known road point sequence (x _r, y _r, x _l, y _l) position, the relation between j, k, n and m is as follows:

4. according to the domain type path tracking control method of a kind of autonomous land vehicle according to claim 1, it is characterized in that, the detailed process of step 4 is:

(1) vehicle kinematics model is set up

Assuming that vehicle is a rigid body, wherein install the wheel that four deformation can not occur, and using vehicle front-wheel as deflecting roller, obtain the kinematics model of vehicle according to kinematical equation and geometric relationship as shown in formula (11):

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=vcos(\mathrm{\ψ}+\mathrm{\β})\\ {\stackrel{\·}{y}}_o=vsin(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(11\right)$

In formula, x _ofor the lengthwise position of vehicle centroid o, unit, m; y _ofor the lateral position of vehicle centroid o, unit, m; V is the speed at vehicle centroid place, unit, m/s; R is the yaw velocity of vehicle, unit, rad/s, formula (4) is updated in formula (11), then the vehicle kinematics model that can be simplified, shown in (12):

$\left\{\begin{array}{c}{\stackrel{\·}{x}}_o=v\\ {\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\end{array}\right.---\left(12\right)$

(2) vehicle dynamic model is set up

If vehicle centroid o is true origin in vehicle dynamic model, along the positive dirction that vehicle body direction is forward transverse axis x, perpendicular to the positive dirction that the direction on X direction is longitudinal axis y, this method is pros by the controlling vehicle always realizing route objects of following the tracks of, so ignore the longitudinal dynamics of vehicle, the lateral dynamics of consideration vehicle and the dynamics of yaw direction, according to Newton second law and equalising torque relation, can obtain such as formula the vehicle dynamic model shown in (13):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{xf}{\mathrm{sin\β}}_f+F_{yf}{\mathrm{cos\β}}_f+F_{yr}\\ I_z\stackrel{\·}{r}=a(F_{xf}{\mathrm{sin\δ}}_f+F_{yf}{\mathrm{cos\δ}}_f)-{\mathrm{bF}}_{yr}\end{array}\right.---\left(13\right)$

In formula, v _xfor the longitudinal velocity at vehicle centroid place, unit, m/s; F _yffor vehicle front-wheel side force, unit, N; F _yrfor vehicle rear wheel side force, unit, N; M is the quality of vehicle, unit, kg; I _zfor vehicle is around the moment of inertia of z-axis, unit, kgm ²; A is the distance of vehicle centroid o to automobile front-axle, unit, m; B is the distance of vehicle centroid o to vehicle rear axle, unit, m; δ _ffor vehicle front wheel angle, unit, rad, the front wheel angle δ of vehicle _fvery little, formula (13) can be simplified, the vehicle dynamic model after simplification is such as formula shown in (14):

$\left\{\begin{array}{c}{\mathrm{mv}}_x(\stackrel{\·}{\mathrm{\β}}+r)=F_{yf}+F_{yr}\\ I_z\stackrel{\·}{r}={\mathrm{aF}}_{yf}-{\mathrm{bF}}_{yr}\end{array}\right.---\left(14\right)$

Assuming that vehicle side does not reach capacity to tire force, now side force F _ysubstantially linear with slip angle of tire α, shown in (15):

$\left\{\begin{array}{c}{F}_{yf}=2C_f{\mathrm{\α}}_f\\ F_{yr}=2C_r{\mathrm{\α}}_r\end{array}\right.---\left(15\right)$

In formula, C _ffor the tire cornering stiffness of vehicle front-wheel, unit, Nrad; C _rfor the tire cornering stiffness of vehicle rear wheel, unit, Nrad; α _ffor the slip angle of tire of vehicle front-wheel, unit, rad; α _rfor the slip angle of tire of vehicle rear wheel, unit, rad, according to the regulation of coordinate system, the slip angle of tire α of front-wheel _fwith the slip angle of tire α of trailing wheel _rbe respectively:

$\left\{\begin{array}{c}{\mathrm{\α}}_f=\mathrm{\β}+\frac{ar}{v_x}-{\mathrm{\δ}}_f\\ {\mathrm{\α}}_f=\mathrm{\β}+\frac{br}{v_x}\end{array}\right.---\left(16\right)$

Convolution (14), (15) and (16), arrangement can obtain binary vehicle dynamic model, shown in (17):

$\left\{\begin{array}{c}\stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{{\mathrm{mv}}_x}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}_x^2}-1)r-\frac{2C_f}{{\mathrm{mv}}_x}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f+{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_z{v}_x}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(17\right)$

(3) state-space model of Vehicular system is set up

Convolution (12) and formula (17), consider simultaneously then can obtain Vehicular system motion and dynamic (dynamical) differential equation, specifically such as formula shown in (18):

$\left\{\begin{array}{c}{\stackrel{\·}{y}}_o=v(\mathrm{\ψ}+\mathrm{\β})\\ \stackrel{\·}{\mathrm{\ψ}}=r\\ \stackrel{\·}{\mathrm{\β}}=\frac{2(C_f+C_r)}{mv}\mathrm{\β}+(\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{{\mathrm{mv}}^2}-1)r-\frac{2C_f}{mv}{\mathrm{\δ}}_f\\ \stackrel{\·}{r}=\frac{2({\mathrm{aC}}_f-{\mathrm{bC}}_r)}{I_z}\mathrm{\β}+\frac{2(a^2{C}_f+b^2{C}_r)}{I_{z}v}r-\frac{2{\mathrm{aC}}_f}{I_z}{\mathrm{\δ}}_f\end{array}\right.---\left(18\right)$

By controlling the front wheel angle of vehicle and then ensureing that the lateral position of vehicle meets the inequality constrain in formula (6), choose the lateral position y of vehicle centroid o _oexport as system, choose front wheel angle δ simultaneously _fas system control amount, system state vector is chosen for x=[y _oψ β r], Vehicular system model can be described as the state-space model shown in step 4 Chinese style (19).

5. according to the domain type path tracking control method of a kind of autonomous land vehicle according to claim 1, it is characterized in that, the detailed process of step 5 is:

Suppose that autonomous land vehicle keeps constant speed drive in a prediction time domain, the Differential Model that formula (19) in step 4 is Vehicular system, in order to the design of the domain type path following control model for vehicle, need formula (19) discretize, obtain the Vehicular system model of discrete time, shown in (20):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ y_o\left(k\right)=C_{c}x\left(k\right)\end{array}\right.---\left(20\right)$

In formula, $A_c=e^{{\mathrm{AT}}_s},B_c={\∫}_0^{T_s}{e}^{A\mathrm{\τ}}d\mathrm{\τ}\·B,C_c=C,$ T in formula _sfor the sampling time;

Assuming that prediction time domain is P, control time domain is N, and meets N≤P, and the controlled quentity controlled variable that supposition controls outside time domain simultaneously remains unchanged, i.e. δ _f(k+N-1)=δ _f(k+N)=...=δ _f(k+P-1), then the status predication equation of P step is gone out based on the Vehicular system model inference of discrete time in formula (20), concrete Ru shown in (21):

$\left\{\begin{array}{c}x(k+1)=A_{c}x\left(k\right)+B_c{\mathrm{\δ}}_f\left(k\right)\\ x(k+2)=A_{c}x(k+1)+B_c{\mathrm{\δ}}_f(k+1)\\ =A_c^{2}x\left(k\right)+A_c{B}_c{\mathrm{\δ}}_f\left(k\right)+B_c{\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ x(k+N)=A_c^{N}x\left(k\right)+A_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+B_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ x(k+P)=A_c^{P}x\left(k\right)+A_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}\right.---\left(21\right)$

The prediction simultaneously deriving P step exports, shown in (22):

$\{\begin{array}{c}{y}_0(k+1)=C_c{A}_{c}x\left(k\right)+C_c{B}_c{\mathrm{\δ}}_f\left(k\right)\\ .\\ .\\ .\\ y_0(k+N)=C_c{A}_c^{N}x\left(k\right)+C_c{A}_c^{N-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+C_c{B}_c{\mathrm{\δ}}_f(k+N-1)\\ .\\ .\\ .\\ y_0(k+P)=C_c{A}_c^{P}x\left(k\right)+C_c{A}_c^{P-1}{B}_c{\mathrm{\δ}}_f\left(k\right)+...+\underset{i=1}{\overset{P-N+1}{\Σ}}{C}_c{A}_c^{i-1}{B}_c{\mathrm{\δ}}_f(k+N-1)\end{array}---\left(22\right)$

Definition control inputs sequence U (k) and control forecasting output sequence Y (k+1|k) are respectively:

$\left\{\begin{array}{c}U\left(k\right)=\left[\begin{array}{c}{\mathrm{\δ}}_f\left(k\right)\\ {\mathrm{\δ}}_f(k+1)\\ .\\ .\\ .\\ {\mathrm{\δ}}_f(k+N-1)\end{array}\right]\\ Y(k+1|k)=\left[\begin{array}{c}{y}_0(k+1)\\ y_0(k+2)\\ .\\ .\\ .\\ y_0(k+P)\end{array}\right]\end{array}\right.---\left(23\right)$

In order to make autonomous land vehicle as far as possible along expecting that the center line of road area travels, define such as formula reference input sequence R (k) shown in (24):

$R\left(k\right)=\left[\begin{array}{c}{y}_r(k+1)\\ y_r(k+2)\\ .\\ .\\ .\\ y_r(k+P)\end{array}\right]---\left(24\right)$

In formula, y _r(k+i), i=1 ..., P is the discrete magnitude expecting road area center line f (x), and discrete interval is v (k) T _s, in order to control vehicle as far as possible along expectation road area center line, realize by the objective function minimized in formula (25):

J ₁＝‖Y(k+1|k)-R(k)‖ ²(25)

In order to the function making domain type path following control model have the controlled route or travel by vehicle of the shortizationest, when adopting the domain type path following control model of model predictive control method design vehicle, realize, shown in (26) by minimizing the objective function be made up of the displacement of vehicle traveling:

$J_2=\underset{i=1}{\overset{P}{\Σ}}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i)|{|}^2+|\left|{\mathrm{\Δy}}_d(k+i)\right|{|}^2)---\left(26\right)$

In formula,

Δx _d(k+i)＝v(k)·T _s,i＝1,…,P

Δy _d(k+i)＝y _o(k+i)-y _o(k+i-1),i＝1,…,P；

In formula, Δ x _d(k+i) be the length travel that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m; Δ y _d(k+i) be the lateral shift that vehicle travels within this period of time of (k+i-1) ~ (k+i), unit, m;

In order to be controlled the control action of controller, make it can not be excessive, turn to ride comfort with what ensure controlled vehicle, the formula (27) that will be made up of control inputs sequence U (k) be as an optimization aim of Controlling model:

J ₃＝‖U(k)‖ ²(27)

Introduce weight coefficient to J ₁, J ₂and J ₃the demand of three optimization aim carries out balance process, and the optimization aim of the domain type path following control model of the vehicle of design is:

$J=\left|\right|{\mathrm{\Γ}}_y\left(Y\right(k+1\left|k\right)-R\left(k\right)\left)\right|{|}^2+\left|\right|{\mathrm{\Γ}}_{u}U\left(k\right)|{|}^2+\underset{i=1}{\overset{P}{\Σ}}{\mathrm{\Γ}}_{d,i}\left(\right|\left|{\mathrm{\Δx}}_d\right(k+i)|{|}^2+\left|\right|{\mathrm{\Δy}}_d(k+i)\left|{|}^2\right)---\left(28\right)$

In formula, Γ _yand Γ _ufor weighting matrix; Γ _d,ifor weight factor;

In order to ensure that controlled vehicle travels all the time in connecting way region, when adopting the domain type path following control model of model predictive control method design vehicle, system is exported and use restraint, inequality relation in integrating step two Chinese style (6), this output constraint can be written to such as formula the form shown in (29):

$\left\{\begin{array}{c}{f}_r^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\≤y_o(k+i)\≤f_l^{\′}(k+i)-l_f\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\\ f_r^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\≤y_o(k+i)\≤f_l^{\′}(k+i)+l_r\left(\mathrm{\ψ}\right(k+i)+\mathrm{\β}(k+i))\end{array}\right.,i=1,...,P---\left(29\right)$

In formula, ψ (k+i)=C _ψx (k+i), and C _ψ=[0 10 0]; β (k+i)=C _βx (k+i), and C _β=[0 00 1]; f _l' (k+i) for expect road area left side boundary line f _l' (x) in the sampled value of moment k+i, unit, m; f _r' (k+i) then for expecting boundary line f on the right of road area _r' (x) in the sampled value of moment k+i, unit, m;

For making vehicle front wheel angle and rate of change thereof not higher than the saturation value of steering mechanism, the control constraints that consideration is shown below when adopting the domain type path following control model of model predictive control method design vehicle and controlling increment constraint:

$\left\{\begin{array}{c}\left|{\mathrm{\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\\ \left|{\mathrm{\Δ\δ}}_f(k+i)\right|\≤{\mathrm{\δ}}_{fsat}\end{array}\right.,i=1,...,N---\left(30\right)$

In formula, δ _f(k+i) be k+i moment vehicle front wheel angle, unit, rad; δ _fsatfor turning to the maximum front wheel angle achieved by topworks, unit, rad; Δ δ _f(k+i)=δ _f(k+i)-δ _f(k+i-1) be the front wheel angle increment in k+i moment, unit, rad; Δ δ _fsatturn to the maximum front wheel angle increment achieved by topworks, unit, rad;

For improving the lateral stability of vehicle, reduce the risk that rollover occurs for it, the domain type path following control model of design should make vehicle centroid side drift angle β be not more than the critical value β of vehicle generation rollover as far as possible _rollover, therefore, consider following state constraint when adopting the domain type path following control model of model predictive control method design vehicle:

|β(k+i)|≤β _rollover,i＝1,…,P (31)

By arranging formula (25) ~ (31), obtain the formula (32) of the zone routing tracing control model in step 5, the optimization problem solved in formula (32) can obtain an optimum control sequence U ^*, the ultimate principle of combination model PREDICTIVE CONTROL, chooses the front wheel angle of controlled quentity controlled variable and current time the best be the formula (33) described in step 5:

${\mathrm{\δ}}_f^{*}=U^{*}\left(1\right)---\left(33\right).$