PD of PID in a balancing robot

Hi,
I managed to implement a balancing algorithm based on many examples on the web. It seems that I get the angle right, fast and reasonable, but I have problems in tuning the PD control law there. I only once got it close to balancing, now can't go back there

My small knowledge is that the "P * angle" will introduces the proportional movement of motor related to the current angle and at the same time, the "D * rate" will introduce a damping effect, so that the robot won't swing all the time, but to stay stable in balance. How best to approach the balance point? I started to keep D zero and first add to P, but this also didn't considerably change the situation. when the robot is falling, the motor response is not that fast at first, but near the 30deg gets enough fast, but it is too late! Also, if I just add to P so that to have faster move on small change in deg. then the robot comes wild and no success again! Confused how to balance it!

I'd be thankful to have suggestions how to tune it. Below comes my code.

#define LPWM 3          
#define RPWM 9         
#define LDIRA  6        
#define LDIRB 5         
#define RDIRA  11        
#define RDIRB 10         

#define A_PIN 5     
#define G_PIN 0        

#define A_ZERO 553 
#define G_ZERO 473      

#define A_GAIN 0.280    
#define G_GAIN 0.33    

#define DT 0.01        
#define A 0.962       

float angle = 0.0;      
float rate = 0.0;       
float output = 0.0;     


void setup()
{
  pinMode(LPWM, OUTPUT);
  pinMode(RPWM, OUTPUT);
  pinMode(LDIRA, OUTPUT);
  pinMode(LDIRB, OUTPUT);
  pinMode(RDIRA, OUTPUT);
  pinMode(RDIRB, OUTPUT);
  

  Serial.begin(9600);
}

void loop()
{
    signed int accel_raw = 0;
    signed int gyro_raw = 0;

      accel_raw = (signed int) analogRead(A_PIN) - A_ZERO;
      gyro_raw =  (signed int) analogRead(G_PIN) - G_ZERO;
  
  rate = (float) gyro_raw * G_GAIN;

  angle = A * (angle + rate * DT) + (1 - A) * (float) accel_raw * A_GAIN;
  
Serial.println(angle);

  if(!(angle>30 || angle < -30))
  {

output = angle * 7 + rate * 2 ;

  if(output < -255.0) { output = -255.0; } 
  if(output > 255.0) { output = 255.0; }
 
  if(output >= 0)
  {
    digitalWrite(LDIRA, HIGH);
    digitalWrite(LDIRB, LOW);
    analogWrite(LPWM, output);
  }
  else
  {
    digitalWrite(LDIRA, LOW);
    digitalWrite(LDIRB, HIGH);
    analogWrite(LPWM, -output);
  }
  
  output = -1 * output;
    if(output >= 0)
  {
    digitalWrite(RDIRA, HIGH);
    digitalWrite(RDIRB, LOW);
    analogWrite(RPWM, output);
  }
  else
  {
    digitalWrite(RDIRA, LOW);
    digitalWrite(RDIRB, HIGH);
    analogWrite(RPWM, -output);
  }



  }//if(!(angle>30 || angle < -30))
  else
  {
    analogWrite(LPWM, 0);
    analogWrite(RPWM, 0);
  }

  delay(10);
}

I understand somewhat the aspect of PID control. Where is your I term? In noisy reading situations you need to set D to zero and only use PI. I will make the robot respond faster but only if I is big enough. You also need to have a maximal I so you dont overshoot.

liudr:
In noisy reading situations you need to set D to zero and only use P.

That's correct, in general, but I fear it's hard to turn an instable balancing system into stable control loop without a D part in your PID controller.
And without maths and knowing the dynamics of your balancing system ( or the model you assume of it ), a try and guess solution is hard to find.

Just out of curiosity: what does |angle| < 30 mean physically in your system?
(having a deadband might be nice for power reduction, but complicates the task of getting your control loop stable)

I haven't made a balance bot myself, but from my understanding of the control theory behind the PID algorithm there is an inherent problem in this application: the PID algorithm is usually linear but your balancing bot is a highly non-linear system. It will be approximately linear when it is almost in balance, but as soon as you get away from that you'll find that the PID parameters that work when it is balanced don't have enough control authority. Or, give it the control authority to work there and it will be overreacting near the balance point. Perhaps you could apply a non-linear transform between your raw sensor data and the PID error input to accomodate this - perhaps a sin transform that gives you the angular acceleration associated with the angular error. Or maybe I'm worrying about something that you can ignore - I'm sure there are plenty of working PID-based solutions to this problem. If you're having trouble tuning the PID then make things easier for yourself at first by giving your algorithm some 'training wheels' - I don't mean literally put wheels on it, but increase the rotational inertia of your bot so that it responds more slowly; this gives the sensors and actuators more authority and gives you an easier problem to solve. Once that works you can reduce the inertia and move back towards your original, harder, problem.

