Pages: [1] 2 3   Go Down
Author Topic: PD of PID in a balancing robot  (Read 5149 times)
0 Members and 1 Guest are viewing this topic.
0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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  smiley-surprise

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.
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);
}
Logged

Central MN, USA
Offline Offline
Tesla Member
***
Karma: 73
Posts: 7198
Phi_prompt, phi_interfaces, phi-2 shields, phi-panels
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged


Germany
Offline Offline
Faraday Member
**
Karma: 59
Posts: 3057
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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) 
Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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!
Logged

SW Scotland
Offline Offline
Edison Member
*
Karma: 17
Posts: 1390
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
...
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 :-)



Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

How are you sensing the lean angle of the robot?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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)
Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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?
Logged

I only provide help via the forum - please do not contact me for private consultancy.

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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..
Logged

Central MN, USA
Offline Offline
Tesla Member
***
Karma: 73
Posts: 7198
Phi_prompt, phi_interfaces, phi-2 shields, phi-panels
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged


SW Scotland
Offline Offline
Edison Member
*
Karma: 17
Posts: 1390
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
« Last Edit: April 23, 2012, 01:38:49 pm by jackrae » Logged

UK
Offline Offline
Shannon Member
****
Karma: 223
Posts: 12630
-
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
Logged

I only provide help via the forum - please do not contact me for private consultancy.

0
Offline Offline
Sr. Member
****
Karma: 0
Posts: 301
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.
http://i45.tinypic.com/op8hlt.jpg
http://i48.tinypic.com/2w73i92.jpg
http://i48.tinypic.com/af7ll5.jpg
http://i47.tinypic.com/346qidt.jpg

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  :-)
Logged

Pages: [1] 2 3   Go Up
Jump to: