Dynamic Equations For The
Second Stage Of The 2-DOF Serial Flexible Link (2DSFL) Robot
© 2008 Quanser Consulting Inc.
Description
The Quanser_Tools Package
Worksheet Initialization
> | restart: interface( imaginaryunit = j ): |
> | with( LinearAlgebra ): |
> | libname := "C:/Program Files/Quanser/Maple Repository", ".", libname: |
> | with( Quanser_Tools ); |
environment variable representing the order of series calculations
> | Order := 2: |
Generalized Coordinates:
's
The generalized coordinates are also called Lagrangian coordinates.
= driving shaft (of harmonic drive #2) angular position: the zero angle is defined as the starting position.
= second link end-effector angular position (relative to the first link end-effector): the zero angle is defined as the starting position.
> | q := [ theta[21](t), theta[22](t) ]; |
Nq = number of Lagrangian coordinates (e.g. here, it is Nq = 2)
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 ); |
State-Space Variables
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 #2 current:
> | subs_U := { I[m2] = U[1] }: |
set the input to be the actuator driving force/torque (if not expressed as a function of the motor voltage):
> | #subs_U := { T[2] = 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 and Kinetic Energies of the System
The total potential and kinetic energies are needed to calculate the Lagrangian of the system.
Total Potential Energy:
The total potential energy can be expressed in terms of the generalized coordinates alone.
Vg[T] = Total Gravitational Potential Energy of the system
Vg[T] = 0 : Because the load vertical displacement from normality is null.
> | Vg[T] := 0: |
Ve[T] = Total Elastic Potential Energy of the system
K[s2] = second flexible link torsional stiffness
> | Ve[T] := potential_energy( 'spring', K[s2], q[2] ); |
V[T] = Total Potential Energy of the system
> | V[T] := simplify( Ve[T] + Vg[T] ); |
Total Kinetic Energy:
The total kinetic energy can be expressed in terms of the generalized coordinates and their first-time derivatives.
= rotational kinetic energy of the second motorized rigid joint (holding the flexible link)
= harmonic drive #2 equivalent output load inertia
qd[1] = drive #2 load angular velocity
> | Tr[21] := kinetic_energy( 'translation', J[21], qd[1] ); |
= rotational kinetic energy of the second flexible link end-effector
= second flexible link end-effector inertia
qd[2] = second link end-effector angular velocity
> | Tr[22] := kinetic_energy( 'translation', J[22], (qd[1] + qd[2]) ); |
= Total Kinetic Energy of the system
> | T[T] := Tr[21] + Tr[22]: |
> | T[T] := collect( T[T], diff ); |
Generalized Forces:
's
The non-conservative forces corresponding to the generalized coordinates are:
and the viscous damping forces, where
= torque generated by the motor #2
B21 = second actuated rigid joint viscous damping coefficient
B22 = second link end-effector viscous damping coefficient
= 0 and
= 0 if both viscous dampings are neglected
> | #B[21] := 0: B[22] := 0: |
Q[i] = generalized force applied on generalized coordinate q[i]
> | Q[1] := T[2] - B[21] * qd[1]; |
> | Q[2] := - B[22] * qd[2]; |
= torque produced by drive #2 at the load: load driving torque
rm: comment the following line out if U = T[1], uncomment it if U = I[m]
> | T[2] := K[t2] * I[m2]; |
> | Q := [ seq( Q[i], i=1..Nq ) ]; |
Euler-Lagrange's Equations
For a N -DOF system, the Lagrange's equations can be written:
for
through
where:
's are special combinations of external forces and called the
generalized forces,
, ...,
, are
N
independent coordinates chosen to describe the system and called the
generalized coordinates
,
and
is the
Lagrangian
of the system.
is defined by:
where
is the total kinetic energy of the system and
the total potential energy of the system.
> | EOM_orig := lagrange_equations( T[T], V[T], q, Q ): |
this is to display the EOM's
> | EOM_orig := collect( EOM_orig, { seq( diff( q[i], t$2 ), i=1..Nq ), seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ) } ); |
Express the Euler-Lagrange equations of motion as functions of the states:
1) substitute (i.e. name) the acceleration states first!
2) then substitute the velocity states!
3) and only after, the position states, and the inputs!
> | EOM_states := subs( subs_Xqd, subs( subs_Xqdd, EOM_orig ) ): |
> | EOM_states := subs( subs_Xq, subs_U, EOM_states ); |
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: |
Here, Fi shows a linear system
> | 'F[i]' = Fi; |
Solving the Euler-Lagrange's Equations
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] = I[m2] }: #subs_U_rev := { U[1] = T[2] }: |
> | eom_collect_list := { seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ), K[s2] }; |
Solution to the (Linear) EOM's
Solve the (linear) equations of motion for the states' second time derivatives
> | Xqdd_solset_ser := solve( convert( EOM_states, set ), convert( Xqdd, set ) ); |
> | assign( Xqdd_solset_ser ); |
pretty display w.r.t. the named system states
> | diff( qd[1], t ) = collect( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd[3] ), eom_collect_list ); |
> | diff( qd[2], t ) = collect( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd[4] ), eom_collect_list ); |
Determine the System State-Space Matrices: A, B, C, and D
> | A_ss := Matrix( Nx, Nx ): |
> | A_ss := deriveA( Xqdd, A_ss, Nq, subs_XUzero ): 'A2' = A_ss; |
> | #lprint( A_ss ); |
> | B_ss := Matrix( Nx, Nu ): |
> | B_ss := deriveB( Xqdd, B_ss, Nq, subs_XUzero ): 'B2' = B_ss; |
> | #lprint( B_ss ); |
> | #C_ss := IdentityMatrix( Ny, Nx ): C_ss := IdentityMatrix( Nx, Nx ): 'C2' = C_ss; |
> | #lprint( C_ss ); |
> | D_ss := Matrix( Nx, Nu, 0 ): 'D2' = D_ss; |
> | #lprint( D_ss ); |
Write A, B, C, and D to a Matlab file
Save the state-space matrices A, B, C and D to a MATLAB file.
> | Matlab_File_Name := "eqn_2DSFL_Stage2.m": |
unassign variables, when necessary, for those present in the "Matlab_Notations" substitution set
> | if assigned( B[21] ) then unassign( 'B[21]' ); end if; |
> | if assigned( B[22] ) then unassign( 'B[22]' ); end if; |
substitution set containing a notation consistent with that used in the MATLAB design script(s)
> | Matlab_Notations := { J[21] = J21, J[22] = J22, K[s2] = Ks2, B[21] = B21, B[22] = B22, K[t2] = Ktg2 }: |
> | Experiment_Name := "2-DOF Serial Flexible Link Robot (Second Stage)": |
> | write_ABCD_to_Mfile( Matlab_File_Name, Experiment_Name, Matlab_Notations, A_ss, B_ss, C_ss, D_ss ); |
Procedure Printing
default:
> | #interface( verboseproc = 1 ); |
> | eval( lagrange_equations ): |
> |