well I did not done math. modeling for my system, but I've read some math. modeling of it, mainly by Kalman filtering.

I used a PD, as in many similar code I found that people don't integrate over angle.

I suppose that you mean sina ~= a when a<30, you meant this right?

Well I turn off wheels when angle is over 30 or -30, as it is a fall off.

It's impossible to set D=0, then swing effect is yet more than what I have now. Well, it is swinging, not balancing but seems very close to balance actually.

Is there any way so I could approach it and get rid of swinging with just choosing parameters of my PD, or anyway I must go through the math?

I've seen people made it by trial and error, just searching online!

Why are you not using any I term (integral)

Basically the three terms of PID are :

P Proportional which produces a correction which is proportional to the deviation from set-point
I Intergral which produces a correction based on how long the measured variable has deviated from the set-point
D Differential which produces a correction based upon the speed with which the deviation is going away from the set-point

So for a balancing system you will need a small integral (I) to correct any tendency to "lean"
You will need a fair amount of differential (D) to compensate for fast movement (the more your robot leans the faster it will tend to fall)
You will need a fair amount to proportional (P) so that correction is capable of reacting fast enough.

Ideally you should be looking at a balance stability of <5 degrees (not the 30 being mentioned) so the non-linear COS function (not SIN) is minimised. When you think about it, the human body finds it awkward to walk along a 5 degree slope but extremely difficult to do likewise along a 30 degree slope. So expecting a robot to do what a human finds difficult is really making things hard for yourself.

Thank you, very clear, actually I tried this "I" term and added to my code like this:

...
int angle_intgr_term = 0;
...

void loop()
{
...
output = angle * 7 + rate * 2  + angle_intgr_term * 0.01;
angle_intgr_term += angle;
...
}

but I didn't get improvement, may be too small? Have no idea how to improve the terms. I think there must be a systematic approach to do it right? I imagined it this way:

1. to set D=0 and I=0 for first, and add up to P (but exactly when to stop?)
2. to add to D to get a damp, so when robot adjusts balance, wheels stop moving on too far to swing
3. to add to the I term

Still no success, as robot is swinging, sometimes very wild may be something else is wrong? Can't be angle for sure. Any more suggestions please?

Tomorrow I'd make some photos of my simple robot so you could see how it is going on, may be I get more help

How are you sensing the lean angle of the robot?

PeterH, thank you, I have an IMU, I get output of gyroscope and accelerometer of it and combine them by the filter you see in my code above and get the angle, it is being updated every 20ms.

Another problem I see is that when I connect the system to PC via serial, I have the zero angle defined correct, but it seems that when I disconnect Arduino and the system lives on battery (Lipo, 21v, 2000mah) the set point slightly changes, but well that must not be a big problem.

From swing of the system I guess that main problem is parameters of PID, but I may be wrong easily.

What else I could bring here that may help you see the problems? (soon photos will be)

Since you don't have an absolute reference for the balance point, you won't be able to get it to balance just using the angle as your input. Can you briefly describe the overall closed loop system in general terms - what are the sensors giving you and how is this used to derive the control output? In other words, what does your error signal represent?

by the way sorry if I have mistakes in English, I'm not a native speaker in it

1. With the system turned off, I try to balance it with hand to find where is max. stability physically, without going to equations,
2. I'd then try to adjust output of accelerometer so that final output of gyroscope and accelerometer are zero, near the point I found, well not that exact, but close to
3. The error from here is defined as: angle_set_point - filtered_angle, or just (0 - filtered_angle)
4. I use PID to get a motor output proportional to this output

Well I wish to go through the theory, starting from free body diagram and I gathered papers to read on it. In that case I'd go this way:

1. analysis of free body diagram and writing mech. equations (deepest respect for The Great Newton)
2. modeling close to my system under matlab
3. ending in working code
4. implementing physical system close to results of 1-3

