1.
I. INTRODUCTION
To
show
the
power
of
dynamics,
an
inverted
pendulum
(shown
below)
that
is
capable
of
driving
the
un-­‐actuated
pendulum
to
the
unstable
equilibrium
position
and
holding
it
there
was
modeled,
designed
and
built.
Fig.
1.
Dr.
Craig’s
inverted
pendulum
design
The
purpose
of
this
paper
is
to
re-­‐analyze
the
dynamics
of
the
system
using
principles
learned
in
MEEN-­‐5220
and
also
simulate
the
dynamics
derived
using
numerical
integration
techniques.
II. PART
1
–
RECURSIVE
NEWTON
EULER
(RNE)
DYNAMIC
ANALYSIS
The
RNE
approach
to
dynamic
modeling
is
a
computer
programmable
way
of
finding
the
dynamics
of
a
system
by
using
an
algorithmic-­‐based
set
of
coordinate
frames.
Often
times,
it
is
used
on
multi
DOF
systems
as
the
dynamic
analysis
of
systems
rapidly
becomes
more
complex
as
the
number
of
DOF
increases.
This
method
was
applied
to
the
inverted
pendulum
problem.
First,
find
coordinate
frames
using
the
Denavit-­‐Hartenberg
(D-­‐H)
Distal
Variant
procedure.
Fig.
2.
Distal
D-­‐H
Coordinate
Frames
applied
to
the
given
2
DOF
inverted
pendulum
N
θn+1
dn+1
an+1
αn+1
0
θ
0
0
-­‐π/2
1
-­‐α
L1
0
0
Using
these,
it
is
possible
to
systematically
compute
each
characteristic
of
the
system
all
the
way
through
to
joint
torques.
The
equation
for
joint
torque
(for
a
revolute
joint
i)
is
shown
below.
See
Appendix
A
for
code
used
to
compute
the
remainder
of
system
characteristics.
𝜏! = 𝑛!!!
!,!!!
!
∗
0
0
1
+ 𝐵! 𝑞! + 𝑇! ∗ 𝑠𝑖𝑔𝑛(𝑞!)
(1)
Note
that
in
Eq.
(1),
𝜏!
will
be
equal
as
the
input
torque
and
𝜏!
will
be
equal
to
zero
as
there
is
no
actuator
on
the
second
joint.
In
addition,
𝑛!!!
!,!!!
!
is
the
moment
on
each
joint
found
using
the
RNE
method.
Completing
the
analysis,
equations
of
motion
(EOM)
were
found
to
be
as
follows:
𝜏! = 𝑇!"#$% = [𝑚! 𝑙!!
!
+ 𝐼!!! + 𝑚! 𝑙!"
!
cos!
𝜙 + 𝐼!!! cos!
𝜙 +
𝐼!!! sin!
(𝜙)]𝜃 + −𝑚! 𝑙! 𝑙!" sin 𝜙 𝜙 + −𝑚! 𝑙! 𝑙!" cos 𝜙 𝜙!
+
−𝑚! 𝑙!"
!
− 𝐼!!! + 𝐼!!! ∗ 2 cos 𝜙 sin 𝜙 𝜃𝜙 − 𝐵! 𝜃 − 𝑇! ∗
𝑠𝑖𝑔𝑛(𝜃)
(2)
𝜏! = 0 = 𝑚! 𝑙!"
!
+ 𝐼!!! 𝜙 + −𝑚! 𝑙! 𝑙!" sin 𝜙 𝜃 + 𝑚! 𝑙!"
!
+
𝐼!!! − 𝐼!!! ∗ cos 𝜙 sin 𝜙 𝜃!
− 𝑚! 𝑔𝑙!" cos 𝜙 + 𝐵! 𝜙 + 𝑇! ∗
𝑠𝑖𝑔𝑛(𝜙)
(3)
Compared
to
the
EOM
derived
by
Dr.
Craig
(given
in
the
project
handout),
the
equations
derived
above
contain
two
discrepancies.
In
Eq.
(3)
the
term
highlighted
in
red
has
the
opposite
sign
from
the
equations
given.
However,
this
may
simply
be
a
discrepancy
in
the
sign
of
g.
In
addition,
Eq.
(2)
is
missing
one
term
(𝑚! 𝑙!
!
)
in
the
𝜃
term
compared
to
equations
given.
However,
this
analysis
did
prove
the
power
of
the
RNE
as
the
script
used
to
derive
the
EOM
was
only
90
lines
long
(approximately)
and
no
free
body
diagrams
were
required.
III. PART
3
–
LAGRANGIAN
DYNAMIC
ANALYSIS
Another
means
by
which
one
can
acquire
the
EOM
of
a
system
is
a
Lagrange
Dynamic
analysis.
This
is
an
energy-­‐
based
method
and
therefor,
one
does
not
need
to
consider
the
many
+/-­‐
values
found
in
Newton-­‐Euler
methods.
However,
it
is
also
not
very
intuitive
and
joint
forces
are
lost
hence
why
RNE
was
completed
as
well.
See
Appendix
D
for
a
complete
derivation
using
Lagrangian
mechanics.
IV. PART
4
–
MODELING
USING
NUMERICAL
INTEGRATION
AND
ODE45
–
A
SIMPLE
CASE
As
the
EOM
of
the
system
are
highly
nonlinear,
numerical
analysis
will
be
required
to
find
the
time
based
position
response
of
the
system.
Using
MATLAB,
two
different
approaches
were
used:
first,
a
proprietary
Euler
algorithm
was
created
and
second,
an
internal
MATLAB
function
(ode45)
was
used.
Each
of
these
codes
can
be
found
in
Appendix
A
of
this
report.
However,
both
of
these
methods
only
work
for
1
st
order
differential
equations.
Since
the
EOM
of
the
system
consists
of
two
2
nd
order
equations,
it
is
required
to
break
each
one

