Self-Balancing Upside Down Pendulum

Introduction: Self-Balancing Upside Down Pendulum

About: I'm an Engineering Associate Professor here in New-Zealand. Originally from near Aberdeen (Stonehaven) Scotland.
More About tmoir »

This is my attempt at the inverted pendulum balancing on a two-wheel chassis cart. I had seen the other ones on here and was inspired to try my own version. The Arduino is wonderfully simple to program and you can pick it up in a few hours. I looked at the Segways as well and compared their approach - which is quite similar. Unlike most of the others I don't use PID control, but State-Feedback control. My background is in control-systems so I found it very interesting. I feedback angular position, velocity and acceleration and add integral action. The integral action is needed to keep the steady-state error at zero. Without it the pendulum flops back and forth and doesn't stay as straight. The pendulum itself is just a length of aluminum (about 500mm long) with an aluminum bob screwed on. I didn't make this part myself but had it lying around from a previous inverted pendulum project on an old x-y plotter which in itself was quite interesting. You could use a piece of bamboo instead o whatever you have lying around. It turn out that the longer is the pendulum shaft the easier it is to control. Try balancing a pencil on your hand - now try a long broom handle and see how much easier it is. You may think that you are wasting your time experimenting with such things, but remember that the control system that keeps a rocket in the air or a walking robot all works on similar principles (but much harder!) Of course the Segway also works along similar lines except you are standing on it and you become part of the control loop.Here is a video of some Auckland University of Technology (AUT) students with the original pendulum (a larger one) on an x-y plotter. entirely analogue. This inspired me to do the one on wheels during the Xmas holidays! Note the structural resonance of the shaft. All mechanical systems have structural resonances and they are a headache when putting feedback around them when operating at high bandwidth. You normally require as high a bandwidth as possible for speed of response.

Teacher Notes

Teachers! Did you use this instructable in your classroom? Add a Teacher Note to share how you incorporated it into your lesson.

1 LiPo battery 11.1v will do - because they are powerful and lightweight. Suggest you buy a custom charger with it. Best to be safe rather than set fire to your house! Can buy a decent battery + charger from HobbyKing

I have over-engineering this a bit but at least the batteries last a long time between charges.

Finally you will need the Gyro-accelerometer - possibly the cheapest of all the parts, the MPU6050. I bought about 4 just in case for other projects. Just search for it on Bay.About $2 US each for the breakout board! You will need to solder header pins onto it. It runs off 3.3V which your Arduino Uno has as an output.

Misc things you will need are a few 10k pots and a battery plug for your Arduino. You will also need a sheet of suitably cut plexiglass or a piece of plastic. I live in New-Zealand and we can get this from Jaycar

I strongly suggest buying zip-ties to bundle the cables together so that the whole thing is robust. It will fall over many times during tests so it needs to be sturdy.

A toggle switch is needed to power on and off the unit.

Step 2: The Chassis

As you can see from the picture, the chassis has the plexiglass sheet bolted onto it. To do this you need to bend the plexiglass at right angles. Fortunately I have a hot-wire system for doing this at work but it is possible to use a heat gun as used for stripping paint off walls! You need to be quick and practice though, otherwise you'll get the wrong shape. Then just drill some holes and attach to the motor chassis. If you don't want to use this approach then you can stack everything on top of each other as others have done. Whatever you do, keep the Gyro as close to the middle of the axle as possible and lay it flat with the pins sticking up. don't mount the Gyro half way up or you will get an error with your readings. I didn't put the aluminum pendulum rod on at first - it is just held on by drilling a few holes and clasping it with cable-ties (zip-ties).Instead of the plexiglass sheet you could use a plastic box maybe if you can find one the right width.Here is an instructional video on how to bend plexiglass.

Once the chassis is built don't be temped to just wire everything up and try the complete system. you first need to test out the motor control via the H bridge on its own - no Gyro (using the Arduino) and then test the Gyro with the Arduino observing the tilt angles in real-time - and no motors. The wiring will be the same to the Arduino when we do the complete pendulum so no need to re-wire.

Step 3: Controlling the Dc Motors Using PWM - No Gyro Connections

