CN104977933B - A kind of domain type path tracking control method of autonomous land vehicle - Google Patents
A kind of domain type path tracking control method of autonomous land vehicle Download PDFInfo
- Publication number
- CN104977933B CN104977933B CN201510377605.7A CN201510377605A CN104977933B CN 104977933 B CN104977933 B CN 104977933B CN 201510377605 A CN201510377605 A CN 201510377605A CN 104977933 B CN104977933 B CN 104977933B
- Authority
- CN
- China
- Prior art keywords
- mrow
- msub
- mtr
- mtd
- vehicle
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Active
Links
Abstract
The invention discloses a kind of domain type path tracking control method of autonomous land vehicle, to overcome tracking and controlling method not consider the shape of vehicle and road and size and then cannot be guaranteed the problem of vehicle does not collide with road boundary or peripheral obstacle, step is:Establish two-dimentional road vehicle model;Establish the mathematical modeling of vehicle route tracking problem;Calculate the connecting way regional edge boundary line in the segment distance of vehicle front one;Establish Vehicular system model;Carry out the design of domain type path following control model and choose controlled quentity controlled variable i.e. current time optimal front wheel angle;Executing agency's action is turned to according to optimal front wheel angle control so that controlled vehicle travels in the segment distance of vehicle front one that vehicle sensory perceptual system provides in connecting way region.This method is in the two-dimentional road vehicle model of foundation, it is contemplated that the shape and size of vehicle and road, reduces the possibility that vehicle collides with road boundary, improves the security of autonomous land vehicle.
Description
Technical field
The invention belongs to the control method of autonomous driving technical field, is related to a kind of path trace control of autonomous land vehicle
Method processed.
Background technology
The wide application prospect of autonomous driving technology makes it be increasingly subject to the concern of people.Typical autonomous driving system bag
Include two big functional module of sensory perceptual system and Ride Control System, sensory perceptual system to obtain vehicle-periphery information and vehicle from
The running state information of body, Ride Control System are then that instead of driver and control vehicle traveling.For autonomous driving technology, road
Footpath tracing control is the most key control problem that Ride Control System needs to solve.Existing autonomous land vehicle
Path following control is mostly the motion control method that have references to robot, the road traffic scanned first according to sensory perceptual system
Information planning goes out a feasible track or path-line, then controls this feasible track of vehicle tracking or path-line.These
Control method is all based on greatly the road vehicle modelling of dotted-line style, and some of typical control methods have pure point tracking
Method, take aim at PID and Stanly methods etc. in advance.They can obtain preferable path following control effect, but due in design process
In neglected the shape and size of vehicle and road, so not ensuring that the barrier that vehicle will not be with road boundary or around it
Thing is hindered to collide.
The content of the invention
The problem to be solved in the present invention is to overcome present in the path tracking control method of existing autonomous land vehicle not
Consider the shape of vehicle and road and size and then it cannot be guaranteed that vehicle does not collide with road boundary or its peripheral obstacle
The problem of, there is provided a kind of domain type path tracking control method of autonomous land vehicle.
A kind of domain type path tracking control method of autonomous land vehicle proposed by the present invention is to use following technical side
What case was realized:
A kind of domain type path tracking control method of autonomous land vehicle, the Ride Control System in autonomous land vehicle
It is excellent according to the connecting way area information and the running state information of vehicle itself that are provided after sensory perceptual system scan process first
Current time optimal front wheel angle is dissolved, the steering executing agency for then controlling vehicle according to the front wheel angle acts, so that
Vehicle travels in feasible road area, it is characterised in that step is as follows:
Step 1: establish two-dimentional road vehicle model:
Two-dimentional road vehicle model is established, if rigid rod RF is auto model, it crosses the barycenter o of vehicle, and length is equal to car
Body length l, expected path is then by expectation road area left side boundary line fl' (x), expectation road area the right boundary line fr' (x) and phase
The expectation road area that road area center line f (x) is formed is hoped to represent, and is met:
In formula, fl(x) it is in the segment distance of front one obtained by the sensory perceptual system scan process in autonomous land vehicle
The left margin in connecting way region;fr(x) front one to be obtained by the sensory perceptual system scan process in autonomous land vehicle
The right margin in connecting way region in segment distance;fl' (x) for it is expected road area left side boundary line, fr' (x) for it is expected road area
The right boundary line, w are vehicle width, unit, m;
Step 2: establish the mathematical modeling of the domain type path trace problem of vehicle:
The two-dimentional road vehicle model established based on step 1, ensure that rigid rod RF is in left by expectation road area all the time
Boundary line fl' (x), expectation road area the right boundary line fr' (x) and it is expected road area center line f (x) composition expectation road
In region, with reference to the geometry and physical characteristic of rigid rod, the mathematical modeling for establishing the domain type path trace problem of vehicle is as follows:
In formula, yoFor vehicle centroid o lateral position, unit, m;lfDistance for vehicle centroid o to vehicle front point F,
Unit, m;lrDistance for vehicle centroid o to rear vehicle end point R, unit, m;ψ is Vehicular yaw angle, unit, rad;β is vehicle
Side slip angle, unit, rad;
Step 3: calculate connecting way regional edge boundary line f in the segment distance of vehicle front onelAnd f (x)r(x):
It is assumed that the sensory perceptual system of autonomous land vehicle can obtain the point sequence of the connecting way zone boundary of du vehicule in real time
Column information (xr,yr,xl,yl), xrFor the segment distance of front one obtained by the sensory perceptual system scan process in autonomous land vehicle
The right margin f in interior connecting way regionr(x) the abscissa sequence of the point on;yrTo pass through the sensory perceptual system in autonomous land vehicle
The right margin f in connecting way region in the segment distance of front one that scan process obtainsr(x) the ordinate sequence of the point on;xlIt is logical
The left margin f in connecting way region in the segment distance of front one that the sensory perceptual system scan process crossed in autonomous land vehicle obtainsl
(x) the abscissa sequence of the point on;ylFor one section of the front obtained by the sensory perceptual system scan process in autonomous land vehicle
Left margin f apart from interior connecting way regionl(x) the ordinate sequence of the point on;Used based on binary search algorithm and drawn three times
The border line function that Ge Lang interpolation formulas obtain connecting way region in the segment distance of vehicle front one is:
In formula, (xr(i),yr(i),xl(i),yl(i) it is) point sequence (x of connecting way zone boundaryr,yr,xl,yl) in
Four groups of coordinate points, wherein i=j, n, m, k;
Step 4: it is state space form to establish autonomous land vehicle system model and arranged:
In formula,
In formula, x is system mode vector, and x=[yo ψ β r]T;δfAlso it is system control amount for vehicle front wheel angle,
Unit, rad;yoExported for system;A is sytem matrix;B is input matrix;C is output matrix;V is the speed at vehicle centroid
Degree, unit, m/s;R be vehicle yaw velocity, unit, rad/s;CfFor the cornering stiffness of vehicle front tyre, unit, N/
rad;CrFor the cornering stiffness of vehicle rear wheel tire, unit, N/rad;M be vehicle quality, unit, kg;IzIt is vehicle around z-axis
Rotary inertia, unit, kgm2;A is vehicle centroid o to the distance of automobile front-axle, unit, m;B is vehicle centroid o to vehicle
The distance of rear axle, unit, m;
Step 5: use the domain type path following control model of model predictive control method design vehicle for:
Meet:
In formula:
Δxd(k+i)=v (k) Ts;
Δyd(k+i)=yo(k+i)-yo(k+i-1);
Δδf(k+i)=δf(k+i)-δf(k+i-1);
Cψ=[0 10 0], Cβ=[0 01 0];
And choose controlled quentity controlled variable i.e. current time optimal front wheel angleFor:
δ in formulaf(k+i) for the k+i moment system control amount, as vehicle front wheel angle, unit, rad;
X (k+i) is the system mode vector at k+i moment;
yo(k+i) exported for the system at k+i moment, i.e. the lateral position of vehicle centroid, unit, m;
P is prediction time domain, and N is control time domain;
ΓyAnd ΓuFor weighting matrix;
Γd,iFor weight factor;
yr(k+i), i=1 ..., P are the discrete magnitude for it is expected road area center line f (x), and discrete interval is v (k) Ts,
Unit, m;
Δxd(k+i) it is the length travel of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
Δyd(k+i) it is the lateral displacement of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
Δδf(k+i) it is the controlling increment at k+i moment, unit, rad;
δfsatTo turn to the maximum front wheel angle achieved by executing agency, unit, rad;
ΔδfsatTurn to the maximum front wheel angle increment achieved by executing agency, unit, rad;
fl' (k+i) for it is expected road area left side boundary line fl' (x) in moment k+i sampled value, unit, m;
fr' (k+i) then for it is expected road area on the right of boundary line fr' (x) in moment k+i sampled value, unit, m;
βrolloverThe critical quantity that may be turned on one's side for vehicle, unit, rad;
TsFor sampling time, unit s;
U*For the optimal control sequence obtained by Optimization Solution;
Step 6: according to the current time provided in step 5 optimal front wheel angleControl turns to executing agency and moved
Make so that the front wheel angle of controlled vehicle is equal to current time optimal front wheel angleSo that controlled vehicle is in vehicle sense
Know in the segment distance of vehicle front one that system provides and travelled in connecting way region, realize the control targe of zone routing tracking.
Further technical scheme is:
The detailed process of step 2 is:
To ensure that rigid rod RF is in by expectation road area left side boundary line f all the timel' (x), expectation road area right margin
Line fr' (x) and it is expected road area center line f (x) composition expectation road area in, need to ensure described in following formula (2)
Relation set up:
In formula, yFFor rigid rod RF forward terminals F lateral position, unit, m;yRFor rigid rod RF aft terminals R lateral position
Put, unit, m;
Following geometrical relationship be present in rigid rod RF forward terminal F and aft terminal R and barycenter o:
In formula, yoFor vehicle centroid o lateral position, unit, m;lfDistance for vehicle centroid o to vehicle front point F,
Unit, m;lrDistance for vehicle centroid o to rear vehicle end point R, unit, m;ψ is Vehicular yaw angle, unit, radian (rad);β
For vehicle centroid side drift angle, unit, rad;
Yaw angle ψ and side drift angle β have following approximation relation:
And then formula (3) can be reduced to:
Formula (5) is updated in formula (2), arrange can obtain the domain type path of vehicle described in step 2 with
The mathematical modulo pattern (6) of track problem.
The detailed process of step 3 is:
In formula (7) in step 3, (xr(i),yr(i),xl(i),yl(i) it is) point sequence of connecting way zone boundary
(xr,yr,xl,yl) four groups of coordinate points, wherein i=j, n, m, k, the selection of this four groups of coordinate points is calculated based on binary search
What method was carried out, the purpose of binary search is to obtain the starting point in connecting way region in the segment distance of vehicle front oneWith
TerminalWherein,It is the starting point on the connecting way region right margin in the segment distance of vehicle front one, includes right margin
Two information of abscissa and ordinate of upper starting point;It is on the connecting way region left margin in the segment distance of vehicle front one
Starting point, two information of the abscissa comprising starting point on left margin and ordinate;Be in the segment distance of vehicle front one can trade
Terminal on the right margin of road region, two information of the abscissa comprising terminal on right margin and ordinate;It is vehicle front one
The terminal on the left margin of connecting way region in segment distance, two letters of the abscissa comprising terminal on left margin and ordinate
Breath;
The specific derivation process of binary search algorithm is as follows:
In the case where not considering reversing, it is assumed that the position coordinates that controlled vehicle is currently located is (xo,yo), take
As the starting point of search, then pointAbscissa xrAnd point (0)Abscissa xl(0) it must be negative value, this time the purpose of search is
Find one group of nearest range points o positioned at vehicle centroid o rears and in the X-axis direction pointThen pointAbscissa xr
(j) and pointAbscissa xl(j) formula (8) must be met:
In formula, xr(j+1) it is positioned at pointFront and the point nearest apart from itAbscissa;xl(j+1) it is positioned at pointFront and the point nearest apart from itAbscissa;
Search out one group of point for meeting formula (8)Afterwards, their information is stored, then carried out second
During search, the starting point using them as search, it is contemplated that the control signal being applied on Autonomous Vehicles, its useful effect phase are general
For 1s or so, the road area that the segment length of vehicle front one is d is only considered.In view of the control signal being applied on Autonomous Vehicles,
Its useful effect phase is generally 1s or so, when the speed at vehicle centroid is v, d=v × 1s therefore, this search target
PointAbscissa must meet inequality relation in formula (9):
In formula, xr(k) it is pointAbscissa;xl(k) it is pointAbscissa;xr(k+1) it is positioned at pointFront and away from
The point nearest from itAbscissa;xl(k+1) it is positioned at pointFront and the point nearest apart from itAbscissa;
The point that will be searchedWithAs two groups of interpolation points of formula (7), while selected pointWithAs other two groups of interpolation points of formula (7), make j, k, n and m represent above-mentioned four groups of interpolation points respectively and having learned that waypoint
Sequence (xr,yr,xl,yl) position, the relation between j, k, n and m is as follows:
The detailed process of step 4 is:
(1) vehicle kinematics model is established
It is assumed that vehicle is a rigid body, wherein four wheels that will not be deformed upon are installed, and with crop rotation before vehicle
For deflecting roller, obtained according to kinematical equation and geometrical relationship shown in the kinematics model such as formula (11) of vehicle:
In formula, xoFor vehicle centroid o lengthwise position, unit, m;yoFor vehicle centroid o lateral position, unit, m;V is
Speed at vehicle centroid, unit, m/s;R is the yaw velocity of vehicle, unit, rad/s, formula (4) is updated into formula (11)
In, then the vehicle kinematics model that can be simplified, as shown in formula (12):
(2) vehicle dynamic model is established
If vehicle centroid o is the origin of coordinates in vehicle dynamic model, along the pros that vehicle body forward direction is transverse axis x
To perpendicular to the positive direction that transverse axis upwardly direction is longitudinal axis y, this method is to realize road by controlling the front wheel angle of vehicle
The purpose of footpath tracking, so ignore the longitudinal dynamics of vehicle, the lateral dynamics of a consideration vehicle and moving for yaw direction
Mechanics, according to Newton's second law and equalising torque relation, it can obtain the vehicle dynamic model as shown in formula (13):
In formula, vxFor the longitudinal velocity at vehicle centroid, unit, m/s;FyfFor vehicle front-wheel side force, unit, N;FyrFor
Vehicle rear wheel side force, unit, N;M be vehicle quality, unit, kg;IzRotary inertia for vehicle around z-axis, unit, kg
m2;A is vehicle centroid o to the distance of automobile front-axle, unit, m;B is vehicle centroid o to the distance of vehicle rear axle, unit, m;δf
For vehicle front wheel angle, unit, rad, the front wheel angle δ of vehiclefVery little, formula (13) can be simplified, the vehicle after simplifying moves
Shown in mechanical model such as formula (14):
It is assumed that the lateral tire force of vehicle is not up to saturation, now side force FyIt is substantially linear with slip angle of tire α,
As shown in formula (15):
In formula, CfFor the tire cornering stiffness of vehicle front-wheel, unit, Nrad;CrIt is firm for the Wheel slip of vehicle rear wheel
Degree, unit, Nrad;αfFor the slip angle of tire of vehicle front-wheel, unit, rad;αrIt is single for the slip angle of tire of vehicle rear wheel
Position, rad, according to the regulation of coordinate system, the slip angle of tire α of front-wheelfWith the slip angle of tire α of trailing wheelrRespectively:
Convolution (14), (15) and (16), the vehicle dynamic model that can obtain two degrees of freedom is arranged, as shown in formula (17):
(3) state-space model of Vehicular system is established
Convolution (12) and formula (17), are considered simultaneouslyVehicular system motion and dynamic (dynamical) differential side can then be obtained
Formula, specifically as shown in formula (18):
Meet the inequality constraints in formula (6) by the lateral position for controlling the front wheel angle of vehicle to ensure vehicle,
Choose vehicle centroid o lateral position yoExported as system, while choose front wheel angle δfAs system control amount, system shape
State vector is chosen for x=[yoψ β r], Vehicular system model is described as the state space shown in step 4 Chinese style (19)
Model.
The detailed process of step 5 is:
Assuming that autonomous land vehicle is predicted at one keeps constant speed drive in time domain, the formula (19) in step 4 is vehicle system
The Differential Model of system, in order to the domain type path following control model for vehicle design, it is necessary to by formula (19) discretization, obtain
To the Vehicular system model of discrete time, as shown in formula (20):
In formula,T in formulasFor the sampling time;
It is assumed that prediction time domain is P, it is N to control time domain, and meets N≤P, while assumes to control the controlled quentity controlled variable outside time domain to protect
Hold constant, i.e. δf(k+N-1)=δf(k+N)=...=δf(k+P-1), the then Vehicular system mould based on discrete time in formula (20)
Type derives the status predication equation of P steps, specific as shown in (21):
The prediction output of P steps is derived simultaneously, as shown in formula (22):
Definition control input sequence U (k) and control forecasting output sequence Y (k+1 | k) be respectively:
In order that autonomous land vehicle travels along the center line for it is expected road area as far as possible, define as shown in formula (24)
Reference input sequence R (k):
In formula, yr(k+i) it is expected road area center line f (x) discrete magnitude, wherein i=1 ..., P, discrete interval is
v(k)·Ts, can be by minimizing in formula (25) in order to control vehicle as far as possible along road area center line traveling it is expected
Object function is realized:
J1=‖ Y (k+1 | k)-R (k) ‖2 (25)
In order that domain type path following control model has the function that most shortization is controlled route or travel by vehicle, using mould
During the domain type path following control model of type forecast Control Algorithm design vehicle, displacement structure can be travelled by vehicle by minimizing
Into object function realize, as shown in the formula (26):
In formula,
Δxd(k+i)=v (k) Ts, i=1 ..., P
Δyd(k+i)=yo(k+i)-yo(k+i-1), i=1 ..., P;
In formula, Δ xd(k+i) it is the length travel of vehicle traveling within this period of time of (k+i-1)~(k+i), unit,
m;Δyd(k+i) it is the lateral displacement of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
In order to be controlled by the control action of controller, make it excessive, to ensure that the steering of controlled vehicle smoothes out
Property, will be by an optimization aim of the formula (27) that control input sequence U (k) is formed as Controlling model:
J3=| | u (k) | |2 (27)
Weight coefficient is introduced to J1、J2And J3The demand of three optimization aims carries out balance processing, design it is pre- based on model
The optimization aim of the domain type path following control model of observing and controlling is:
In formula, ΓyAnd ΓuFor weighting matrix;Γd,iFor weight factor;
In order to ensure that controlled vehicle travels in connecting way region all the time, using model predictive control method design car
Domain type path following control model when control system output is used restraint, with reference in step 2 Chinese style (6)
Formula relation, the output constraint can be written to the form as shown in formula (29):
In formula, ψ (k+i)=CψX (k+i), and Cψ=[0100];β (k+i)=CβX (k+i), and Cβ=[0001];fl'(k+
I) it is expectation road area left side boundary line fl' (x) in moment k+i sampled value, unit, m;fr' (k+i) then for it is expected roadway area
Boundary line f on the right of domainr' (x) in moment k+i sampled value, unit, m.
To make vehicle front wheel angle and its rate of change be not higher than the saturation value of steering mechanism, Model Predictive Control side is being used
Control constraints and the controlling increment constraint being shown below are considered during the domain type path following control model of method design vehicle:
In formula, δf(k+i) it is the vehicle front wheel angle at k+i moment, unit, rad;δfsatBy turn to executing agency institute energy reality
Existing maximum front wheel angle, unit, rad;Δδf(k+i)=δf(k+i)-δf(k+i-1) increase for the front wheel angle at k+i moment
Amount, unit, rad;ΔδfsatTurn to the maximum front wheel angle increment achieved by executing agency, unit, rad;
To improve the lateral stability of vehicle, reduce its risk turned on one's side, the domain type path of the vehicle of design with
Track Controlling model should make vehicle centroid side drift angle β be not more than the critical value β that vehicle is turned on one's side as far as possiblerollover, therefore,
Following state constraint is constantly considered using 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), the formula (32) of the zone routing tracing control model in step 5 is obtained, is asked
Optimization problem in solution formula (32) can obtain an optimal control sequence U*, the general principle of binding model PREDICTIVE CONTROL,
It is current time optimal front wheel angle to choose controlled quentity controlled variableFormula (33) as described in step 5:
Compared with prior art, the invention has the advantages that:
1. during the two-dimentional road vehicle model that the present invention establishes, it is contemplated that the shape and size of vehicle and road, reduce car
The possibility to be collided with road boundary, improve the security of autonomous land vehicle.
2. the Controlling model of the present invention is designed based on dynamics of vehicle and kinematic relation, same in low speed and high speed
Sample has preferable path trace performance.
3. the present invention asks intact stability and energy consumption of vehicles in the domain type path following control model of design vehicle
Topic takes into account, while autonomous land vehicle path trace performance is ensured, it is ensured that the riding stability of vehicle, reduce it
Oil consumption, improve the economy of Autonomous Vehicles.
Brief description of the drawings
Fig. 1 is a kind of flow chart of the domain type path tracking control method of autonomous land vehicle of the present invention;
Fig. 2 is the two dimension established in a kind of domain type path tracking control method of autonomous land vehicle of the present invention
The schematic diagram of road vehicle model;
Before and after Fig. 3 is the vehicle body in a kind of domain type path tracking control method of autonomous land vehicle of the present invention
The geometrical relationship figure of end points and vehicle centroid;
Fig. 4 is the acquisition vehicle in a kind of domain type path tracking control method of autonomous land vehicle of the present invention
The principle schematic in connecting way regional edge boundary line in the segment distance of front one;
Fig. 5 is the vehicle movement in a kind of domain type path tracking control method of autonomous land vehicle of the present invention
Learn model schematic;
Fig. 6 is the vehicle power in a kind of domain type path tracking control method of autonomous land vehicle of the present invention
Learn model schematic;
Fig. 7 is the domain type path following control system block diagram of autonomous land vehicle in embodiment of the present invention;
Fig. 8 is the road condition figure that off-line simulation is tested in embodiment of the present invention;
Fig. 9 a to Fig. 9 d are the simulation results of first group of off-line simulation experiment in embodiment of the present invention, and wherein Fig. 9 a are quilt
The driving path of vehicle is controlled, Fig. 9 b are the front wheel angle of controlled quentity controlled variable, i.e. vehicle that controller optimization goes out, and Fig. 9 c are the barycenter of vehicle
Side drift angle, Fig. 9 d are the yaw velocity of vehicle;
Figure 10 a to Figure 10 d are the simulation results of second group of off-line simulation experiment in embodiment of the present invention, wherein Figure 10 a
To be controlled the driving path of vehicle, Figure 10 b are the front wheel angle of controlled quentity controlled variable, i.e. vehicle that controller optimization goes out, and Figure 10 c are vehicle
Side slip angle, Figure 10 d be vehicle yaw velocity.
Embodiment
The present invention is described in detail below in conjunction with the accompanying drawings:
The present invention proposes a kind of domain type path tracking control method of autonomous land vehicle, and its specific implementation step is such as
Under:
Step 1: establish two-dimentional road vehicle model:
The present invention establishes a kind of new two-dimentional road vehicle model, as shown in Figure 2.Ignore the width on left and right vehicle wheel both sides
Degree, used vehicle centroid o rigid rod RF represent vehicle, and rigid rod RF length is equal to length of wagon l.Expected path is then by the phase
Hope road area left side boundary line fl' (x), expectation road area the right boundary line fr' (x) and expectation road area center line f (x) group
Into expectation road area represent, wherein it is expected road area left side boundary line fl' (x) and expectation road area the right boundary line fr'
(x) can be calculated by following formula:
Wherein, fl(x) to be scanned by sensory perceptual system, connecting way region in the obtained segment distance of front one is then handled
Left margin;fr(x) to be scanned by sensory perceptual system, the right side in connecting way region in the obtained segment distance of front one is then handled
Border;W is vehicle width, unit, m.
Step 2: establish the mathematical modeling of the domain type path following control problem of vehicle:
The target of path trace is so that autonomous driving automobile travels along expected path.The two dimension established based on the present invention
Road vehicle model, it is known that the main target of path trace is to ensure that rigid rod RF is in by it is expected roadway area all the time in the present invention
Domain left side boundary line fl' (x), expectation road area the right boundary line fr' (x) and it is expected road area center line f (x) composition expectation
In road area, thus the present invention path trace problem also known as be domain type path trace.
Rigid rod RF, which has, is moving the characteristic constant with size and shape under stressing conditions, as long as so ensureing rigid rod
RF forward terminal F and aft terminal R is in given expectation road area, and whole rigid rod RF is at the expectation road area
It is interior.Therefore, domain type path trace problem proposed by the invention, its main target are the relations described in guarantee formula (2)
Set up:
Wherein, yFFor rigid rod RF forward terminals F lateral position, unit, m;yRFor rigid rod RF aft terminals R lateral position
Put, unit, m.
As shown in Figure 3, with barycenter o there is following geometrical relationship in rigid rod RF forward terminal F and aft terminal R:
Wherein, yoFor vehicle centroid o lateral position, unit, m;lfDistance for vehicle centroid o to vehicle front point F,
Unit, m;lrDistance for vehicle centroid o to rear vehicle end point R, unit, m;ψ is Vehicular yaw angle, unit, radian (rad);β
For vehicle centroid side drift angle, unit, rad.
In view of the sensory perceptual system of autonomous land vehicle, its distance that can observe every time is about 50m, and the song of road
Rate is also mostly smaller, it is believed that yaw angle ψ when vehicle travels in this section of region is very little.It is also contemplated that car
Side slip angle β very littles, the present invention use following approximation relation:
And then formula (3) can be reduced to:
Formula (5) is updated in formula (2), arranges the domain type path trace problem that can obtain vehicle proposed by the present invention
Mathematical modeling, be shown below:
Step 3: calculate connecting way zone boundary line function f in one segment distance of frontlAnd f (x)r(x)
Current invention assumes that vehicle sensory perceptual system can obtain the vehicle periphery road connecting way region under vehicle body coordinate system in real time
Point sequence (the x on borderr,yr,xl,yl).Based on this, the road point sequence (x using Lagrange's interpolation formula three times to acquisitionr,
yr,xl,yl) interpolation processing is carried out, obtain the left side boundary line f in connecting way region in the segment distance of vehicle front onelAnd the right (x)
Boundary line fr(x), specifically as shown in formula (7):
In formula, (xr(i),yr(i),xl(i),yl(i) it is) point sequence (x of connecting way zone boundaryr,yr,xl,yl) in
Four groups of coordinate points, wherein i=j, n, m, k;.The selection of this four groups of coordinate points is carried out based on binary search algorithm, tool
Body is as shown in Figure 4.The purpose of binary search is to obtain the starting point in connecting way region in the segment distance of vehicle front oneWith terminalThe specific derivation process of binary search algorithm is as follows:
In the case where not considering reversing, it is assumed that the position coordinates that controlled vehicle is currently located is (xo,yo), take
As the starting point of search, then pointAbscissa xrAnd point (0)Abscissa xl(0) it must be negative value.This time the purpose of search is
Find one group of nearest range points o positioned at vehicle centroid o rears and in the X-axis direction pointThen pointAbscissa xr
(j) and pointAbscissa xl(j) formula (8) must be met:
Wherein, xr(j+1) it is positioned at pointFront and the point nearest apart from itAbscissa;xl(j+1) it is positioned at pointFront and the point nearest apart from itAbscissa.
Search out one group of point for meeting formula (8)Afterwards, their information is stored, then carried out second
During search, the starting point using them as search.Only consider the road area that the segment length of vehicle front one is d.In view of effect
Control signal on to Autonomous Vehicles, its useful effect phase are generally 1s or so, described when the speed at vehicle centroid is v
Length d=v × 1s of the road area of the segment length of vehicle front one.Therefore, the target point of this searchAbscissa
The inequality relation in formula (9) must be met:
Wherein, xr(k) it is pointAbscissa;xl(k) it is pointAbscissa;xr(k+1) it is positioned at pointFront and away from
The point nearest from itAbscissa;xl(k+1) it is positioned at pointFront and the point nearest apart from itAbscissa.
The point that the present invention will searchWithAs two groups of interpolation points of formula (7), while selected pointWithAs other two groups of interpolation points.J, k, n and m is made to represent above-mentioned four groups of interpolation points respectively in known road
Point sequence (xr,yr,xl,yl) position, then in the presence of following relation:
Step 4: establish Vehicular system model and be organized into state space form:
Kinematics and kinetics relation in view of vehicle have to low speed and autonomous land vehicle when running at high speed respectively
Great influence, the present invention considers when establishing Vehicular system model, while by the kinematics of vehicle and kinetics relation
It is interior.
(1) vehicle kinematics model is established
The schematic diagram of vehicle kinematics model is as shown in Figure 5, it is assumed here that vehicle is a rigid body, wherein installing
Four wheels that will not be deformed upon, and wheel was used as deflecting roller in the past.According to the geometry shown in kinematical equation and accompanying drawing 5
Relation can be obtained shown in the kinematics model such as formula (11) of vehicle:
In formula, xoFor vehicle centroid o lengthwise position, unit, m;yoFor vehicle centroid o lateral position, unit, m;V is
Speed at vehicle centroid, unit, m/s;R be vehicle yaw velocity, unit, rad/s.Formula (4) is updated to formula (11)
In, then the vehicle kinematics model that can be simplified, as shown in formula (12):
(2) vehicle dynamic model is established
As shown in Figure 6, wherein vehicle centroid o is the origin of coordinates to the schematic diagram of vehicle dynamic model, along vehicle body to
Preceding direction is transverse axis x positive direction, perpendicular to the positive direction that transverse axis upwardly direction is longitudinal axis y.Because the present invention is to pass through
The front wheel angle of control vehicle carrys out the purpose of realizing route tracking, so ignoring the longitudinal dynamics of vehicle, and only considers car
Lateral dynamics and yaw direction dynamics.According to Newton's second law and equalising torque relation, can obtain such as formula
(13) vehicle dynamic model shown in:
In formula, vxFor the longitudinal velocity at vehicle centroid, unit, m/s;FyfFor vehicle front-wheel side force, unit, N;FyrFor
Vehicle rear wheel side force, unit, N;M be vehicle quality, unit, kg;IzRotary inertia for vehicle around z-axis, unit, kg
m2;A is vehicle centroid o to the distance of automobile front-axle, unit, m;B is vehicle centroid o to the distance of vehicle rear axle, unit, m;δf
For vehicle front wheel angle, unit, rad.The front wheel angle δ of vehiclefVery little, so formula (13) can be simplified, the car after simplifying
Shown in kinetic model such as formula (14):
It is assumed that the lateral tire force of vehicle is not up to saturation, now side force FyIt is substantially linear with slip angle of tire α,
As shown in formula (15):
In formula, CfThe tire cornering stiffness of vehicle front-wheel, unit, Nrad;CrFor 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:
Convolution (14), (15) and (16), the vehicle dynamic model that can obtain two degrees of freedom is arranged, as shown in formula (17):
(3) state-space model of Vehicular system is established
Convolution (12) and formula (17), are considered simultaneouslyVehicular system motion and dynamic (dynamical) differential side can then be obtained
Formula, specifically as shown in formula (18):
The domain type path trace problem of vehicle proposed by the present invention, its main target are the preceding rotation by controlling vehicle
The lateral position of angle and then guarantee vehicle meets the inequality constraints in formula (6).So choose vehicle centroid o lateral position
yoExported as system, while choose front wheel angle δfAs system control amount, selecting system state vector x=[yo ψ β r
]T.Based on this, Vehicular system model is described as the state-space model shown in formula (19):
In formula,
Wherein, A is sytem matrix;B is input matrix;C is output matrix.
Step 5: carrying out the design of domain type path following control model using model predictive control method, obtain current
Moment optimal front wheel angle
The system block diagram for the domain type path following control system that the present invention designs as shown in Figure 7, characterizes vehicle operation
Position (the x of the vehicle centroid of stateo,yo), Vehicular yaw angle ψ, the speed v of vehicle, the yaw velocity r and vehicle of vehicle
Side slip angle β can all be obtained by the high-precision GPS sensor RT3002 measurements of device in sensory perceptual system.In view of model
Superiority of the forecast Control Algorithm in processing constraint, the present invention carry out domain type path trace based on model predictive control method
The design of Controlling model.
First, it can be seen from the mathematical modeling for the domain type path trace problem above established, in the region of design vehicle
It must enter row constraint to the lateral position of vehicle centroid during type path following control model, it is met the inequality in formula (6)
Relation.Secondly, it is contemplated that security is the major issue that vehicle has to concern in the process of moving, and for carrying out area
It is substantially safest traveling along the center line traveling in its front connecting way region for the Autonomous Vehicles of domain type path trace
Scheme, so the domain type path following control model of the vehicle of design must can guarantee that Autonomous Vehicles travel in region as much as possible
On center line.The driving safety that lateral turnover also drastically influence vehicle occurs for vehicle.By experience, when vehicle centroid lateral deviation
Angle beta is more than a certain amount, and (we are referred to as the critical point β that turns on one's siderollover) when, vehicle just has the danger turned on one's side, so designing
Vehicle centroid side drift angle must be used restraint during the domain type path following control model of vehicle.Again, it is contemplated that wide now
The oil consumption problem paid close attention to both at home and abroad, the present invention propose to reduce energy consumption and then realization to subtract by the travel route of most shortization vehicle
The purpose of few oil consumption.Finally, it is contemplated that the ride performance of Autonomous Vehicles, also need to limit the size of control action, to keep away
Exempt from excessive control action occur.The steering mechanism of vehicle is a mechanical system, can there is the problem of mechanical saturation, if so
Control action can effectively be played by ensureing the controlled quentity controlled variable of controller output, in the domain type path following control mould of design vehicle
The saturation problem for turning to executing agency must also be taken into account during type.To sum up analyze, the domain type for the vehicle that the present invention designs
Path following control model needs to realize following some targets:
Target 1) lateral position of vehicle centroid is met constraint in formula (6);
Target 2) vehicle is travelled as much as possible on the center line for it is expected road area, to reduce vehicle and road edge
Or the danger that barrier collides;
Target 3) vehicle centroid side drift angle β is not more than the critical value β that vehicle is turned on one's siderollover;
Target 4) make the path of vehicle traveling as short as possible, to reduce the oil consumption of vehicle;
Target 5) ensure controller output controlled quentity controlled variable be vehicle front wheel angle it is steady all the time, avoid the occurrence of excessive control
Braking is made.
Target 6) front wheel angle and its rate of change is not higher than the saturation value of steering mechanism;
According to above-mentioned control targe, using the domain type path following control model of model predictive control method progress vehicle
Design, detailed process is as follows:
Understand, speed is a slowly varying continuous quantity, so the present invention makes hypothesis below:Assuming that autonomous driving car
One predict time domain in keep constant speed drive.
Formula (19) is the Differential Model of Vehicular system, for the design of the domain type path following control model for vehicle,
Need, by formula (19) discretization, the Vehicular system model of discrete time to be obtained, as shown in formula (20):
In formula,Wherein TsFor the sampling time.
It is assumed that prediction time domain is P, it is N to control time domain, and meets N≤P.Assume to control the controlled quentity controlled variable outside time domain to protect simultaneously
Hold constant, i.e. δf(k+N-1)=δf(k+N)=...=δf(k+P-1), the then Vehicular system mould based on discrete time in formula (20)
Type, the status predication equation of P steps can be derived, it is specific as shown in (21):
The prediction output of P steps is derived simultaneously, as shown in formula (22):
Definition control input sequence U (k) and control forecasting output sequence Y (k+1 | k) be respectively:
According to the above-mentioned analysis on control targe, it is known that one of control targe is to make autonomous land vehicle as far as possible
Travelled along the center line for it is expected road area, so invention defines the reference input sequence R (k) as shown in formula (24):
In formula, yr(k+i) it is expected road area center line f (x) discrete magnitude, wherein i=1 ..., P, discrete interval is
v(k)·Ts.So, control vehicle as far as possible can be by most along this control targe for it is expected road area center line traveling
Object function in smallization formula (25) is realized:
J1=‖ Y (k+1 | k)-R (k) ‖2 (25)
Target 4) point out:Domain type path following control model should have the function that most shortization is controlled route or travel by vehicle,
The purpose of energy-saving and emission-reduction is realized with this.
When using the domain type path following control model of model predictive control method design vehicle, this demand for control
It can be realized by minimizing the object function being made up of vehicle traveling displacement, as shown in formula (26):
In formula,
Δxd(k+i)=v (k) Ts, i=1 ..., P
Δyd(k+i)=yo(k+i)-yo(k+i-1), i=1 ..., P;
Wherein, Δ xd(k+i) it is the length travel of vehicle traveling within this period of time of (k+i-1)~(k+i), unit,
m;Δyd(k+i) it is the lateral displacement of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m.Target 5 simultaneously)
Point out:The controlled quentity controlled variable of reply controller output is that the size of vehicle front wheel angle is controlled by, and makes it excessive, to ensure quilt
Control the steering ride comfort of vehicle.Therefore, the present invention will be by the formula (27) that control input sequence U (k) is formed as Controlling model
One optimization aim:
J3=‖ U (k) ‖2 (27)
There is J for such a1、J2And J3The optimization problem of three optimization aims, weight coefficient need to be introduced and come to each
The demand conflict of optimization aim is weighed and handled, to obtain a most suitable optimum results.Therefore, the present invention designs
The optimization aim of domain type path following control model based on Model Predictive Control is:
In formula, ΓyAnd ΓuFor weighting matrix;Γd,iFor weight factor.
Know again, the most important control targe that domain type path following control needs to realize is to ensure that controlled vehicle begins
Traveling is in connecting way region eventually.Therefore, the present invention is in the domain type path using model predictive control method design vehicle
System output is used restraint during tracing control model.Inequality relation in convolution (6), the output constraint can be written to as
Form shown in formula (29):
In formula, ψ (k+i)=CψX (k+i), and Cψ=[0 10 0];β (k+i)=CβX (k+i), and Cβ=[0 00
1];fl' (k+i) for it is expected road area left side boundary line fl' (x) in moment k+i sampled value, unit, m;fr' (k+i) then schedule to last
Hope boundary line f on the right of road arear' (x) in moment k+i sampled value, unit, m.
There is mechanical saturation in the steering executing agency of vehicle, excessive or too fast controlled quentity controlled variable can not effectively be made
Use on controlled vehicle.To avoid controller from providing invalid controlled quentity controlled variable, the present invention proposes target 6):Make front wheel angle and its
Rate of change is not higher than the saturation value of steering mechanism, and this is then needed on the domain type road using model predictive control method design vehicle
Control constraints and the controlling increment constraint being shown below are considered during the tracing control model of footpath:
In formula, δf(k+i) it is k+i moment vehicle front wheel angles, unit, rad;δfsatTo turn to achieved by executing agency
Maximum front wheel angle, unit, rad;Δδf(k+i)=δf(k+i)-δf(k+i-1) it is the front wheel angle increment at k+i moment,
Unit, rad;ΔδfsatTurn to the maximum front wheel angle increment achieved by executing agency, unit, rad.It is simultaneously raising car
Lateral stability, reduce its risk turned on one's side, the present invention proposes that the domain type path following control model of design should
Vehicle centroid side drift angle β is set to be not more than the critical value β that vehicle is turned on one's side as far as possiblerollover.Therefore, model prediction control is being used
Consider following state constraint during the domain type path following control model of method design vehicle processed:
|β(k+i)|≤βrollover, i=1 ..., P (31)
In summary, the domain type path following control model for the vehicle that the present invention is designed using model predictive control method
Following form can be organized into:
Meet:
In formula:
Δxd(k+i)=vTs;
Δyd(k+i)=yo(k+i)-yo(k+i-1);
Δδf(k+i)=δf(k+i)-δf(k+i-1);
Cψ=[0 10 0], Cβ=[0 01 0];
Optimization problem in solution formula (32) can obtain an optimal control sequence U*, binding model PREDICTIVE CONTROL
General principle, it is current time optimal front wheel angle that the present invention, which chooses controlled quentity controlled variable,For:
Choose optimal control sequence U*First amount be applied to as controlled quentity controlled variable on controlled vehicle.To subsequent time,
Domain type path following control model based on Model Predictive Control will recalculate an optimal control according to current vehicle condition
Amount processed, it is reciprocal with this, that is, realize rolling optimization control.
Step 6: according to the current time provided in step 5 optimal front wheel angleControl turns to executing agency and moved
Make so that the front wheel angle of controlled vehicle is equal to current time optimal front wheel angleSo that controlled vehicle is in vehicle sense
Know in a certain segment distance of vehicle front that system provides and travelled in connecting way region, realize the control mesh of zone routing tracking
Mark.
To verify the validity of designed path following control model, base is built under Matlab/Simulink environment
In the domain type path following control model of Model Predictive Control, made using high-precision vehicle dynamics simulation software veDYNA
For controlled device, carry out off-line simulation and analyze simulation result.
The HQ430 auto model parameters of table 1
Vehicular system model employed in emulation experiment is red flag HQ430 models, and its major parameter is as shown in table 1.It is real
Test road condition as shown in Figure 8, be asphalt roads.Fully to verify the performance of designed path following control model, this
Invention has carried out two groups of emulation experiments altogether;
One group is carried out under dry bituminous paving operating mode, and the friction coefficient μ of road surface and tire is taken as 0.9.In reality
During testing, vehicle carries out Acceleration of starting first, then carries out constant speed drive respectively with 60km/h and 85km/h longitudinal velocity.
Another group is carried out under the bituminous paving operating mode of humidity, and the friction coefficient μ of road surface and tire is taken as 0.6.
In experimentation, vehicle carries out Acceleration of starting first, then carries out constant speed respectively with 60km/h and 82km/h longitudinal velocity
Traveling.
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, that is, is existed
The simulation result under asphalt roads operating mode (μ=0.9) is dried, wherein Fig. 9 a are the driving path of controlled vehicle, and Fig. 9 b are control
The front wheel angle of the controlled quentity controlled variable of device optimization, i.e. vehicle, Fig. 9 c are the side slip angle of vehicle, and Fig. 9 d are the yaw angle speed of vehicle
Degree;Accompanying drawing 10 is second group of the simulation result under moist bituminous paving operating mode (μ=0.6), and wherein Figure 10 a are controlled vehicle
Driving path, Figure 10 b are the front wheel angle of controlled quentity controlled variable, i.e. vehicle that controller optimization goes out, and Figure 10 c are the barycenter lateral deviation of vehicle
Angle, Figure 10 d are the yaw velocity of vehicle.
By off-line simulation, it can be seen that the domain type path following control based on Model Predictive Control that the present invention designs
Model can control controlled vehicle to travel all the time in given road area, and the front wheel angle of optimization is less than steering mechanism
Saturation value, while the driving safety and lateral stability of controlled vehicle can be ensured, and the change tool of road pavement coefficient of friction
There is certain robustness.
Claims (5)
1. a kind of domain type path tracking control method of autonomous land vehicle, the Ride Control System in autonomous land vehicle is first
First optimized according to the running state information of the connecting way area information and vehicle itself provided after sensory perceptual system scan process
Go out current time optimal front wheel angle, the steering executing agency for then controlling vehicle according to the front wheel angle acts, so that car
Travelled in feasible road area, it is characterised in that step is as follows:
Step 1: establish two-dimentional road vehicle model:
Two-dimentional road vehicle model is established, if rigid rod RF is auto model, it crosses the barycenter o of vehicle, and length is equal to vehicle body length
L is spent, it is expected road area then by expectation road area left side boundary line fl' (x), expectation road area the right boundary line fr' (x) and phase
The expectation road area that road area center line f (x) is formed is hoped to represent, and is met:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>f</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mfrac>
<mi>w</mi>
<mn>2</mn>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>f</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mfrac>
<mi>w</mi>
<mn>2</mn>
</mfrac>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>f</mi>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfrac>
<mrow>
<msub>
<mi>f</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>f</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
</mrow>
<mn>2</mn>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, fl(x) being can trade in the segment distance of front one obtained by the sensory perceptual system scan process in autonomous land vehicle
The left margin in road region;fr(x) segment distance of front one to be obtained by the sensory perceptual system scan process in autonomous land vehicle
The right margin in interior connecting way region;fl' (x) for it is expected road area left side boundary line, fr' (x) for it is expected road area right margin
Line, w are vehicle width, unit, m;
Step 2: establish the mathematical modeling of the domain type path trace problem of vehicle:
The two-dimentional road vehicle model established based on step 1, ensure that rigid rod RF is in by expectation road area left margin all the time
Line fl' (x), expectation road area the right boundary line fr' (x) and it is expected road area center line f (x) composition expectation road area
Interior, with reference to the geometry and physical characteristic of rigid rod, the mathematical modeling for establishing the domain type path trace problem of vehicle is as follows:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>6</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, yoFor vehicle centroid o lateral position, unit, m;lfDistance for vehicle centroid o to vehicle front point F, unit,
m;lrDistance for vehicle centroid o to rear vehicle end point R, unit, m;ψ is Vehicular yaw angle, unit, rad;β is vehicle centroid
Side drift angle, unit, rad;
Step 3: calculate connecting way regional edge boundary line f in the segment distance of vehicle front onelAnd f (x)r(x):
It is assumed that the sensory perceptual system of autonomous land vehicle can obtain the point sequence letter of the connecting way zone boundary of du vehicule in real time
Cease (xr,yr,xl,yl), xrFor can in the segment distance of front one that is obtained by the sensory perceptual system scan process in autonomous land vehicle
The right margin f of row road arear(x) the abscissa sequence of the point on;yrTo be scanned by the sensory perceptual system in autonomous land vehicle
Handle the right margin f in connecting way region in the obtained segment distance of front oner(x) the ordinate sequence of the point on;xlFor by certainly
The left margin f in connecting way region in the segment distance of front one that sensory perceptual system scan process in main driving vehicle obtainsl(x) on
Point abscissa sequence;ylFor in the segment distance of front one that is obtained by the sensory perceptual system scan process in autonomous land vehicle
The left margin f in connecting way regionl(x) the ordinate sequence of the point on;It is Lagrangian three times based on the use of binary search algorithm
The border line function that interpolation formula obtains connecting way region in the segment distance of vehicle front one is:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>f</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>&Sigma;</mo>
<munder>
<mo>&Pi;</mo>
<mrow>
<mi>i</mi>
<mo>&NotEqual;</mo>
<mi>p</mi>
</mrow>
</munder>
<mfrac>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mo>(</mo>
<mi>p</mi>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mfrac>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>f</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mo>&Sigma;</mo>
<munder>
<mo>&Pi;</mo>
<mrow>
<mi>i</mi>
<mo>&NotEqual;</mo>
<mi>p</mi>
</mrow>
</munder>
<mfrac>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mo>(</mo>
<mi>p</mi>
<mo>)</mo>
<mo>-</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mo>(</mo>
<mi>i</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mfrac>
<msub>
<mi>y</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>p</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mtable>
<mtr>
<mtd>
<mrow>
<mi>p</mi>
<mo>=</mo>
<mi>j</mi>
<mo>,</mo>
<mi>n</mi>
<mo>,</mo>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
<mo>;</mo>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mi>j</mi>
<mo>,</mo>
<mi>n</mi>
<mo>,</mo>
<mi>m</mi>
<mo>,</mo>
<mi>k</mi>
<mo>;</mo>
</mrow>
</mtd>
</mtr>
</mtable>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>7</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, (xr(i),yr(i),xl(i),yl(i) it is) point sequence (x of connecting way zone boundaryr,yr,xl,yl) in four
Group coordinate points, wherein i=j, n, m, k;
Step 4: it is state space form to establish autonomous land vehicle system model and arranged:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>A</mi>
<mi>x</mi>
<mo>+</mo>
<msub>
<mi>B&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>C</mi>
<mi>x</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>19</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula,
C=[1 00 0];
In formula, x is system mode vector, and x=[yo ψ β r]T;δfAlso it is system control amount for vehicle front wheel angle, it is single
Position, rad;yoExported for system;A is sytem matrix;B is input matrix;C is output matrix;V is the speed at vehicle centroid,
Unit, m/s;R be vehicle yaw velocity, unit, rad/s;CfFor the cornering stiffness of vehicle front tyre, unit, N/
rad;CrThe respectively cornering stiffness of vehicle rear wheel tire, unit, N/rad;M be vehicle quality, unit, kg;IzFor vehicle around
The rotary inertia of z-axis, unit, kgm2;A is vehicle centroid o to the distance of automobile front-axle, unit, m;B is that vehicle centroid o is arrived
The distance of vehicle rear axle, unit, m;
Step 5: use the domain type path following control model of model predictive control method design vehicle for:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<munder>
<mi>min</mi>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
</mrow>
</munder>
<mi>J</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>J</mi>
<mo>=</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Gamma;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>Y</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
<mo>)</mo>
<mo>-</mo>
<mi>R</mi>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Gamma;</mi>
<mi>u</mi>
</msub>
<mi>U</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>p</mi>
</munderover>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>d</mi>
<mo>,</mo>
<mi>i</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;x</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;y</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>32</mn>
<mo>)</mo>
</mrow>
</mrow>
Meet:
In formula:
<mrow>
<mi>Y</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>R</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>U</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>;</mo>
</mrow>
<mrow>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<mo>=</mo>
<msup>
<mi>e</mi>
<mrow>
<msub>
<mi>AT</mi>
<mi>s</mi>
</msub>
</mrow>
</msup>
<mo>,</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<mo>=</mo>
<msubsup>
<mo>&Integral;</mo>
<mn>0</mn>
<msub>
<mi>T</mi>
<mi>s</mi>
</msub>
</msubsup>
<msup>
<mi>e</mi>
<mrow>
<mi>A</mi>
<mi>&tau;</mi>
</mrow>
</msup>
<mi>d</mi>
<mi>&tau;</mi>
<mo>&CenterDot;</mo>
<mi>B</mi>
<mo>,</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<mo>=</mo>
<mi>C</mi>
<mo>;</mo>
</mrow>
Δxd(k+i)=v (k) Ts;
Δyd(k+i)=yo(k+i)-yo(k+i-1);
Δδf(k+i)=δf(k+i)-δf(k+i-1);
Cψ=[0 10 0], Cβ=[0 01 0];
And choose controlled quentity controlled variable i.e. current time optimal front wheel angleFor:
<mrow>
<msubsup>
<mi>&delta;</mi>
<mi>f</mi>
<mo>*</mo>
</msubsup>
<mo>=</mo>
<msup>
<mi>U</mi>
<mo>*</mo>
</msup>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>33</mn>
<mo>)</mo>
</mrow>
</mrow>
δ in formulaf(k+i) for the k+i moment system control amount, as vehicle front wheel angle, unit, rad;
X (k+i) is the system mode vector at k+i moment;
yo(k+i) exported for the system at k+i moment, i.e. the lateral position of vehicle centroid, unit, m;
P is prediction time domain, and N is control time domain;
ΓyAnd ΓuFor weighting matrix;
Γd,iFor weight factor;
yr(k+i), i=1 ..., P are the discrete magnitude for it is expected road area center line f (x), and discrete interval is v (k) Ts, unit,
m;
Δxd(k+i) it is the length travel of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
Δyd(k+i) it is the lateral displacement of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
Δδf(k+i) it is the controlling increment at k+i moment, unit, rad;
δfsatTo turn to the maximum front wheel angle achieved by executing agency, unit, rad;
ΔδfsatTurn to the maximum front wheel angle increment achieved by executing agency, unit, rad;
fl' (k+i) for it is expected road area left side boundary line fl' (x) in moment k+i sampled value, unit, m;
fr' (k+i) then for it is expected road area on the right of boundary line fr' (x) in moment k+i sampled value, unit, m;
βrolloverThe critical quantity that may be turned on one's side for vehicle, unit, rad;
TsFor sampling time, unit s;
U*For the optimal control sequence obtained by Optimization Solution;
Step 6: according to the current time provided in step 5 optimal front wheel angleControl turns to executing agency's action, makes
The front wheel angle that vehicle must be controlled is equal to current time optimal front wheel angleSo that controlled vehicle is in vehicle sensory perceptual system
Travelled in the segment distance of vehicle front one provided in connecting way region, realize the control targe of zone routing tracking.
2. according to a kind of domain type path tracking control method of autonomous land vehicle described in claim 1, it is characterised in that
The detailed process of step 2 is:
To ensure that rigid rod RF is in by expectation road area left side boundary line f all the timel' (x), expectation road area the right boundary line fr'
(x) and it is expected in the expectation road area that road area center line f (x) is formed, the relation described in following formula (2) need to be ensured
Set up:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>F</mi>
</msub>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>R</mi>
</msub>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>x</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, yFFor rigid rod RF forward terminals F lateral position, unit, m;yRIt is single for rigid rod RF aft terminals R lateral position
Position, m;
Following geometrical relationship be present in rigid rod RF forward terminal F and aft terminal R and barycenter o:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>F</mi>
</msub>
<mo>=</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>R</mi>
</msub>
<mo>=</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mi>sin</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>3</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, yoFor vehicle centroid o lateral position, unit, m;lfDistance for vehicle centroid o to vehicle front point F, unit,
m;lrDistance for vehicle centroid o to rear vehicle end point R, unit, m;ψ is Vehicular yaw angle, unit, radian (rad);β is car
Side slip angle, unit, rad;
Yaw angle ψ and side drift angle β have following approximation relation:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
<mo>&ap;</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
<mo>&ap;</mo>
<mn>1</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>4</mn>
<mo>)</mo>
</mrow>
</mrow>
And then formula (3) can be reduced to:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>F</mi>
</msub>
<mo>=</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>R</mi>
</msub>
<mo>=</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>5</mn>
<mo>)</mo>
</mrow>
</mrow>
Formula (5) is updated in formula (2), the domain type path trace for being to can obtain the vehicle described in step 2 is arranged and asks
The mathematical modulo pattern (6) of topic.
3. according to a kind of domain type path tracking control method of autonomous land vehicle described in claim 1, it is characterised in that
The detailed process of step 3 is:
In formula (7) in step 3, (xr(i),yr(i),xl(i),yl(i) it is) point sequence (x of connecting way zone boundaryr,
yr,xl,yl) four groups of coordinate points, wherein i=j, n, m, k, the selection of this four groups of coordinate points are entered based on binary search algorithm
Capable, the purpose of binary search is to obtain the starting point in the connecting way region in the segment distance of vehicle front oneWith end
PointWherein,It is the starting point on the connecting way region right margin in the segment distance of vehicle front one, comprising on right margin
Two information of abscissa and ordinate of starting point;It is rising on the connecting way region left margin in the segment distance of vehicle front one
Point, two information of the abscissa comprising starting point on left margin and ordinate;It is the connecting way in the segment distance of vehicle front one
Terminal on the right margin of region, two information of the abscissa comprising terminal on right margin and ordinate;It is one section of vehicle front
The terminal on the left margin of connecting way region in distance, two information of the abscissa comprising terminal on left margin and ordinate;
The specific derivation process of binary search algorithm is as follows:
In the case where not considering reversing, it is assumed that the position coordinates that controlled vehicle is currently located is (xo,yo), takeAs
The starting point of search, then pointAbscissa xrAnd point (0)Abscissa xl(0) it must be negative value, this time the purpose of search is to find
One group of nearest range points o point positioned at vehicle centroid o rears and in the X-axis directionThen pointAbscissa xr(j) and
PointAbscissa xl(j) formula (8) must be met:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>)</mo>
</mrow>
<mo>&CenterDot;</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>j</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>8</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, xr(j+1) it is positioned at pointFront and the point nearest apart from itAbscissa;xl(j+1) it is positioned at pointFront
And the point nearest apart from itAbscissa;
Search out one group of point for meeting formula (8)Afterwards, their information is stored, is then carrying out second of search
When, the starting point using them as search, only consider the road area that the segment length of vehicle front one is d, it is contemplated that be applied to certainly
Control signal on main car, its useful effect phase are generally 1s or so, when the speed at vehicle centroid is v, d=v × 1s, because
This, the target point of this searchAbscissa must meet inequality relation in formula (9):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
<mo>-</mo>
<mi>d</mi>
<mo>)</mo>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>r</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mo>)</mo>
<mo>-</mo>
<mi>d</mi>
<mo>)</mo>
<mo>&le;</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
<mo>-</mo>
<mi>d</mi>
<mo>)</mo>
<mo>(</mo>
<msub>
<mi>x</mi>
<mi>l</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
<mo>)</mo>
<mo>-</mo>
<mi>d</mi>
<mo>)</mo>
<mo>&le;</mo>
<mn>0</mn>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>9</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, xr(k) it is pointAbscissa;xl(k) it is pointAbscissa;xr(k+1) it is positioned at pointFront and apart from it
Nearest pointAbscissa;xl(k+1) it is positioned at pointFront and the point nearest apart from itAbscissa;
The point that will be searchedWithAs two groups of interpolation points of formula (7), while selected pointWith
As other two groups of interpolation points of formula (7), make j, k, n and m represent above-mentioned four groups of interpolation points respectively and having learned that waypoint sequence
(xr,yr,xl,yl) position, the relation between j, k, n and m is as follows:
4. according to a kind of domain type path tracking control method of autonomous land vehicle described in claim 2, it is characterised in that
The detailed process of step 4 is:
(1) vehicle kinematics model is established
It is assumed that vehicle is a rigid body, wherein install four wheels that will not be deformed upon, and using vehicle front-wheel as turning
To wheel, obtained according to kinematical equation and geometrical relationship shown in the kinematics model such as formula (11) of vehicle:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>v</mi>
<mi>c</mi>
<mi>o</mi>
<mi>s</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>y</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>v</mi>
<mi>s</mi>
<mi>i</mi>
<mi>n</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>r</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>11</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, xoFor vehicle centroid o lengthwise position, unit, m;yoFor vehicle centroid o lateral position, unit, m;V is vehicle
Speed at barycenter, 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, as shown in formula (12):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>x</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>v</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>y</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>r</mi>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>12</mn>
<mo>)</mo>
</mrow>
</mrow>
(2) vehicle dynamic model is established
If vehicle centroid o is the origin of coordinates in vehicle dynamic model, the positive direction along vehicle body forward direction for transverse axis x,
Perpendicular to transverse axis upwardly direction be longitudinal axis y positive direction, this method be by control the positive direction of vehicle come realizing route with
The purpose of track, so ignore the longitudinal dynamics of vehicle, a lateral dynamics of consideration vehicle and the dynamics of yaw direction,
According to Newton's second law and equalising torque relation, the vehicle dynamic model as shown in formula (13) can obtain:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>mv</mi>
<mi>x</mi>
</msub>
<mrow>
<mo>(</mo>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>r</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>x</mi>
<mi>f</mi>
</mrow>
</msub>
<msub>
<mi>sin&delta;</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>f</mi>
</mrow>
</msub>
<msub>
<mi>cos&delta;</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>r</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>a</mi>
<mrow>
<mo>(</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>x</mi>
<mi>f</mi>
</mrow>
</msub>
<msub>
<mi>sin&delta;</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>f</mi>
</mrow>
</msub>
<msub>
<mi>cos&delta;</mi>
<mi>f</mi>
</msub>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>bF</mi>
<mrow>
<mi>y</mi>
<mi>r</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>13</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, vxFor the longitudinal velocity at vehicle centroid, unit, m/s;FyfFor vehicle front-wheel side force, unit, N;FyrFor vehicle
Trailing wheel side force, unit, N;M be vehicle quality, unit, kg;IzRotary inertia for vehicle around z-axis, unit, kgm2;a
For vehicle centroid o to automobile front-axle distance, unit, m;B is vehicle centroid o to the distance of vehicle rear axle, unit, m;δfFor car
Front wheel angle, unit, rad, the front wheel angle δ of vehiclefVery little, formula (13) can be simplified, the dynamics of vehicle after simplifying
Shown in model such as formula (14):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>mv</mi>
<mi>x</mi>
</msub>
<mrow>
<mo>(</mo>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>+</mo>
<mi>r</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>f</mi>
</mrow>
</msub>
<mo>+</mo>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>r</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<msub>
<mi>aF</mi>
<mrow>
<mi>y</mi>
<mi>f</mi>
</mrow>
</msub>
<mo>-</mo>
<msub>
<mi>bF</mi>
<mrow>
<mi>y</mi>
<mi>r</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>14</mn>
<mo>)</mo>
</mrow>
</mrow>
It is assumed that the lateral tire force of vehicle is not up to saturation, now side force FyIt is substantially linear with slip angle of tire α, such as formula
(15) shown in:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>f</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
<msub>
<mi>&alpha;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>F</mi>
<mrow>
<mi>y</mi>
<mi>r</mi>
</mrow>
</msub>
<mo>=</mo>
<mn>2</mn>
<msub>
<mi>C</mi>
<mi>r</mi>
</msub>
<msub>
<mi>&alpha;</mi>
<mi>r</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>15</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, CfFor the tire cornering stiffness of vehicle front-wheel, unit, Nrad;CrIt is single for the tire cornering stiffness of vehicle rear wheel
Position, 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-wheelfWith the slip angle of tire α of trailing wheelrRespectively:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>f</mi>
</msub>
<mo>=</mo>
<mi>&beta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mi>a</mi>
<mi>r</mi>
</mrow>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
</mfrac>
<mo>-</mo>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&alpha;</mi>
<mi>r</mi>
</msub>
<mo>=</mo>
<mi>&beta;</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mi>b</mi>
<mi>r</mi>
</mrow>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
</mfrac>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>16</mn>
<mo>)</mo>
</mrow>
</mrow>
Convolution (14), (15) and (16), the vehicle dynamic model that can obtain two degrees of freedom is arranged, as shown in formula (17):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>mv</mi>
<mi>x</mi>
</msub>
</mrow>
</mfrac>
<mi>&beta;</mi>
<mo>+</mo>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
<mo>-</mo>
<msub>
<mi>bC</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msubsup>
<mi>mv</mi>
<mi>x</mi>
<mn>2</mn>
</msubsup>
</mrow>
</mfrac>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mi>r</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
</mrow>
<mrow>
<msub>
<mi>mv</mi>
<mi>x</mi>
</msub>
</mrow>
</mfrac>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
<mo>-</mo>
<msub>
<mi>bC</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
</mfrac>
<mi>&beta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msup>
<mi>a</mi>
<mn>2</mn>
</msup>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msup>
<mi>b</mi>
<mn>2</mn>
</msup>
<msub>
<mi>C</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
<msub>
<mi>v</mi>
<mi>x</mi>
</msub>
</mrow>
</mfrac>
<mi>r</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
</mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
</mfrac>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>17</mn>
<mo>)</mo>
</mrow>
</mrow>
(3) state-space model of Vehicular system is established
Convolution (12) and formula (17), are considered simultaneouslyVehicular system motion and dynamic (dynamical) differential equation can then be obtained
Formula, specifically as shown in formula (18):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mover>
<mi>y</mi>
<mo>&CenterDot;</mo>
</mover>
<mi>o</mi>
</msub>
<mo>=</mo>
<mi>v</mi>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>+</mo>
<mi>&beta;</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&psi;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mi>r</mi>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>&beta;</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<mi>m</mi>
<mi>v</mi>
</mrow>
</mfrac>
<mi>&beta;</mi>
<mo>+</mo>
<mrow>
<mo>(</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
<mo>-</mo>
<msub>
<mi>bC</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msup>
<mi>mv</mi>
<mn>2</mn>
</msup>
</mrow>
</mfrac>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mi>r</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
</mrow>
<mrow>
<mi>m</mi>
<mi>v</mi>
</mrow>
</mfrac>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mover>
<mi>r</mi>
<mo>&CenterDot;</mo>
</mover>
<mo>=</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
<mo>-</mo>
<msub>
<mi>bC</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
</mfrac>
<mi>&beta;</mi>
<mo>+</mo>
<mfrac>
<mrow>
<mn>2</mn>
<mrow>
<mo>(</mo>
<msup>
<mi>a</mi>
<mn>2</mn>
</msup>
<msub>
<mi>C</mi>
<mi>f</mi>
</msub>
<mo>+</mo>
<msup>
<mi>b</mi>
<mn>2</mn>
</msup>
<msub>
<mi>C</mi>
<mi>r</mi>
</msub>
<mo>)</mo>
</mrow>
</mrow>
<mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
<mi>v</mi>
</mrow>
</mfrac>
<mi>r</mi>
<mo>-</mo>
<mfrac>
<mrow>
<mn>2</mn>
<msub>
<mi>aC</mi>
<mi>f</mi>
</msub>
</mrow>
<msub>
<mi>I</mi>
<mi>z</mi>
</msub>
</mfrac>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>18</mn>
<mo>)</mo>
</mrow>
</mrow>
Meet the inequality constraints in formula (6) by the lateral position for controlling the front wheel angle of vehicle to ensure vehicle, choose
Vehicle centroid o lateral position yoExported as system, while choose front wheel angle δfAs system control amount, system mode to
Amount is chosen for x=[yoψ β r], Vehicular system model is described as the state-space model shown in step 4 Chinese style (19).
5. according to a kind of domain type path tracking control method of autonomous land vehicle described in claim 1, it is characterised in that
The detailed process of step 5 is:
Assuming that autonomous land vehicle is predicted at one keeps constant speed drive in time domain, the formula (19) in step 4 is Vehicular system
Differential Model, in order to the domain type path following control model for vehicle design, it is necessary to by formula (19) discretization, obtain from
The Vehicular system model of time is dissipated, as shown in formula (20):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>20</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula,T in formulasFor the sampling time;
It is assumed that prediction time domain is P, it is N to control time domain, and meets N≤P, while assumes to control the controlled quentity controlled variable outside time domain to keep not
Become, i.e. δf(k+N-1)=δf(k+N)=...=δf(k+P-1), then the Vehicular system model based on discrete time in formula (20) pushes away
The status predication equation of P steps is exported, it is specific as shown in (21):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>=</mo>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mn>2</mn>
</msubsup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mi>N</mi>
</msubsup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mi>P</mi>
</msubsup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>P</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>P</mi>
<mo>-</mo>
<mi>N</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</munderover>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>21</mn>
<mo>)</mo>
</mrow>
</mrow>
The prediction output of P steps is derived simultaneously, as shown in formula (22):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msub>
<mi>A</mi>
<mi>c</mi>
</msub>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mi>N</mi>
</msubsup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mi>P</mi>
</msubsup>
<mi>x</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>P</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<mo>...</mo>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mrow>
<mi>P</mi>
<mo>-</mo>
<mi>N</mi>
<mo>+</mo>
<mn>1</mn>
</mrow>
</munderover>
<msub>
<mi>C</mi>
<mi>c</mi>
</msub>
<msubsup>
<mi>A</mi>
<mi>c</mi>
<mrow>
<mi>i</mi>
<mo>-</mo>
<mn>1</mn>
</mrow>
</msubsup>
<msub>
<mi>B</mi>
<mi>c</mi>
</msub>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>22</mn>
<mo>)</mo>
</mrow>
</mrow>
Definition control input sequence U (k) and control forecasting output sequence Y (k+1 | k) be respectively:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mi>U</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>N</mi>
<mo>-</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mi>Y</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>23</mn>
<mo>)</mo>
</mrow>
</mrow>
In order that autonomous land vehicle travels along the center line for it is expected road area as far as possible, the ginseng as shown in formula (24) is defined
Examine list entries R (k):
<mrow>
<mi>R</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>=</mo>
<mfenced open = "[" close = "]">
<mtable>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mn>2</mn>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mo>.</mo>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msub>
<mi>y</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>P</mi>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>24</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, yr(k+i) it is expectation road area center line f (x) discrete magnitude, wherein i=1 ..., P, discrete interval v
(k)·Ts, can be by minimizing the target letter in formula (25) in order to control vehicle as far as possible along road area center line it is expected
Count to realize:
J1=‖ Y (k+1 | k)-R (k) ‖2 (25)
In order that domain type path following control model has the function that most shortization is controlled route or travel by vehicle, pre- using model
When surveying the domain type path following control model of control method design vehicle, it can travel what displacement was formed by vehicle by minimizing
Object function is realized, as shown in the formula (26):
<mrow>
<msub>
<mi>J</mi>
<mn>2</mn>
</msub>
<mo>=</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>P</mi>
</munderover>
<mrow>
<mo>(</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;x</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;y</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>26</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula,
Δxd(k+i)=v (k) Ts, i=1 ..., P
Δyd(k+i)=yo(k+i)-yo(k+i-1), i=1 ..., P;
In formula, Δ xd(k+i) it is the length travel of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;Δyd
(k+i) it is the lateral displacement of vehicle traveling within this period of time of (k+i-1)~(k+i), unit, m;
In order to be controlled by the control action of controller, make it excessive, will to ensure the steering ride comfort of controlled vehicle
An optimization aim of the formula (27) being made up of control input sequence U (k) as Controlling model:
J3=‖ U (k) ‖2 (27)
Weight coefficient is introduced to J1、J2And J3The demand of three optimization aims carries out balance processing, the domain type road of the vehicle of design
The optimization aim of footpath tracing control model is:
<mrow>
<mi>J</mi>
<mo>=</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Gamma;</mi>
<mi>y</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>Y</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mn>1</mn>
<mo>|</mo>
<mi>k</mi>
</mrow>
<mo>)</mo>
<mo>-</mo>
<mi>R</mi>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Gamma;</mi>
<mi>u</mi>
</msub>
<mi>U</mi>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<munderover>
<mo>&Sigma;</mo>
<mrow>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
</mrow>
<mi>P</mi>
</munderover>
<msub>
<mi>&Gamma;</mi>
<mrow>
<mi>d</mi>
<mo>,</mo>
<mi>i</mi>
</mrow>
</msub>
<mrow>
<mo>(</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;x</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>+</mo>
<mo>|</mo>
<mo>|</mo>
<msub>
<mi>&Delta;y</mi>
<mi>d</mi>
</msub>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>|</mo>
<msup>
<mo>|</mo>
<mn>2</mn>
</msup>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>28</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, ΓyAnd ΓuFor weighting matrix;Γd,iFor weight factor;
In order to ensure that controlled vehicle travels in connecting way region all the time, using model predictive control method design vehicle
System output is used restraint during domain type path following control model, should with reference to the inequality relation in step 2 Chinese style (6)
Output constraint can be written to the form as shown in formula (29):
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mi>&beta;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>-</mo>
<msub>
<mi>l</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mi>&beta;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<msubsup>
<mi>f</mi>
<mi>r</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mi>&beta;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msub>
<mi>y</mi>
<mi>o</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>&le;</mo>
<msubsup>
<mi>f</mi>
<mi>l</mi>
<mo>&prime;</mo>
</msubsup>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>+</mo>
<msub>
<mi>l</mi>
<mi>r</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>&psi;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>+</mo>
<mi>&beta;</mi>
<mo>(</mo>
<mrow>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
</mrow>
<mo>)</mo>
<mo>)</mo>
</mrow>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mo>...</mo>
<mo>,</mo>
<mi>P</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>29</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, ψ (k+i)=CψX (k+i), and Cψ=[0 10 0];β (k+i)=CβX (k+i), and Cβ=[0 00 1];fl'
(k+i) it is expectation road area left side boundary line fl' (x) in moment k+i sampled value, unit, m;fr' (k+i) then for it is expected road
Boundary line f on the right of regionr' (x) in moment k+i sampled value, unit, m;
To make vehicle front wheel angle and its rate of change be not higher than the saturation value of steering mechanism, set using model predictive control method
Control constraints and the controlling increment constraint being shown below are considered when counting the domain type path following control model of vehicle:
<mrow>
<mfenced open = "{" close = "">
<mtable>
<mtr>
<mtd>
<mrow>
<mo>|</mo>
<msub>
<mi>&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<mo>&le;</mo>
<msub>
<mi>&delta;</mi>
<mrow>
<mi>f</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
<mtr>
<mtd>
<mrow>
<mo>|</mo>
<msub>
<mi>&Delta;&delta;</mi>
<mi>f</mi>
</msub>
<mrow>
<mo>(</mo>
<mi>k</mi>
<mo>+</mo>
<mi>i</mi>
<mo>)</mo>
</mrow>
<mo>|</mo>
<mo>&le;</mo>
<msub>
<mi>&Delta;&delta;</mi>
<mrow>
<mi>f</mi>
<mi>s</mi>
<mi>a</mi>
<mi>t</mi>
</mrow>
</msub>
</mrow>
</mtd>
</mtr>
</mtable>
</mfenced>
<mo>,</mo>
<mi>i</mi>
<mo>=</mo>
<mn>1</mn>
<mo>,</mo>
<mn>...</mn>
<mo>,</mo>
<mi>N</mi>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>30</mn>
<mo>)</mo>
</mrow>
</mrow>
In formula, δf(k+i) it is k+i moment vehicle front wheel angles, unit, rad;δfsatTo turn to achieved by executing agency most
Big front wheel angle, unit, rad;Δδf(k+i)=δf(k+i)-δf(k+i-1) it is the front wheel angle increment at k+i moment, it is single
Position, rad;ΔδfsatTurn to the maximum front wheel angle increment achieved by executing agency, unit, rad;
To improve the lateral stability of vehicle, its risk turned on one's side, the domain type path following control model of design are reduced
Vehicle centroid side drift angle β should be made to be not more than the critical value β that vehicle is turned on one's side as far as possiblerollover, therefore, using, model is pre-
Consider following state constraint during the domain type path following control model for surveying control method design vehicle:
|β(k+i)|≤βrollover, i=1 ..., P (31)
By arranging formula (25)~(31), the formula (32) of the zone routing tracing control model in step 5 is obtained, solves formula
(32) optimization problem in can obtain an optimal control sequence U*, the general principle of binding model PREDICTIVE CONTROL, choose
Controlled quentity controlled variable is current time optimal front wheel angleFormula (33) as described in step 5:
<mrow>
<msubsup>
<mi>&delta;</mi>
<mi>f</mi>
<mo>*</mo>
</msubsup>
<mo>=</mo>
<msup>
<mi>U</mi>
<mo>*</mo>
</msup>
<mrow>
<mo>(</mo>
<mn>1</mn>
<mo>)</mo>
</mrow>
<mo>-</mo>
<mo>-</mo>
<mo>-</mo>
<mrow>
<mo>(</mo>
<mn>33</mn>
<mo>)</mo>
</mrow>
<mo>.</mo>
</mrow>
9
Priority Applications (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510377605.7A CN104977933B (en) | 2015-07-01 | 2015-07-01 | A kind of domain type path tracking control method of autonomous land vehicle |
Applications Claiming Priority (1)
Application Number | Priority Date | Filing Date | Title |
---|---|---|---|
CN201510377605.7A CN104977933B (en) | 2015-07-01 | 2015-07-01 | A kind of domain type path tracking control method of autonomous land vehicle |
Publications (2)
Publication Number | Publication Date |
---|---|
CN104977933A CN104977933A (en) | 2015-10-14 |
CN104977933B true CN104977933B (en) | 2017-11-14 |
Family
ID=54274529
Family Applications (1)
Application Number | Title | Priority Date | Filing Date |
---|---|---|---|
CN201510377605.7A Active CN104977933B (en) | 2015-07-01 | 2015-07-01 | A kind of domain type path tracking control method of autonomous land vehicle |
Country Status (1)
Country | Link |
---|---|
CN (1) | CN104977933B (en) |
Families Citing this family (49)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
CN105335797B (en) * | 2015-11-03 | 2019-07-16 | 浙江大学 | A kind of autonomous parking track optimizing method based on full simultaneous dynamic optimization frame |
CN105974917B (en) * | 2016-05-11 | 2018-12-14 | 江苏大学 | A kind of vehicle obstacle-avoidance path planning research method based on novel artificial potential field method |
FR3051756B1 (en) * | 2016-05-24 | 2020-03-20 | Renault S.A.S | VEHICLE TRAJECTORY CONTROL DEVICE |
CN106114511B (en) * | 2016-07-21 | 2018-03-06 | 辽宁工业大学 | A kind of automobile cruise system core target identification method |
CN107963120B (en) * | 2016-10-19 | 2020-11-10 | 中车株洲电力机车研究所有限公司 | Automatic steering control method for rubber-tyred low-floor intelligent rail train |
CN107963126B (en) * | 2016-10-19 | 2020-03-17 | 中车株洲电力机车研究所有限公司 | Large-curvature automatic driving steering control method for multi-axle steering vehicle |
JP6603197B2 (en) * | 2016-11-29 | 2019-11-06 | 株式会社デンソー | Nonlinear optimization program, route search program, and route search device for continuous value optimization problem |
US10345812B2 (en) * | 2017-01-10 | 2019-07-09 | GM Global Technology Operations LLC | Methods and apparatus for optimizing a trajectory for an autonomous vehicle |
CN108445750B (en) * | 2017-02-16 | 2022-04-08 | 法拉第未来公司 | Method and system for vehicle motion planning |
CN107015477B (en) * | 2017-04-05 | 2019-10-18 | 同济大学 | Vehicle route based on state feedback tracks H ∞ control method |
CN107168319B (en) * | 2017-06-01 | 2020-07-28 | 同济大学 | Unmanned vehicle obstacle avoidance method based on model predictive control |
CN107600073B (en) * | 2017-08-10 | 2019-07-05 | 同济大学 | A kind of vehicle centroid side drift angle estimating system and method based on Multi-source Information Fusion |
US10571921B2 (en) * | 2017-09-18 | 2020-02-25 | Baidu Usa Llc | Path optimization based on constrained smoothing spline for autonomous driving vehicles |
CN107878453B (en) * | 2017-11-07 | 2019-07-30 | 长春工业大学 | A kind of automobile emergency collision avoidance integral type control method for hiding dynamic barrier |
CN110065491B (en) * | 2018-01-24 | 2021-01-12 | 宝沃汽车(中国)有限公司 | Wheel protection method and device and vehicle |
CN108334086B (en) * | 2018-01-25 | 2021-06-22 | 江苏大学 | Unmanned vehicle path tracking control method based on soft-constraint quadratic programming MPC |
CN108549367A (en) * | 2018-04-09 | 2018-09-18 | 吉林大学 | A kind of man-machine method for handover control based on prediction safety |
CN108646743A (en) * | 2018-06-01 | 2018-10-12 | 浙江亚特电器有限公司 | Path following method for mobile carrier |
CN108973769B (en) * | 2018-06-15 | 2020-01-17 | 吉林大学 | Full-line control electric automobile path tracking control method |
CN108958245A (en) * | 2018-06-29 | 2018-12-07 | 北京理工大学 | A kind of unmanned vehicle path tracking algorithm based on time series |
CN108897216A (en) * | 2018-07-03 | 2018-11-27 | 吉林大学 | A kind of intelligent automobile model prediction decision-making technique based on the virtual field of force in region |
CN109017793B (en) * | 2018-07-26 | 2020-04-07 | 中南大学 | Autonomous parking navigation and control method based on front-rear axis fusion reference |
CN109017804B (en) * | 2018-07-31 | 2022-01-18 | 大连民族大学 | Method for distributing driving torque for each hub motor of vehicle by torque distribution controller |
CN109017760B (en) * | 2018-07-31 | 2022-04-12 | 大连民族大学 | Vehicle expected trajectory tracking method and device and rolling time domain optimization algorithm |
CN109017759B (en) * | 2018-07-31 | 2022-04-08 | 大连民族大学 | Desired path vehicle yaw control method |
CN109017778B (en) * | 2018-07-31 | 2022-04-15 | 大连民族大学 | Active steering control method for expected path of four-wheel independent drive vehicle |
US10739768B2 (en) * | 2018-08-08 | 2020-08-11 | Toyota Motor Engineering & Manufacturing North America, Inc. | Smoothed and regularized Fischer-Burmeister solver for embedded real-time constrained optimal control problems in autonomous systems |
CN109017808B (en) * | 2018-08-16 | 2020-06-05 | 天津大学 | Design method of model predictive controller of unmanned automobile |
CN110370267B (en) * | 2018-09-10 | 2021-08-20 | 北京京东尚科信息技术有限公司 | Method and apparatus for generating a model |
CN109292018B (en) * | 2018-09-13 | 2021-01-12 | 吉林大学 | Four-wheel steering track tracking control method based on coaxial wheel leg structure |
CN109085840B (en) * | 2018-09-21 | 2022-05-27 | 大连维德集成电路有限公司 | Vehicle navigation control system and control method based on binocular vision |
CN111399489B (en) * | 2018-12-14 | 2023-08-04 | 北京京东乾石科技有限公司 | Method and device for generating information |
CN110134042B (en) * | 2019-05-10 | 2021-11-05 | 吉林大学 | Vehicle path tracking prediction controller and FPGA heterogeneous acceleration implementation method |
CN110244721B (en) * | 2019-06-05 | 2022-04-12 | 杭州飞步科技有限公司 | Automatic driving control method, device, equipment and storage medium |
CN110187639B (en) * | 2019-06-27 | 2021-05-11 | 吉林大学 | Trajectory planning control method based on parameter decision framework |
CN110435640A (en) * | 2019-07-25 | 2019-11-12 | 禾多科技(北京)有限公司 | The efficient valet parking planing method cooperateed with based on field end, vehicle end |
CN110377039B (en) * | 2019-07-26 | 2021-07-30 | 吉林大学 | Vehicle obstacle avoidance track planning and tracking control method |
CN110968095A (en) * | 2019-11-23 | 2020-04-07 | 奇瑞汽车股份有限公司 | Automatic driving control method |
CN110989625B (en) * | 2019-12-25 | 2020-11-27 | 湖南大学 | Vehicle path tracking control method |
WO2021168669A1 (en) * | 2020-02-25 | 2021-09-02 | 华为技术有限公司 | Vehicle control method and apparatus |
CN111688704B (en) * | 2020-06-24 | 2021-05-25 | 吉林大学 | Man-machine torque cooperative control method based on driving state prediction |
CN111891118A (en) * | 2020-08-05 | 2020-11-06 | 桂林电子科技大学 | Model predictive control algorithm-based electric vehicle rollover prevention control method |
CN112305911B (en) * | 2020-09-11 | 2023-03-03 | 北京智行者科技股份有限公司 | Feedback prediction control method and device under complex environment and vehicle |
CN112590773B (en) * | 2020-12-15 | 2022-08-19 | 恒大新能源汽车投资控股集团有限公司 | Automatic vehicle warehousing method and device, computer equipment and storage medium |
CN112904719B (en) * | 2021-01-15 | 2022-08-02 | 哈尔滨工程大学 | Annular area tracking control method suitable for underwater robot position |
CN113008258A (en) * | 2021-03-02 | 2021-06-22 | 京东鲲鹏(江苏)科技有限公司 | Path planning method, device, equipment and storage medium |
CN113204236B (en) * | 2021-04-14 | 2022-05-20 | 华中科技大学 | Intelligent agent path tracking control method |
CN114435399B (en) * | 2022-01-27 | 2023-09-12 | 上海工程技术大学 | Automatic driving automobile stability path tracking method based on predictive model |
CN116279596B (en) * | 2023-05-26 | 2023-08-04 | 禾多科技(北京)有限公司 | Vehicle control method, apparatus, electronic device, and computer-readable medium |
Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2020169A2 (en) * | 2007-07-31 | 2009-02-04 | Deere & Company | Method and system for generating end turns |
CN101842278A (en) * | 2007-11-02 | 2010-09-22 | 丰田自动车株式会社 | Vehicle control device and vehicle control method |
CN103085816A (en) * | 2013-01-30 | 2013-05-08 | 同济大学 | Trajectory tracking control method and control device for driverless vehicle |
CN103204162A (en) * | 2012-01-11 | 2013-07-17 | 通用汽车环球科技运作有限责任公司 | Lane Tracking System With Active Rear-steer |
-
2015
- 2015-07-01 CN CN201510377605.7A patent/CN104977933B/en active Active
Patent Citations (4)
Publication number | Priority date | Publication date | Assignee | Title |
---|---|---|---|---|
EP2020169A2 (en) * | 2007-07-31 | 2009-02-04 | Deere & Company | Method and system for generating end turns |
CN101842278A (en) * | 2007-11-02 | 2010-09-22 | 丰田自动车株式会社 | Vehicle control device and vehicle control method |
CN103204162A (en) * | 2012-01-11 | 2013-07-17 | 通用汽车环球科技运作有限责任公司 | Lane Tracking System With Active Rear-steer |
CN103085816A (en) * | 2013-01-30 | 2013-05-08 | 同济大学 | Trajectory tracking control method and control device for driverless vehicle |
Non-Patent Citations (2)
Title |
---|
Automatic path-tracking controller of a four-wheel steering vehicle;Toshihiro Hiraoka et al.;《Vehicle System Dynamics》;20091031;第47卷(第10期);第1205-1227 * |
自主驾驶车辆的预测避障控制;余如 等;《信息与控制》;20150215;第44卷(第1期);第117-124 * |
Also Published As
Publication number | Publication date |
---|---|
CN104977933A (en) | 2015-10-14 |
Similar Documents
Publication | Publication Date | Title |
---|---|---|
CN104977933B (en) | A kind of domain type path tracking control method of autonomous land vehicle | |
CN102495631B (en) | Intelligent control method of driverless vehicle tracking desired trajectory | |
Gao et al. | Robust lateral trajectory following control of unmanned vehicle based on model predictive control | |
CN104859650B (en) | A kind of vehicle yaw stability rolling optimization control method of Multiple Time Scales | |
Cai et al. | Implementation and development of a trajectory tracking control system for intelligent vehicle | |
Chen et al. | Path tracking and handling stability control strategy with collision avoidance for the autonomous vehicle under extreme conditions | |
CN107161207A (en) | A kind of intelligent automobile Trajectory Tracking Control System and control method based on active safety | |
CN108569336A (en) | Vehicle kinematics model rotating direction control method is based under Dynamic Constraints | |
Yu et al. | MPC-based regional path tracking controller design for autonomous ground vehicles | |
Attia et al. | Coupled longitudinal and lateral control strategy improving lateral stability for autonomous vehicle | |
CN107867290A (en) | A kind of automobile emergency collision avoidance layer-stepping control method for considering moving obstacle | |
CN104881025A (en) | Reactive navigation control method for underground mining vehicle | |
Xu et al. | Model predictive control for lane keeping system in autonomous vehicle | |
CN109823393A (en) | A kind of intelligent driving Vehicle tracing control method | |
Zhang et al. | Automatic vehicle parallel parking design using fifth degree polynomial path planning | |
Subosits et al. | Autonomous vehicle control for emergency maneuvers: The effect of topography | |
CN207328574U (en) | A kind of intelligent automobile Trajectory Tracking Control System based on active safety | |
CN107839683A (en) | A kind of automobile emergency collision avoidance control method for considering moving obstacle | |
CN112578672B (en) | Unmanned vehicle trajectory control system based on chassis nonlinearity and trajectory control method thereof | |
CN110161865A (en) | A kind of intelligent vehicle lane-change method for planning track based on Nonlinear Model Predictive Control | |
Liu et al. | Optimum path-tracking control for inverse problem of vehicle handling dynamics | |
Shi et al. | Trajectory planning of autonomous trucks for collision avoidance with rollover prevention | |
Domina et al. | Modelling the dynamic behavior of the steering system for low speed autonomous path tracking | |
Choi et al. | Human driver model and sliding mode control-road tracking capability of the vehicle model | |
Kanchwala et al. | Model predictive tracking controller for a high fidelity vehicle dynamics model |
Legal Events
Date | Code | Title | Description |
---|---|---|---|
C06 | Publication | ||
PB01 | Publication | ||
C10 | Entry into substantive examination | ||
SE01 | Entry into force of request for substantive examination | ||
GR01 | Patent grant | ||
GR01 | Patent grant |