2. up
into
two
1
st
order
equations.
This
can
be
done
by
using
the
following
definition
of
a
system
of
equations
of
motion:
𝑀 𝑞 𝑡 = {𝐹 𝑞, 𝑞, 𝑡 }
(4)
!
!"
𝑥 =
𝑞
𝑀 !! 𝐹
(5)
𝑥 =
𝑞
𝑞
(6)
See
Appendix
C
for
the
values
of
the
mass
matrix
and
force
vector,
which
were
derived
through
the
rearranging
of
the
EOM
(Eq.
(2)
&
(3)).
To
test
the
model,
a
simple
case
that
is
intuitively
understandable
would
be
employed.
The
case
tested
is
one
where
there
is
no
actuator
torque
and
the
pendulum
is
released
from
rest
at
the
near
vertical
position.
In
theory,
the
pendulum
should
oscillate
(with
decaying
amplitude
due
to
damping)
about
the
stable
equilibrium
position.
In
addition,
link
1
should
rotate
due
to
the
momentum
of
the
pendulum.
Results
are
plotted
below.
(note:
plots
found
using
EOM
from
handout):
From
these
plots,
it
is
immediately
obvious
that
the
modeled
data
does
not
perfectly
match
up
with
experimental
data.
In
addition,
the
two
sets
of
modeled
data
do
not
match
up
with
one
another.
However,
this
should
be
expected
as
ODE45
is
far
more
sophisticated
than
the
Euler
integration
used,
which
is
a
0
th
order
integration.
To
improve
the
Euler
method,
a
higher
order
integration
(trapezoidal
rule,
etc.)
should
be
used.
V. PART
6-­‐
COMPARISON
TO
KNOWN
MODEL
In
addition,
a
Simulink
model
used
to
determine
control
torque
at
the
actuator
to
drive
the
pendulum
to
the
unstable
equilibrium
position
and
hold
it
there
was
provided.
In
theory,
if
the
model
generated
matches
up
with
the
one
used
by
Dr.
Craig,
the
angle
profiles
of
the
system
will
be
the
same
regardless
of
which
model
is
used.
Fig.
5.
Comparison
of
Θ(t)
values
for
modeled
values
Fig.
6.
Comparison
of
Φ(t)
values
for
modeled
values
From
the
plots,
it
is
immediately
clear
that
the
model
generated
for
this
project
does
not
match
up
with
Dr.
Craig’s
Simulink
model.
The
reasons
for
this
could
be
many
and
will
be
discussed
in
Section
VI,
Conclusion.
VI. CONCLUSION
To
conclude,
it
was
found
that
the
model
used
for
the
purpose
of
this
report
did
not
match
perfectly
with
the
one
used
by
Dr.
Craig.
This
could
be
for
one
of
two
reasons:
first,
the
EOM
used
for
the
purposes
of
this
report
may
not
be
correct.
A
reference
to
a
product
of
inertia
(Ixy2)
was
found
in
the
parameters
given
however,
it
was
not
used
in
the
final
system
model
which
draws
concern.
In
addition,
Euler
integrations
are
characterized
by
their
propensity
for
instability.
This
issue
is
only
exacerbated
by
the
attempt
to
position
the
pendulum
at
the
unstable
equilibrium
position.
This
would
explain
why
Euler
matches
up
with
Dr.
Craig’s
analysis
until
approximately
the
time
at
which
unstable
equilibrium
is
reached.
Finally,
a
great
deal
was
learned
through
this
project
as
it
did
encompass
many
if
not
every
topic
touched
upon
in
the
course.
Specifically,
the
difference
between
intelligent
methods
(ode45)
and
brute
force
methods
(Euler)
was
reinforced
through
this
project.
In
the
end,
it
can
be
said
summarily
as
thus:
if
you
have
an
analytical
solution,
great!
But
if
you
don’t
and
need
to
use
a
numerical
method,
it
is
best
to
work
smarter,
not
harder.

