Simple Planar Vehicle

H. G. Kwatny

Department of Mechanical Engineering & Mechanics
Drexel University

Introduction

We consider a simple, 3-wheeled planar vehicle. It illustrates the calculations required to assemble a model involving differential constraints. This system is useful for illustrating basic properties of nonlinear system controllability.

The system is assumed to be composed of a main body with two rear wheels and one front wheel. The front wheel is both the steering and drive wheel. The rear wheels rotate freely about an axle fixed in the body. The assumption of pure rolling imposes a sideslip constraint, but they play no other essential role in the system behavior. Thus, we consider them to have no mass or inertia. The front wheel, on the other hand is assumed to have nontrivial inertia properties and both steering and drive torques are applied to it.. It is assumed to undergo rolling.

In summary, the model is composed of two bodies: (1) the main vehicle body including the rear wheels and (2) the and the front wheel. The model has two joints: (1) the main body joint -- a three degree of freedom (to displacements and a rotation) joint characterizing the motion of the body in a space fixed frame, and (2) the main body/front wheel joint -- a two degree of freedom (steering column and front axle rotations) joint that characterizes the relative motion of the front wheel relative to the main body.

Modeling

<<ProPac`;

?ProPac`Dynamics`*

Main body joint

Note that ϵ defines the wheel castor. ϵ>0 implies that axis points down and forward.

ϵ=0;

r1={3};q1={ϕ,x,y};p1={ωb,vx,vy};
H1={{0,0,0},{0,0,0},{1,0,0},{0,1,0},{0,0,1},{0,0,0}};

r2={1,1};q2={θf,θ};p2={ωf,ω};
H2={{0,-ϵ},{1,0},{0,1},{0,0},{0,0},{0,0},{0,0}};

JointLst={{r1,H1,q1,p1},{r2,H2,q2,p2}};

Body data

cm1={-d/2,0,0};out1={{2,{0,0,0}},{3,{-d,0,-R}},{5,{0,ym,0}} } ;  I1={{Jxx,0,Jxz},{0,Jyy,0},{Jxz,0,Jzz}};
cm2={0,0,0};out2={{4,{0,R Sin[θf],-R Cos[θf]}}};
I2=DiagonalMatrix[{Ix,J,Ix}];

BodyLst={{cm1,out1,m1,I1},{cm2,out2,m2,I2}};
TreeLst={{{1,1},{2,2}}};
q=Flatten[{q1,q2}]
p=Flatten[{p1,p2}]

Q={0,0,0,Tdr,Tst};
{V,X,H,M,Csys,Fsys,psys,qsys}=
	CreateModel[JointLst,BodyLst,TreeLst,g,0,Q];

Z=EndEffector[4,TreeLst,BodyLst,X][[3,4]]

Z/.{θf->0}

Ybar=EndEffector[5,TreeLst,BodyLst,X][[2,4]]

Constraints

Vrear=NodeVelocity[3,TreeLst,BodyLst,X,H,q,p];
Vfront=NodeVelocity[4,TreeLst,BodyLst,X,H,q,p];
c1=Vrear[[5]]
c2=FullSimplify[Vfront[[5]]/.{θf->0}]
c3=FullSimplify[Vfront[[4]]/.{θf->0}]

FullSimplify[Vfront[[6]]/.{θf->0},ϵ∈Reals]

{Mm,Cm,Fm,Vm,Trans,phat}=
Simplify[DifferentialConstraints[M,Csys,Fsys,V,{c1,c2,c3},p,q,{1,2,3}]];

Vm//MatrixForm

q

Vm[[{2,3,1,5}]]//MatrixForm

Simplify[Mm]

f=Simplify[(Cm.phat+Fm)]

Simplify[(Fm)]//MatrixForm

Vm.phat

phat

Stabilizing the Kinematics

RHS=Simplify[(Vm.phat)/.ωf->v/R][[{1,2,3,5}]]

X={x,y,ϕ,θ};Xbar={xbar,ybar,ϕbar,0}
L=(1/2) (X-Xbar).(X-Xbar);
Ldot=Simplify[Jacob[L,X].RHS]

s1=Coefficient[Ldot,v]

s2=Coefficient[Ldot,ω]

RHScl=(RHS/.{v->-Sign[s1],ω->-Sign[s2]})/.{xbar->0,ybar->0,ϕbar->0,d->1}

?MakeODEs

eqns=MakeODEs[X,RHScl,t]

InitialConditions={x[0]==-0.2,y[0]==1,ϕ[0]==0,θ[0]==0};
Eqns=Join[eqns,InitialConditions];

$MachinePrecision

sols=NDSolve[Eqns,X,{t,0,1.0},PrecisionGoal->16]

Plot[Evaluate[y[t]/.sols],{t,0,1}]

Plot[Evaluate[x[t]/.sols],{t,0,1}]

Plot[Evaluate[θ[t]/.sols],{t,0,1}]

Plot[Evaluate[ϕ[t]/.sols],{t,0,1}]

Stability

Equilibria

An equilibrium point requires ω=0, and f=0.

EE=phat.Mm.phat
Edot=Simplify[Jacob[EE,phat].(Inverse[Mm].f)+Jacob[EE,{θ}].{ω}]

Wheelset

<<ProPac`