So you initially need to mount the Uno and the L298 H bridge. Just drill some holes and attach via small screws and spacers if you like. Because it's a plastic backing you need not worry about insulation. I mounted the Uno at the top and the H bridge below it. The battery is held by zip-ties. Originally I used an external power supply to do the initial testing. I suggest you wire up the L298 Bridge to the dc motors and do some testing first. The motors are driven by PWM waveforms. The waveforms which come out of the Arduino are shown in the diagram above. The Arduino cannot drive a motor directly so this PWM drives an H-Bridge controller to step up the voltage and increase the driving current. Some H bridges have two logic inputs - one is held High and the other Low for forward, and the opposite for reverse. This is the case for the one we use here. Others have internal logic and just use High/Low for forward and reverse. The analogWrite(pin,number from 0 to 255) command gives the strength of the PWM which essentially when averaged out gives us a dc component which varies from 0 to a maximum 255 for full speed!! For example with analogWrite(10,255) writes max PWM to pin 10 (which is just dc 5v) , analogWrite(10,128) gives 50% speed to pin 10 and analogWrite(10,64) gives quarter speed. Pin 10 would then have to be connected to the PWM input of your H bridge. See waveform diagrams.

Then run his example and check that both motors turn in the same direction!! If not then reverse the polarity on one of the offending motors. I leaned a lot from his Arduino tutorial.

Connections are as follows:

Arduino------> L298 H Bridge

D10 -------------> 7

D5 --------------->12

D9---------------> IN1

D8---------------> IN2

D7 ---------------> IN3

D6----------------> IN4

Plus the motor connections.

Don't forget to connect zero volts from the Arduino to the zero volts on the H-Bridge. In fact the zero volts connector on the H bridge board became rather crowded and I ended up soldering all of them together to make them robust.

His demo program is as follows. Make sure it works first before continuing to the Gyro part.

// connect motor controller pins/to Arduino digital pins

// motor one

int enA = 10;

int in1 = 9;

int in2 = 8;

// motor two

int enB = 5;

int in3 = 7;

int in4 = 6;

void setup()

{

// set all the
motor control pins to outputs

pinMode(enA,OUTPUT);

pinMode(enB,OUTPUT);

pinMode(in1,OUTPUT);

pinMode(in2,OUTPUT);

pinMode(in3, OUTPUT);

pinMode(in4,OUTPUT);

}

void demoOne()

{

// this function will run the motors in both directions at a fixed speed

// turn on motor A

digitalWrite(in1,HIGH);

digitalWrite(in2,,LOW);

// set speed to 200 out of possible range 0~255

analogWrite(enA,200);

// turn on motor B

digitalWrite(in3,HIGH);

digitalWrite(in4,LOW);

// set speed to 200 out of possible range 0~255

analogWrite(enB,200);

delay(2000);

// now change motor directions

digitalWrite(in1,LOW);

digitalWrite(in2,HIGH);

digitalWrite(in3,LOW);

digitalWrite(in4,HIGH);

delay(2000);

// now turn off motors

digitalWrite(in1,LOW);

digitalWrite(in2,LOW);

digitalWrite(in3,LOW);

digitalWrite(in4,LOW);

}

void demoTwo()

{

// this function will run the motors across the range of possible speeds

// note that maximum speed is determined by the motor itself and the operating voltage

// the PWM values sent by analogWrite() are fractions of the maximum speed possible by your hardware

// turn on motors

digitalWrite(in1,LOW);

digitalWrite(in2,HIGH);

digitalWrite(in3,LOW);

digitalWrite(in4,HIGH);

// accelerate from zero to maximum speed

for (int i = 0; i< 256; i++)

{

analogWrite(enA,i);

analogWrite(enB,i);

delay(20);

}

// decelerate from maximum speed to zero

for (int i = 255; i>= 0; --i)

{

analogWrite(enA,i);

analogWrite(enB,i);

delay(20);

}

// now turn off motors

digitalWrite(in1,LOW);

digitalWrite(in2,LOW);

digitalWrite(in3,LOW);

digitalWrite(in4,LOW);

}

void loop()

{

demoOne();

delay(1000);

demoTwo();

delay(1000);

}

Attachments

Step 4: The MPU6050 3 Axis Gyro and Accelerometer

What we need is a signal proportional to the angle of tilt - which is the Y axis in the software and this Gyro when it is sitting flat. Once again I don't recommend just jumping to the final solution I have, test the Gyro on its own first and watch on the serial port what the angle is as you tilt it back and forth (with no motor drive). so you need to use the program from this link by Jeff Rowberg who wrote the driver software

