PD of PID in a balancing robot

Projects General Guidance
21

ironbot:
http://i46.tinypic.com/4ghrmd.png

I've only just looked at your sketch. That's not what I meant at all. The additional piece needs to be higher up and turned through ninety degrees so that when the 'bot pivots around the axle, the ends of your additional piece move a long way. This is what gives it the inertia about the axis.

Responding to your later post, could you explain what signal your sensors give you and how you get the 'bot's lean angle from them? Are you actually working out the lean angle at all? Given an accelerometer at the top of the 'bot, you might only be detecting for/aft acceleration; that wouldrespond when the 'bot tips rotates around its axle, but will also respond to acceleration from your motor as you try to balance the 'bot. That's going to need more analysis to separate the two signals if that's what you're hoping to do. So, how are you determining the lean angle of your 'bot?

22

Ok, I changed it, now is this the idea? Please have a look:
http://i48.tinypic.com/34zyp9t.png

So, how are you determining the lean angle of your 'bot?

The x axis of accelerometer is set in the direction of movement of the robot and horizontally, as seen in the picture, gyroscope feels rotation around 'y' axis, i.e. the rotation of x axis between -g and g. So I combine both of their output to get the lean angle. I learned this by reading a lot online, I never seen a technique somewhere used to differentiate the signal of the motor rotation from the robot lean. May be that is the reason it is oscillating this much fast?

Well, here how I combine accel and gyro output:

      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;
23

Besides all good hints about how to get the system slower
(BTW:for me as a "normal human", balancing a 1 ft (30cm) rod is too hard, but I can rather easily keep a 6 ft (2m) rod upright, even in 2 dimensions)
When I studied Control Systems at university long ago, we also had an exercise similar to this one (upright rod balanced in one direction by moving the ground point) Of course the goal at that time was to learn doing the maths, but the real experience was to analyze what data comes out of the sensor(s).

I fear the "30" means "degrees" in your sketch: that warns me that your sensors might not be sensitive enough. When you have an open control loop and keep the rod balanced manually:

  • is 0 calibrated properly?
  • is the slightest leaning against one finger signaled as != 0 ( and in the correct direction ; ) ?

When you close the loop and lean the rod slightly against one finger, the motor should start immediately and follow your finger.

  • with which P does it start swinging ?
  • does it still react properly with half of that instable P ?
  • does a small D make it act random ( noise is the second problem, I agree with liudr ) or simply instable as with a too big P ?

What you'll need is an automatic 0 calibration ( I )
Hope you can one day add a position control. Good luck!

24

Michael_x, thank you, I answer some of questions in one post and first time possible I do a video so you and everybody kindly helping me here can see it in the practice and provide information about P and its variations.

Current state of project:
A) holding robot in air with hand, not putting on the ground:
There indeed is a +/-5deg with relation to vertical state, where leaning the robot won't start motors, if I lean some 2deg, motors won't start, but a "sound of noise" only goes sharper and sharper until they start. If I add to P or D, or both, the reaction happens faster and with even 1deg, motors react correctly, but then they also remain wildly active: just little bit of movement and they stay rotate too fast.

B) putting on the ground:
Same behavior nearly, but now that +/-5deg becomes +/-15deg,

Now, if I add any "I" term, robot becomes totally crazy!

I can't say that sensors are giving wrong data or giving it right but late: if I connect the bot via serial to PC and monitor output of filter, angle changes very realistic: on 0 it stays on, like, 0.40 and all the time oscillates like 0.34, 0.26, 0.66, ...but it is on 0 stable! then a 1 deg. movement outputs 1deg in same form: 1.33, 1.53, 1.37, 1.22,...

All the problem I see is in parameters of PID, and as I've been pointed to, the chassis construction for what I have to add to momentum.

Also, I can't say that zero is calibrated correctly: I have no way to set it exactly where I feel the system mechanically has equilibrium. I turn it off and guess in what position it is so close to equilibrium, then if I turn on by connecting Arduino to PC and reprogram to adjust the A_ZERO (zero of accelerometer) to have it know the more correct zero position, then when I disconnect from PC and connect battery source, I get a different zero! It changes based on what power is powering.

I have no encoder available, I can make optical encoders for motors but it takes time. I think I will first try to change construction to add to momentum as described for me here, to see some more balance, then go for encoders.

