LQR and Iterative Learning Control strategies
for Cart Pole Control - Swingup problem
Dynamics of Cart Pole
Deriving equation of motion through Lagrangian, and then applying Partial Feedback Linearization (PFL) we get the following dynamics equation.
Position of cart - x
Pole angle - theta
Control Input to cart - f
Energy Shaping
Control input 'f' is given to cart and energy is pumped into the system so that the pole (pendulum) reaches vertically upright position finally or into a region (basin of attraction), after which we can apply LQR to the system locally and stabilize the system about its unstable fixed point.
Energy shaping is a concept to apply energy to the system and get the system into basin of attraction as described in this paper.
The concept was implemented in MATLAB and the following are the plots and results.
Basin Of Attraction for Implementing An LQR
It is interpreted from the obtained plot that in the set of initial conditions (angle theta, angular velocity) for the pole marked in the red colored region, an LQR controller will stabilize and bring the system to vertically upright position.
Cart Pole Swingup Problem
We need to have the knowledge of linearized dynamics in order to apply LQR control to the cart-pole system after swinging up into the basin of attraction.
So the following were the A and B matrices after computing the jacobian.
Mass of pole - m
Mass of cart - M
Length of pole - L
A = [0 0 1 0
0 0 0 1
0 m*g/M 0 0
0 (m+M)*g/(M*L) 0 0]
B = [0
0
1/M
1/(M*L)]
Cost matrices used for this problem are
Q = I and R = 1
State Trajectory Of The Pole
State trajectory starting from stable equilibrium position to vertically upright position as a result of the controller - Energy shaping and LQR. We apply a switching a controller basically - until the pole goes into the basin of attraction we apply energy shaping controller, and later this controller is switched to a LQR controller which makes sure the pole always remains vertically upright.
Iterative Learning Control - Hardware Implementation
LQR control strategy for balancing the pendulum at inverted position and Iterative learning control strategy for the system to track a sine wave repeatedly with least error is implemented through MATLAB Simulink.
LQR makes the pendulum vertical by choosing gains, and we supply an additional input (error in tracking the sine curve from the previous iteration) as a result of ILC, which tries to compensate the error in every subsequent iteration while following the sine curve.