When you run the test program you may well se that when the pendulum is at 90 degrees vertical that the angle is offset by say anything up to +/- 10 degrees!! This varies from device to device (I am led to believe) and that is why I put a trim pot on the final design.

The wiring of the MPU6050 Gyro is straightforward

Connect VCC and Gnd to 3.3v and ground of the Arduino (NOT 5V!)

connect Int (the interrupt) pin to Arduino Pin 2

connect SCL and XDA to the corresponding same lettered pins on the Arduino. This is for the i2c bus.

The other pins are not used.

I mounted the Gyro on a small piece of plastic via two small screws (holes are already drilled on the device of course). I then used a glue-gun to glue the assembly to the wheel chassis. Here is the test program for the Gyro. It doesn't power the motors, just use it to test. Use the serial monitor on the Arduino to see Gyro outputs.

Attachments

Step 5: The Final System

Once you have tested the motors using the test program and the Gyro using a second test program as per the previous steps then you are nearly ready for the final software. I added two 10K potentiometers - both connected across the regulated 5v to zero volt line (from the H bridge). Then I connected the wipers of the pots to A0 and A1 of the analogue to digital convertor of the Arduino. In this way I can read in dc voltages and create a small offset via A1 and an overall gain from A0. Other than that the wiring is the same as per the two test programs. The overall software is shown in the attached text file.

Attachments

Step 6: The Hard Sums!

So how does it balance? Negative feedback is used by creating an error between the desired angle of inclination and the measured value from the gyro. If the two differ then a control voltage is applied to both motors simultaneously to drive the cart forward or backwards to correct this angle. Such an approach is known as Proportional control and suffers from the fact that this can be quite a large error that is created. Theory tells us that by increasing the amount of gain to the error path that this error reduces. Unfortunately to make the error small enough requires such a large gain that the loop becomes unstable and the cart falls over. We can improve matters by adding a second term which is proportional to the differential of the error signal time a second gain term. This is known as Differential control and when added to the Proportional term is known as PD or Proportional plus Derivative control. The D term stabilizes the system and reduces the instability. That would be the answer except that the error doesn't always go to zero and there is a finite difference left over. The cart will flop back and forward and not stay up straight. We can add a third term which is an integrator which takes the error away. Unfortunately this third so-called I term if given in too big a quantity leads to further instability. Hence a compromise must be met between the three terms in what is known as a PID controller. Tuning PID controllers is a bit of an art and there are tuning rules, but they aren't quite so good when you have an inherently unstable system as we do. However, PID does work and other instructibles have used it.

The approach we use here is the approach that many are using for the Segway and is not PID control. PID is often known as Classical control theory whereas what we use here is known as Modern Control theory. In fact the Classical approach isn't that old and the Modern approach isn't that modern! The Modern approach we use is known as Pole-Placement State-Feedback. In it we feedback proportions of angular position,velocity and acceleration. The Segway indestructibles only use position and velocity feedback to the best of my knowledge. I added a third state of acceleration and a fourth state which is integration or integral action. This sort of thing is well established in text books in fact but seldom seen in demonstrations. We would normally require a mathematical model i.e the differential equations describing the complete dynamic system. Here I use a simpler approach and tune it like a PID. There is a similarity between PD control and state feedback that only uses position and velocity feedback. The velocity feedback is known as Rate feedback and is just a differential term which is read from a sensor rather than being explicitly calculated. The difference here is that differentiation is in the Feedback path rather than the forward path as is the case with PID. This gives a little more damping at the expense of a slightly slower system - but can easy be compensated for by increasing the overall gain. So here we measure directly angular error using the Gyro and angular velocity from the accelerometer.

The third state is acceleration which we compute (there are better ways but for now I use this method) directly by differentiating velocity. By controlling acceleration we can control the current drawn from the motors and improve disturbance rejection. If you cannot measure acceleration with a transducer (which is the usual case) then you must create it by differentiation or by using a state Observer or Kalman filter.

So the control signal is

u(k)=-K*[Kp*y(k)+Kd*y(k)_dot+Ka*y(k)_double_dot]

