Generalized Coordinates: 's

The generalized coordinates are also called Lagrangian coordinates.

 =  driving cart (i.e. HFLC) linear position

Relative Pendulum angles:

$\alpha$  = Lower inverted pendulum position angle about the vertical: the zero angle [mod 2*pi] is defined when the IP is in the perfect upright position.

$\theta \left(t\right)$  = Upper inverted pendulum position angle about the vertical of the lower pendulum: the zero angle [mod 2*pi] is defined when the IP is in the perfect upright position w.r.t. to the lower pendulum.

Np = number of links (i.e. pendulums)

>

Np := 2:

>	q := [ x[c](t), alpha(t), theta(t) ];

Nq = number of Lagrangian coordinates

Nq is also the number of position states.

>	Nq := nops( q ):

qd = first-order time derivative of the generalized coordinates, e.g. position and angular velocities

>	qd := map( diff, q, t );

Transformation Matrices

Translation from the base to cart position.

>	T[0,c] := HTM('trans',q[1],0,0);

Rotation about $\alpha$  joint.

>	R[alpha] := HTM('rot','Z',q[2]);

Translation to pendulum 1 cog.

>	T[p1] := HTM('trans',0,l[p1],0);

Translation to hinge.

>	T[ph] := HTM('trans',0,L[p1],0);

Cart to pendulum 1 cog.

>	T[c,1] := Multiply(R[alpha],T[p1]);

Cart to hinge

>	T[c,h] := Multiply(R[alpha],T[ph]);

Rotation about $\theta$ .

>	R[theta] := HTM('rot','Z',q[3]);

Translation to cog 2.

>	T[p2] := HTM('trans',0,l[p2],0);

Hinge to cog 2

>	T[h,2] := Multiply( R[theta], T[p2] );

Cart to cog 2.

>	T[c,2] := Multiply( T[c,h], T[h,2] );

Base coordinate system to cog 1.

>	T[0,1] := Multiply( T[0,c], T[c,1] );

Base coordinate system to hinge.

>	T[0,h] := Multiply( T[0,c], T[c,h] );

Base coordinate system to the cog 2.

>	T[0,2] := Multiply( T[0,c], T[c,2] );

Cartesian Coordinates of the Pendulum's Centre Of Gravity

conventions:

1) $\left[\alpha =0,\theta =0\right]$  corresponds to both pendulums perfectly vertical, pointing downswards.

2) positive rotation is CCW  when facing the cart.

 = x-coordinate of each pendulum's Centre Of Gravity (COG) absolute Cartesian position

 = y-coordinate of each pendulum's Centre Of Gravity (COG) absolute Cartesian position

>	x[p,1] := T[0,1][1,4]; y[p,1] := T[0,1][2,4]; x[p,2] := T[0,2][1,4]; y[p,2] := T[0,2][2,4];

 = x-coordinate of the hinge COG absolute Cartesian position

 = y-coordinate of the hinge COG absolute Cartesian position

>	x[h] := T[0,h][1,4]; y[h] := T[0,h][2,4];

 = x-coordinate of each pendulum's Centre Of Gravity (COG) absolute Cartesian velocity
 = y-coordinate of each pendulum's Centre Of Gravity (COG) absolute Cartesian velocity

>	xd[p,1] := diff(x[p,1],t); yd[p,1] := diff(y[p,1],t); xd[p,2] := diff(x[p,2],t); yd[p,2] := diff(y[p,2],t);

 = x-coordinate of the hinge COG absolute Cartesian position

 = y-coordinate of the hinge COG absolute Cartesian position

>	xd[h] := diff(x[h],t); yd[h] := diff(y[h],t);

State-Space Variables

• The chosen states should at least include the generalized coordinates and their first-time derivatives.
• X is the state vector.
• In the state vector X: Lagrangian coordinates are first, followed by their first-time derivatives, and finally any other states, as required.

Substitution sets for the states (to obtain time-independent state equations).

>	subs_Xq := { seq( q[i] = X[i], i=1..Nq ) }; subs_Xqd := { seq( qd[i] = X[i+Nq], i=1..Nq ) };

Substitution set for the input(s).

set the input to be the motor voltage:

>	subs_U := { V[m] = U[1] }:

set the input to be the cart's driving force:  (if not expressed as a function of the motor voltage):

>	#subs_U := { F[c] = U[1] }:

Nu = number of inputs; U = input (row) vector (e.g. U = [ V[m] ] )

>	Nu := nops( subs_U ):

substitution set for the position states' second time derivatives