3. APPENDIX
A. Matlab
Code
%Alex Folz
%MEEN-5220 Project
%Recursive Newton Euler Calculation
clc
clear
%initialize symbolic variables
syms L21 t td tdd a ad add L1 ixx2 izz2 m2 g iyy2 L1 L11 ixx1 iyy1
izz1 ixy2 m1
z = [0;0;1];
%omega,alpha,v,a values of the zero frame
w0 = [0;0;0];
al0 = [0;0;0];
v0 = [0;0;0];
a0 = [0;0;0];
%Inertia values of each of the links
I1 = [ixx1 0 0; 0 iyy1 0; 0 0 izz1];
I2 = [ixx2 0 0; 0 iyy2 0; 0 0 izz2];
%Distance to each frame and center of mass
r1 = [0;0;0];
r2 = [0;0;L1];
rc1 = [0;0;L11];
rc2 = [0; -L21; 0];
%Rotation matrices and inverse rotation matrices between each
frame
R01 = [cos(t) 0 -sin(t); sin(t) 0 cos(t); 0 -1 0];
R12 = [cos(-a) -sin(-a) 0; sin(-a) cos(-a) 0; 0 0 1];
R10 = [cos(t) sin(t) 0; 0 0 -1; -sin(t) cos(t) 0];
R21 = [cos(-a) sin(-a) 0; -sin(-a) cos(-a) 0; 0 0 1];
%gravity in the zero frame
g0 = [0;0;g];
%calculation of the angular velocity of each frame
r.w1 = R10*(w0+z*td);
r.w2 = R21*(r.w1+z*ad);
%calculation of the angular acceleration of each frame
r.al1 = R10*(al0+z*tdd+cross(w0,(z*td)));
r.al2 = R21*(r.al1+z*add+cross(r.w1,(z*ad)));
%calculation of the velocity of each frame
r.v1 = R10*v0+cross(r.w1,r1);
r.v2 = R21*r.v1+cross(r.w2,r2);
%calculation of the acceleration of each frame
r.a1 = R10*a0+cross(r.al1,r1)+cross(r.w1,cross(r.w1,r1));
r.a2 = R21*r.a1+cross(r.al2,r2)+cross(r.w2,cross(r.w2,r2));
%calculation of the acceleration of each center of mass
r.ac1 = r.a1+cross(r.al1,rc1)+cross(r.w1,cross(r.w1,rc1));
r.ac2 = r.a2+cross(r.al2,rc2)+cross(r.w2,cross(r.w2,rc2));
%calculation of the gravity vector in each frame
r.g1 = R10*g0;
r.g2 = R21*r.g1;
%forces acting on joint 2, in both frame 1 and 2
r.f221 = [0;0;0]+m2*r.g2+m2*r.ac2;
r.f121 = R12*r.f221;
%forces acting on join 1, in frame 1
r.f110=r.f121+m1*r.g1+m1*r.ac1;
%moments on joint 2, in both frame 1 and 2
r.n221=[0;0;0]+cross((r2+rc2),r.f221)-
cross(rc2,[0;0;0])+I2*r.al2+cross(r.w2,(I2*r.w2));
r.n121=R12*r.n221;
%moments on joint 1, in both frame 1 and 0
r.n110=r.n121+cross((r1+rc1),r.f110)-
cross(rc1,r.f121)+I1*r.al1+cross(r.w1,(I1*r.w1));
r.n010=R01*r.n110;
%actuator torque at each joint
r.T1 = transpose(r.n010)*z;
r.T2 = transpose(r.n121)*z;
%Use matlab to simplify the expressions of torque as much as
possible
r.m1 = collect(r.T1,ad^2);
r.n1 = collect(r.m1,tdd);
r.o1 = collect(r.n1,add);
r.T1f = r.o1;
r.m2 = collect(r.T2,td^2);
r.n2 = collect(r.m2,tdd);
r.o2 = collect(r.n2,add);
r.T2f = r.o2;
%Alex Folz & Dr. Phil Voglewede
%MEEN-5220 Project
% Euler's Method solver
%clear variables generated by this code
clc
clear x1 y1 x2 y2 m f x2d y2d om al theta_eu alpha_eu tout_eu
% Initial conditions for Euler's method
x1(1) = 0;
x2(1) = 0;
y1(1) = -pi()/2;
y2(1) = 0;
x2d(1) = 0;
y2d(1) = 0;
om(:,:,1) = [x2(1); y2(1)];
al(:,:,1) = [x2d(1); y2d(1)];
% Increment for time1
deltat1 = 0.001;
tspan_eu = [0:deltat1:10];
% Euler's method1
for i=1:length(tspan_eu)-1
%Determine actuator torque; 0 for experimental test case and
%control_torque to compare with Dr. Craig's model
Torque(i) = control_torque(i);
%Calculate componenents of the mass Matrix
m1(i) =
M1*L11^2+I_bar_1_z1+M2*L1^2+M2*L21^2*cos(y1(i))^2+I_bar_2_z
2*cos(y1(i))^2+I_bar_2_y2*sin(y1(i))^2;
m2(i) = -(M2*L1*L21)*sin(y1(i));
m3(i) = -(M2*L1*L21)*sin(y1(i));
m4(i) = M2*L21^2+I_bar_2_x2;
%Calculate components of the force vector

