Balancing robot - How to control the velocity and angle

Question

We are currently making a balancing robot as a school project.

The robot has a gyroscope and an encoder to get the angle of the robot and the rpm of the wheels. By using the current angle of the robot and defining a target angle we are able to make the robot stand still and not fall over.

We don't understand how to incorporate the encoder (that measures the angular velocity of the wheels) to make the robot stand still when he motor speed is changing. The robot only knows the angle it is at so it won't compensate for the speed. Could one perhaps use the encoder in such a way that dictates the target angle and keep the robot still or even better control it?

We would also like to know if there is a way to automatically adjust TargetAngle to the needed one, like if the floor angle were to change.

Here is a sample of our code:

error = TargetAngle - CurrentAngle;
integral = integral + error*ki;
derivative = (error - lastError)*kd;

lastError = error;

pidsum = derivative+error*kp+integral;//this is what is used for throttling the engines

Answer 1

First of all, well done on getting your robot to balance! If you’ve got this far the rest should be easy. A cascading PI loop (no D) works very well for this. You have an outer loop to control the speed and an inner loop to control the stability. In the outer PI loop, you set the speed and the necessary angle is calculated (hint: angular velocity can be calculated simply from the conversion of PE to KE). This is then passed down as the angle set point for the inner PI loop. This is what has been used here. I hope this helps.

Answer 2

Consider doing a PID loop on the motor angle.

Target angle can be calculated tick-by-tick based on the motion profile you want the robot to move compensated with the desired robot angle (you will likely want to lean forward when beginning to more forward, etc. like a person would do).

Actual angle can be determined by the encoder position combined with the gyro angle.

Answer 3

Balancing problem is all about eliminating all the torque on the robot with the motors. We are currently working on a balancing system which is more advanced but the idea is almost the same. You should use torque controller using the encoders.

$$\frac{V - Bw}{R} = I$$

Where $$V$$ is the voltage you give to the motor and $$w$$ is the angular velocity of the motor. $$-Bw$$ part is the back EMF from the rotating motor. Since torque is directly proportional to current, you can use PID to calculate the required velocity of the motors. You can calculate the required torque by using the gyro.