LQR Control for the Inverted Pendulum on a Cart: Full-State Design

[00:00:02]okay welcome back so in the last few segments we've taken this inverted pendulum on a cart we have simulated the full nonlinear system without control we've linearized about the fixed point where the pendulums up to get an a matrix and a D matrix we've checked that the system is in fact controllable so if I look at the rank of the controllability matrix it has dimension 4 so it spans the four dimensional state space of the position and velocity of both the carts and the pendulum and then we've also shown that since the system is controllable I can design a full state feedback u equals minus KX so that I can place the eigenvalues of the closed-loop system anywhere I want okay and it's one line in MATLAB where you specify the eigenvalues and it will find this gain matrix K - to move your system to those eigenvalues we then take that control law apply it to the full nonlinear system and we show that in fact we can stabilize this up unstable inverted pendulum configuration which is really cool but it begs the question where do we place the poles where do we place Augen values where are the best eggs and that's a really interesting kind of open-ended question you could spend a long time trying lots of different combinations of eigenvalues and how do you know when you find the best one so this is just an amazingly powerful tool in control theory called the linear quadratic regulator so I'm going to write it out you have something called the linear quadratic regulator that essentially answers this question linear quadratic regulator or lqr control and the idea is if I can cook up a cost function that tells me how how bad is it if my state is really slow to converge to where I want it to so if I make this thing really lightly damp barely stable it will take forever to stabilize to move this thing from point A to point B can I put a cost on how much that bothers me it how bad that is if this thing is slow and can I also put a cost on my actuation you may be it actually costs fuel or something maybe it costs energy or you know I have to buy a bigger beefier motor to make this thing move really fast into the stable position so I'm going to build a cost function it's going to be J equals the integral now I'm going to integrate from 0 to infinity but really you could just integrate for a long time from 0 to 10 or 0 to 30 or whatever so I'm going to integrate from 0 to infinity and what I'm going to do is I'm going to develop a matrix Q that is going to tell me how bad of a penalty it is if X is not where I tell it to be so this is an N by n matrix in this case a 4 by 4 matrix and I'm going to take X transpose QX so this is you know I want this to be a positive number so I want Q to be non-negative so kind of positive semi-definite and I'm also going to have a similar term u transpose R U and I'm going to add an X and u are both functions of T so I'm going to integrate with respect to T I'm going to add up these penalties if my state is not where I tell it to be this thing's trying to stabilize to zero for example then this is going to be big ok so I need this thing to stabilize to where I tell it to quickly for this to be small and I want to not spend a lot of energy doing so or else this is going to be big and so Q and R are these matrices so for example if I wanted to build Q equals let's say 1 1 10 and 100 and zeros everywhere else essentially what that would do is it would say the first variable in X which is the position of the cart I penalize with one if it's not where I tell it to be quickly same with X dot this is the theta dot penalty it's bigger I want I want my thing to be stabilized in the vertical position quickly so I'm going to make this number bigger so that it's more penalized if it's not where I tell it to be in the vertical position and I'm making the theta dot penalty even larger and you get to play around with these numbers basically what you do I might start with the identity matrix and then if I think that the theta variable is really important I'm going to bump up data and theta dot by factors of ten until I get good performance and four are maybe in this case you know electricity is cheap maybe I have a really beefy electric motor maybe I overspent my motor and so I'm going to say R is really small point zero one basically free I can really actuate this thing aggressively and so in MATLAB if you have Q and R it turns out that there is an optimal K matrix and honest-to-goodness best K protocol there's a best control law that minimizes this cost function and that's called the linear quadratic regulator okay so this controller is the linear quadratic regulator so let's break it out linear it's a linear controller it's linear matrix times the state it's a linear full state feedback controller it minimizes a quadratic cost function if you plot this thing it's a quadratic cost in the state and the control and it has a well-defined minimum cost for some protocol so we're finding the linear controller that minimizes this quadratic cost function and regulator just means this will stabilize my system this system will be stable so that X will go to zero okay and if I tell it a reference value of up and move over you know four units it will stabilize that system okay so that's what a linear quadratic regulator is and again the great thing in that is that this is just incredibly easy k equals lqr of a B Q and R its decibel so I need to cook up a Q matrix of an R matrix I think mine are actually pretty similar to this I need an a matrix and a B matrix for the linearization and lqr will give me the optimum controller for that cost function with these cost weights of how much I weight the state deviations and how much I weigh the control expenditure okay so we're going to try this out let's just fire up MATLAB so again same script except now this is lqr cart pen so I have the same parameters for the cart and the pendulum I have the same a matrix same B matrix but now you'll see I have a Q matrix okay so this is exactly my Q matrix my Q matrix penalize this data and data dot more than X and X dot and my our matrix says there's not much of a penalty on control expenditure controls cheap okay so I run this one simple command K equals lqr and that gives me my optimal state feedback law for this case okay we run the same code to simulate the nonlinear system of OD 45 and plot so I think we're ready to go let's try this out l QR cart pend enter let's go so it finds K and it simulates and it does this nice aggressive stabilization to that position I told it to so this works like a dream and in fact I could compute the I guess be K and see where lqr chose to place them and so interestingly it takes one eigenvalue and makes it really aggressive probably theta dot you know it's probably corresponds to theta dot and then the other ones are not so aggressive and if I wanted to probe this I actually could I could what would I do i would look at T comma D equals I of a minus BK so I get my eigen vectors and eigen values the look at let's say the real part of D the diag of the real parts of D so these are my eigenvalues the real part and then if I want to see what the eigenvector looked like that corresponds to this most stable eigenvalue all I have to do is look at the first column of T and interestingly it actually looks like the most stabilizing directions are X dot and theta dot so what this controller is saying is if I want good performance you better have really good aggressive control of X dot and theta dot that's how that's what you want to be super fast super aggressive that's got the biggest negative eigenvalue and I didn't tell it to do this I just gave it a weight on how much I want it to converge and how much I care about control expenditure and it turned it for me so there's a lot of stuff I'm glossing over for example how you actually find the minimum the K that minimizes this is very interesting and kind of complicated so that you could do it with l'orange multipliers but you can do it by hand using optimization and then solving for that K involves solving something called a riccati equation and so this is actually expensive for high dimensional systems this can be I think order n cubed in the state dimension X in RN so for example if I'm trying to control a million by million you know a million dimensional state system this is going to be intractable I'm not going to be able to build an lqr easily for a million state system ok but for our little 4 dimensional system really easy to do this in MATLAB in real time okay so there's considerations now what I think is really important to tell you is when I was cooking up this example and a few hours doing it I took about an hour making that nice visualization that actually plops the pendulum in cool colors I spent a couple of hours getting the nonlinear equations and the linearization that's a longest time developing the control law took me less than five minutes okay getting the control law is the easy part building a model is the hard part okay building a model and the linearization is generally speaking the hard part the control is incredibly simple in MATLAB one line it fits in perfectly it's very clear what it means okay and if you wanted to play around with this what I would recommend is try making are more expenses so let's do that let's say let's say we're going to make our actually kind of expensive so I'm going to save this and run it it's not going to be as aggressive takes it's much slower floor and if I look at my eggs they're a lot less aggressive right they're different and they're less aggressive I could make my control really expensive I could say well I've got a wimpy motor and electricity super expensive so I'm going to really penalize control expenditure and all I have to do is change that one number and everything else is exactly the same that's what I love about control design especially in MATLAB is if I want to try a bunch of these things all I have to do is change you know a couple of numbers nothing else changes and notice it was pretty sluggish and my eigenvalues are now you know even less damped okay so that's the mile-high overview of lqr we can see that it actually works on a real non linear system this inverted pendulum on a cart very simple to design optimal in some cost function I didn't tell you how to derive it but you can look that up in a book or on on the internet but it really is optimal linear full state feedback control with respect to this quadratic cost function okay thank you