I recently took the edX / MIT Underactuated Robotics course (which by the way is excellent, though not easy), and in order to try and consolidate some of the concepts, have been working on the swingup and stabilization of a double pendulum cart system.
A double pendulum cart consists of a cart free to travel along a linear rail, with two pendulums in series dangling from the cart. The only actuator is a force input into the cart parallel to the rail - the pendulums are completely unactuated (hence underactuated, in this case one actuator, three degrees of freedom). The challenge is to design a controller that can swing the pendulums into the vertical position, or some other stable pose, e.g. one pendulum hanging down and one up, and to keep them stable in that configuration in response to disturbances. Obviously it doesn't serve a useful purpose in of itself, but pendulum-cart systems are often use as a vehicle for developing control systems, precisely because they're well understood but difficult to control (partly because of the non-linearity of the dynamics), and because other more complex systems can be approximated by pendulums. A Segway (or balancing robot) is an example of a similar system, albeit one in which you don't have to solve the swingup problem (it's already too late if it's not upright!).
At the moment the project is purely software based, but the intention is ultimately to build a real system. The basic concept is to generate an optimal trajectory using a process called trajectory optimization (direct collocation specifically), linearize the system along the trajectory, and then use a time varying LQR to control the system in closed loop. All the code is implemented in Matlab, and consists of four sections:
- Functions to generate the equations of motion for a solid body system given the Lagrangian
- A small animation library
- A trajectory optimization library using fmincon which implements direct collocation and direct transcription (using 4th order Runge Kutta)
- A function which creates a closed loop controller for the system along the nominal trajectory
The code (which is still in a somewhat messy state) is now up on github. There's still quite a bit of work to do, but all the constituent elements are working pretty well, and I'm confident now that the closed loop controller should be robust enough to control a real system.
If you've never heard these terms before none of this is likely to make much sense, but hopefully these videos will start to explain what it's all about.
Nominal (optimal) swing up trajectory, using a weighted sum of control effort squared and time squared as the objective cost function (above).
Above: Now the physical properties of the system are perturbed (mass and moment of intertia increased by 5%, damping doubled). Using the nominal force trajectory from the trajectory optimization, it's unsurprisingly not possible to swing up the pendulum.
Incidentally this is simulated using ode45 which uses a variable time step, hence the slightly unsteady playback speed. I need to re-write my animation export function to pick animation frames at a roughly constant interval of time (it's on the to-do list!).
Above: Here's the system with the same pertubations to the physical parameters, but this time using the closed loop controller. Now it can successfully swing the pendulum up. Notice that the two pendulums are near vertical by the end, but the cart is about 0.6 units to the right of the nominal trajectory final position. The LQR is weighted to favour pendulum angle over cart position.
Next - building the real thing!