where K is overall gain adjusted from the potentiometer and Kp,Kd and Ka are the proportional gain, derivative gain and acceleration gain respectively. Here y(k) represents the present reading from the Gyro sensor (angle), y(k)_dot is present reading of angular velocity and y(k)_double_dot is the present reading of acceleration all at time instant k. In the diagram Ki is integral gain.

To digitally differentiate we can use a simple Euler approach. For a sampling interval dT seconds (here it is dT=0.01 for a 100Hz sampling rate) we can differentiate velocity omega to give acceleration accel as

// calculate angular acceleration from angular velocity

omega_old = omega; // store last value of acceleration

omega = ...read velocity from accelerometer here

// differentiate

accel=(omega-omega_old)/dT;

Usually we leave dT out (i.e set dT=1) as it becomes a gain term which we can adjust separately.

This is quite a crude differentiator and in fact any type of pure differentiator is a bad idea for a control loop because it amplifies high-frequency noise and excites structural resonance. Therefore we low-pass filter the differentiated output. we use a first order low-pass filter - would take a while to explain all of the theory here. this filter is not shown on the diagram above. (only the ideal case).

To integrate we use a simple Euler integration. eg to integrate an error

//Integration of error

// setpoint = 0 for zero degrees vertical

error=setpoint-output_value;

y_out_past=y_out;/store /past value of integrator output

y-out=y_out_past+dT*error;

// yes you can use the y+= type notation in code but it hides what is happening and the understanding

where y_out is the integrated output. usually we leave out dT and scale later i.e dT=1. You could in principle use a summation to do integration, but the above method is a better - recursive method sample by sample and only requires minimal storage. Summation would require us to store a number of past values. You can also perform integration via z-transforms and get a slightly more accurate version called a Trapezoidal Integrator. No need for that complication here, though I did use z-transforms for our simple low-pass filter.

The sampling frequency: I used previous Segway code to set the sampling frequency to 100Hz and could not get it to go faster. I checked with an oscilloscope that this was the true sampling frequency by flipping a logic flag up and down - true and false very execution of the loop. I send this to a digital out and got a square way. I measured the half-period and it was spot on 0.01secs=dT. The block diagram is shown with full state-feedback and integral action.

I also used the special so-called complementary filters to combine the accelerometer and gyro readings. This is because the accelerometer is good at low frequencies whilst the gyro is good at high frequencies whilst the opposite is the case for poor performance. This attempts to reduce the effect of gyro drift. Another approach is to use a Kalman filter but this is not shown here. Details of complementary filter method is shown in the link below.

The main idea is shown in the diagram (shown below the state-feedback diagram) taken from this link.

Step 7: A Note on the Savitsky Golay Filter

Many Segway projects appear to use what is known as a Savitsky Golay filter to try and average out the Gyro readings and remove erroneous readings. This special filter is what is known as a finite-impulse filter (FIR) and unfortunately its phase-shift is not too good. Any negative phase-shift has a destabilizing effect on a control loop. so when you do ordinary filtering of say music or speech, phase maybe isn't as important, but when you are working inside a control-loop you need to be careful and not be wasteful on phase-lag. Whilst I agree that we do need a filter, I do not agree that this filter is suitable. I tried it and it caused less stability than without it. You could trade this off by going to an even lower bandwidth I suppose, but I decided to just use a simple low-pass filter instead. (1/(1+sTau) if you understand such things. Make sure the cut-off was far enough away (14Hz) from the unity-gain crossover frequency of the loop (which I take to be around 1 Hz). A rule often used in control-engineering is to sample at least 10 times higher than the highest frequency of interest. The usual Nyquist sampling rate in signal processing is taken to be only twice as high but in control it needs to be much higher because digital control has an inherent one-step time-delay from input to output due to the computer! Any delay in a control loop causes negative going phase which in turn destabilizes the loop. The Savitsky Golay filter frequency and phase response (top graph)was plotted on MATLAB and although it has a nice magnitude response it's phase turns hard negative with a steep slope. In contrast the simple first-order filter I use (bottom graph) has a minor phase shift only at low frequencies.The main purpose of my filter is to attenuate structural resonances at higher frequencies. Without it the whole mechanism shakes! Another problem with some FIR filters is that they can quite often be non-minimum phase - and this one is just that. That means it gives much greater phase shift than an equivalent filter that is minimum phase.

Step 8: Switching On

