inverted pendulum 2 wheel robot

Hi,

I am writing a PID control loop for an inverted pendulum 2 wheel robot and am having some trouble getting it to balance.

I have a gyro, accelerometer and wheel encoders to use.

For the P (proportional) term I use the platform angle in degrees which is generated from the gyro and accelerometer.

For the I (integral) term I am not sure how to calculate this and also over what time to integrate the platform angle. Can I just use a running average for the platform angle for this?

For the D (derivative) term I use the gyro's rate output in degrees per second.

Once I calculate all 3 of the above variables (multiplying them each by their gain factors) do I just add them together and then use the result to set the motors dutycycle? So I would have:

motor_dutycycle = P*p_gain + I*i_gain + D*d_gain

Using various combinations of P and D terms, I have not been successful to balance it yet.

There are also wheel encoders that provide 1200 counts per wheel rotation. Would it help to use the wheel encoders for balancing? I would like to just get the robot balancing in one place as a start.

The way I generate the platform angle may be a problem I think: I integrate the gyroscope output and then use the accelerometer to slowly correct that integrated angle, up to a maximum of 2degrees/second of correction. The problem this has is the accelerometer is sensitive to the accelerations of the pendulum jerking back and forth while it tries to balance, so it may cause an error in the actual platform angle. Would a 2 state kalman filter help for this? Also I currently use only the horizontal axis of the accelerometer, if I used the vertical and horizontal axis' of the 2axis accelerometer (adxl202e) would that help? The gyroscope is an adxrs150 and both the gyro and accelerometer are filtered with first order 40Hz -3dB lowpass digital filters. the balancing loop runs at 30Hz currently.

cheers, Jamie

Reply to
Jamie Morken
Loading thread data ...

First and foremost check out the ORIGINAL inverted pendulum, 2 wheel robot (at least the first to be documented on the net): the nBot:

formatting link

It has a description of the original algorithm used (in block diagram and pseudo-C):

formatting link

Notice that the algorithm is basically two PID loops (actually, just P and D) summed together. If you look at the videos you'll see that the very simple algorithm is extremely robust.

Balancing the robot is simply maintaining the angle and position = 0. If you already have the angle then the algorithm on the page above works fine. Problem is, none of your sensors directly measures angle. So angle is roughly the double integral of the rate gyro adjusted by the long term integral of the accelerometer. The long term integral of the accelerometer is basically the force of gravity, which, if measured on the horizontal axis should average out to zero if perfectly balanced.

So, converting all this into an algorithm, I would do something like:

angular_a = read_gyro(); angular_v += angular_a; // integrate acceleration to get velocity angle_rel += angular_v; // integrate velocity to get position

horiz_a = read_accelerometer(); horiz_v += horiz_a; angle_abs += horiz_v;

angle = (X*angle_rel) + (Y*angle_abs).

Now plug that in to your PID loop:

angular_adjustment = A*angle + B*integrate(angle) + C*derive(angle);

As I noted earlier, the original balancing robot did not bother with the integral and the code is in fact the above with B=0. Also, if you notice, we calculated angle by double integrating acceleration you can simply re-use angle_v and horiz_v as your D term. Now do the same for position adjustment using your wheel encoders.

As for your question of how to do the integration I usually (like the code above) integrate from zero (when you turn on the power) to infinity (when the batteries run out).

Reply to
slebetman

I'd also like to suggest, since you have a dual axis accelerometer, that you try mounting it at 45 degree angle. I once considered a scheme like this to do balancing without a gyro using trigonometry to cancel out the horizontal acceleration from both axis but a couple of russian guys (succesfully) beat me to it:

formatting link

Reply to
slebetman

As long as your accelerometer signal isn't saturating I think you have a good way of estimating the platform angle. You'll want to use lots of integrated gyro and just a bit of accelerometer for the angle, because of the jerking around.

A Kalman filter is just a better way of designing your corrected integrator, but yes, it may help. Of particular help will be getting the thing oriented correctly and fast enough on startup that you'll know where 'up' is in time to not fall down.

Do _not_ use a running average for the integral term! Just integrate the angle forever. A running average is just a low pass, not an integrator. You'll find that your accelerometer will have a bit of bias, which will inevitably lead to your pendulum system wanting to run away at some constant (hopefully low) acceleration. You'll have to correct for this by monitoring the wheel position. Once you do this you may find that you don't need the accelerometer at all.

My web site has some resources that may help. Take a look in the 'articles' section, and at the book.

--
Tim Wescott
Wescott Design Services
 Click to see the full signature
Reply to
Tim Wescott

Thanks, I have it working now! I am still fine-tuning the PID loop but it is able to balance very smoothly now. I still need to add the wheel encoder variables into the PID loop, but the angle position and angle rate seem to balance it quite well, although without the wheel position the robot slowly rolls across the floor.

Also doing a curve fit between the PID loop calculated motor torque and the motors dutycycle helped a lot, as the motors I am using generate no torque until 50% dutycycle , so I used this formula to correct this:

dutycycle = 0.5 * torque + 50;

A question on the I term of PID loops: For integrated values, once an error has built up it will correct the platform angle back to horizontal, but also for the integrated error to disappear, the platform angle must overshoot to allow the integration error to decrease to zero. The proportional and derivative term can cancel this out, but why not zero the integration variable every time you are acceptably close to a horizontal platform angle?

What is the next step in control theory beyond PID loops?

cheers, Jamie

Reply to
Jamie Morken