Also, Please let me know, if it really helps I can reconstruct it in 2m height. That is no problem and if you see it pays back in easier balance, I'd do it.

I want to balance it :smiley:

25

Of course you need to know where deviation=0 is:
If this is just slightly wrong, your bot at best will drift away more or less fast.
If there's no way to detect this properly, you just can solve it by contolling both angle and location in an even more complex 2 level control loop, controlling the average output to be 0.

2deg -> noise, but motor won't start

This is another main problem, IMO: due to motor characteristics, control does not start linear at 0 but has a dead band.

2° is rather late already, but you'd need something that makes your bot act "immediately" (even a decent Integral term might sum up rather late). When on ground, motor needs more power to move, of course. If linear control ( pure PID with a good I in it ) does not help, experiment with a nonlinear term:

    analogWrite(LPWM, output+M_BIAS);
...
    analogWrite(RPWM, -output+M_BIAS);

Another idea ( I do not know Arduino's servo or stepper software, though ): What about instead of using normal ( high frequency ) PWM, do a similar PWM on a low frequency: switch the motor really ON (such that it turns) for a more or less long moment during your control cycle.
BTW:
I did not get how you measure positive and negative angles.
And I understand you have 2 outputs for forward and backward ( or left/right , depending on your point of view )

I do not fully understand how your code in the initial post around the line
output = -1 * output;
works, and why you need those 2*2 cases.

26

Probably this changing set-point puts its effect to the wild oscillation. I don't get you here:

but about the negative angle, it is coming from sensor: if I lean forward, the angle I get is positive, if I lean backward, I have negative angle, this gives negative and positive motor output based on formulation, so I use the code as you pointed:

  if(output >= 0)
  {
    digitalWrite(LDIRA, HIGH);
    digitalWrite(LDIRB, LOW);
    analogWrite(LPWM, output);
  }
  else
  {
    digitalWrite(LDIRA, LOW);
    digitalWrite(LDIRB, HIGH);
    analogWrite(LPWM, -output);
  }

and check if the final output (to be sent as PWM to motrs) is negative or positive. I only send positive PWM to my motor controller, from 0 to 255, so in case output is negative, I just switch A and B but multiply the output with -1 to have it always positive.

All the time, behavior of the system seems logical, but late reaction

I'm preparing for reconstrcution...

27

ironbot:
The x axis of accelerometer is set in the direction of movement of the robot and horizontally, as seen in the picture, gyroscope feels rotation around 'y' axis, i.e. the rotation of x axis between -g and g. So I combine both of their output to get the lean angle. I learned this by reading a lot online, I never seen a technique somewhere used to differentiate the signal of the motor rotation from the robot lean. May be that is the reason it is oscillating this much fast?

I can see what the code is doing, but I don't understand the logic behind it. The accelerometer doesn't differentiate between rotation about the axle and lateral acceleration, and I don't see the purpose of trying to incorporate that into your angle calculation. Did all your reading explain the theory behind this algorithm?

Going back to basics, what is the output of your gyro? Is it giving you deflection, speed or acceleration? How sensitive is it - what is the minimum value it can read (in terms of the bot's angle, angular speed or angular acceleration)?

28

I combine output of accelerometer and gyroscope to get a clean angle, as the gyro is fast but has a drift and accelerometer is slow on changes. This is the theory I learned: use not one of them (gyro or angle) but both of them and call it IMU, then filter output to get the angle.

gyro gives me the rate of change, but from accelerometer I receive the angle, knowing that it changes from -g to g.

I also submit the datasheet of both of my sensors so you have full information about them,

new_ADXL203.pdf (438 KB)

new_ADXRS401_0.pdf (404 KB)

29

check if the final output (to be sent as PWM to motrs) is negative or positive. I only send positive PWM to my motor controller

This is clear to me, and the now included code snippet as well.
In your initial post code, there is some stuff after that, which I don't understand:

...
    analogWrite(RPWM, -output);
  }  [color=green]// probably not good enough, but understood up to here  ;)[/color]
  
  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);
  }

