View ECE4960-2020 on GitHub

Cornell University: ECE 4960

Return to main page

Lab 11b: LQR on the Inverted Pendulum on a Cart

Objective

The purpose of this lab is to familiarize yourself with linear controllers, LQR control and their cost functions. Although we do not expect you to make this code work on your actual robot, we expect you to approach the lab with that goal in mind. That means adapting parameters to fit your particular system, checking how sensitive the control is to imperfections in your model parameters and to realistic noise in your measurements. For the purposes of this lab, we will assume that you can measure the full state of your system at a sufficient time interval.

Prelab

Check out lectures 15, 16, and 17 for a recap of linear systems, how to linearize non-linear systems, controllability, and derivation of the inverted pendulum on a cart dynamics. Note that the position of the cart referenced as ‘x’ in lectures, is denoted ‘z’ in the code.

Download lab code, and familiarize yourself with the functions in the folder.

  • pendulumParam.py: initializes physical pendulum parameters and state space equations.
  • pendulumControllerDynamics.py: describes the nonlinear dynamics of the system.
  • signalGenerator.py: computes various reference functions to test the pendulum.
  • pendulumAnimation.py: helper functions for animating the system.
  • plotDataZ.py: helper functions to plot state variables and reference values for debugging.
  • runSimulation.py: integrates the entire vector field using the controller and the true nonlinear dynamics, and plots the results.

Lab

Feedback control of an idealized pendulum on a cart system

  1. Adapt the system parameters to fit your robot – you can assume that the pendulum length and mass are as stated in the downloaded code.
  2. Run the system open loop (without control) to convince yourself that the nonlinear dynamics are behaving as expected.

  3. Add a Kr controller. Try different pole/eigenvalue placements, including some that are too passive and some that are very aggressive. Argue for a reasonable choice:

     import control
 dpoles = np.array([..insert eigenvalue1.., \
                    ..insert eigenvalue2.., \
                    ..insert eigenvalue3.., \
                    ..insert eigenvalue4..])
 Kr = control.place(A,B,dpoles)
  4. Compute and test LQR K-gains.
    • Try different values for Q and R.
    • Try to find the highest amount of penalty you can add to the energy expenditure before the cart is no longer able to stabilize the pendulum.
    • Argue for your choice of Q and R given the knowledge of your real actuators and sensors.
    • Hint: Check the eigenvalues and eigenvectors of the closed loop system to understand what is required of your controller.
     Q = np.matrix([[..insert penalty1.., 0.0, 0.0, 0.0],
                [0.0, ..insert penalty2.., 0.0, 0.0],
                [0.0, 0.0, ..insert penalty3.., 0.0],
                [0.0, 0.0, 0.0, ..insert penalty4..]])
 R = np.matrix([..insert penalty5..])
 #solve algebraic Ricatti equation (ARE)
 S = scipy.linalg.solve_continuous_are(A, B, Q, R) 
 # Solve the ricatti equation and compute the LQR gain
 Kr = np.linalg.inv(R).dot(B.transpose().dot(S))

Feedback control of a non-ideal pendulum on a cart system

  1. Your actual robot has limits both on the minimum (deadband) and maximum (saturation) velocity. Implement realistic saturation and deadband values (given your robot) on your nonlinear system and check whether you can still implement an LQR controller that works.
    • Note that you may have to adjust your R and Q cost functions.
    • Pay special attention to units. Your motors are set in values of 0-255, whereas u is a force. Theta ha to be in radians for the small angle approximation to work.

Feedback control with sensor noise

  1. Building on the non-ideal system from (5), explore how sensitive your controller is.
    • What happens if you add realistic noise to the sensor measurements? You can model the noise as Gaussian, fitted around the actual output from your sensors. Remember that the noise from your IMU may increase if the motors are spinning.
    • What happens if the parameters of your model are slightly wrong? Focus on parameters that are hard to get ground truth for.
    • Can you still make control work in simulation?
    • Discuss other factors that may cause the model, and therefore the implemented K-gain to fail.

Write-up

To demonstrate that you’ve successfully completed the lab, please upload a brief lab report (<1.000 words), with code snippets (not included in the word count), photos, and/or videos documenting that everything worked and what you did to make it happen.