Control of the single-track vehicle


Full-linear vehicle modelling

> restart: libname := libname, "C:/MBSymba": with(MBSymba_r6):
LA:=LinearAlgebra:

> PDEtools[declare]({x(t),y(t),u(t),v(t),psi(t),omega(t),delta(t)},prime=t,quiet);

Definitio of small variables

> linear_modeling({v(t),y(t),psi(t),omega(t),delta(t)});

Warning, Linear Modeling option has been choosen for the following variables:

Definition of the Moving Frame

> TV := translate(U*t,y(t),0) . rotate('Z',psi(t)):

The forward speed U is assumed constant

> track_XY := use_moving_frame(U,v(t),0,0,0,omega(t),TV):

Trajectory equations

> Vector(track_XY);

Warning, The base frame <moving_frame> has been selected.

vehicle chassis

define main frame

> TV := moving_frame:

define vehicle body

> vehicle := make_BODY(TV,M,IX,IY,IZ): show(vehicle,'synthetic');

rear tire

> R := make_POINT(TV,-b,0,0): show(R);

rear tire kinematics

> VR:=velocity(R):show(VR);

> lambda[R] := linearize( -arctan( comp_Y(VR,TV)/comp_X(VR,TV) )):

force

> Fr := KLr * lambda[R];
make_VECTOR(TV, 0, Fr ,0):
rear_tire := make_FORCE(%, R, vehicle): show(rear_tire);

front tire

> TF := TV * Translate(a,0,0) * Rotate('Z',delta(t));

contact point

> F := origin(TF): show(F,'synthetic');

front tire kinematics

> VF := project(velocity(F),TF): show(VF);

> lambda[F] := linearize ( -arctan( comp_Y(VF,TF)/comp_X(VF,TF) ) );

force

> Ff := KLf * lambda[F];
make_VECTOR(TF, 0, Ff ,0):
front_tire := make_FORCE(%, F, vehicle): show(front_tire);

Newton-Euler equations

Newton

> eqnsN := newton_equations({vehicle,rear_tire,front_tire}): show(eqnsN);

Euler

> eqnsE := euler_equations({vehicle,rear_tire,front_tire},CoM(vehicle)):
show(eqnsE);

State space formulation

> lin_eqns := track_XY[2..3] union [comp_Y(eqnsN), comp_Z(eqnsE)]: Vector(lin_eqns);

state variables

> xx := [y(t),psi(t),v(t),omega(t)];

system inputs

> uu := [delta(t)];

observed variables

> yy := [y(t), 'Ff', 'Fr'];

state space formulation

> AA,BB,CC,DD := state_space(lin_eqns,t,xx,uu,yy);

> AA := map(simplify,AA):

Classical PD control

define PD control on the lateral position

> PD := delta(t) = ( KP*(yT-y(t)) - KD*diff(y(t),t) );

> PD := subs( solve({lin_eqns[1]},diff(y(t),t)), PD);

> ctrl_eqns := subs(PD,lin_eqns): Vector(ctrl_eqns);

> AA, BB, CC,DD := state_space(ctrl_eqns, t, xx, [yT], subs(PD,yy)):

Stability Analysis

> understeer_vehicle :=[
a=1.3, from Cr to G
b=1.3, from G to Cf
M=1000.,mass
IZ=600,yaw inertia
KLr=24.*5000,rear tire cornering stiffness
KLf=20.*5000 front tire cornering stiffness
];

> controlled_stability := proc(vehicle,speed, myKP,myKD)
local ei,i:
printf("speed = %5.1f\t\tKP = %+6.3f\t KD = %+6.3f \neigenvalues:\n",speed, myKP,myKD);
ei := LinearAlgebra[Eigenvalues](subs(vehicle,U=speed,KP=myKP,KD=myKD,AA)):
for i from 1 to 4 do printf("%+8.3f%+8.3f i\n",Re(ei[i]),Im(ei[i])); end do;
map(Re,ei); max(op(convert(%,list))):
if %>0 then printf("the vehicle is UNSTABLE") else printf("the vehicle is STABLE") end if:
printf("\n "):
end:

> AAK;

1. the free-vehicle is stable