Main body joint

r1={3};q1={ϕ,x,y};p1={ωb,vx,vy};
H1={{0,0,0},{0,0,0},{1,0,0},{0,1,0},{0,0,1},{0,0,0}};

JointLst={{r1,H1,q1,p1}};

Body data

cm1={0,0,0};out1={{2,{0,d,0}},{3,{0,0,0}},{4,{0,-d,0}} } ;  I1={{Jxx,0,Jxz},{0,Jyy,0},{Jxz,0,Jzz}};

BodyLst={{cm1,out1,m1,I1}};
TreeLst={{{1,1}}};
q=q1;
p=p1;

Q={T,F,0};
{V,X,H,M,Csys,Fsys,psys,qsys}=
	CreateModel[JointLst,BodyLst,TreeLst,g,0,Q];

Constraints

Vset=NodeVelocity[3,TreeLst,BodyLst,X,H,q,p];
c1=Vset[[5]]

p

{Mm,Cm,Fm,Vm,Trans,phat}=
Simplify[DifferentialConstraints[M,Csys,Fsys,V,{c1},p,q,{1,2}]];

Vm//MatrixForm

q

Simplify[Mm]

f=Simplify[(Cm.phat+Fm)]

Vm.phat

phat

RHS=Simplify[Join[(Vm.phat),-f]/.d->1]

Control

s1=vx-vbar/.vbar->(1/10) (Abs[x]+Abs[y])/(1+Abs[x]+Abs[y]);
s2=(ωb+λ (ϕ-ArcTan[-x,-y]))/.λ->5;

X=Join[q,phat]

RHScl=(RHS/.{F->-Sign[s1],T->-4 Sign[s2]})

eqns=MakeODEs[X,RHScl,t]

Simulations 1

InitialConditions={x[0]==0,y[0]==2/10,ϕ[0]==0,vx[0]==0,ωb[0]==0};
Eqns=Join[eqns,InitialConditions];

sols=NDSolve[Eqns,X,{t,0,50.0},AccuracyGoal->12,PrecisionGoal->12,WorkingPrecision->12,MaxSteps->50000,MaxStepSize->0.01,Method->{FixedStep,Method->ExplicitRungeKutta}]

Plot[Evaluate[y[t]/.sols],{t,0,50}]

Plot[Evaluate[vx[t]/.sols],{t,0,50},PlotRange->All]

Plot[Evaluate[((ωb[t]+λ (ϕ[t]-ArcTan[-x[t],-y[t]]))/.λ->5)/.sols],{t,0,50},PlotRange->All]

Plot[Evaluate[ϕ[t]/.sols],{t,0,50},PlotRange->All]

gr1=ParametricPlot[Evaluate[{x[t],y[t]}/.sols],{t,0,50},PlotPoints->1000,PlotRange->All,AxesLabel->{"x","y"}]

Simulations 2

InitialConditions={x[0]==2/10,y[0]==0,ϕ[0]==0,vx[0]==0,ωb[0]==0};
Eqns=Join[eqns,InitialConditions];

Tfinal=50;
sols=NDSolve[Eqns,X,{t,0,Tfinal},AccuracyGoal->12,PrecisionGoal->12,WorkingPrecision->12,MaxSteps->50000,MaxStepSize->0.01,Method->{FixedStep,Method->ExplicitRungeKutta}]

Plot[Evaluate[y[t]/.sols],{t,0,Tfinal}]

Plot[Evaluate[vx[t]/.sols],{t,0,Tfinal},PlotRange->All,AxesLabel->{"t","vx"}]