I assume output should represent the speed you want your robot to move. It does not need to be exactly proportional, but you should work on that a bit, too.

  • What's the minimum/maximum speed your bot can do ? For smooth and easy contol, motor should react immediately with slow speed. Max speed should rather tell you when to give up.
  • What's the dynamic behavior of your balancing rod when it falls ? angle / angular velocity over time ?
  • I think you don't need sin/cos even (5.7° = 0.1 * R is sufficient around your "set point") but a bit of math won't hurt.
  • Did you consider that the more your sensor has left the 0° position, the more moving the motor will accelerate it ?
30

ironbot:
I combine output of accelerometer and gyroscope to get a clean angle, as the gyro is fast but has a drift and accelerometer is slow on changes. This is the theory I learned: use not one of them (gyro or angle) but both of them and call it IMU, then filter output to get the angle.

gyro gives me the rate of change, but from accelerometer I receive the angle, knowing that it changes from -g to g.

I also submit the datasheet of both of my sensors so you have full information about them,

I'm not convinced that approach will work. The description in the datasheet of using the accelerometer as a tilt sensor only works when the sensor itself is stationary. Attached to your bot, all you'll see is the the accelerations of the sensor itself, the tiny signal of the direction of gravity changing will be lost. You can test it yourself if you believe otherwise, but I think you're barking up the wrong tree there. Don't lose hope, though; there may well be another use for that accelerometer later on.

I suggest you focus on the gyro to start with. If I read it correctly the data sheet shows that the output is proportional to angular velocity and the output sensitivity is about 15mV per degree-per-second. By the time it's gone through the Arduino's ADC what is the smallest angular rate you can read, and what latency (delay) is there between the movement and you being able to see the input change?

31

PeterH, I'm little bit confused that you say we don't need accel, actually without filtering, if using only gyro output, the big drift I'd have and change of set-point that happens, I get nothing, I tried it long before this experiment!

It is impossible in my practice to not to combine output of gyro and accel, and yet balance the bot! I've read so many works (of other people already done it) and everybody is doing it this way, writing that impossible to use only gyro!

Of course, I'm in no way on an authority to claim anything in control, just I report what I've read. Actually if you personally can do it, is because of your skills that I unfortunately don't have, all i have is a background in math and a lot of reading online about this project, and still with a bot that can't balance really :slight_smile:

32

michael_x:
In your initial post code, there is some stuff after that, which I don't understand:

...

analogWrite(RPWM, -output);
 }  // probably not good enough, but understood up to here  ;)
 
 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);
 }




I assume *output* should represent the speed you want your robot to move. It does not need to be exactly proportional, but you should work on that a bit, too. 
- What's the minimum/maximum speed your bot can do ? For smooth and easy contol, motor should react immediately with slow speed. Max speed should rather tell you when to give up. 
- What's the dynamic behavior of your balancing rod when it falls ? angle / angular velocity over time ?
- I think you don't need sin/cos even (5.7° = 0.1 * R is sufficient around your "set point") but a bit of math won't hurt.
- Did you consider that the more your sensor has left the 0° position, the more moving the motor will accelerate it ?

oh, sorry, I just got that you are pointing this:

  output = -1 * output;

well, I just changed motors and swapped the left/right wires of motors and one of them started to turn not in the right direction, instead of changing in hardware, I added that line to just turn the output sign to the opposite one, which in turn effects on the 'if' decision and I got the motor turning again on the right direction.

I set 255 to get max. speed and it is claimed by the seller that this motor turns 300rpm, but I did not pass any motor identification process to give a reliable data of the speed.

dynamic behavior - I'm not sure if I got what you mean by that, when it falls: it takes some 10 deg and only by then the motor reaction happens, which is late, and robot starts to fall, and motors get to turn so fast, until it falls completely and motors stop :frowning:

I think you don't need sin/cos even - also didn't get you! sorry!

the more moving the motor will accelerate it ? - no, I don't feel that, but I'd submit a behavioral video, so you all who are kindly helping see it, that is better than me describing it, yet with my English which is noisy :slight_smile:

33

ironbot:
PeterH, I'm little bit confused that you say we don't need accel, actually without filtering, if using only gyro output, the big drift I'd have and change of set-point that happens, I get nothing, I tried it long before this experiment!

It is impossible in my practice to not to combine output of gyro and accel, and yet balance the bot! I've read so many works (of other people already done it) and everybody is doing it this way, writing that impossible to use only gyro!

