The goal of this project is to make a robot that can balance itself, but has only two points of contact with the ground. As an additional design challenge, the center of mass of the robot must be at least 1.5 times wheel-radius above the ground (so that the robot does not balance just because of the very low center of mass) - essentially, a mini Segway!
This post will focus on the controls aspect of the problem, since the hardware and circuitry are quite simple.
So you have the following system that works in a loop -
This means that the simplest algorithm would be -
So let us take a look at the effects of having a mocrocontroller that takes some time (albeit in tens of milliseconds) to process IMU data and figure out the motor PWM and direction, and motors that are slack in following these commands. And then we will be able to 'anticipate' them and thus correct for them.
Suppose you free your robot from its vertical position. Eventually it will tip in one direction. The controller takes a few milliseconds to realize this, and tells the motors to speed forward, slowly at first, because the angle is small. The motors take about 40 ms to react to these commands. Note that the robot has been tipping all this time. Two things can happen now -
sumErr = 0;
loop:
end
An integral term in your controller responds to the (time) integral of your control variable. Here is how it behaves for our robot: Say the robot tipped over one side and the proportional gain was not set strong enough to arrest this tipping. The integral gain will respond in proportion to the time build-up of your error (here, angle from the vertical). As you might have realized, this gives a 'kick' (strong response) when the error builds up too much.
D = kD * (currErr - prevErr);
Differential control acts proportionally to the rate of change of error. This simply means that differential response depends on how fast the error is increasing i.e. how fast your robot is falling. This is very important for inherently unstable systems like the self-balancing robot. Let us see how it works in detail with different cases-
Now suppose the robot changed directions due the controller's action and is now going up, but it is still on the positive side of error. Successive values will be smaller than the previous ones, causing the derivative term to be negative and subtract from the proportional term (which will be positive since the robot is actually on the positive side of the vertical). This acts as a 'damper' and prevents the robot from overshooting too much. In inherently unstable systems like the self-balancing robot, damping is absolutely necessary for control; otherwise oscillations will feed on themselves and go out of control.
The overall control,
C = P + I + D;
This post will focus on the controls aspect of the problem, since the hardware and circuitry are quite simple.
Systems viewpoint
You will need motors that actuate the wheels, a drive circuit for them, a controller that controls the motors and a device that measures the angle of the robot with respect to ground. There are a couple of choices for such a device -
- An Inertial Measurement Unit (IMU) is a MEMS device that usually has a gyroscope and an accelerometer on a single chip - this can be used to measure electronically the angle of the IMU chip. If the IMU chip is mounted rigidly on the robot this becomes the angle of the robot (with a constant offset depending on the mounting method). If you 'zero' the IMU angle when the robot is standing vertically, the IMU angle will then be the angle of the robot with respect to ground. More on zeroing below.
- A couple of IR LED-phototransistor pairs can be mounted on two sides of the robot, facing the ground. Based on the voltage given out by the two photo-transistors you can know which way the robot is tripping and how much.
So you have the following system that works in a loop -
Control viewpoint
Realize that the robot will have to balance itself by driving out - if it tips forward, the robot will have to accelerate forward at a sufficient rate to stop the tipping and vice versa.This means that the simplest algorithm would be -
- Drive the motors forward if the robot is tipping forward and vice versa;
- Make the speed of the motors proportional to the robot tipping angle.
So let us take a look at the effects of having a mocrocontroller that takes some time (albeit in tens of milliseconds) to process IMU data and figure out the motor PWM and direction, and motors that are slack in following these commands. And then we will be able to 'anticipate' them and thus correct for them.
Suppose you free your robot from its vertical position. Eventually it will tip in one direction. The controller takes a few milliseconds to realize this, and tells the motors to speed forward, slowly at first, because the angle is small. The motors take about 40 ms to react to these commands. Note that the robot has been tipping all this time. Two things can happen now -
- If Kp is smaller than a 'magic value', the speed commanded to the motors will not be enough to arrest the tipping of the robot and it will fall all its merry way
- If Kp is larger than the 'magic value',the robot will over-accelerate a bit, so then it will cross over and tip in the other direction. There will be a sharp response to this too, which will send the robot tipping in the previous direction - this will go on. This is an inherently unstable system, so the oscillations will feed themselves and keep growing, causing the robot to actually throw itself to ground.
- You cannot, in a time comparable to the lifetime of the universe, find the exact 'magic value' for Kp - so forget about using just proportional control.
Proportional control
P = kP * err;
Note that error is the difference between the set-point and the control variable (angle of the robot measured from the vertical). Since the set-point is 0 degrees for us, we will take the angle as our error.
Integral control
sumErr = 0;
loop:
sumErr = sunErr + err;
I = kI * sumErr;end
An integral term in your controller responds to the (time) integral of your control variable. Here is how it behaves for our robot: Say the robot tipped over one side and the proportional gain was not set strong enough to arrest this tipping. The integral gain will respond in proportion to the time build-up of your error (here, angle from the vertical). As you might have realized, this gives a 'kick' (strong response) when the error builds up too much.
Now you know how integral control works. How can you deploy it? Integral control can be used to add an extra punch to the proportional control when you don't want to set your proportional gain above a certain limit (to avoid the robot oscillating very frequently around the vertical, for example).
Bear in mind that for this project, integral control is optional. Use it only if you need to, because there is a fundamental disadvantage associated with it - wind-up. Let me elaborate on that. Say your robot tipped over to one side, integral control kicked in, and shoved the robot over to the other side. Now, the integral error that has built up during that process will take some time to 'unwind' and change its sign. Meanwhile, the robot might have crossed the vertical line during that time due to the integral kick. If your your integral error has not become zero or very small by that time, you will end up pushing to robot towards ground! So that same 'kick' that saved the robot from falling down on one side will push it down on the other side, because it is too powerful (and a bit slow in changing directions).
To prevent the sumErr from increasing too much, it makes sense to put a cap on the maximum value it can take.
To prevent the sumErr from increasing too much, it makes sense to put a cap on the maximum value it can take.
Differential control
D = kD * (currErr - prevErr);
Differential control acts proportionally to the rate of change of error. This simply means that differential response depends on how fast the error is increasing i.e. how fast your robot is falling. This is very important for inherently unstable systems like the self-balancing robot. Let us see how it works in detail with different cases-
Robot on the positive side, going down
Since the robot is accelerating down, each successive error will be increasing non-linearly as shown below-
You can imagine that a simple proportional gain will not be enough to combat this. However, if we have a differential term in our controller, its response will increase more if the robot tries to fall more sharply, because it depends on the rate of change of the error. Successive values will be greater than the previous ones, causing the derivative term to be positive and add to the proportional term.
Robot on the positive side, going up
Now suppose the robot changed directions due the controller's action and is now going up, but it is still on the positive side of error. Successive values will be smaller than the previous ones, causing the derivative term to be negative and subtract from the proportional term (which will be positive since the robot is actually on the positive side of the vertical). This acts as a 'damper' and prevents the robot from overshooting too much. In inherently unstable systems like the self-balancing robot, damping is absolutely necessary for control; otherwise oscillations will feed on themselves and go out of control.
The overall control,
C = P + I + D;
Tuning the gains
- Initially set the proportional and derivative gains to zero, proportional gain to 1.
- Switch your robot on and increase the proportional gain till the robot is able to change directions once it crosses the vertical i.e. it does not simply flop down. At this stage, it is OK if the robot is not able to stay up for more than a few seconds. You just need to ensure that the proportional part works.
- Now, slowly increase the integral gain so that now the robot does not fall down for a minute or so. You should be able to see the integral 'kick', the power being added to the proportional control at extreme angles which enables the robot to stay up longer. However, the robot will promptly fall down if you poke it, because it is not damped in any way and all that is keeping it up right now is the power of the motors.
- Increase the derivative gain and you should be able to see the damping effect - smoother oscillations about the vertical. Poke it, and the robot should be able to recover reasonably again due to the damping. Practically, the derivative gain Kd will be a lot higher than the proportional and integral gains, because we did not divide by the interval between two measurements in the equation for derivative term above.
That is all there is to making a nice little self-balancing robot! The tuning gains part usually takes up some time; but works out in the end.
Finally, a little video of how my bot was also able to do a 'self-start' :)
Finally, a little video of how my bot was also able to do a 'self-start' :)