4. f1(i) = M2*L1*L21*cos(y1(i))*y2(i)^2-(I_bar_2_y2-M2*L21^2-
I_bar_2_z2)*(2*cos(y1(i))*sin(y1(i)))*y2(i)*x2(i)+Torque(i)-
B_theta*x2(i)-Tf_theta*sign(x2(i));
f2(i) = -(M2*L21^2-
I_bar_2_y2+I_bar_2_z2)*cos(y1(i))*sin(y1(i))*x2(i)^2-
M2*g*L21*cos(y1(i))-B_phi*y2(i)-Tf_phi*sign(y2(i));
%Initialize the mass matrix and force vector
m(:,:,i) = [m1(i) m2(i); m3(i) m4(i)];
F(:,:,i) = [f1(i); f2(i)];
%Find x1 and y1 of the next cycle, where x1 and y1 are theta and
alpha respectively
x1(i+1) = x1(i)+deltat1*x2(i);
y1(i+1) = y1(i)+deltat1*y2(i);
%Find dx2 and dy2 of the current cycle, where dx2 and dy2 are
the angular
%accelerations of theta and alpha,respectively
acc(:,:,i) = m(:,:,i)^(-1)*F(:,:,i);
x2d(i) = acc(1,1,i);
y2d(i) = acc(2,1,i);
%Find x2 and y2 of the next cycle, where x2 and y2 are the
angular
%velocity of theta and alpha, respectively
x2(i+1) = x2(i)+deltat1*x2d(i);
y2(i+1) = y2(i)+deltat1*y2d(i);
%convert x and y values to known variables
alpha_eu(i) = (y1(i)-pi()/2)*360/(2*pi());
theta_eu(i) = (x1(i))*360/(2*pi());
tout_eu(i) = i*deltat1;
end
%plot relevant data
figure(1)
plot(tout_eu,theta_eu)
figure(2)
plot(tout_eu,alpha_eu)
%Alex Folz & Dr. Phil Voglewede
%Meen-5220 Project
%ODE initialization code
% Initial conditions for ODE45
ic = [0 0 pi()/2-0.0061 0];
tspan45 = [0 10];
% Run the solver
[tout45,pout45] = ode45('alex_model',tspan45,ic);
%Convert pout45 values into known variables
theta45 = pout45(:,1)*360/(2*pi());
theta45_dot = pout45(:,2)*360/(2*pi());
alpha45 = pout45(:,3)*360/(2*pi())-90;
alpha45_dot = pout45(:,4)*360/(2*pi());
%Plot Results
figure(1)
plot(tout45,theta45);
figure(2)
plot(tout45,alpha45);
%Alex Folz
%Meen-5220 Project
%System Model to be used with ODE45
%p - [theta;dtheta;alpha;dalpha]
%pdot - derivative of p
function pdot = alex_model(t,p)
%Establish Global Variables
global g M1 M2 L1 L11 L21 I_bar_1_z1 I_bar_2_x2 I_bar_2_y2
I_bar_2_z2 B_phi B_theta Tf_theta Tf_phi
%No accuator torque in test case
Torque = 0;
pdot = zeros(4,1); %initialize column vector
%From the state model definition, p(2) & p(4) are the derivatives of
%p(1) & p(3), respectively
pdot(1) = p(2);
pdot(3) = p(4);
%Calculate components of the Mass matrix
m1 =
M1*L11^2+I_bar_1_z1+M2*L1^2+M2*L21^2*cos(p(3))^2+I_bar_2_z
2*cos(p(3))^2+I_bar_2_y2*sin(p(3))^2;
m2 = -(M2*L1*L21)*sin(p(3));
m3 = -(M2*L1*L21)*sin(p(3));
m4 = M2*L21^2+I_bar_2_x2;
%Caculate components of the Force vector
f1 = M2*L1*L21*cos(p(3))*p(4)^2-(I_bar_2_x2-M2*L21^2-
I_bar_2_z2)*(2*cos(p(3))*sin(p(3)))*p(4)*p(2)+Torque-B_theta*p(2)-
Tf_theta*sign(p(2));
f2 = -(M2*L21^2-
I_bar_2_y2+I_bar_2_z2)*cos(p(3))*sin(p(3))*p(2)^2-
M2*g*L21*cos(p(3))-B_phi*p(4)-Tf_phi*sign(p(4));
%Initialize Mass Matrix and Force vector
m = [m1 m2; m3 m4];
F = [f1; f2];
%Using the Mass Matrix and Force vector, determine pdot(2)
&pdot(4)
%which are the angular acceleration of theta and alpha,
respectively
acc= m^(-1)*F;
pdot(2) = acc(1,1);
pdot(4) = acc(2,1);
end
B. Matlab
Output
for
RNE
T1:
(-­‐L1*L21*m2*cos(a))*add
+
(iyy1
+
cos(a)*(m2*cos(a)*L1^2
+
iyy2*cos(a))
-­‐
L11*(L1*m2*cos(a)^2
+
L1*m2*sin(a)^2)
+
L11*(L1*m2*cos(a)^2
+
L1*m2*sin(a)^2
+
L11*m1)
+
sin(a)*(m2*sin(a)*L1^2
+
m2*sin(a)*L21^2
+
ixx2*sin(a)))*tdd
-­‐
cos(a)*(L1*(L21*m2*cos(a)*sin(a)*td^2
+
g*m2*sin(a))
+
ad*ixx2*td*sin(a)
-­‐
ad*iyy2*td*sin(a)
-­‐
ad*izz2*td*sin(a))
+
sin(a)*(L1*(g*m2*cos(a)
-­‐
L21*m2*td^2*sin(a)^2)
+
L21*m2*(L1*td^2*cos(a)^2
+
L1*td^2*sin(a)^2
-­‐
2*L21*ad*td*cos(a))
-­‐
ad*ixx2*td*cos(a)
+
ad*iyy2*td*cos(a)
-­‐
ad*izz2*td*cos(a))
+
(-­‐
L1*L21*m2*sin(a))*ad^2

