This is a quick prototype robot designed to test certain mechanisms before spending my time building a more polished robot. It has two wheels and drives them forwards or backwards as the body tilts in order to keep the robot balanced upright.
This prototype is made up of an Arduino Nano, an MPU-6050 Inertial Measurement Unit (IMU), two 28BYJ-48 stepper motors, and two ULN2003 motor controllers all powered off of a bench power supply at 7.4V and a little over 1A.
The wheels are wooden circles with slots in the middle for a press-fit with the motors. Everything not plugged directly into the breadboard is hot glued to the side. This prototype was built and tested over the course of several hours.
The code for this project is hosted on GitHub
IMU feedback
The Arduino communicates with the MPU-6050 Inertial Measurement Unit over an I2C bus. The IMU gives back angular velocity from a gyroscope and acceleration data from an accelerometer. It is used to determine the pitch of the robot.
My first thought was to integrate the angular velocity over time to get the angular displacement.
/*
Gyro is an array of the current angular velocity around
the X, Y, and Z axes.
The sensor's configuration is such that any pitch will occur
around the Z axis.
SENSITIVITY variables convert the data to the appropriate units.
Gyro: degrees per second
*/
Pitch += Gyro[2] * GYROSCOPE_SENSITIVITY * DeltaMS / 1000.0;
During testing I learned that, while the gyroscope is useful for obtaining quick angular velocity data, drift makes the result of this integration inaccurate.
An accelerometer can be used to calculate it’s pitch by detecting the direction of the net external force, gravity. This method can also be used to determine initial pitch.
float GetAccelerometerPitch(){
/*
Accel is an array of the current acceleration on the X, Y,
and Z axes.
The sensor's configuration is such that pitch will occur
around the Z axis.
*/
return atan2f(Accel[0], Accel[1]) * 180.0 / 3.14159;
}
However other forces act on this sensor, such as when the wheels drive to correct it’s angle, and a solution relying solely on the accelerometer is accurate only when external forces are minimal.
One solution for this problem is to use a Complimentary Filter to combine the two. The purpose of the complimentary filter is to use the gyroscope data for quick changes, and use the accelerometer data to find the calculated pitch from gravity over time.
/*
Gyro is an array of the current angular velocity around the X, Y,
and Z axes.
Accel is an array of the current acceleration on the X, Y,
and Z axes.
The sensor's configuration is such that any pitch will occur
around the Z axis.
SENSITIVITY variables convert the data to the appropriate units.
Gyro: degrees per second
Accel: g's (9.8 meters per second^2)
GYRO_WEIGHT was 0.96f in testing
*/
Pitch += Gyro[2] * GYROSCOPE_SENSITIVITY * DeltaMS / 1000.0;
int forceMagnitude = abs(Accel[0]) + abs(Accel[1]) +
abs(Accel[2]);
forceMagnitude *= ACCELEROMETER_SENSITIVITY;
if(abs(forceMagnitude) < 2.0){
// If there is not a lot of force
float p = GetAccelerometerPitch();
Pitch = Pitch * GYRO_WEIGHT + p * (1.0 - GYRO_WEIGHT);
}
This implementation is responsive within milliseconds, eliminates drift (observed over the course of 30 minutes), and has noise of 0.02 degrees.
Motors
There is a theoretical target angle where the robot is balanced and still. The motors will drive forwards or backwards to bring the pitch closer to the target angle. Due to the nature of floating-point numbers, it is expected that the two will never be equal. The motors will run at a speed dependent on the error between the current angle and the target angle.
All I had on hand for this prototype was several stepper motors which run in discrete steps. I control the speed of these motors by adjusting the frequency of the steps.
As seen in the video at the beginning of this post this prototype works, but stepper motors are not optimal for this scenario. The motors/gearing that I have step in 5.625°/64 = 0.088° increments. This allows the motors to run at about 10.25 RPM (700Hz) with minimal torque. This is usually fast enough to balance when the error is on the scale of individual degrees.
One feature I would like to add is controlled movement. To move, the robot would tilt in the direction of movement and drive while holding that angle. The torque resulting from driving forwards would be equal in magnitude to the torque from gravity but in the opposite direction. To stop, the robot would temporarily speed up so there is greater torque to revert the angle back to a balanced angle, and then stop.
The slow speed of the stepper motors eliminates any possibility of implementing the control described above. My stepper motors with this gearing cannot turn fast enough to hold any significant angle while moving. I could get a stepper motor with a different gear ratio for increased speed, but this would result in bigger steps and less precision.
A standard DC motor would turn faster and we would control the speed rather than the timing of the steps. Since the positioning of the DC motor does not rely on steps, it’s precision relies on the sensors. A faster DC motor would have less torque, but this project does not require a great deal of torque.
Due to the above described control, I will replace the stepper motors with dc motors + encoders.
Closed-loop speed control
When the pitch of the robot is further away from the target angle, the wheels should turn faster to correct it. I went with a Proportional-feedback system to control the speed of the motors.
float targetAngle = 15;
const int MIN_STEP_DELAY_MS = 2;
const float MAX_PITCH_OFFSET = 5.0;
const float P = MIN_STEP_DELAY_MS / MAX_PITCH_OFFSET;
/*
targetAngle is the angle at which the robot is balanced.
MIN_STEP_DELAY_MS is the lowest time (ms) between steps
without skipping.
MAX_PITCH_OFFSET is the angle from the targetAngle where the wheels
will spin full speed.
*/
int GetMSBeforeNextStep(){
float error = abs(targetAngle - Pitch);
float result = error * P;
float reciprocal = 1.0 / max(0.01, result); // max is to avoid dividing by zero
return max(MIN_STEP_DELAY_MS, reciprocal);
}
The feedback system used to control the speed of the motors is simply proportional to the error and then I find the reciprocal because a larger error should result in a smaller delay between steps.
The target angle is not directly vertical, at 0 degrees, because the robot is not perfectly balanced front-to-back. Through testing I found that a good balanced angle for this robot is 15 degrees tilted back (positive 15 degrees in my scheme). This is reflected in the assignment of the variable targetAngle to 15.
Hard-coding this value to 15 is not ideal. Best-case, there would be a startup routine to calculate a balanced angle by comparing the observed rotation due to gravity with the expected rotation due to gravity if the balanced angle was zero. These motors are not fast enough to handle any significant deviation from balanced, so starting in an unbalanced state (where this algorithm would be used) is impractical for this prototype.
I have updated this code to embedded-c and have a post describing this process.