>	subs_Xqdd := { seq( diff( q[i], t$2 ) =  Xd[i+Nq], i=1..Nq ) };

second time derivatives of the position states (written as time-independent variables).

The set of unknowns is obtained from this list to solve the Lagrange's equations of motion.

>	Xqdd := [ seq( Xd[i+Nq], i=1..Nq ) ];

substitution set to linearize the state-space matrices (i.e. A and B)

about the quiescent null state vector (small-displacement theory)

>	subs_XUzero := { seq( X[i] = 0, i=1..2*Nq ), seq( U[i] = 0, i=1..Nu ) }:

Nx = dim( X ) = total number of states (should be greater than or equal to: 2 * Nq)

Ny = chosen number of outputs

>	Nx := 2 * Nq + 0: Ny := Nq:

Total Potential Energy:  

The total potential energy can be expressed in terms of the generalized coordinates alone.

V[e] = Total Elastic Potential Energy of the system

>	V[e] := 0;

Pendulum 1 potential energy

>	Vg[1] := potential_energy( 'gravity', M[p1], g, y[p,1] );

Pendulum 2 potential energy

>	Vg[2] := potential_energy( 'gravity', M[p2], g, y[p,2] );

Hinge potential energy

>	Vg[h] := potential_energy( 'gravity', M[h], g, y[h] );

Vg = Total Gravitational Potential Energy of the system

>	V[g] := Vg[1] + Vg[2] + Vg[h];

V[T] = Total Potential Energy of the system

>	V[T] := simplify(V[g] + V[e]);

Total Kinetic Energy:

The total kinetic energy can be expressed in terms of the generalized coordinates and their first-time derivatives.

 = translational kinetic energy of the motorized cart (e.g. HFLC)

 = cart total mass

>	Tt[c] := kinetic_energy( 'translation', M[c], qd[1] );

The cart's directions of translation and rotation are orthogonal.

 = kinetic energy due to rotation tied to the motorized cart

 = motor armature inertia;  = gear ratio;  = motor pinion radius

>	Tr[c] := kinetic_energy( 'rotation', J[m], omega[m](t));

Angular rate of motor w.r.t. linear velocity (no gearbox).

>	REL_W_X := omega[m](t) = K[g] * qd[1] / r[mp]: REL_W_X;

Express rotational kinetic energy in terms of linear velocity.

>	K_ROT_CART := subs(REL_W_X,Tr[c]): K_ROT_CART;

Evaluate coefficient of above term.

>	K_ROT_CART_COEFF := coeff(K_ROT_CART,diff(x[c](t),t),2): K_ROT_CART_COEFF = evalf(subs(IP02_PARAM,K_ROT_CART_COEFF),3)*Unit(kg);

Given that 0.0680 kg << Mc (0.14 kg), the rotational kinetic energy from the cart can be neglected. Therefore

>	Tr[c] := 0;

Pendulum 1 motion is composed of one rotation and one translation, both directions are orthogonal.

$\frac{d}{dt}\alpha \left(t\right)$  = pendulum 1 absolute angular velocity;

Tr[p,1] = pendulum 1 rotational kinetic kinergy;

>	#Tr[p,1] := kinetic_energy( 'rotation', J[p1], qd[2] ); Tr[p,1] := 0;

v[p,1] = pendulum 1 cartesian velocity (magnitude);

Tt[p,1] = pendulum 1 translational kinetic kinergy

>	v[p,1] := n_norm( [ xd[p,1], yd[p,1] ], 2 ): Tt[p,1] := kinetic_energy( 'translation', M[p1], v[p,1] );

Pendulum 2 motion is composed of one rotation and one translation, both directions are orthogonal.

$\frac{d}{dt}\theta \left(t\right)$  = pendulum 2 absolute angular velocity

Tr[p,2] = pendulum 2 rotational kinetic kinergy

>	#Tr[p,2] := kinetic_energy( 'rotation', J[p2], qd[3] ); Tr[p,2] := 0;

v[p,2] = pendulum 2 cartesian velocity (magnitude)

Tt[p,2] = pendulum 2 translational kinetic kinergy

>	v[p,2] := n_norm( [ xd[p,2], yd[p,2] ], 2 ): Tt[p,2] := kinetic_energy( 'translation', M[p2], v[p,2] );

The hinge motion is composed one translation

v[h] = hinge cartesian velocity (magnitude);

Tt[h] = hinge translational kinetic kinergy

>	v[h] := n_norm( [ xd[h], yd[h] ], 2 ): Tt[h] := kinetic_energy( 'translation', M[h], v[h] );