5. T2:
(m2*cos(a)*sin(a)*L21^2
+
ixx2*cos(a)*sin(a)
-­‐
iyy2*cos(a)*sin(a))*td^2+
add*(m2*L21^2
+
izz2)
+
L21*g*m2*sin(a)-­‐
L1*L21*m2*tdd*cos(a)
C. M
matrix
and
F
vector
[𝑀] = [
𝑚! 𝑙!!
!
+ 𝐼!!! + 𝑚! 𝑙!"
!
cos!
𝜙 + 𝐼!!! cos!
𝜙 + 𝐼!!! sin!
𝜙 −𝑚! 𝑙! 𝑙!" sin 𝜙
−𝑚! 𝑙! 𝑙!" sin 𝜙 𝑚! 𝑙!"
!
+ 𝐼!!!
]
𝐹
= {
𝑚! 𝑙! 𝑙!" cos 𝜙 𝜙!
− −𝑚! 𝑙!"
!
− 𝐼!!! + 𝐼!!! ∗ 2 cos 𝜙 sin 𝜙 𝜃𝜙 + 𝑇 − 𝐵! 𝜃 − 𝑇! ∗ 𝑠𝚤𝑔𝑛(𝜃)
− 𝑚! 𝑙!"
!
+ 𝐼!!! − 𝐼!!! ∗ cos 𝜙 sin 𝜙 𝜃!
− 𝑚! 𝑔𝑙!" cos 𝜙 − 𝐵! 𝜙 − 𝑇! ∗ 𝑠𝑖𝑔𝑛(𝜙)
}
D.
Lagrangian
Derivation
1. Draw
FBD
of
the
system
2.
Fig.
7.
FBD
of
system,
to
be
used
for
Lagrangian
Analysis
3. Choose
generalized
coordinate
so
that
N=M
𝑞! = 𝜃
𝑞! = 𝜙
*Note:
𝜙 =
!
!
− 𝛼
4. Identify
non-­‐conservative
force
𝜏, 𝐵! 𝜃, 𝑇! ∗ 𝑠𝑖𝑔𝑛 𝜃 , 𝐵! 𝜙, 𝑇! ∗ 𝑠𝑖𝑔𝑛(𝜙)
5. Formulate
generalized
forces
for
each
generalized
coordinate
𝛿𝜃 = 𝛿𝜃𝚤
𝛿𝜙 = 𝛿𝜙𝚥
𝛿𝑊! = 𝜏 − 𝐵! 𝜃 − 𝑇! ∗ 𝑠𝑖𝑔𝑛 𝜃 𝚤 ∗ 𝛿𝜃𝚤
𝛿𝑊! = −𝐵! 𝜙 − 𝑇! ∗ 𝑠𝑖𝑔𝑛(𝜙) 𝚥 ∗ 𝛿𝜃𝚥
𝑞! = 𝜏 − 𝐵! 𝜃 − 𝑇! ∗ 𝑠𝑖𝑔𝑛 𝜃
𝑞! = −𝐵! 𝜙 − 𝑇! ∗ 𝑠𝑖𝑔𝑛(𝜙)
6. Formulate
potential
energy
𝑉 = 𝑉! + 𝑉!
𝑉 = 0 + 𝑚! 𝑔𝑙!"sin (𝜙)
7. Formulate
kinetic
energy
𝑇 = 𝑇! + 𝑇!
𝑇! =
1
2
𝑚! 𝑉!,!
!
+
1
2
𝜔. 𝐼 𝜔
𝑇! =
1
2
𝜔!. 𝐼! 𝜔!
𝑇! =
1
2
𝜃
0
0
.
𝐼!!! 0 0
0 𝐼!!! 0
0 0 𝐼!!!
𝜃
0
0
𝑇! =
1
2
𝐼!!! 𝜃!
𝑇! =
1
2
𝑚! 𝑉!,!
!
+
1
2
𝜔!. 𝐼! 𝜔!
DNC
8. Evaluate
Lagrange’s
equation
DNC