> restart: libname := libname,"C:/MBSymba": with(MBSymba_r6):
System modelling
References:
A SYMBOLIC APPROACH FOR AUTOMATIC GENERATION OF THE EQUATIONS OF MOTION OF MULTIBODY SYSTEMS
Roberto Lot and Mauro Da Lio
Proceedings of the ECCOMAS Thematic Conference Multibody Dynamics 2003
on Advances in Computational Multibody Dynamics
Instituto Superior Técnico Lisboa, Portugal, July 1-4, 2003
'sin' and 'cosine' functions
> alias(c=cos,s=sin);
> PDEtools[declare](x(t),y(t),psi(t),phi(t),theta1(t),theta4(t),delta(t),psi4(t), Yr(t),Yf(t),gamma1(t),gamma2(t),gamma3(t),gamma4(t), prime=t,quiet):
Bodies
rear chassis
frame T1, fixed to the rear chassis in absence of roll angle
> T1 := translate(x(t),y(t),0) * rotate('Z',psi(t)):
frame T2, fixed to the rear chassis
> T2 := T1 * rotate('X',phi(t));
center of mass
> Gr := make_POINT(T2,0,0,-h): show(Gr);
> Cryz:=0: Crxy:=0: #because of the simmetry with respect XY plane
> rear_chassis := make_BODY(Gr,mr,Irx,Iry,Irz,Cryz,Crxz,Crxy): show(rear_chassis);
rear wheel
rear wheel reference frame
> TWR := T2 * translate(-b,0,-Rr) * rotate('Y',theta1(t)):
> rear_wheel := make_BODY(TWR,0,0,iry,0): show(rear_wheel,'syntethic');
front chassis
frame T4, fixed to the front chassis (see Sharp paper)
> T4 := T2*rotate('Y',epsilon) * translate(a,0,0)* rotate('Z',delta(t));
> Gf := make_POINT(T4,e,0,f): show(Gf,'syntetic');
> front_chassis := make_BODY(Gf,mf,Ifx,0,Ifz): show(front_chassis,'syntetic');
front wheel
> E := project( make_POINT(T4,lx,0,lz) ,ground): show(E,'syntethic'); wheel center
> TWF := translate(comp_XYZ(E)) * rotate('Z',psi4(t)) * rotate('X',phi4(t)) * rotate('Y',theta4(t));
> front_wheel := make_BODY(TWF,0,0,ify,0): show(front_wheel,'syntethic');
Forces
rear wheel contatc point
> Cr := make_POINT(TWR, -Rr*sin(theta1(t)) , 0 , Rr*cos(theta1(t))):
show(Cr,'synthetic');
rear wheel contatc force
> make_VECTOR(rotate('Z',psi(t)), 0,Yr(t),Zr):
rear_tire_forces := make_FORCE (%, Cr, rear_wheel, NULL):
show(rear_tire_forces);
front wheel contatc point
> Cf := make_POINT(TWF, -Rf*sin(theta4(t)) , 0 , Rf*cos(theta4(t))):
show(Cf,'synthetic');
front wheel contatc force
> make_VECTOR(rotate('Z',psi4(t)), 0,Yf(t),Zf):
front_tire_forces := make_FORCE (%, Cf, front_wheel):
show(front_tire_forces);
handlebar
> make_VECTOR(T4, 0,0,tau(t)-C[delta]*diff(delta(t),t)):
steering_torque := make_TORQUE(%, front_chassis,rear_chassis):
show(steering_torque,'syntetic');
Constraints
front wheel spin axis, defined using front wheel frame
> spin_axis := project(make_VECTOR(TWF,0,1,0),ground): show(spin_axis);
front wheel spin axis, defined using frame T4
> spin_axis_copy :=project( make_VECTOR(T4 ,0,1,0),ground): show(spin_axis_copy);
both definitions must yield the same vector, thus the following constraint equations must be satisfied
It must be observed that only two components of the vectorial equation are independent !!
> spin_X := comp_X(spin_axis)-comp_X(spin_axis_copy);
spin_Z := comp_Z(spin_axis)-comp_Z(spin_axis_copy);
no longitudinal slip allowed
> VCr_euler := project(eulerian_velocity(Cr),T1): show(VCr_euler);
> no_slip_rear := comp_X(VCr_euler);
front wheel
> VCf_euler := project(eulerian_velocity(Cf),rotate('Z',psi4(t))): #show(VCf_euler);
> no_slip_front := comp_X(VCf_euler);
Tires
> get_no_objects();
rear lateral force (relaxation equation)
> VCr_abs := project(velocity(Cr),T1);
VSr := comp_X(VCr_abs); VNr := comp_Y(VCr_abs);
> equ_Yr := sigma/'VSr'*diff(Yr(t),t) + Yr(t) - ( Cr1*arctan(-'VNr'/'VSr') +Cr2*phi(t) );
front lateral force (relaxation equation)
> VCf_abs := project(velocity(Cf),rotate('Z',psi4(t))):
VSf := comp_X(VCf_abs); VNf := comp_Y(VCf_abs);
equ_Yf := sigma/'VSf'*diff(Yf(t),t) + Yf(t) - ( Cf1*arctan(-'VNf'/'VSf') +Cf2*phi4(t) );
collect additional equations and variables
> hh := [equ_Yr,equ_Yr]: pp:=[Yr(t),Yf(t)];
Non-Linear Equations of Motion
Assembly Multibody System
> bodies := [rear_wheel,rear_chassis,front_chassis,front_wheel];
> constraints := make_CONSTRAINT([no_slip_rear,no_slip_front,spin_X,spin_Z], [gamma1(t),gamma2(t),gamma3(t),gamma4(t)]): show(constraints);
gravity forces
> for i from 1 to nops(bodies) do
FG[i]:=make_FORCE( make_VECTOR(ground,0,0,bodies[i][mass]*g), CoM(bodies[i]) ,bodies[i]);
end do:
forces
> forces := [rear_tire_forces,front_tire_forces,steering_torque] union [seq(FG[i],i=1..nops(bodies))];
dependent variables
> qq := [x(t), y(t), psi(t), delta(t),phi(t), psi4(t) , phi4(t), theta1(t), theta4(t) ];
nc:=nops(qq);
equations of motion
> leqns := lagrange_equations(bodies union forces union [constraints] ,qq):
these equations are vety long and complex, indeed
> codegen[cost](leqns);
> #for i from 1 to nc do qq[i]; leqns[i]; od;
> leqns[1];
variable list
> qq union constraints[vars] union [Yr(t),Yf(t)];
Linearized equations for Straight Running Motion
straight running conditions
> straight_run := [ x(t)=V*t ,
y(t)=0, psi(t)=0, phi(t)=0, delta(t)=0, psi4(t)=0, phi4(t)=0, theta1(t)=-V*t/Rr, theta4(t)=-V*t/Rf, Yr(t)=0, Yf(t)=0, seq(constraints[vars][i]=0,i=1..nops(constraints[vars]))];
verify the solution
> map(expand,subs(straight_run,tau(t)=0, leqns));
linearization of Lagrange's equations
> lin_leqns := linearize(leqns, diff(straight_run,t,t) union diff(straight_run,t) union straight_run ):
> codegen[cost](lin_leqns);
> for i from 1 to nops(lin_leqns) do qq[i]; lin_leqns[i]; od;
> lin_constraints := linearize(constraints,qq union diff(qq,t)): show(lin_constraints);
even if non-linear constraints are non-holonomic, the linearization yields to holonomic constraints!!
> lin_constraints[expr][1] :=int(lin_constraints[expr][1],t):
lin_constraints[expr][2]:= int(lin_constraints[expr][2],t):
print("after integration"): show(lin_constraints);
independent coordinates
> zz := [x(t), y(t), psi(t), delta(t), phi(t)];
dependent coordinates
> yy := [phi4(t),psi4(t),theta1(t),theta4(t)];
> change_vars := op(solve(lin_constraints[expr],yy));
velocity projection matrix RR
> #II := Matrix(1..nops(zz),1..nops(zz),1,shape=diagonal): identity matrix - independent coordinates
> #IL := -linalg[genmatrix](change_vars,zz): compute dependent coordinates
> #RR := linalg[stackmatrix](II,IL); assembly the velocity projection matrix
Velocity projection matrix RR (the simplest way)
> subs(change_vars,qq);
#RR := linalg[genmatrix](%,zz);
(RR,bb):=LinearAlgebra[GenerateMatrix](%,zz):'RR'=RR;
> lin_independent_leqns := LinearAlgebra[Transpose](RR).Vector(subs(change_vars,lin_leqns)):
> for i from 1 to nops(zz) do zz[i]; collect(lin_independent_leqns[i],diff(zz,t,t) union diff(zz,t) union zz); od;
completion with tires equations
> lin_tire_eqns :=subs(change_vars, linearize([equ_Yr,equ_Yf],diff(straight_run,t) union straight_run) );
assembly equations
> all_eqns := convert(lin_independent_leqns,list) union lin_tire_eqns:
> all_vars := zz union [Yr(t),Yf(t)];
Sharp's equations
Introducing the velocity of point A in moving coordinates (as in the Sharp's paper)
> PDEtools[declare](x1[dot](t),y1[dot](t),prime=t,quiet):
> VA := make_VECTOR(T1,x1[dot](t),y1[dot](t),0): show(VA);
> VA := linearize( project(VA,ground) , [x1[dot](t)=V,y1[dot](t),psi(t)]): show(VA);
> moving_vars := [ diff(x(t),t)=comp_X(VA), diff(y(t),t)=comp_Y(VA) ];
simplify using relations between geometric dependent parasmeters
> global_properties := {
#l = 1/c(epsilon)*(-an+c(epsilon)*k-e+j*s(epsilon)),
f = s(epsilon)*k-c(epsilon)*j,
lx = -an+Rf*s(epsilon),
a = c(epsilon)*k-e+j*s(epsilon),
lz = -1/c(epsilon)*(-s(epsilon)*k*c(epsilon)+c(epsilon)^2*Rf+s(epsilon)*e-j+c(epsilon)^2*j+s(epsilon)*an)
}:
for i from 1 to nops(global_properties) do global_properties[i]: od;
change vars and neglect the X equation
> sharp_leqns := subs(global_properties,moving_vars,all_eqns[2..nops(all_vars)]):
sharp_leqns := map(expand,map(simplify,sharp_leqns,trig)):
sharp_vars := [y1[dot](t), psi(t), delta(t), phi(t), Yr(t), Yf(t)];
> for i from 1 to nops(sharp_leqns) do sharp_vars[i]; collect(sharp_leqns[i], diff(sharp_vars,t) union diff(sharp_vars,t,t) ); od;
reduction to 1st order
> (xx,state_eqns) := first_order(sharp_leqns,sharp_vars,t,flag):
for i from 1 to nops(state_eqns) do xx[i]; collect(state_eqns[i],xx union diff(xx,t)); od;
> 'xx'=xx;
state space formulation
matrix form A1*Xdot = A*(X-X0) + B*(U-U0)
> uu := [tau(t)]; driving vector
(A1,A,B) := implicit_state_space(state_eqns,t,xx,uu):
> j*s(epsilon) = a -c(epsilon)*k+e; A := map(expand,subs(%,map(simplify,A))):
> 'A1'=evalm(A1);
'A'=evalm(A), 'B'=evalm(B);
Stability Analysis
> motorcycle_data := {g = 9.81,
rtf = 0, Rf = 0.3048, Cf1 = 11174, Cf2 = 938.6, sigma = 0.2438,
rtr=0, Rr = 0.3048, Cr1 = 15831, Cr2 = 1325.6,
an = 0.1158, epsilon = 0.4715, l = 0.93468, a=0.9485376,
C[delta] = 6.77, Zf = -1005.3,
mf = 30.65, e = 0.0244, f = 0.0283, Ifx = 1.2338, Ifz = 0.4420, ify = 0.7186,
mr = 217.45, b = 0.4798, h = 0.6157, Irx = 31.18, Irz = 21.07, Crxz = -1.7355, iry = 1.0508 }:
> {k = a*c(epsilon)+f*s(epsilon)+c(epsilon)*e, j = s(epsilon)*a-c(epsilon)*f+s(epsilon)*e}:
> motorcycle_data := motorcycle_data union evalf(subs(motorcycle_data,%));
save
> save A1,A,B,xx,uu,motorcycle_data,"state_space_symb.m";
numerical state space matrix
> A1N := convert(evalf(subs(motorcycle_data,evalm(A1))),Matrix):
> AN := convert(evalf(subs(motorcycle_data,evalm(A ))),Matrix):
> interface(displayprecision=3):
Eigenvalues and Frequecy Responce Functions
> speed := 20;
> eivals := LinearAlgebra[Eigenvalues](subs(V=speed,AN),subs(V=speed,A1N)):
Frequecy Responce Functions
> freq := 2;
> GG := evalf(subs(V=speed,I*2*Pi*freq*A1N-AN)):
> FRFs := evalm(linalg[inverse](GG) &* B):
> 'eivals'=eivals, convert(convert(xx,array),matrix), 'FRFs'=evalm(FRFs);
Eigenvalues varying speed
speed vector
> speed := [ seq(1.*i,i=1..60)]:
> np := nops(speed):
nn := nops(xx):
eigenvalues
> eigen_vals := 'eigen_vals':
for i from 1 to np do
(eigen_vals[i],eigen_vect[i]) := LinearAlgebra[Eigenvectors](subs(V=speed[i],eval(AN)),subs(V=speed[i],eval(A1N)));
od:
Nyquist plot
> #plot( [seq( [[Re(eigen_vals[n][i]),Im(eigen_vals[n][i])] $n=1..np ] ,i=1..nn)], style=point, symbol=circle);
> Xrange := -25..5: Yrange := 0..70:
> plot( [seq( [[Re(eigen_vals[n][i]),Im(eigen_vals[n][i])] $n=1..np ] ,i=1..nn)], real = Xrange, imag = Yrange, style=point, symbol=circle, color=blue); #, title="Nyquist Plot"