Total kinetic energy.

>	T[T] := collect( simplify( Tr[c]+Tt[c]+Tr[p,1]+Tt[p,1]+Tr[p,2]+Tt[p,2]+Tt[h] ), diff );

Linearization  in the EOM's   of the Trigonometric Functions

Linearization of the equations of motion around the quiescent point of operation (in order to solve them).

Here, linearization around the zero angles, i.e. for small-amplitude oscillations.

Linearization around: alpha0 = 0, alpha_dot0 = 0, alpha1 = 0, and alpha_dot1 = 0

>	EOM_ser := EOM_states:

Generalized series expansions of the trigonometric functions is used (for small angles).

>	for i from 1 to Nq do   for k from 1 to Np do     EOM_ser[i] := subsop( 1 = convert( series( op( 1, EOM_ser[i] ), X[ k+1 ] ), polynom ), EOM_ser[i] );   end do:   EOM_ser[i] := simplify( EOM_ser[i] ); end do:

>

EOM_ser;

Additional Insight: Inertia (or mass) Matrix: Fi

The nonlinear system of equations resulting from the Lagrangian mechanics can be written in the following matrix form:

F( q ) . qdd + G( q, qd ) . qd + H( q ) . q = L( q, qd, u )

F, G, and H are called, respectively, the mass, damping, and stiffness matrices.

They are symmetric in form.

The inertia (a.k.a. mass) matrix, F, gives indications regarding the coupling existing in the system.

>	Fi := Matrix( Nq, Nq ):

>	for i from 1 to Nq do   for k from 1 to Nq do     Fi[ i, k ] := simplify( diff( op( 1, EOM_states[i] ), Xd[k+Nq] ) );     Fi[ i, k ] := collect( combine( Fi[ i, k ], trig ), cos );   end do; end do:

>	'F[i]' = Fi;

Linearization of the inertia matrix for small-displacements

>	Fi_lin := Matrix( Nq, Nq ):

>	for i from 1 to Nq do   for k from 1 to Nq do     Fi_lin[ i, k ] := Fi[ i, k ];     Fi_lin[ i, k ] := convert( series( Fi_lin[ i, k ], X[ 2 ] ), polynom );     Fi_lin[ i, k ] := subs( subs_XUzero, Fi_lin[ i, k ] );   end do; end do:

>	'F_lin' = Fi_lin;

Reverse State Substitution for Pretty Display of the Solved EOM's

only for pretty print

>	subs_Xq_rev := { seq( X[i] = q[i], i=1..Nq ) }: subs_Xqd_rev := { seq( X[i+Nq] = qd[i], i=1..Nq ) }:

>	subs_U_rev := { U[1] = V[m] }: #subs_U_rev := { U[1] = F[c] }:

>	eom_collect_list := { seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ) };

>	if not assigned( J[p1] ) then   eom_collect_list := eom_collect_list union { J[p1] }; end if;

>	if not assigned( J[p2] ) then   eom_collect_list := eom_collect_list union { J[p2] }; end if;

Solution to the Non-Linear Equations of Motion

Solve the non-linear form of the equations of motion for the states' second time derivatives

>	Xqdd_solset_nl := solve( convert( EOM_states, set ), convert( Xqdd, set ) ):

>	assign( Xqdd_solset_nl );

>	for i from 1 to Nq do   Xd_nl[i+Nq] := simplify( Xd[i+Nq] ):   unassign( 'Xd[i+Nq]' ): end do:

pretty display w.r.t. the named system states

>	for i from 1 to Nq do   Xd_nl[i+Nq] := simplify( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd_nl[i+Nq] ) ): end do:

>	for i from 1 to Nq do   diff( qd[i], t ) = collect( Xd_nl[i+Nq], eom_collect_list ); end do:

Solution to the Linearized EOM's

Solve the linear form of the equations of motion for the states' second time derivatives

>	Xqdd_solset_ser := solve( convert( EOM_ser, set ), convert( Xqdd, set ) ):

>	assign( Xqdd_solset_ser );

Moreover, for small angles

>	subs_avsq_list := { X[Nq+2]^2 = 0, X[Nq+3]^2 = 0 }:

>	for i from 1 to Nq do   Xd[i+Nq] := subs( subs_avsq_list, Xd[i+Nq] ); end:

pretty display w.r.t. the named system states

>	for i from 1 to Nq do   diff( qd[i], t ) = collect( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd[i+Nq] ), eom_collect_list ); end do;