Of course, I'm in no way on an authority to claim anything in control, just I report what I've read. Actually if you personally can do it, is because of your skills that I unfortunately don't have, all i have is a background in math and a lot of reading online about this project, and still with a bot that can't balance really :slight_smile:

If you are in contact with somebody who has actually done it, then follow their advice. I haven't - I am only telling you my understanding of the theory.

In my opinion, the accelerometer will not do what you intend it to do. The tilt calculations are based on measuring very small lateral accelerations that occur when the sensor is not perfectly level. This only works on a stationary object, and of course your bot is not stationary. As soon as your bot starts to move sideways the sensor will get a big horizontal acceleration that swamps the tiny signal you were using to calculate the direction of gravity.

Now, you can try using that instead of calculating the lean angle, if you like. As soon as the bot starts to fall to the left you move the wheels in the direction it's falling. To get that working reliably you'd really need to be able to work out and control the acceleration caused by your motors and model the relationship between the movement of the base and the top of your bot. Although it would be possible to do, it'd look nothing like the algorithm you have got there and would be quite hard to get working.

I think you are better off with the approach you're using, of trying to measure the orientation of the bot. The primary sensor for doing this is the gyro. You have it sitting in front of you; I don't. What signal do you get off the gyro in practice? What is the minimum angular rate you can read from it, and how much latency is there in reading it?

You are right that you need to cope with drift. If it was my project, the way I'd do that was to add a second feedback loop which tried to drive the amount of correction towards zero. In effect it would self-calibrate towards the vertical at startup, and inherently deal with gyro frift. But there's no point thinking about that until you have got the basic underlying feedback loop working. And that depends on know what your sensors are telling you. Hence I keep asking the question: what are you getting from that gyro?

34

PeterH thank you for this much support, my aim is to do the project to learn something and I'm ready to try different direction that I took at first. So let's do it the way you suggest, but I've doubt I fully understand you in this (to measure gyro).

Do you mean that I post here simply information like:

  1. if I keep robot stationary, output of gyro is x
  2. leaning forward fast to around 45deg I get y
    3...

or what best way to answer your question "output of gyro" please?

35

ironbot:
2. leaning forward fast to around 45deg I get

Don't bother about 30° or more: If your motor at full speed is able to catch that, it's nice, but you should rather get numbers (or a feeling, at least) about what happens and what your sensor says, when your bot begins leaving the balanced position. (In that region where degrees and displacement in x direction is nearly proportional...)

36

Ok, thank you all for continuous support!

Just received my new sensors: a separate gyro and a separate accel (my current sensor as you see on photos, has both of them combined on one board).

I can't rely on my old sensor, not really sure about its health, she is too old!

With new ones I'd first build a new chassis, with the hope that it gives me room to easily apply changes as you suggest and instruct. This will take probably around 2 weeks and I'd be back here with a new bot which (hopefully) also helps to answer the questions you kindly put (how goes the output of gyro if you do this/that?) easy and fast.

Please, if you have suggestions for the new chassis I'd build, proportionality info., etc. let me know. What I know for now is that, I'd for instance build the vertical axis in around 70cm height, so that to have it falling not too fast, and keep the base in a way I can add and remove weight to easily change the moment of inertia. Any other suggestions will be very welcome, I will apply them in 3D modeling of chassis in software and finally cut with CNC.

Thank you again,

37

BTW: there is a dedicated Robotics thread http://arduino.cc/forum/index.php/board,14.0.html , discussing "balancing robot" similar to yours (1 ft high, two wheels) http://arduino.cc/forum/index.php/topic,60170.0.html

38

Man, I just visited the links and the video clip there: when he leans the bot even just a degree, motors react, in my version, I have some 5 deg before reaction, I must take record a video before destructing the current one and upload here!

39

ironbot:
Man, I just visited the links and the video clip there: when he leans the bot even just a degree, motors react, in my version, I have some 5 deg before reaction, I must take record a video before destructing the current one and upload here!

I think you're looking at the real problem now, but I'm still not clear whether the slow reaction is caused by the sensors, or your algorithm. I really think you need to write a sketch that just outputs the raw data from your gyro and accelerometer and find out how sensitive they are and how quickly they respond. Don't try to do any clever mapping or anything - just output the raw data as promptly and simply as you can.

40

The fact that you reported the motor producing a sound only on low values and started turning with higher pwm output and without load only, is something you should definitely try to solve.