Go Down

### Topic: PD of PID in a balancing robot (Read 10315 times)previous topic - next topic

#### PeterH

#15
##### Apr 26, 2012, 12:26 am
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.

#### ironbot

#16
##### Apr 26, 2012, 12:05 pm
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..

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

#### jackrae

#17
##### Apr 26, 2012, 12:22 pm
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.

#### PeterH

#18
##### Apr 26, 2012, 01:38 pm
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.

#### ironbot

#19
##### Apr 26, 2012, 01:46 pm
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:

Code: [Select]
`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).
http://i47.tinypic.com/wkirs9.jpg
http://i45.tinypic.com/2lln1jc.jpg

#### PeterH

#20
##### Apr 27, 2012, 12:39 am

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?

#### ironbot

#21
##### Apr 27, 2012, 07:34 am
Ok, I changed it, now is this the idea? Please have a look:
http://i48.tinypic.com/34zyp9t.png

Quote
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:
Code: [Select]
`      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; `

#### michael_x

#22
##### Apr 27, 2012, 05:04 pm
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!

#### ironbot

#23
##### Apr 27, 2012, 07:01 pm
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

#### michael_x

#24
##### Apr 27, 2012, 08:03 pmLast Edit: Apr 27, 2012, 08:17 pm by michael_x Reason: 1
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:
Code: [Select]
`    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.

#### ironbot

#25
##### Apr 27, 2012, 09:33 pm
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:
Code: [Select]
`  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...

#### PeterH

#26
##### Apr 28, 2012, 01:33 am

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

#### ironbot

#27
##### Apr 28, 2012, 09:20 am
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,

#### michael_x

#28
##### Apr 28, 2012, 02:39 pm
Quote
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:
Code: [Select]
`...    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 ?

#### PeterH

#29
##### Apr 28, 2012, 02:57 pm

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?

Go Up