Dynamic Equations for the 2 DOF Helicopter
© 2006 Quanser Consulting Inc.
NOTE: This worksheet presents the general dynamic equations modelling a 2 DOF Helicopter.
Description
The Quanser_Tools Package
References
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.
= pitch angle
= yaw angle
> | q := [ theta(t), psi(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 ); |
Transformation Matrices
Rotate base coordinate about yaw: T_0_1 = Rot(Z0, psi)
> | T_0_1 := matrix(4,4,[ cos(-q[2]),-sin(-q[2]),0,0, sin(-q[2]),cos(-q[2]),0,0, 0,0,1,0, 0,0,0,1]); |
Rotate about pitch: T_1_2 = Rot(Y1, theta)
> | T_1_2 := matrix(4,4,[ cos(-q[1]),0,sin(-q[1]),0, 0,1,0,0, -sin(-q[1]),0,cos(-q[1]),0, 0,0,0,1]); |
Translate about O2 to center of mass of helicopter: T_2_3 = Trans(X2, lcm)
> | T_2_3 := matrix(4,4,[ 1,0,0,l[cm], 0,1,0,0, 0,0,1,0, 0,0,0,1]); |
Base coordinate system, O0, to center of mass , O3: T_0_3 = T_0_1 * T_1_2 * T_2_3
> | T_0_3 := evalm( T_0_1 &* T_1_2 &* T_2_3); |
Cartesian Coordinates of the Helicopter's Centre Of Gravity
conventions:
1)
corresponds to the helicopter being horizontal
2) positive rotation about pitch axis,
, is CCW
3) positive rotation of about yaw axis,
, is CW.
Absolute 3-dimensional Cartesian position: CM of 2 DOF Helicopter.
> | x[cm] := T_0_3[1,4]; y[cm] := T_0_3[2,4]; z[cm] := T_0_3[3,4]; |
Absolute 3-dimensional Cartesian velocities.
> | xd[cm] := diff( x[cm], t ); yd[cm] := diff( y[cm], t ); zd[cm] := diff( z[cm], 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 voltage:
> | subs_U := { V[m_p] = U[1], V[m_y] = U[2] }: |
set the input to be the torque applied by the motor shaft,
(if not expressed as a function of the motor voltage):
> | #subs_U := { tau[p] = U[1], tau[y] = U[2] }: |
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.
V[e] = Total Elastic Potential Energy of the system
> | V[e] := 0: |
= potential energy of helicopter.
> | V[g] := potential_energy( 'gravity', m[heli], g, z[cm] ); |
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.
= kinetic energy due to rotation of arm
= moment of inertia of motor and rotary arm.
> | Tr[1] := kinetic_energy( 'rotation', J[eq_p], qd[1] ); |
> | Tr[2] := kinetic_energy( 'rotation', J[eq_y], qd[2] ); |
v[cm] = cm cartesian velocity (magnitude)
Tt[2] = cm translational kinetic kinergy
> | v[cm] := n_norm( [ xd[cm], yd[cm], zd[cm] ], 2 ): Tt := kinetic_energy( 'translation', m[heli], v[cm] ); |
Total kinetic energy
> | T[T] := Tr[1] + Tr[2] + Tt; 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 acting on pitch axis
= torque acting on yaw axis
= pitch viscous friction torque coefficient (a.k.a. viscous damping)
= yaw viscous friction torque coefficient (a.k.a. viscous damping)
= 0 and
if both viscous dampings are neglected
> | #B[eq_p] := 0: #B[eq_y] := 0: |
Q[i] = generalized force applied on generalized coordinate q[i]
> | Q[1] := tau[p] - B[p] * qd[1]; |
> | Q[2] := tau[y] - B[y] * qd[2]; |
rm: comment the following line out if
, uncomment it if
> | tau[p] := K[pp] * V[m_p] + K[py] * V[m_y]; |
> | tau[y] := K[yy] * V[m_y] + K[yp] * V[m_p]; |
> | 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 ): |
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:
,
,
, and
> | EOM_ser := EOM_states: |
Generalized series expansions of the trigonometric functions is used (for small angles).
> | for i from 1 to Nq do EOM_ser[i] := subsop( 1 = convert( series( op( 1, EOM_ser[i] ), X[ 1 ] ), polynom ), EOM_ser[i] ); 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[ 1 ] ), polynom ); Fi_lin[ i, k ] := subs( subs_XUzero, Fi_lin[ i, k ] ); end do; end do: |
> | 'F_lin' = Fi_lin; |
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] = V[m_p], U[2] = V[m_y] }: #subs_U_rev := { U[1] = tau[m] }: |
> | eom_collect_list := { seq( diff( q[i], t ), i=1..Nq ), seq( q[i], i=1..Nq ), J[eq_p], J[eq_y], V[m_p], V[m_y] }; |
Solution to the Non-Linear Equations of Motion
Solve the non-linear form of the equations of motion for the states' second time derivatives
NOTE: These computations take long and therefore they have been commented.
> | 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; |
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 ): |
> | 'A' = A_ss; |
> | B_ss := Matrix( Nx, Nu ): |
> | B_ss := deriveB( Xqdd, B_ss, Nq, subs_XUzero ): |
> | 'B' = B_ss; |
> | C_ss := IdentityMatrix( Nx, Nx ): 'C' = C_ss; |
> | D_ss := Matrix( Nx, Nu, 0 ): 'D' = 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 := "HELI_2D_ABCD_eqns.m": |
unassign variables, when necessary, for those present in the "Matlab_Notations" substitution set
> | if assigned( B[p] ) then unassign( 'B[p]' ); end if; |
> | if assigned( B[y] ) then unassign( 'B[y]' ); end if; |
> | if assigned( J[eq_p] ) then unassign( 'J[eq_p]' ); end if; |
> | if assigned( J[eq_y] ) then unassign( 'J[eq_y]' ); end if; |
substitution set containing a notation consistent with that used in the MATLAB design script(s)
> | Matlab_Notations := {m[heli] = m_heli, l[cm] = l_cm, J[eq_p] = J_eq_p, J[eq_y]=J_eq_y, B[p] = B_p, B[y] = B_y, K[pp] = K_pp, K[yy] = K_yy, K[py] = K_py, K[yp] = K_yp, R[m_p] = R_m_p, R[m_y] = R_m_y, r[p] = r_p, r[y] = r_y}: |
> | Experiment_Name := "2 DOF Helicopter": |
> | 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 ): |
> |
> |
> |
> |
> |
> |
> |
> |
> |
> |
> |