Balancing robot PID tuning trouble

Hi all,

I'm making a "simple" balancing robot Here it is:

I'm using a Uno, with an Arduino motor shield to drive 2x 6V mini metal gearmotors. 130 RPM 0.3 kg.cm. With 32mm wheels.

An MPU 6050 from Sparkfun. And pots to tune the PID parameters.

Motors and pots have their own supply (9.6V with 5V voltage regulator for the pots).
The Uno only powers the MPU 6050.

For the code I mostly followed the advice from Kas' thread which has been super helpful (thanks heaps Kas!).

I use the raw readings from the 6050 (I use "getmotion6" from the MPU6050 lib to get 6 raw readings from accel and gyro). Each loop takes an average of 3 sets of readings, then I use the Kalman filter from Kas to get a clean angle.

Angle readings I'm quite confident with, nice symmetric and smooth (although it still picks up a bit of rotation when I shake it in "translation" but it's hard to control the shake so not 100% sure).

Then the PID part. It's my problem.

I start with all 3 coeffs P,I,D at 0 and increase P with a pot.

Problem is, I can't seem to get proper balance with the P. When I increase the P it goes like that:

• phase 1: robot cant recover from the 1st tilt, ie; it's always gonna fall on the side it tilted first. I increase P it just falls slower.
  -phase 2: robot shakes. It's super jerky.

Then I tried everything, play with many values for the I term, I can somehow get it to balance for ~20sec, but it's still supre shaky and ends up countering the slow oscillations too much and slamming itself in the floor.

I tried playing with D but because robot is super shaky it just picks up the "noise" and makes it shakier If I keep the value super low it does nothing and if I increase it starts shaking more.

So there I am... I've been fiddling with the PID values for a couple hours now. I thought it could be backlash, so I tried to map the motor speed from (0,255) to (30,255) so they wouldn't "stick" and not turn at low PWM. Didn't work.

I graphed the command PWM (output of the PID function) and it's all over the place, basically it varies from -255 to +255 multiple times per second.

I was thinking doing some kind of averaging of the command. The whole code loop runs in ~5ms so is it a good idea to try and run the loop say 4 times and only send command every 20ms? Or do a rolling average?

I don't even knoow if the PWM all over the place is a bad thing. The motors need ~50 to get unstuck from stopped so I guess that's the problem, but there could be others.

I would appreciate any advice! thanks! (I can post a video of shakybot if needed)

OK I'll answer to myself

I got it to work! Well it balances on carpet, not on hard floor (yet )

The robot was actually way too heavy... i had a pack of 6AA on top and moved it down and now it's much better.
I'll redo the body so it's smaller and lighter. Thinking about the best design now.

Hi Dwacito

For the code I mostly followed the advice from Kas' thread which has been super helpful (thanks heaps Kas!)

Thanks, glad it helped for your project 8) 8)

I suggest you have a look at Balanduino open project for additional ideas
You can contact Kristian, he is very knowledgeable and helpfull

Finally, when you bot is resonably stable, add a bluetooth card and move it !!!

Should you own an Android phone or tablet, you can use Kristian code or my general purpose Joystick bluetooth Commander application.

Discussion and support [here](Arduino Forum?

PHPSESSID=jfqqvuet348fipgf2pv1osj0c7&topic=173246.0)

Enjoy this fascinating and educational balancing project

Hey thanks Kas!

I got better motors (Pololu 12V 29:1) and a new frame and it works much better. I got it to balance on hard floor

I still don't understand PID very well.

With only PI, I can get it to balance quite well. It keeps balance for several minutes. However it always move back and forth by about 5-10cm, with a period of about 3-5 seconds. I can't seem to get it to hold position better (like stay in a 2cm zone). I've seen some videos of people whose robots stay within 1cm, without using encoders. That frustrates me

Also I don't understand the D term. It does nothing for me it seems. I downloaded your code (Kasbot V2) and it looks like your D term is negative? Like you have P = 6 I = 1 and D = -5. I don't get that. Thinking about it, if the error increases (de/dt > 0) then you want the D term to have the same action as the P term, don't you? And if error decreases, ie. P term is doing its job, then D term has opposite action to P to prevent it from overcompensating. That's my understanding at least. But in that case you would want P and D to have the same sign, no?

Anyway, I'll keep fudging and see what happens. I'll try to upload a video of the new bot. EDIT: here's a (bad) video to show the large "oscillations":

After that I wantto include the encoders, and try to make it move (ie. control global robot position) but not quite sure how to go about that. I'll first try to get better balance. Then I'll make it autonomous, kinda like an obstacle avoidance (I already made one of these) but on 2 wheels. Then add remote control! I ordered some cheap 2.4Ghz transceivers (the nordic chip) on Ebay.

