You can pursue a "model free" approach where you manually adjust gains until the robot balances, or you can pursue a "model based" approach where you attempt to model the robot, and use that model to select appropriate gains. You can also do both, perhaps by initially manually finding gains that allow the robot to stay upright long enough that you can collect data to make a model.
Here is what I was able to achieve using a model-based approach and LQR control design: My video
I would suggest putting "training wheels" on the robot to limit how far
it can fall and the damage caused by falling, and putting strain relief
on your USB cable (tying one end of the cable to the robot and the other
end to a heavy object (desk leg)) so you don't destroy
USB connectors on either end when the robot drives off with the cable
attached.
Here is some data from my first attempts
at manually choosing gains to
balance the robot.
The commands to the motor largely were saturated, and every time they
switched from one extreme to the other the robot would vibrate. I noticed
that the motor commands matched the wheel accelerations fairly well, so
1) the angle and angular velocity don't seem to affect what the wheel
does, and 2) I can make an independent model of just the wheel responding
to the motor commands. Here are the accelerations and commands plotted together:
Here are my best attempts to model the body rotation. Body angular velocities:
The question is whether this imperfect model is good enough to design a model-based controller and Kalman filter.
Today I tried improving the model. First I weighed all the parts of the
robot, and tried to locate where they are relative to the wheel axle.
I then tried to compute a body center of mass, and moments of inertia.
Note that the rotor of the motor rotates with the wheel, at gear_ratio^2
times the wheel velocity. The stator of the motor, and the gearbox,
rotate with the body. I guessed that the rotor was 1/4 the weight of the
entire motor, and modeled it as a uniform density cylinder with half the
radius of the motor. Because it is geared, it contributes gear_ratio^2
times its moment of inertia, which makes it dominate all other moments
of inertia. Here are my notes.
Next I derived a new model whose state variables are the amount of
motor axle angle (not x = r_w*wheel_angle measured with respect
to a vertical
axis) and body angle. The reason I did this is that the encoder measures this relative angle between the wheel and the body, not the wheel angle with respect to vertical.
Note that motor_axle_angle = x/r_w - body_angle.
The state vector order is also rearranged: [ wheel_angle, wheel_angular_velocity, body_angle, body_angular_velocity ], so the A matrix will be closer
to block diagonal.
I added viscous friction to this model. I adjusted this friction
and scale parameters for the motor command, since I don't know what
torques the commands generate. Here is the fit I got for the early part
of data file run526/f000. (This data is mentioned above and is available
in this file.)
Here is a typical fit:
So the above fits are pretty good for matching an unstable system with
an open loop simulation (just input the commands into a model).
One remaining mystery is why the body velocity is delayed about 4
ticks (12ms) relative to the wheel velocity. This is mentioned above,
and is clear if you look at the real and model body velocity, and try
to match up the slope discontinuities caused by the command going from
one extreme to another. The discontinuities are synchronized for the
relative wheel angular velocity plot.
I am going to look if the delay is in the
gyro hardware, and we are not reading
it with the lowest latency possible.
After investigating, I decided the latency was the same for the IMU
data and the wheel encoder data. My low pass filtering and differentiation
made the wheel velocity data spread out in time, which made it look
like it changed sooner than the IMU data. When I removed the filtering
this went away.
It turns out there is something that looks like gear rattle that occasionally appears:
Here is the above data where the same phases in the sinusoid are
plotted on top of each other. Sometimes the gear rattle happens,
and sometimes it doesn't.
What are ways to handle this? Reduce all gains to a minimum?
Use a dead zone? Do something fancier?
Here are the actual and predicted velocities. It is pretty unusual that an
open loop simulation that is just driven by commands matches
the actual behavior for so long (about 3 seconds):
Since the simulated angular velocities match the actual angular velocities well,
it is not surprising that the simulated wheel angle(s), which are integrations of the simulated angular velocities, match the actual wheel angles(s):
You can use this data to create a model of the entire robot, including
the body which acts as an inverted pendulum.
Body angle predictions, which are quite bad. Then again, this is an unstable
mode, so open loop prediction was never going to work well.
I had to delay the commands to get this
model to match the timing of
the gyro slope discontinuities.
The new model twip2.m is in the model2 directory,
which should be inserted in the balance-data1 directory mentioned above next to the
model1 directory. Look at model2/notes.txt to see how to generate the plots
above.