Welcome back. So now we're actually going to start coding
up real examples in Matlab, seeing if they're
controllable, designing controllers to stabilize the
dynamics. So this should be really fun and really powerful. Okay? We did a lot
of math to understand why and when and how to control things,
but now the rubber is going to hit the road and we're actually going to
see how easy it is to really control a system in
Matlab. Okay? So one of the systems I really like to work
with is imagine that I have an
inverted pendulum. But that inverted pendulum
is sitting on top of a cart,
okay? And so I'm going to have my inverted
pendulum, but now that pendulum is on a cart and I can move
that cart around. So for some reason, this has always felt much more physical
to me than just, you know, having a pendulum with some torque
control on the bottom. That seems too easy. This is a little more interesting,
right? I have some cart, and I can move that cart around, and I'm trying
to stabilize this inverted pendulum. Okay,
so let's think about what numbers we need.
Well, first of all, we need to define the state of the system,
okay? And so the state of the system is given by the
position of this cart, we're going to call that x.
And the angle of this pendulum arm,
we're going to call that theta. So our kind of high dimensional
dynamical system state,
I'm going to change it up and I'm going to call it a vector y.
So this state y dot.
Yeah, maybe. I'm not going to call it y because that'll be confusing with the
output measurements. Let's keep it x. The vector
of states, I'm going to say is
this position script x its
velocity script x dot theta
and theta dot. Okay? And so this is a two degree
of freedom system. There's two degrees of freedom x and theta,
the position of the cart and the angle of the pendulum. And because it's two
degrees of freedom with Newton's second law, I get
four coupled odes. Okay? So I essentially
have some d dt
of x equals some
nonlinear function of that vector x. So this is four
nonlinear row equations. I'm not going to bore you with the details
of how to actually derive these, but you could use something
like Euler lagrange equations or Hamilton's equations, and you
could get these coupled first order non linear odes.
Okay? So this is an honest to goodness nonlinear problem,
but we can linearize the system and just intuitively,
we know that there are a couple of key fixed points,
okay? So the fixed points of the system,
fixed points,
we're going to have a fixed point corresponding
to the pendulum down position. So what we
know has to happen is theta has to equal either
zero for the pendulum down or PI for
the pendulum up. Theta dot
has to equal zero. X dot has to equal zero.
And if I wrote down all of these equations, you'd see that none of these
rates of change actually depend on x itself.
And so x is a free variable.
And that kind of makes sense, right? It doesn't matter if this thing is here
or here or here. If I have the pendulum
up or down and there's no velocities, it's a fixed point.
So I've got two basic fixed points corresponding to,
let's say the theta equals
zero case is the pendulum
down with the cart fixed,
and the theta equals PI is going to be the
pendulum up with the cart fixed.
Okay? And so if you had
the Euler Lagrange equations and you had these fixed points, you could compute the
jacobian and you could essentially get,
you know, so through d f dx evaluated
at a fixed point, you could get a linear
system of equations. X dot equals axief
plus bu. Okay? And now,
I haven't told you what, what u is. This is super important.
So u the control input for this system, what we're going to get to control,
we're going to say that that
u equals a force on
the cart in the
XT direction.
Okay? And this is actually very reasonable.
What I can do is I could have like a motor on one
of these wheels and I could have some controller. So that I
could, if I specified, I want, you know, twice the force,
I could give it twice the voltage and make this thing move.
Okay? So this is actually very realistic. You could build a
cart that would hold this pendulum and drive it around, and you could
actuate with force. Okay? And so now what I'm going to do is
I'm just going to fire up Matlab and show you I've already cooked
up all of these examples. So I
have this nice function. So in Matlab, right,
I build these right hand side functions. This is my nonlinear ode.
It's called cart pend, the cart pendulum system.
And it gives me the time derivative
given the state. So here it's called y. That's why
I wanted to call it y. Sometimes in Matlab, you call these states y.
That's just kind of Matlab convention and a couple of
other numbers. I just have a bunch of parameters out here. So I say
that this little mass has mass. Little m.
The cart has mass, big m.
This inverted pendulum has a length
lithe, we know that gravity pulls down
and then there's d, which is essentially some damping term. Okay?
So in these equations, there could be some friction,
I believe. I mean, the damping to be as opposing the x
dot. Okay, so it's a damping on the cart. Maybe this cart has some
friction. And I'm assuming the pendulum has a lot less friction.
And then finally I have this u. So if I was going to
apply a force, you can derive Euler Lagrange or Hamilton's
with some forsyth applied. And I'm not going to bore you with all
the details. There's going to be a link to this on YouTube, so you can
download this code and read through it yourself. But the right hand side, these dynamics
are pretty nasty. Okay? This actually takes a long time to
derive by hand on pencil and paper for the system.
This is the most time consuming part of this whole demo is actually getting this
right hand side vector field, right?
But I'm not going to bore you with the details. You can see,
like if I go on and on, it's a pretty long, nasty expression,
okay? And the input, the force
enters in the x dot. And the theta dot equations,
when you go through Euler Lagrange, that's what you find,
okay, no big deal. It's just nonlinear right
hand side. And this is relatively easy to simulate the system
with or without forcing in MATlab. So I'm going to go
up to this function called Sim cartpend.
And all I do is I specify some parameters. So I say
this is mass one, this is mass. Five. Length is two,
gravity is normal, and there's some moderate dissipation
on the cart. And in MATLab,
I can integrate vector fields using OD 45. So I can
integrate this if I set up some time span.
So I want to integrate from zero to time ten with
this initial condition. So again, this initial condition means
x is zero, x dot is zero,
theta is PI. So it's up, and theta dot
is 0.5. So it starts in the up position,
but it's moving to the right, I think, or,
you know, in the positive theta direction, maybe it's to the left. But anyway,
then I go through and I write this ode 45.
I tell it that I want to integrate x,
y dot equals this function of y.
And I'm going to lock in these parameters and there's zero control.
So for right now, we're just going to simulate this thing in free physics
with no forcing. Okay. And down here at the bottom,
I have a function I've written called drawcartpend.
And so, essentially, it just plots a movie of the pendulum.
And again, you can download this and see what I did. So let's
actually try to run this sim cart
pend. Okay, let's hope this runs.
And you can see this is basically what you expect to happen. You let this
thing go, and it's going to just swing and swing and swing. And eventually,
if I integrated it for longer, it would come to rest. Maybe I'll
just show you that, because it's pretty easy. I'll integrate it for a
little bit longer.
Okay. It's gonna go, and it's just gonna keep swinging and swinging and
swinging, and eventually, friction will kick in, and it'll kind of come
to rest. Okay. And it's only stuck
here because I stopped integrating. So that's the system we're gonna control.
Okay. We're in a great shape to actually control this
system. So what we're going to do, we know the fixed points.
I, you know, offline, I computed this jacobian matrix.
It was ugly and kind of a mess, and I plugged in these
fixed points. So I have linearized equations about the
pendulum up condition, and I have linearized conditions about
the pendulum down condition. And so what we're going to do
is we're going to simulate.
We're going to design controllers based on those linearizations
and then apply them to the full, honest to goodness nonlinear system.
Okay? So the first thing we need to do is get this
jacobian. Then we plug in the fixed points in either
the pendulum up or pendulum down condition to get this linear system.
And then the first thing we need to do is check and see is this
controllable. Okay, so that's what we're going to do.
Now, I have this code here called
pull place cart pend.
Okay? And what
I've done here is offline. I've essentially cooked
up the a matrix and the b matrix
for this linearized system of equations.
And because the up linearization and the down
linearization share so much in common, I decided to
have this little switch where if the pendulum is up, the switch
is one, and if the pendulum is down, the switch is negative one.
And so, essentially, this is the linearized matrix, the a
matrix, um, for both conditions.
And if I want to go from pendulum up to pendulum down, I just
switch this s, this s and this s
to negative values. Okay? So this is the a
matrix and the b matrix. Now, all I want to do at this
point is,
first of all, I'm going to run this code. So I'm going to run my,
I'm going to run pol.
It's going to load the a matrix and the b matrix. Oh, I think that
ran the whole thing. So I'm going to load my
a matrix and I'm going to look at eigenvalue.
So these are the eigenvalues of my a matrix in the pendulum
up condition. And notice that one of these eigenvalues is unstable.
So that thing is eventually, if it's, if it's epsilon off of vertical,
it's going to accelerate away.
Now, what's really important is I want to know what
is a and b controllable. Okay? So this was something we've
been working up for up towards, and it should be really easy to test now.
So I'm going to try, first of all,
ctrb of a comma b.
This will give me that controllability matrix. It's a big matrix.
I'm just going to hit enter. You'll see.
Right. It's a big matrix. It's a four by four matrix.
So if I look at the rank of this CTRB matrix,
if this rank is four, my system's controllable. If this rank
is not four, the system is not controllable.
Okay, so with this a matrix and this b matrix
corresponding to force in the x direction of the cartae,
because the rank of my controllability matrix is four,
I can span all of my state space with this controllability
subspace. And that means that I can actually develop
a controller to control the system. So what's coming next?
This will be in the next video. We know it's controllable.
And so what that means is that I can cook up some control law,
u equals minus kx so
that the closed loop system is x dot equals a
minus bkx.
And I can develop, I can design k so
that the eigenvalues of this are anywhere I want them to be. So what I'm
going to do next is I'm going to specify, I want stable eigenvalues.
I want to move these eigenvalues into the stable left
half plane. So we had that one unstable eigenvalue. I'm going to move it into
the stable left half plane with some proportional feedback
of x. And just to remember what this diagram looks
like, we have our system,
in this case, the cart pendulum. We're going to measure the
full state x. We're going to feed it back.
So u is
going to be minus kx. Okay,
so we're literally going to say u equals minus this matrix
k times the measurements of x. We're going to measure everything and
that's going to stabilize the system to have these eigenvalues.
So that's what we're going to do next. We're going to use the place command
in Matlab to place these eigenvalues and design this matrix
k. Then we're going to develop an optimal
eigenvalue placement using the linear quadratic regulator or LQR
control. So that's coming up next.