Thanks again!

Sorry for the late response, didn't get the reply notification

I understand encoders are not yet implemented; you can't get proper balancing w/o encoders, as they are part of the PID algorithm. this will be now your next task

PID tuning is (at least for us ) a pure hit and miss, experimental task
I just checked my current setup, both values are positive

Before adding R/C control, make sure that you bot is able to balance forever, even being pushed rather hard
You should end up with a slight back and forth move, due to the motors blackslash

Bigger/softer wheels as shown here also really help balancing

Thanks again Kas!

I added encoders but don't use them for stability, only speed control.

Then I added a ultrasonic sensor and made it autonomous. Hope I get my 2.4Ghz modules soon!

Some details:

I used two cascading PI loops. The global input to the system is the velocity of the Centre Of Gravity (Vcog_target). To have a stationary robot it's Vcog_target = 0 etc.

The IMU raw readings are fed to a Kalman filter to get the robot tilt angle (theta) and tilt velocity:
theta_dot = angle - last_angle

Encoders read from each motors (928 counts / turn each) then I get wheel velocity:
Vwheel = count - last_count

The actual cog velocity Vcog is calculated from wheel velocity and tilt velocity:
Vcog = K x theta_dot + Vwheel

The factor K is to account for various things such as difference in units and height of the cog.

This then feeds the first PI loop which output is the desired tilt angle of the robot:

theta_target = PI (Vcog_target, Vcog)

The target tilt angle is fed into the second PI loop, the "classic" one used for balance:

motor_PWM = PI (theta_target, theta)

and this is sent to the motors.

The autonomous mode works by setting a desired speed. Then it reads the ultrasound sensor, and when an obstacle is detected, sets target speed to 0, waits a small delay, then spins the robot until it finds a path. Then sets speed again, etc.

With the two PI loops the robot is very stable to perturbation (pushes) but doesn't maintain a stationary position very well. I think it's due to my very crappy tuning of the stability PI parameters

I still can't get proper, clean balance when robot stays stationary. I looked at the Balanduino and it's very still when stationary, it uses the same motors as me so I know it's doable.

I still have the same problem, either the robot is too "soft" (low P and I values) and doesn't balance properly, or when I increase P and I it starts jittering a lot.

I think the root of the problem is backlash, and I'm looking at ways to deal with it. Then I will have better stability. I'm thinking of adding a 3rd PID loop to control motor speed: the 2nd PID output (desired motor speed) is fed to a 3rd PID which takes desired speed and actual speed (from encoders) to drive PWM. this loop would have to run very fast though to achieve anything. I don't know if that could work, gonna try it now!

I'm turning this topic into a "project progress" kind of post, hope it's OK.

I'm turning this topic into a "project progress" kind of post, hope it's OK.

It's Ok for me, if you keep on posting XD

I might do a simular project, once I've finished my current project [whenever that may be], so any experience/knowledge is usefull to me.

Impressed
Your project is really moving in in the right direction

I still can't get proper, clean balance when robot stays stationary. I looked at the Balanduino and it's very still when stationary, it uses the same motors as me so I know it's doable.

I suggest you come back to the basics and, before moving it, make sure you bot can stay still forever
Encoders feedback will help staying at the same place

int updatePid(float targetPosition, float currentPosition)   {
  float error = targetPosition - currentPosition; 
  pTerm = Kp * error;
  integrated_error += error;                                       
  iTerm = Ki * constrain(integrated_error, -GUARD_GAIN, GUARD_GAIN);
  dTerm = Kd * (error - last_error);                            
  last_error = error;

  count = -count_R;
  pTerm_Wheel = Kp_Wheel * count;
  dTerm_Wheel = Kd_Wheel * (count - last_count);                            
  last_count = count;
  return -K*(pTerm + iTerm + dTerm + pTerm_Wheel + dTerm_Wheel);
}

This is how bot should behave when pushed:

I also used (count_R - count_L) to compensate for motor speed difference, and follow straight lines

I'm thinking of adding a 3rd PID loop to control motor speed

I had the same idea, as motor torque is very weak at low PWM.
Have a look at the second source code here...

Ho... I can't see your sensor board
Make sure it is located near the COR (R for rotation)

Good luck for your Quest for Balance

kas:
Impressed
Your project is really moving in in the right direction

Thanks

kas:
I suggest you come back to the basics and, before moving it, make sure you bot can stay still forever
Encoders feedback will help staying at the same place

I am coming back to the basics now, I was getting bored of turning potentiometers for hours to adjust the PID, so I decided to move along and add some stuff and see what happens Now I'll definitely work on the balance control to get it clean.

