The objective of this lab was to balance a rod on top of a cart using an LQR controller to stabilize it. This was done both with an ideal situation and trying to incorporate deadband, saturation, and noise.
Ideal System
The first thing I tested was the ideal system. I first ran the simulation out of the box, ensuring that the dynamics worked as I expected. As I did in fact expect, the pendulum without any control quickly fell over and swung to a close. I changed the system parameters to fit my robot by adjusting the weight of the cart to around 560g, but other than that they all looked pretty good.
Next, I tried implementing Kr control first using the eigenvalues suggested in lecture, which worked like a charm. for my state value I took the error measurement of the desired state differenced with the current state.
I tried a few different values to test a reasonable range. I made sure to keep the values negative, so that it would be stable. If they are too small, the control is not strong enough and the system becomes unstable. Around [-.5, -.6, -.7, -.8], it almost goes off screen indicating it is not stable, but recovers and stabilizes, but much smaller the control is not strong enough. Implementing too high of values actually produces a really interesting result where the control overreacts, and moves far quicker than the robot would actually be able to realistically move.
After toying with that, I set to setting up an LQR controller. Again, using the suggested values from lecture worked very well.
Q = np.matrix([[1, 0.0, 0.0, 0.0],
[0.0, 1, 0.0, 0.0],
[0.0, 0.0, 10, 0.0],
[0.0, 0.0, 0.0, 100]])
R = np.matrix([.002])
#solve algebraic Ricatti equation (ARE)
S = scipy.linalg.solve_continuous_are(P.A, P.B, Q, R)
# Solve the ricatti equation and compute the LQR gain
Kr = np.linalg.inv(R).dot(P.B.transpose().dot(S))
self.u = Kr*np.subtract(des_state, curr_state)
Increasing R never causes the system to become unstable. Knowing that the actual sensors will have some kind of noise, and that the actual actuators will have both a deadband and a saturation point, we can't have the system reacting too quickly, and it also can't make that fine of adjustments.
Unideal System: Deadband and Saturation
Next, I incorporated some of the realistic non-idealities of my system into my simulated controller. First, I implemented a saturation acceleration: I know that I cannot accelerate infinitely quickly, as noted earlier when observing the quickly reacting system. I went back to lab 3 and checked out the class consensus on max acceleration, noting that it was around 330cm/s^2. Using this and the knowledge of the mass of the cart (560g), I calcuated that u could be no larger than around 1.85 (F=MA). Therefore, I set a condition that if abs(u)>1.85, it would just have to max out at 1.85.
This simulation ran exactly the same as before with the LQR system, without a hitch. Next, I tried implementing a deadband. The deadband was taken from the minimum velocity I could move the robot; with the tape on the wheels, this was around 80-90 out of 255. To translate this to a force, I multiplied this by the saturation velocity out of 255, to get a deadband force of around .6, conservatively.
This was very difficult, and at first no matter what my Q and R, I could not seem to get a stable controller while implementing deadband. The deadband was implemented much the same as the saturation velocity, like so
In addition, I tried splitting up the deadband, where anything below .3 would be set to zero and anything above would be set to .6, but that was not useful. Every time, I would get an error along the lines of "ODEintWarning: Excess work done on this call (perhaps wrong Dfun type). Run with full_output = 1 to get quantitative information.
warnings.warn(warning_msg, ODEintWarning)"
After some back and forth on campuswire, Kirstin and Sadie pointed out that instead of comparing my signal to a square wave, I could instead compare to a sine wave, such that the controller would always have to be moving, and never crash. At first, I was trying to change zref in pendulumControllerDynamics.py, but I learned from trial, error, and Greg that it should actually be the ref and ctrl signals in runSimulation.py. After playing around with amplitude and frequency values for this sine wave, I finally got a stable, albiet dance-y, inverted pendulum with deadband. Increasing the frequency and amplitude helped to consistently change the value it was being compared to, which made it not as stationary, which is what was causing errors. I found the graphs interesting; here are two with different values of frequency, 1 and .6 respectively. I increased the penalty on R a lot so it wouldn't move too much and roam off screen, as it was wont to do when the R was too low.
Finally, I incorporated noise into my system. For x noise, I used the accelerometer, as that was how I determined how far I had moved, and for the theta noise I used gyro noise, as that was how I determined which direction I was facing on the real robot. From lab 6 I could see that my accelerometer data had a std dev of noise around .2 m/s^2. The gyroscope was actually fairly low noise, with the drift being a much larger issue. I calculated the noise on the gyro to be around .1 deg/s based on lab 6. To implement this, I added gaussian noise with the standard deviation based on these lab 6 readings.
I was getting many errors of the type "ODEintWarning: Repeated convergence failures (perhaps bad Jacobian or tolerances). Run with full_output = 1 to get quantitative information."
Following Sadie's suggestion on campus wire, I turned down my deadband to a less conservative value and tried only applying noise to one variable, theta or x, rather than both. Unfortunately, this still would not run for more than 10 seconds without failing with the same errors as listed above. They happened mostly when the pendulum slowed down too much, so I tried penalizing x less, but that did not help enough.
There were similar results when parameters were changed. I may have been able to retune the system to work better with retuned parameters; for instance, saturation velocity will change as the battery dies on the robot. However, with things like noise, if the noise is increased, which would likely happen if the motors were running, I can imagine the control would be even worse than it already is, which was confirmed by turning the noise higher and observing that the simulation failed even faster.
Some factors that cause the simulation to fail likely involve the robot moving too slowly, and therefore falling into the deadband too fast and cutting immediately from a high motion value to static, which would cause a discontinuity. Also, if there is too high a penalty on motion or movement of the pendulum, in x or in theta, the robot may be stuck between moving slowly and letting the pendulum fall, whereas if it is set too low, it tends to spin wildly in circles and wheel right off screen.
