HW5: Controlling a single axis robot, Part Deux
Tomasz Malisiewicz, Mark Palatucci, and Geoff Hollinger
Abstract
For this assignment, we implemented a velocity state estimator using a Kalman filter. We then repeated a number of the experiments from assignment 4 using this velocity estimator rather than the velocity feedback from the motor tachometer. We calculated the max gains for a PD controller and also repeated the dynamic programming solution to perform an optimal swing trajectory. Finally, we implemented gravity and friction compensation to swing the pendulum at a constant velocity. The assignment 5 page further describes the task.
Part 1: Implementation of a Kalman Filter for Velocity Estimation
To estimate the velocity of the pendubot, we implemented a Kalman Filter. Our Kalman Filter implementation uses the angle and the angular velocity as the state variables, and it takes advantage of the process model derived in the previous assignment. Our Kalman Filter uses both the raw angle measurement and the numerical derivative the angle measurement as measurements. The derivation is given below:
The matlab implementation is given in the tarball as kalFilter.m. This takes a log file and runs the Kalman Filter velocity estimator on it offline.
When implementing the Kalman Filter on the actual robot, we determined that we could not trust the process model as much as we had expected. This is likely due to hardware changes since the previous lab. To account for this, we decreased the measurement noise of the angle derivative to 0.1 (diagonal of R = [0.1 0.1]). This made the estimated velocity somewhat noisier but produced graphs closer to the measured velocity. The process model still provides some smoothing for the estimated velocity. Kalman.c contains the code for the C implementation of the Kalman Filter.
Part 2: PD Control of Robot - Comparison of PD Gains
After implementing the Kalman Filter state estimator, we attemped to drive the robot with the estimated velocity gain. For the full swingup, the robot went unstable at k = 0.01 and b = 0.009. These constants are the same as in the previous assignment. For the horizontal swingup, the robot went unstable at k = 0.01 and b = 0.007. This is slightly higher than in the previous assignment (b = 0.006). Generally, the robot seemed to perform very similarly with the Kalman Filter as with the raw velocity measurements.
To quickly move the pendulum to a horizontal position, we empirically optimized the PD constants. As in the previous lab, the proportional gain was increased to reduce steady-state error, and then the velocity gain was increased to eliminate ringing. After some testing, the constants of k = 0.08 and b = 0.003 were selected. These were the same constants selected in the previous lab. With these constants, the robot settled to within 0.1 radians of horizontal in 0.2801 seconds. This is slightly faster than the time achieved with the raw velocity measurements. The figures below give the angle, estimated velocity, and measured velocity for the run.
Part 3: Swingup Trajectory Using a Kalman Filter for Velocity Estimation
Next, we ran our dynamic programming solution from the previous assignment using the estimated velocity from the Kalman Filter. The pendulum successfully swung up to the vertical position, and the swing up looked very similar to the one in the previous lab. The score achieved was 36.1677, which is similar to the score achieved in the previous lab (33.9555). The graphs below give the angle, measured velocity, and estimated velocity for the swing up. The dp swing up can be run by executing ./sample in the tarball.
Part 4: Gravity and Friction Compensation
In this part, we wanted the pendulum to swing with a constant angular velocity. We achieved this
by compensating for the gravity and friction torques. Our torque model was:
In order to swing with a constant velocity, we wanted our angular acceleration to be zero. We
did this by calculating the sum of the torques from friction and gravity and then applied the
negative amount of torque.
(sign of angular_velocity)*CoulumbFrictionConstant -
ViscousFrictionConstant*angular_velocity = I*angular_acceleration
After some experimentation, we determined that the constants calculated for the system model in the previous assignment did not provide a sufficient model to swing the pendulum at a constant velocity. This was likely due to the amplifier change on the pendulum and the lack of high velocity trials in the modeling data sets. To swing at constants velocity, we empirically selected gravity, coulomb friction, and viscous friction constants (Cg = 0.5, Cc = 0.2, Cv = 0.001). Furthermore, the process noise in the Kalman Filter was increased to put more emphasis on the measurements (diagonal of Q = [100 100]). The figures below show the measured and estimated velocity for a constant velocity run. After being hit initially to a constant velocity, the pendulum maintains a constant velocity with some minor oscillations.