I was pondering this today, at a client site when I was waiting for _their_ control software to build. Since the pendulum's angular acceleration is proportional to the angle plus some of the wheel acceleration, you could ditch the accelerometer and just use the gyro. Too bad the gyro's the expensive part, but you could do something similar with the gyro.

This is not a bad thing to do, but were you to make a product out of this you'd find that the threshold would change with temperature, aging and manufacturing variations. It's better to use the wheel encoder to make a speed loop around the motor; you can expect to be able to make that significantly faster than the pendulum loop and so control the motor startup torque issue.

There are more than one way to solve this particular problem. In fact, there's at least two for every serious control engineer in the world, perhaps more.

The simplest way is to just hard limit the integrator state to something small. Because it's the simplest it's the only one I detailed in my PID article on the Embedded.com website.

My favorite method when speed isn't paramount is to back off the integrator state so that the sum of the integral term and the proportional term just kiss the limit in question. This means that the integrator is pulling back strongly, so the system will settle with little or no overshoot -- but it'll be slow.

With the inverted pendulum you'll _always_ see the base overshoot -- the only way to slow down the bob is for the base to get ahead of it. That's just basic physics, and you must always allow for this. About the only thing you can count on is arranging your system so that the _bob_ never overshoots, and possibly that the settling is gentle enough so that the base doesn't overshoot much.

Buy my book!

--
Tim Wescott
Wescott Design Services
 Click to see the full signature
Reply to
Tim Wescott

So as you approach the limit do you reduce the integrator state manually?

For tuning the PID loop, I started with the PID loop only having the platform angle and the gyro rate, and got this balancing and recovering from pushes well with no oscillations and then added the wheel position and wheel rate, and it now is balancing as well as holding position, but it now has an unrecoverable oscillation that happens sometimes now. Does adding more terms to the PID loop mean that I may have to tune the balancing variables again? I am not sure if you need to retune different parts of the PID loop when you add new functions. It is difficult to know which variables to change, especially as I add more terms to the PID loop torque calculation. Any pointers on this? Do I need to go back and retune the balance variables when I add position and steering functionality? This can get pretty complicated if that is the case. Here is my PID loop right now:

torque = ((platform_angle) * K1) + (gyro2_degrees_per_second * K2); //balance torque = torque + (wheel_position_error * K3) + (wheel_velocity * K4); //position

torque1 = torque + (steer_rate_error * K5) + (integrated_encoder_steer * K6) + (encoder_steer_rate * K7); //wheel1 steering

torque2 = torque - (steer_rate_error * K5) + (integrated_encoder_steer * K6) + (encoder_steer_rate * K7); //wheel2 steering

I make two torque variables for steering using the encoders. I think this will work (haven't tested the steering yet).

Here is a video of the robot balancing and holding position (it moves towards the position of the wheel encoders == 0)

"

formatting link
" (785KB)

Here's a video of an oscillation which starts at about 20seconds in: "

formatting link
" (790KB)

cheers, Jamie

Reply to
Jamie Morken

More like when it's far away from the target the integrator state is limited, then it's allowed to settle naturally as the target is approached.

"Happens sometimes" is a key word for one of two things: either your system is on the edge of stability and some change in it's characteristics pushes it over, or your system is nonlinear and some large excursion pushes it into a hard limit cycle.

If it's just sitting there, then suddenly starts oscillating I would suspect the former, but this should be preceded by a fairly undamped recovery from upset before it starts oscillating.

Possibly.

This is what state space control was invented for -- you've got a multiplicity of sensors, and a variety of behaviors that you want to influence. State space control lets you formulate the problem in matrix math, which makes the solution clearer.

But trust me -- you don't want to go there until you know more about control theory.

I think your position loop may be backwards. The most direct way I know to control the position and angle of an inverted pendulum is to calculate the position of the center of mass of the pendulum, and servo that. Why? Because it automatically takes care of the weirdness of controlling an unstable system with it's requisite bass-ackwardness in the primary control, and when you're done the code is more compact and it's more intuitively satisfying. Just remember these two things:

cg position = wheel position + sin(angle) * (cg height)

and

cg acceleration = g * sin(angle).

Now you just need to have a good fast loop on the wheel position to place the pendulum base wherever it needs to be to drive the cg where it needs to go.

--
I really think you would benefit from my book.  It was written for folks 
who are otherwise adept technically, but who lack a foundation in 
 Click to see the full signature
Reply to
Tim Wescott

If you are integrating the platform angle, then the integrator state won't settle as you approach horizontal though, as you need to pass horizontal to start subtracting off the integrator state. So how does it settle naturally as you approach horizontal?

Thanks, I will try this!

It sounds like a great book. I will buy a copy once my bank balance is a bit to the right in the colour spectrum. I'm in the infra-red right now :)

cheers, Jamie

>
Reply to
Jamie Morken

That's why I prefer the method I use -- coming off of the limit the integrator is as negative as it can get, with the proportional term supplying all the positive impetus. As the integrator builds up the system approaches the target closer and closer, until finally everything is on target.

-- snip --

Been there, done that. AFAIK Amazon has the whole thing on their site as a "preview", so you can read it for free if you don't mind being chained to your PC.

--
Tim Wescott
Wescott Design Services
 Click to see the full signature
Reply to
Tim Wescott

Hi Tim,

I understand now! I forgot that the platform angle flips past horizontal as it is going back to horizontal, so I finally understand how the integrated value limits itself. I am slow in the head I think :)

cheers, Jamie

Reply to
Jamie Morken

ElectronDepot website is not affiliated with any of the manufacturers or service providers discussed here. All logos and trade names are the property of their respective owners.