With my configuration of life I'd need a 6 to 8 months to finish that, counting on my not-bad basis in math, but to take it I really need some feeling of 'close success' out of this trial and error

May be there is any chance that I can get it at least swinging close to balance point (angle_set_point=0)?

Please help..

I agree with jackrae about all the definitions. I still recommend using P and I only and disable the D. Some people did it with D doesn't mean they did it correctly. If you take derivative on a noisy signal, you will get large errors. The integral term will bring the system to balance more quickly than just P, while D only makes it more unstable.

Can you send a picture of your robot so that we know what we are discussing.

I presume your robot is capable of 2-axis (X and Y) movement (back-to-front and side-to-side), therefore you must have at least 2 motors to perform correction. Your controller will also have to run two equation sets, one for each motor, each of which will be a PID control.

Based on the robot being a humanoid shape, may I suggest the following :

Tie a cord via a long spring from the top of the robot's head and pass this over a pulley (vertically above the robot) then route it over a second pulley (clear of the robot) and to the free end of the cord attach a weight sufficient to support the robot upright. With the robot upright the spring should be slightly extended.

If the robot now shows an inclination to fall the spring will extend sufficiently to prevent a complete fall.

Now, with "D" and "I" turned off,( D = 0 I = infinity) adjust the gain "P" such that the robot is capable of holding itself up.

To test the control give the robot a gentle nudge and it should try to get back to a vertical position. You will need to perform this for both "X" and "Y" directions

It may not be fully upright but there must be a tendency to get there. If you cannot achieve this then your sensors are supplying insufficient or incorrect data to your controller to achieve an assisted balance. If you cannot achieve assisted balance, you will never achieve auto-balance.

If you set the gain too high the robot will oscillate. If too low the robot will fail to hold itself upright. The optimum level of P is that where it holds itself upright but is just off the point of oscillation.

Once you have P set there will (may) be a slight error and a little bit of "I" should correct this. I'd guess you are probably looking at say I = 5 seconds

Now you need to set some "D" component to compensate for rapid movement so you might want around D = 0.05 seconds

You will of course have to adjust the values to suit the application and please be aware that adjusting any one of the PID factors will/ may require adjustment of the other two to maintain stability.

Lots of tweaking but hopefully a guide to a way ahead.

ironbot:
by the way sorry if I have mistakes in English, I'm not a native speaker in it

1. With the system turned off, I try to balance it with hand to find where is max. stability physically, without going to equations,
2. I'd then try to adjust output of accelerometer so that final output of gyroscope and accelerometer are zero, near the point I found, well not that exact, but close to
3. The error from here is defined as: angle_set_point - filtered_angle, or just (0 - filtered_angle)
4. I use PID to get a motor output proportional to this output

Well I wish to go through the theory, starting from free body diagram and I gathered papers to read on it. In that case I'd go this way:

1. analysis of free body diagram and writing mech. equations (deepest respect for The Great Newton)
2. modeling close to my system under matlab
3. ending in working code
4. implementing physical system close to results of 1-3

With my configuration of life I'd need a 6 to 8 months to finish that, counting on my not-bad basis in math, but to take it I really need some feeling of 'close success' out of this trial and error

May be there is any chance that I can get it at least swinging close to balance point (angle_set_point=0)?

Please help..

It has been an uncomfortably large number of years since I did the math but I don't think the math is the issue here - what matters is understanding that the balancing robot constitutes a feedback loop and you need to understand what the feedback algorithm is. It's all too easy to design it in such a way that it won't converge.

If you identify an orientation so that the bot is approximately balanced, you should be able to design a PID algorithm to keep it at that orientation. But, since the balance won't be perfect, the bot would slowly accelerate sideways until it was going too fast for your motor to keep up and then fall over. For a small bot, that would probably happen very quickly. To avoid that you would need to discover the equilibrium point dynamically by adjusting your 'set point' until the lateral acceleration at that set point was zero. One way to achieve that would be using a separate PID loop. If you want your bot to settle in a particular position (rather than just coming to a halt wherever it happens to be when it gets balanced that would require yet another layer of feedback to vary the acceleration target so that the bot tended to wobble towards the target point. This could be done with another PID.

