inverted pendulum 2 wheel robot

[Jamie Morken]

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

 

[[email protected]]

Hi,

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

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

http://www.geology.smu.edu/~dpa-www/robo/nbot/

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

http://www.geology.smu.edu/~dpa-www/robo/nbot/bal2.txt

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.

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

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

 

[[email protected]]

the accelerometer, if I used the vertical
and horizontal axis' of the 2axis accelerometer (adxl202e) would that
help?

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:

http://www.sensi.org/~svo/rakipaki/

 

[Tim Wescott]

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

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
http://www.wescottdesign.com

Posting from Google? See http://cfaj.freeshell.org/google/

"Applied Control Theory for Embedded Systems" came out in April.
See details at http://www.wescottdesign.com/actfes/actfes.html

 

[Jamie Morken]

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.

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

 

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

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.

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;

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.

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?

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.

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

Buy my book!

--

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Posting from Google? See http://cfaj.freeshell.org/google/

"Applied Control Theory for Embedded Systems" came out in April.
See details at http://www.wescottdesign.com/actfes/actfes.html

 

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

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)

"http://www.rocketresearch.org/new/robot-temp/robot1-320x240.avi"
(785KB)


Here's a video of an oscillation which starts at about 20seconds in:
"http://www.rocketresearch.org/new/robot-temp/robot2-320x240.avi"
(790KB)

cheers,
Jamie

 

[Tim Wescott]

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

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.

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.

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

Does adding more terms to the PID loop mean that
I may have to tune the balancing variables again?
Possibly.

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.

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.

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

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
control theory. It's heavily slanted toward implementing the
controllers in software, but that's where 95% of the loops get
implemented these days. It discusses integrator anti-windup as well as
general nonlinear systems in far more detail than I could devote to a
single newsgroup posting, as well as giving you the tools you need to
answer your own questions about tuning.

--

Tim Wescott
Wescott Design Services
http://www.wescottdesign.com

Posting from Google? See http://cfaj.freeshell.org/google/

"Applied Control Theory for Embedded Systems" came out in April.
See details at http://www.wescottdesign.com/actfes/actfes.html

 

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

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?

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


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.

Thanks, I will try this!

----

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
control theory. It's heavily slanted toward implementing the
controllers in software, but that's where 95% of the loops get
implemented these days. It discusses integrator anti-windup as well as
general nonlinear systems in far more detail than I could devote to a
single newsgroup posting, as well as giving you the tools you need to
answer your own questions about tuning.

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

 

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

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

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

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
http://www.wescottdesign.com

Posting from Google? See http://cfaj.freeshell.org/google/

"Applied Control Theory for Embedded Systems" came out in April.
See details at http://www.wescottdesign.com/actfes/actfes.html

 

[Jamie Morken]

Tim Wescott wrote:

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.

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