Plot[Evaluate[((ωb[t]+λ (ϕ[t]-ArcTan[-x[t],-y[t]]))/.λ->5)/.sols],{t,0,Tfinal},PlotRange->All.AxesLabel->{"t","s2"}]

Plot[Evaluate[ϕ[t]/.sols],{t,0,Tfinal},PlotRange->All]

gr2=ParametricPlot[Evaluate[{x[t],y[t]}/.sols],{t,0,Tfinal},PlotPoints->1000,PlotRange->All,AxesLabel->{"x","y"}]

Show[gr1,gr2]

Simulations 3

InitialConditions={x[0]==2/10,y[0]==-2/10,ϕ[0]==0,vx[0]==0,ωb[0]==0};
Eqns=Join[eqns,InitialConditions];

Tfinal=50;
sols=NDSolve[Eqns,X,{t,0,Tfinal},AccuracyGoal->12,PrecisionGoal->12,WorkingPrecision->12,MaxSteps->50000,MaxStepSize->0.01,Method->{FixedStep,Method->ExplicitRungeKutta}]

Plot[Evaluate[y[t]/.sols],{t,0,Tfinal}]

Plot[Evaluate[vx[t]/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[((ωb[t]+λ (ϕ[t]-ArcTan[-x[t],-y[t]]))/.λ->5)/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[ϕ[t]/.sols],{t,0,Tfinal},PlotRange->All]

gr3=ParametricPlot[Evaluate[{x[t],y[t]}/.sols],{t,0,Tfinal},PlotPoints->1000,PlotRange->All,AxesLabel->{"x","y"}]

Show[gr1,gr2,gr3]

Simulations 4

InitialConditions={x[0]==-2/10,y[0]==-2/10,ϕ[0]==0,vx[0]==0,ωb[0]==0};
Eqns=Join[eqns,InitialConditions];

Tfinal=50;
sols=NDSolve[Eqns,X,{t,0,Tfinal},AccuracyGoal->12,PrecisionGoal->12,WorkingPrecision->12,MaxSteps->50000,MaxStepSize->0.01,Method->{FixedStep,Method->ExplicitRungeKutta}]

Plot[Evaluate[y[t]/.sols],{t,0,Tfinal}]

Plot[Evaluate[vx[t]/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[((ωb[t]+λ (ϕ[t]-ArcTan[-x[t],-y[t]]))/.λ->5)/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[ϕ[t]/.sols],{t,0,Tfinal},PlotRange->All]

gr4=ParametricPlot[Evaluate[{x[t],y[t]}/.sols],{t,0,Tfinal},PlotPoints->1000,PlotRange->All,AxesLabel->{"x","y"}]

Show[gr1,gr2,gr3,gr4]

Simulations 5

InitialConditions={x[0]==-2/10,y[0]==2/10,ϕ[0]==0,vx[0]==0,ωb[0]==0};
Eqns=Join[eqns,InitialConditions];

Tfinal=50;
sols=NDSolve[Eqns,X,{t,0,Tfinal},AccuracyGoal->12,PrecisionGoal->12,WorkingPrecision->12,MaxSteps->50000,MaxStepSize->0.01,Method->{FixedStep,Method->ExplicitRungeKutta}]

Plot[Evaluate[y[t]/.sols],{t,0,Tfinal}]

Plot[Evaluate[vx[t]/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[((ωb[t]+λ (ϕ[t]-ArcTan[-x[t],-y[t]]))/.λ->5)/.sols],{t,0,Tfinal},PlotRange->All]

Plot[Evaluate[ϕ[t]/.sols],{t,0,Tfinal},PlotRange->All]

gr5=ParametricPlot[Evaluate[{x[t],y[t]}/.sols],{t,0,Tfinal},PlotPoints->1000,PlotRange->All,AxesLabel->{"x","y"}]

Show[gr1,gr2,gr3,gr4,gr5]

Control Constraints

ForceEqns={F==F1+F2,T==(F1-F2) d};
Solve[ForceEqns,{F1,F2}]

ForceEqns2={F==F1+F2,T==(F1-F2) ,F1≤1,F1≥-1,F2≤1,F2≥-1};
Reduce[ForceEqns2,{F1,F2},Reals]

RegionPlot[(-2<T≤0&&-2-T≤F≤2+T)||(0<T<2&&-2+T≤F≤2-T),{T,-3,3},{F,-3,3},AxesLabel->{"T","F"}]