If you just want to get the darned thing balancing I suggest you increase the moment of inertia of your bot to slow the responses down, and start with I and D elements very low and focus on P. In this mode it should tend to right itself but it will be prone to oscillation and the oscillation will probably amplify until it goes out of control. Then you wind up the D component to give you some damping. As you introduce damping you may find you need to slightly increase the P component. You should use the least amount of damping which prevents the bot from overshooting when it corrects. Since you have designed your bot to be perfectly balanced at the set point, you should not need to ue the I component; this would only really be needed if you wanted it to resist a constant force trying to deflect it away from the set point. If you decided to go ahead with the other two layers of feedback then the I component would become more important.

THat's my view, from a purely theoretical perspective. I'm sure there are thousands of people who have done this in practice and an ounce of practice is worth a ton of theory, so you would be well advised to go scouring the robot forums for advice. But starting from scratch without help, that's how I'd approach the problem.

Still swing, no success in balancing, it is clear that the response time of sensor is lower than needed: if I start to slowly lean the bot, I have proportional response of motors, if I lean fast, only after some 10deg motors start to react, may be my IMU is damaged simply?

Sorry, photos not high quality, but you can see it. I made a simplified Arduino copy and also my own version of L293E motor driver.

I'd try to also make a video so you see how it is working,

I'd be so happy to have your support on

Hard to get an impression of scale, but I'm guessing it's about a foot high.

In the early days while you get the algorithm working I suggest that you slow the system's response down to give you an easier problem to solve. The way to do that is to add weight away from the axis of rotation, for example by adding a piece of wood parallel to the floor and perpendicular to the axle. It needs to be high enough to remain clear of the floor over the range of motion that you are going to support - its purpose is to increase the moment of inertia of the system, not to act as a prop.

I just made a measurement:
height: 28.5cm
width: 30cm (outside of one wheel, to the outside of the other one measured)

By my cnc machine and good software, I can easily change construction, to hit the goal I'm ready to reconstruct it as many times as needed. Could you please tell me more suggestion and I do a complete reconstruction?

May be you know a better model, or a proportionality of sizes that helps in addition to increasing the moment of inertia, or may be there is some numeric proportionality already known for such models of IPR?

I can make design close to specification if I have one, in inventor software, then cut it with cnc so to at least be more sure about the chassis

I also draw simply what I understood from your post, you know I'm not native in English and not even very good in it, I sometimes can't get the right imagination from reading the text..

You have in effect an upside down pendulum since all the weight is above the centre of rotation.

Rather than a piece of wood below the axle (centre of rotation) might I suggest a lead or steel weight such that the robot is almost self-balanced. Then, once you have your algoritms working you can start reducing the weight. It's like teaching a child how to walk, small steps first then as things progress you can get more ambitious.

By the way your comment that fast movements are not reacted to by the motor means you need D factor. D stands for Differential NOT damping. Differential has exactly the opposite effect to that of damping - it actually speeds up reaction to rapid errors. If you want to introduce damping then you need to think about adjusting gain (1/P) Less gain (larger values of P) = more damping

Can you also identify the sensor in your photograph and which axis of the sensor relates to the rotation about the axle.

A copy of your circuit diagram would also be beneficial.

I did not mean that the additional beamneeded to be below the axle. The goal of that beam is not to turn the 'bot into a weeble, it is just to increase the moment of inertia about the axle so that the 'bot responds slower. This makes it easier for your algorithm to control. Once you have got something that will balance at all, and confirmed that the sensors are giving you the input you think they are and your control algorithm is basically sound, you can fine-tune the control algorithm and remove the extra inertia.

Thanks, I will have to find my schematics I did in eagle and upload it, but they already are in my posts on the forum, as I built both of those circuits by the help and support of this great forum. They are simply one Arduino board copy and an L293E motor driver with standard components of datasheet, nothing more.

I have D terms now, actually, after filtering angle, currently I get a PID this way:

output= angle * 3 + rate * 2 + angle_integrated * 0.0

I tried a lot, in fact 2 whole days in changing various possible P.I.D. values. Giving even a 0.05 to "I" term introduces wildness: motors start to run fast and never stop. Values of P and D terms other than what above, also introduce wildness.

I think I'd anyway need a reconstruction of a chassis with first designing a 3D model, based on some model I still don't know of! So that at least the body gives me room for applying suggestions: adding weight, removing weight...

Please have a look at where my sensor is. The accelerometer X axis is horizontally placed, toward the forward movement of the machine, and is the only axis of the accelerometer being used (I have a Y axis too).