It's best to tun the gain down to zero on your gain potentiometer and monitor the phase angle coming from the Gyro. Then adjust the offset pot so that the output as viewed on the serial port is close to zero when vertical.

You can then increase the gain slowly until it balances. I have a video of it working

You may of course have to change the various state-feedback gains for your system. You can do this fastest by having pots for each gain - which is a lot of pots of course! I did it the hard way in software. Now a demonstration of what I mean by structural resonance. I deliberately set the gain too high and the entire structure shakes.

I also have a demo of the PID version. Similar to other projects here but this one has a filter on the derivative action.

I then swapped the smaller half metre pendulum with the one metre one. I had to change the parameters of the program. I reduced the filter 3dB frequencies to 7Hz (from 14Hz). This attenuated structural resonances. I also had to tweak a few more of the state-feedback gains and add a little more integral action on the error.

These motors also have encoders built in which will be useful for future modifications.

The system then worked nearly perfectly and was very robust. You can knock it and it still stays up and even balance things on the top as illustrated in the video below. The original perspex chassis for the project broke when I dropped it on the ground and so the one you see in the video is a smarter one knocked up in a few hours by one of our best technicians using a laser cutter.

I found that all of the electronic circuit boards lifted off all at once linked together by the wiring and I managed to slot them into an exact replica made from the original template- only better looking. The job took about 15 mins.

Good day sir. if i only put 1 motor and still using this code. and i will use a flywheel to balance on a stick (still inverted pendulum concept) will it balance? because i am working on a self-balancing two in-line wheel or bicycle using flywheel and mpu6050.

I tuned the control system for a length of about half a metre. For it to work without the pendulum you would need to change the various gains. First try it with a pendulum. You can use a length of Bamboo for instance with a blob of plastercine or something on the end. You can tie the Bamboo with zip-ties for instance. If that works you can take off the Bamboo and re-tune the gains. That can take time though.

I suppose naming is something we could argue about but I am not too bothered what people call it! It is a pendulum in that it pivots about the wheels upside down. It cannot stay in the same place, it has to move horizontally. In text books it is normally called inverted pendulum on a cart. Variations are shown in most control-engineering text books. I would settle for Inverted Pendulum on a cart but remember in this case the cart has only two wheels. In many text books the cart has four wheels.

Make sure your pots are around 10k ohms and connect the first and last pin to 0 and 5v respectively. If you get them the wrong way around the pot will be backwards ie max clockwise will be min and max will be min anti-clock. It won't burn out. Centre pin goes to A0 or A1 ADC inputs.

I have 2 queries on the inverted pendulum’s shape design in general.It is very much appreciated if you can enlighten me.

First query is why the wheels are located beside the body, not underneath of the body.I can understand if wheels are big (compared with the body size), those should be located beside the body, but even the inverted pendulum with small wheels, wheels are located beside the body, not underneath.For me, it seems that Steamboat Willie’s design is succeeded by into the inverted pendulum.

Second one is why the most of inverted pendulums are designed as top heavy.My understanding is that the long pendulum (not inverted one) has a long period of swing, and the top-heavy inverted pendulum also has a long period to fall down. During such a period we can control the balancing. However, the top-heavy design means a large inertia moment of the body, and that requires a larger torque for motor.If my understanding is correct, how can we meet a period and a motor torque ?

Wheels are at the side because the chassis I bought had them there. Some amateur Segways have them underneath and it still works fine. I wouldn't say it was that top heavy, only that the longer a pendulum is the easier it is to balance! The bob at the top is just to make it look like a pendulum, I could take it off and it would still work though I would need to change the controller a bit maybe. You don't need the pendulum at all, plenty designs just have the cart and no pendulum at all. You may find they are more unstable however but some are pretty good. The motors themselves are chosen to be powerful enough for most practical loads. They take up to 2A. Torque in a dc motor is directly proportional to current drawn. The period is not dependent on the mass at the top, but on g and the length. So my answer is to over-engineer things so that it meets with most practical loads. Chose a tiny motor with a small current and it may have difficulties. Whilst this may not be best practice, this is a one-off demo so doesn't really matter much.

The same principle is used to stabilise a rocket or missile. If you add a joint in the middle and have a double inverted pendulum it's like a walking machine ie two legs. It's taken 30 odd years or more to get half decent machines that walk on rough terrain.