Looking at your PID I realised PD(wheel position) is actually the same as PI(wheel speed)

because wheel speed is the derivative of wheel pos, PI(wheel_speed) = PI (d/dt (wheel pos)) = DP(wheel_pos)
(derivative of P is D, derivative of I is P)
The difference is I actually use 2 separate loops, and don't use wheel speed but cog_speed.

kas:
This is how bot should behave when pushed:

[/quote]

My bot does behave a bit like that (the "lean back" to stop when pushed), you can hear it in the video: when the robot comes close to a wall, motors accelerate briefly to get ahead of the cog and brake the bot, you can hear the sharp acceleration of motors before the stop. It does the same when stationary and pushed, but of course it's not as perfectly stable as Beautifulsmall's bot. I read all his advice in your thread btw I actually tried his method to reduce backlash but no luck with it.

kas:
Ho... I can't see your sensor board
Make sure it is located near the COR (R for rotation)

The sensor is at the centre of the bottom level right next to the arduino and motor driver, so very close to motors axles.

kas:
Good luck for your Quest for Balance

Thanks! I won't stop until I get the same stability as in the video you posted / the balanduino

Hi Dwacito,

I am facing the exact same problem as you had described in your first post. My robot is super-jerky for a specific value of P and I gains. Decreasing the gains dosent help the robot to balance whereas increasing the gains makes it more jerky. Also, D gain is making it more jerky. So I am keeping it 0. When the robot is standing still the fluctuation in the pitch angle reading is about +-1 deg.

Can you please tell me about what did you exactly do to reduce the jerky behaviour of your robot other than reducing its weight.

Dwacito:
I got better motors (Pololu 12V 29:1) and a new frame and it works much better. I got it to balance on hard floor

Hey, Dwacito. I'm having the exact same problem like you, but my robot is a lots worse. I can't even get it balances on its own around 3 second when playing with the gain like you said. When I increase the P gain, it stands a little bit and then slams on the floor immediately. It can't even balances when I set the I gain. It's been 1 week of frustrating Do you have any idea for that problem? Thank you so much

Sancho:
Hey, Dwacito. I'm having the exact same problem like you, but my robot is a lots worse. I can't even get it balances on its own around 3 second when playing with the gain like you said. When I increase the P gain, it stands a little bit and then slams on the floor immediately. It can't even balances when I set the I gain. It's been 1 week of frustrating Do you have any idea for that problem? Thank you so much

Even though this thread first started in maybe 2013.....

Make sure that your robot's power supply can supply the needed amount of power to the robot's motor drivers. And make sure that the motor drivers can supply the adequate amount of current to the motors.

And make sure that your angle sensing measurement module is working reliably.

And...for your control loop software.... make sure you don't create a big lag in your loop by using slow serial communications speeds (that's if you're using serial comms to do serial prints to the serial monitor).

And... choose a small enough sampling period .... eg 1 millisecond.

Thanks Southpark!!!
I finally got it balance But I'm having another problem with controlling it. I added a bluetooth module and control it through Serial pin. I move the robot back and fort by changing the Setpoint of the PID, but once I change the Setpoint, the robot falls down immediately. You have any idea?
P/s: Thanks again for still replying this thread.

Sancho:
Thanks Southpark!!!
I finally got it balance But I'm having another problem with controlling it. I added a bluetooth module and control it through Serial pin. I move the robot back and fort by changing the Setpoint of the PID, but once I change the Setpoint, the robot falls down immediately. You have any idea?
P/s: Thanks again for still replying this thread.

Hi Sancho. Great work. Maybe you can start off with very small changes in setpoint to begin with. To start with..... just go easy on the setpoint. Like..... change it by a small amount only, and see what happens. Eg..... for going forwards, set the maximum allowed change in setpoint to be something small .... like 5 degrees..... just for testing.

Southpark:
Hi Sancho. Great work. Maybe you can start off with very small changes in setpoint to begin with. To start with..... just go easy on the setpoint. Like..... change it by a small amount only, and see what happens. Eg..... for going forwards, set the maximum allowed change in setpoint to be something small .... like 5 degrees..... just for testing.

It goes crazy even when I change a really small amount only, well actually everytime I send the signal from the module to the arduino. Here is my code:

double kp = 700;         // Proportional value
double ki = 1;           // Integral value
double kd = 6;  
double Setpoint = -2; 

void loop() 
      {
runEvery(10)
      {                         // Exetutes this part of the code every 10 miliseconds -> 100Hz 
    timer = micros();     // Reset the time
    MotorControl(Compute(getData()));
      }
      ReceiveData();
      /*Serial.print(kp);
      Serial.print("\t");
      Serial.print(kd);
      Serial.print("\t");
       Serial.print(ki);*/
      }

