Self-Balancing Robot keeps moving forward and backward

Projects Discussion and Showcase Robotics
1

Hi,
I am building a self balancing robot and I finally made it to balance but the only problem I am running into is the robot keeps moving. It does not stop.

Here are the parts I am using.

  • MPU6050
  • 30:1 Metal Gearmotor 37Dx68L mm 12V with 64 CPR Encoder (Helical Pinion)

I have connected the MPU such that when the robot is standing vertically:
+X is up
-X is down
+Y is left
-Y is right
+Z is forward
-Z is backward

The "input angle" that I refer to here after is the angle the robot makes with the +X axis. It is the tilt angle.

I am using cascade controllers where the inner controller controls the tilt angle and the outer controller controls the speed of the robot.

For the outer controller, the desired speed is 0 and the input speed is measured from the encoder, and the output of the outer controller will be the desired angle for the inner angle controller.

As for the inner angle controller, the desired angle comes from the output of the outer speed controller, the input angle comes from MPU6050, and the output of the inner angle controller is used to run the motors.

I have a strong feeling that the robot never stops because of an error that I have in the way I calculate the input angle. Here is the function which takes one parameter "firstTime". In the "setup" function I call calculateInputAngle(true). This will allow me to read the input angle (tilt angle) from accelerometer before running the motors.

The accelerometer gives good result when there is no vibration. Once I start the motors, I call calculateInputAngle(false) so that I get the change in angle from the gyroscope which does not get affected by the vibrations of the motors.

Here is my calculateInputAngle function that I suspect my error is coming from. Does anyone see any problem with it?

void calculateInputAngle(bool firstTime) {
  // Read the raw ACC/GYRO data from MPU-6050
  readIMUData();

  // Subtract the offset calibration value
  gyroRaw.x -= calGyroX;
  gyroRaw.y -= calGyroY;
  gyroRaw.z -= calGyroZ;
  accRaw.x -= calAccX;
  accRaw.y -= calAccY;
  accRaw.z -= calAccZ;

  // Convert to instantaneous degrees per second
  gyro.z = (double)gyroRaw.z / (double)gyroScaleFactor;
  gyro.y = (double)gyroRaw.y / (double)gyroScaleFactor;
  acc.x = (double)accRaw.x / (double)accScaleFactor;
  acc.z = (double)accRaw.z / (double)accScaleFactor;


  accAngleX = atan2(acc.z, acc.x) * RAD_TO_DEG;

  if (firstTime) {
    inputAngle = accAngleX;
    return;
  }

  // Complementary filter
  // Too much vibration in the  systeem makes the acc reading unreliable.
  // gyro is less sensetive to vibrations.
  // gyro drift a little over time.
  // acc term is added to compensate for the drift
  inputAngle = a * (inputAngle + gyro.y * dT) + (1 - a) * accAngleX;
}
2

Me2. You don't take into account the x acceleration forced by the motors. Your calculation is correct only if the robot is standing still or moving at constant speed. As soon as you start controlling the motors the angle is incorrect.

4

Thanks for your help. Can you please elaborate on how I should take into account the x acceleration forced by motor. I thought I am already doing this like this:

  accAngleX = atan2(acc.z, acc.x) * RAD_TO_DEG;

Shouldn’t this be the angle made by the vertical x-axis?

5

acc.x is affected by the inclination of the sensor and the acceleration by the motor.

6

I have changed my function to look like this but still the robot has a tendency to move forward and backward. It does not stand still.

void calculateInputAngle(bool firstTime) {
  // Read the raw ACC/GYRO data from MPU-6050
  readIMUData();

  // Subtract the offset calibration value
  gyroRaw.x -= calGyroX;
  gyroRaw.y -= calGyroY;
  gyroRaw.z -= calGyroZ;
  accRaw.x -= calAccX;
  accRaw.y -= calAccY;
  accRaw.z -= calAccZ;

  accXAngle = asin(accRaw.z / sqrt(accRaw.x*accRaw.x + accRaw.y*accRaw.y + accRaw.z*accRaw.z)) * RAD_TO_DEG;

  if (firstTime) {
    inputAngle = accXAngle;
    return;
  }

  pitchAngle = gyroRaw.y / gyroScaleFactor * dt * .001;
  rollAngle = gyroRaw.z / gyroScaleFactor * dt * .001;

  pitchAngle += rollAngle * sin(gyroRaw.x / gyroScaleFactor * dt * .001 * DEGREE_TO_RAD);
//  rollAngle -= pitchAngle *sin(gyroRaw.x / gyroScaleFactor * dt * .001 * DEGREE_TO_RAD);
  
  
  // Complementary filter
  // Too much vibration in the  system makes the acc reading unreliable.
  // gyro is less sensetive to vibrations.
  // gyro drift a little over time.
  // acc term is added to compensate for the drift
  inputAngle = a * (inputAngle + pitchAngle) + (1 - a) * accXAngle;
}

Does anybody have an idea what exactly the problem is?
Thank you.

7

I know for a fact my IMU is tilted by a couple of degrees. I am not sure how to make it 100% perpendicular to the surface.

8

Add a regulator (PID...).

9

I already use two PIDs. An outer that controls the speed and an inner that controls the angle.

10

How do you "control" the angle without feedback from speed change (acceleration)? Doesn't such an interwoven system call for a Kalman filter?

11

These are the PIDs that I use.

image
image1094×232 5.82 KB

Vdesired is set to 0.

12

I suspect the problem is in the PID control parameters, not in the reading of the sensor. I think you need to tune the PID.

13

I have built a self balancing robot and I am running into the same. problem. The robot can stand without any problem but it can't stop. It moves either forward or backward. Tuning the PID for the angle was not difficult. I only needed a value of kp more than 10. Kd was not. doing much. Kd of 2 was able to reduce bouncing. The speed PID did not seem to add any value.

I have placed the IMU vertically and horizontally but there was no difference. The robot keeps moving. I feel that the center of gravity is not on top of the access of rotation. Could that be the problem?

Could someone out there share his / her knowledge.
Thanks

14

Looks like the MPU-6050 the data rate can be programmed from 4Hz to 8000Hz and the filter response can be from 5Hz to 256Hz. These rates should be considerably faster than the speed PID and angle PID computation rates or oscillation (hunting) will occur.

15

You are thinking in static terms about a dynamic problem.

In order to get vertical it must move and while you may get very close to desired, the thing will tilt and your system will compensate by moving.

Have you tried using a gyroscope?

16

PJRC dot com sells Arduino-compatible ARM-duinos with high clock rates (Teensy 4.1 is 600MHz) and Floating Point Units in the 32-bit cores. Floating point calculations without FPU except for powers of 10 operations magnitudes slower than using integers.

17

This topic was automatically closed 180 days after the last reply. New replies are no longer allowed.