> controlled_stability (understeer_vehicle,30,0,0);

speed = 30.0 KP = +0.000 KD = +0.000

eigenvalues:

+0.000 +0.000 i

+0.000 +0.000 i

-12.482 +0.000 i

-15.507 +0.000 i

the vehicle is STABLE

2. the properly controlled vehicle is more stable

> controlled_stability (understeer_vehicle,30,+0.2,+0.2);

speed = 30.0 KP = +0.200 KD = +0.200

eigenvalues:

-37.820 +0.000 i

-4.576 +15.792 i

-4.576 -15.792 i

-1.017 +0.000 i

the vehicle is STABLE

3. the unproperly controlled vehicle is unstable

> controlled_stability (understeer_vehicle,30,-0.2,-0.2);

speed = 30.0 KP = -0.200 KD = -0.200

eigenvalues:

+23.732 +0.000 i

-15.369 +14.466 i

-15.369 -14.466 i

-0.984 +0.000 i

the vehicle is UNSTABLE

Stability Analysis varying control gains

> maxRE := proc(vehicle,speed, myKP,myKD)
LinearAlgebra[Eigenvalues](subs(vehicle,U=speed,KP=myKP,KD=myKD,AA)):
max(op(convert(map(Re,%),list))):
end:

> KPmin:= -0.06: KPmax:= 0.4: KPstep:= .02:set KP limit
KDmin:= -0.06: KDmax:= 0.4: KDstep:= .02:set KD limit

> eiS10 := [seq([ seq([myKP,myKD, maxRE(understeer_vehicle,10,myKP,myKD)], myKP=KPmin..KPmax,KPstep)], myKD=KDmin..KDmax,KDstep)]:
eiS30 := [seq([ seq([myKP,myKD, maxRE(understeer_vehicle,30,myKP,myKD)], myKP=KPmin..KPmax,KPstep)], myKD=KDmin..KDmax,KDstep)]:
eiS50 := [seq([ seq([myKP,myKD, maxRE(understeer_vehicle,50,myKP,myKD)], myKP=KPmin..KPmax,KPstep)], myKD=KDmin..KDmax,KDstep)]:

> p10 := plots[surfdata](eiS10,contours=[0],style=contour,thickness=2,color=black):
p30 := plots[surfdata](eiS30,contours=[0],style=contour,thickness=2,color=blue):
p50 := plots[surfdata](eiS50,contours=[0],style=contour,thickness=2,color=red):

> plots[display](p10,p30,p50, orientation=[270, 0], view=[-KPmax/2..KPmax, -KDmax/2..KDmax, -10..10], axes=normal, labels=["proportional gain KP","derivative gain KD","Real[1/s]"]);

Frequency Response

> HX := LinearAlgebra[MatrixInverse](s-AA) . BB:

> DEN = denom(HX[1,1]);



> 'HX' = 1/DEN * map(numer,HX);

> HY := CC.HX+DD:

> 'HY' = 1/DEN * map(numer,HY);

> interface(displayprecision=2):

> HXN := unapply( subs(understeer_vehicle,s=I*omega,HX), U,KP,KD,omega):

> HYN := unapply( subs(understeer_vehicle,s=I*omega,HY), U,KP,KD,omega):

> iy := 1: iFr:=2: iFf:=3: row indices

Bode plot

> plots[loglogplot]([abs(HYN(30,0.2,0.2,omega)[iy,1])],omega=1e-2..1e2,title="LATERAL POSITION",color=[magenta],thickness=2);

> HYC := HYN(30,0.2,0.2,omega):

> pM := plots[loglogplot]([abs(HYC[iFr,1]),abs(HYC[iFf,1])],omega=1e-2..1e2,title="REAR & FRONT TIRE FORCES",color=[red,blue]):

> pPH := plots[semilogplot]([arctan(Im(HYC[iFr,1]),Re(HYC[iFr,1])), arctan(Im(HYC[iFf,1]),Re(HYC[iFf,1])) ]
,omega=1e-2..100,title="phase",view=[1e-2..1e3,-Pi..Pi],color=[red,blue,black]):

> plots[display](array(1..2,1..1,[[pM],[pPH]]));

Comments are closed.