Inverted Pendulum on a Cart [Control Bootcamp]

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 dynamic so this should be really fun and really powerful ok we did a lot of math to understand why and when and how to control things but now the rubbers are going to hit the road and we're actually going to see how easy it is to really control a system in MATLAB ok 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 only 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 so 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 States I'm going to change it up and I'm going to call it a vector Y so this state Y dot and maybe I'm not going to call it like so that'll be confusing with the output measure mental to keep it X the vector of states I'm going to say is this position script X it's the loss of the 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 OD E's okay so I essentially have some D DT of x equals some nonlinear function of that vector X so this is for 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 equation or Hamilton's equations and you could get these coupled first order nonlinear oh jeez 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 point 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 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 can 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 ax plus B u ok and now I haven't told you what what u is this is a super important so you 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 X 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 can 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 OTE 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 you 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 L 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 the damping on the cart maybe this cart has action 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 oiler LeGrande or Hamilton's with some four six 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 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 Lagrangian Oh big deal it's just a 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 cart pen and all I do is I specify some parameters so I say this is mass one this is mass five length this to gravity is normal and there's some moderate dissipation on the cart and in MATLAB I can integrate vector field using OD 45 so I can integrate this if I set up some time span so I want 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 state a dot is point five so it starts in the up position but it's moving in the to the right I think or you know in the positive theta direction maybe it's so left but anyway then I go through and I write this OD e45 I tell it that I want to integrate XY 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 the physics with no forcing okay and down here at the bottom I have a function I written called draw cart penned 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 penned okay so 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 going to go and it's just going to keep swinging and swinging and swinging and eventually frictional 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 going to 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 that 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 linearization 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 and 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 penned 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 equation and because the up linearization and the down linearization shares so much in common I decided to have this little switch where if the pendulum is up the switch is 1 and if the pendulum is down the switch is negative 1 and so essentially this is the linearized matrix the a matrix for both conditions and if I want to go for 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 pull place carp end so 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 I of a 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 CPR B 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 4 by 4 matrix so if I look at the rank of this CTR B matrix if this rank is 4 my system is controllable if this rank is not for the system is not controllable okay so with this a matrix and this B matrix corresponding to force in the x-direction of the cart because the rank of my controllability matrix is 4 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 BK X 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 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 eigen values and design this matrix K then we're going to develop an optimal eigen value placement using the linear quadratic regulator or lqr control so that's coming up next

Channel: Steve Brunton

Views: 157,287

Keywords: Control, control theory, Linear algebra, Eigenvalues, Closed-loop, Feedback, Controllability, Reachability, Applied Math, Matlab, SVD, Singular value decomposition, Controllability Gramian, Gramian, pendulum

Id: qjhAAQexzLg

Length: 15min 8sec (908 seconds)

Published: Sun Jan 29 2017