The first thing to do is to read the IMU, which tells the robot which way is up and how the body is rotating. Here is example code that reads the IMU, and in theory could balance the robot. It is similar to the previous servo code. I had to change the sampling interval to 3ms to accomodate reading the IMU. The numbers for the Kalman filter in this code are not right, but should be in the ballpark. Figuring out the bias of each sensor when you turn the robot on for each session is important. Here is a program to print out averaged IMU readings. Prop the robot upright to find out the biases. My experience is the gyro biases are stable day to day, but the accelerometer biases change within a single session of working with the robot (or maybe I can't reliably prop the robot up in the same position).
Figuring out how to calibrate and filter the
sensor outputs is also important.
Here is some data and a program that might
help you. This data was taken with the wheels locked in a stand:
Note that the gyro saturates at around +/-32400, and when it saturates with
a negative value, a positive saturated value is sent. There is some code
in the final balance software to avoid this negative to positive flip.
Here is an example of gyro saturation where the robot is locked in the stand,
so the wheel angular velocity and the gyro output should be the same:
Another issue is that angular acceleration of the body affects what the
Y accelerometer measures.
A Kalman Filter should introduce much less delay that a
simple low pass filter, since it uses
a model of the process
dynamics to predict future states such as velocity.
Here are the results from my implementation, where the
Kalman Filter has much less delay, except towards the
end of the movement. Any ideas why that might be happening?
and the robot was moved by hand. Accelerometers is atan2(Y accelerometer, Z accelerometer). Online Kf is a Kalman filter that integrated the gyro velocity
to get body angle (prediction step), and then used the Y accelerometer
reading to fix any bias in the gyro signal. The "velocity from encoders" is from a numerical differentiation of the encoder signals.
So the encoder, accelerometer, and gyro should all match. Here are
the signals after calibration. From data file run527/f000:
From data file run527/f002:
The calibration coefficients are in the regress.m program.
To get these figures,
Get the data and program,
Set up the in_stand directory,
cd in_stand/model1/,
run Matlab,
and then type:
c527
plot_it(ff000)
plot_it(ff002)
into Matlab
Here is another plot showing all the signals after calibration everything
from in-stand data:
This data is taken when the robot is manually driven back and forth with
the wheels locked in the stand. The wheel angle provides the ground
truth body angle. The accelerometer is scaled to match the slower
angle oscillations at the beginning and end of the trial. We see
that when the body oscillates faster, with bigger angular accelerations,
the accelerometer excursions are about 2-3 times bigger than the
wheel angle excursions. To show that the angular acceleration explains
this difference, we predicted the Y accelerometer signal we expect to get based and the differentiated gyro signal, and we get the following match:
There is still some error in the predicted accelerometer signal that
is due to gyro saturation (see above), but gyro saturation occurs
when the angular acceleration is small, and the peaks of the
accelerometer signal are correctly matched. We chose to ignore this
effect of angular acceleration on the accelerometer measurement,
since compensating for it would require online estimation and us
of angular acceleration, which would potentially create and inject
noise into the controller, and the accelerometer error
did not seem to degrade control much.
Here is a comparison of Kalman filters with ground truth data (the robot
wheels are locked in the stand, so wheel angle readings should be
the same as body angle estimates). The robot is manually driven in the
stand. Wheel angle is compared to a causal Kalman filter (online kf)
and an acausal Kalman smoother (offline smoother,
running the causal Kalman filter both
forwards and then backwards in time).