void ReceiveData()
{
  
  if (Serial.available()>0)
  {
    char a = Serial.read();
  switch(a)
  {
      case 'q':
        kp = kp + 1;
      break;
      case 'w':
         kp = kp - 1;
      break;
      case 'e':
        kd = kd + 1;
      break;
      case 'r':
        kd = kd - 1;
      break;
      case 't':
        ki = ki + 1;
      break;
      case 'y':
        ki = ki - 1;
      break;
  }
      if(a == '1') 
      {
         Setpoint = Setpoint + 1;
         delay(10); 
         //MotorControl(Compute(getData()));
      }else if(a == '2') 
      {
         Setpoint = Setpoint - 1 ;
         delay(10); 
        // MotorControl(Compute(getData()));
      }

Hi Sancho. It's possible that your Kp value may be too large. At the moment, you have a value of 700 for Kp.

I guess it all depends on the relative scaling factors for your error signal...... but I think values of Kp are typically in the range of 10 to 20 or so. Having very large values of Kp can make the bot become unstable.

For testing..... just set Ki to zero for now. And leave Kd as it is. And, if possible.....make Kp much smaller than 700..... such as 15. But then again, it all depends on the what sort of error voltages you have. Although, 700 appears way too large.

Also, the code that you posted....... this code doesn't run within your robot, right? If it does.... then avoid using delay() functions. Delay() functions will slow the loop iteration time, and can make a control system sluggish and maybe glitchy.

Southpark:
Hi Sancho. It's possible that your Kp value may be too large. At the moment, you have a value of 700 for Kp.

Thanks Southpark.
For my robot, only kp around 500 and 700 can keep it balance(even without kd), the value of kp from 0 to 100 makes it slow-responding and from 200 to 500 makes it swings back and forward (with my hand-holding), but that case it's just when I use the Kalman filter, I have tried the Complementary Filter before but I can't get it balance as easy as the Kalman filter, meanwhile the data from the Kalman seems like not really accurate than the Complementary filter.
And until now, I am sill not having a right way to tune my robot.

Hi Friends,
I have a question. I am making a self-balancing robot and I want to use encoders to increase the stability with two PID's. One PID for MPU 6050 and other PID for encoders. The question is: kp, ki, kd are the same values for both PID's??
Thanks,

Dwacito:
Thanks again Kas!

I added encoders but don't use them for stability, only speed control.

Then I added a ultrasonic sensor and made it autonomous. Hope I get my 2.4Ghz modules soon!

http://www.youtube.com/watch?v=SxB7yf4_Tj0

---

Some details:

I used two cascading PI loops. The global input to the system is the velocity of the Centre Of Gravity (Vcog_target). To have a stationary robot it's Vcog_target = 0 etc.

The IMU raw readings are fed to a Kalman filter to get the robot tilt angle (theta) and tilt velocity:
theta_dot = angle - last_angle

Encoders read from each motors (928 counts / turn each) then I get wheel velocity:
Vwheel = count - last_count

The actual cog velocity Vcog is calculated from wheel velocity and tilt velocity:
Vcog = K x theta_dot + Vwheel

The factor K is to account for various things such as difference in units and height of the cog.

This then feeds the first PI loop which output is the desired tilt angle of the robot:

theta_target = PI (Vcog_target, Vcog)

The target tilt angle is fed into the second PI loop, the "classic" one used for balance:

motor_PWM = PI (theta_target, theta)

and this is sent to the motors.

The autonomous mode works by setting a desired speed. Then it reads the ultrasound sensor, and when an obstacle is detected, sets target speed to 0, waits a small delay, then spins the robot until it finds a path. Then sets speed again, etc.

With the two PI loops the robot is very stable to perturbation (pushes) but doesn't maintain a stationary position very well. I think it's due to my very crappy tuning of the stability PI parameters

I still can't get proper, clean balance when robot stays stationary. I looked at the Balanduino and it's very still when stationary, it uses the same motors as me so I know it's doable.

I still have the same problem, either the robot is too "soft" (low P and I values) and doesn't balance properly, or when I increase P and I it starts jittering a lot.

I think the root of the problem is backlash, and I'm looking at ways to deal with it. Then I will have better stability. I'm thinking of adding a 3rd PID loop to control motor speed: the 2nd PID output (desired motor speed) is fed to a 3rd PID which takes desired speed and actual speed (from encoders) to drive PWM. this loop would have to run very fast though to achieve anything. I don't know if that could work, gonna try it now!

I'm turning this topic into a "project progress" kind of post, hope it's OK.

Hi, I see that you V_cog PID takes velocity as an input but you get "Angle" as an output. How is that transformation happening?