Dynamic Equations For The

First Stage Of The 2-DOF Serial Flexible Link (2DSFL) Robot

©  2008 Quanser Consulting Inc.

URL: http://www.quanser.com

Description

The Quanser_Tools  Package

Worksheet Initialization

>    restart: interface( imaginaryunit = j ):

>    with( LinearAlgebra ):

>    libname := "C:/Program Files/Quanser/Maple Repository", ".", libname:

>    with( Quanser_Tools );

[HTM, deriveA, deriveB, deriveF, kinetic_energy, lagrange_equations, moment_of_inertia, n_norm, potential_energy, write_ABCD_to_Mfile]
[HTM, deriveA, deriveB, deriveF, kinetic_energy, lagrange_equations, moment_of_inertia, n_norm, potential_energy, write_ABCD_to_Mfile]

environment variable representing the order of series calculations

>    Order := 2:

Notations

Generalized Coordinates: q[i] 's

The generalized coordinates are also called Lagrangian coordinates.

theta[11](t)  =  driving shaft (of harmonic drive #1) angular position: the zero angle is defined as the starting position.

theta[12](t)  = first link (absolute) end-effector angular position: the zero angle is defined as the starting position.

>    q := [ theta[11](t), theta[12](t) ];

q := [theta[11](t), theta[12](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 );

qd := [diff(theta[11](t),t), diff(theta[12](t),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 ) };

subs_Xq := {theta[11](t) = X[1], theta[12](t) = X[2]}

subs_Xqd := {diff(theta[11](t),t) = X[3], diff(theta[12](t),t) = X[4]}

Substitution set for the input(s).

set the input to be the motor #1 current:

>    subs_U := { I[m1] = 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[1] = 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 ) };

subs_Xqdd := {diff(theta[11](t),`$`(t,2)) = Xd[3], diff(theta[12](t),`$`(t,2)) = Xd[4]}

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 ) ];

Xqdd := [Xd[3], Xd[4]]

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: V[T]  

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[s1] = first flexible link torsional stiffness

>    Ve[T] := potential_energy( 'spring', K[s1], q[2] );

Ve[T] := 1/2*K[s1]*theta[12](t)^2

V[T] = Total Potential Energy of the system

>    V[T] := simplify( Ve[T] + Vg[T] );

V[T] := 1/2*K[s1]*theta[12](t)^2

Total Kinetic Energy: T[T]

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

Tr[11]  = rotational kinetic energy of the first motorized rigid joint (holding the flexible link)

J[11]  = harmonic drive #1 equivalent output load inertia

qd[1] = drive #1 load angular velocity

>    Tr[11] := kinetic_energy( 'translation', J[11], qd[1] );

Tr[11] := 1/2*J[11]*diff(theta[11](t),t)^2

Tr[12]  = rotational kinetic energy of the first flexible link end-effector (compounded with the second link assembly)

J[12]  = first flexible link end-effector load inertia compounded with the second link assembly

qd[2] = first link end-effector angular velocity

>    Tr[12] := kinetic_energy( 'translation', J[12], (qd[1] + qd[2]) );

Tr[12] := 1/2*J[12]*(diff(theta[11](t),t)+diff(theta[12](t),t))^2

T[T]  = Total Kinetic Energy of the system

>    T[T] := Tr[11] + Tr[12]:

>    T[T] := collect( T[T], diff );

T[T] := (1/2*J[11]+1/2*J[12])*diff(theta[11](t),t)^2+J[12]*diff(theta[12](t),t)*diff(theta[11](t),t)+1/2*J[12]*diff(theta[12](t),t)^2

Generalized Forces: Q[i] 's

The non-conservative forces corresponding to the generalized coordinates are: T[1]  and the viscous damping forces, where

T[1]  = torque generated by the motor #1

B11 = first actuated rigid joint viscous damping coefficient

B12 = first link end-effector viscous damping coefficient (with the second link connected)

B[11]  = 0 and B[12]  = 0 if both viscous dampings are neglected

>    #B[11] := 0: B[12] := 0:

Q[i] = generalized force applied on generalized coordinate q[i]

>    Q[1] := T[1] - B[11] * qd[1];

Q[1] := T[1]-B[11]*diff(theta[11](t),t)

>    Q[2] := - B[12] * qd[2];

Q[2] := -B[12]*diff(theta[12](t),t)

T[1]  = torque produced by drive #1 at the load: load driving torque

rm: comment the following line out if U = T[1], uncomment it if U = I[m]

>    T[1] := K[t1] * I[m1];

T[1] := K[t1]*I[m1]

>    Q := [ seq( Q[i], i=1..Nq ) ];

Q := [K[t1]*I[m1]-B[11]*diff(theta[11](t),t), -B[12]*diff(theta[12](t),t)]

Euler-Lagrange's Equations

For a N -DOF system, the Lagrange's equations can be written:

Diff(Diff(L,qdot[i]),t)-Diff(L,q[i]) = Q[i]   for i = 1  through N

where:

  Q[i] 's are special combinations of external forces and called the generalized forces,

  q[1] , ..., q[N] , are N independent coordinates chosen to describe the system and called the generalized coordinates ,

 and L  is the Lagrangian  of the system.

L  is defined by:

L = T-U

where T  is the total kinetic energy of the system and U  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 ) } );

EOM_orig := [(J[11]+J[12])*diff(theta[11](t),`$`(t,2))+J[12]*diff(theta[12](t),`$`(t,2)) = K[t1]*I[m1]-B[11]*diff(theta[11](t),t), diff(theta[11](t),`$`(t,2))*J[12]+J[12]*diff(theta[12](t),`$`(t,2))+K[...
EOM_orig := [(J[11]+J[12])*diff(theta[11](t),`$`(t,2))+J[12]*diff(theta[12](t),`$`(t,2)) = K[t1]*I[m1]-B[11]*diff(theta[11](t),t), diff(theta[11](t),`$`(t,2))*J[12]+J[12]*diff(theta[12](t),`$`(t,2))+K[...

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 );

EOM_states := [(J[11]+J[12])*Xd[3]+J[12]*Xd[4] = K[t1]*U[1]-B[11]*X[3], Xd[3]*J[12]+J[12]*Xd[4]+K[s1]*X[2] = -B[12]*X[4]]

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;

F[i] = Matrix(%id = 19307344)

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[m1] }:
subs_U_rev := { U[1] = T[1] }:

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

eom_collect_list := {theta[11](t), theta[12](t), diff(theta[11](t),t), diff(theta[12](t),t), K[s1]}

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 ) );

Xqdd_solset_ser := {Xd[4] = -(J[12]*K[t1]*U[1]-J[12]*B[11]*X[3]+J[12]*B[12]*X[4]+J[11]*K[s1]*X[2]+J[11]*B[12]*X[4]+J[12]*K[s1]*X[2])/J[11]/J[12], Xd[3] = (K[t1]*U[1]-B[11]*X[3]+B[12]*X[4]+K[s1]*X[2])/J...
Xqdd_solset_ser := {Xd[4] = -(J[12]*K[t1]*U[1]-J[12]*B[11]*X[3]+J[12]*B[12]*X[4]+J[11]*K[s1]*X[2]+J[11]*B[12]*X[4]+J[12]*K[s1]*X[2])/J[11]/J[12], Xd[3] = (K[t1]*U[1]-B[11]*X[3]+B[12]*X[4]+K[s1]*X[2])/J...
Xqdd_solset_ser := {Xd[4] = -(J[12]*K[t1]*U[1]-J[12]*B[11]*X[3]+J[12]*B[12]*X[4]+J[11]*K[s1]*X[2]+J[11]*B[12]*X[4]+J[12]*K[s1]*X[2])/J[11]/J[12], Xd[3] = (K[t1]*U[1]-B[11]*X[3]+B[12]*X[4]+K[s1]*X[2])/J...

>    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(theta[11](t),`$`(t,2)) = K[s1]/J[11]*theta[12](t)-B[11]/J[11]*diff(theta[11](t),t)+B[12]/J[11]*diff(theta[12](t),t)+K[t1]^2*I[m1]/J[11]

>    diff( qd[2], t ) = collect( subs( subs_U_rev, subs_Xq_rev, subs_Xqd_rev, Xd[4] ), eom_collect_list );

diff(theta[12](t),`$`(t,2)) = -(J[11]+J[12])/J[11]/J[12]*K[s1]*theta[12](t)+B[11]/J[11]*diff(theta[11](t),t)-(J[12]*B[12]+J[11]*B[12])/J[11]/J[12]*diff(theta[12](t),t)-K[t1]^2*I[m1]/J[11]
diff(theta[12](t),`$`(t,2)) = -(J[11]+J[12])/J[11]/J[12]*K[s1]*theta[12](t)+B[11]/J[11]*diff(theta[11](t),t)-(J[12]*B[12]+J[11]*B[12])/J[11]/J[12]*diff(theta[12](t),t)-K[t1]^2*I[m1]/J[11]

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 ): 'A1' = A_ss;

A1 = Matrix(%id = 19692852)

>    #lprint( A_ss );

>    B_ss := Matrix( Nx, Nu ):

>    B_ss := deriveB( Xqdd, B_ss, Nq, subs_XUzero ): 'B1' = B_ss;

B1 = Matrix(%id = 19729472)

>    #lprint( B_ss );

>    #C_ss := IdentityMatrix( Ny, Nx ):
C_ss := IdentityMatrix( Nx, Nx ):
'C1' = C_ss;

C1 = Matrix(%id = 19786840)

>    #lprint( C_ss );

>    D_ss := Matrix( Nx, Nu, 0 ):
'D1' = D_ss;

D1 = Matrix(%id = 19796088)

>    #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_Stage1.m":

unassign variables, when necessary, for those present in the "Matlab_Notations" substitution set

>    if assigned( B[11] ) then
  unassign( 'B[11]' );
end if;

>    if assigned( B[12] ) then
  unassign( 'B[12]' );
end if;

substitution set containing a notation consistent with that used in the MATLAB design script(s)

>    Matlab_Notations := { J[11] = J11, J[12] = J12, K[s1] = Ks1, B[11] = B11, B[12] = B12, K[t1] = Ktg1}:

>    Experiment_Name := "2-DOF Serial Flexible Link Robot (First 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 ):

>   

Click here to go back to top: Description Section.