Self Balancing Robot

Hello, i've been working on a self balancing robot, based on Arduino.
I used an RD01 drive system, an IMU combo board with 2 accelerometer and a gyroscope.

The system features a kalman filter to acquire data from the IMU and a PID controller to control the motors.

The robot is actually able to stand by itself indefinitely, but it's not completely stable, either becouse of my software or becouse of the motors that have a little "loosiness" (a couple of degrees of "free" rotation). I'm still working on it

You can check some video here:

More info here:
http://letsmakerobots.com/node/1505

400×533 57.7 KB

Nice job!

I didn't see any details on the code, perhaps I missed it. Are you sharing the software implimentation?

Sure, i'll post the code this evening as i get home. Be warned that the code is a real mess

Anyway, i adapted the code for the kalman filter from these two sites:
http://diydrones.com/profiles/blog/show?id=705844%3ABlogPost%3A23188
http://tom.pycke.be/

The rest is the pid controller (not that complex), something to use the LCD and other glue.

It can't be all that bad, it seems to be working a treat

here's the code
i don't guarantee for wrong and outdated comments

#include <AXELCD.h>

#include <MD23.h>
#include <Wire.h>

#include <math.h>


/////////////////////////////////////////
#define NUMREADINGS 5 //Gyro noise filter
int readings[NUMREADINGS];                // the readings from the analog input (gyro)
int index = 0;                            // the index of the current reading
int total = 0;                            // the running total
int average = 0;                          // the average
int inputPin =0;                          //Gyro Analog input
///////////////////////////////////////

float dt = .1; // .06; //( 1024.0 * 256.0 ) / 16000000.0; (Kalman)  // what does this means ?? hand tuned
int mydt = 20; //in ms.
static float PP[2][2] = { //(Kalman)
  { 
    1, 0   }
  , //(Kalman)
  { 
    0, 1   }
  ,//(Kalman)
}; //(Kalman)


/*
 * Our two states, the angle and the gyro bias.  As a byproduct of computing
 * the angle, we also havef an unbiased angular rate available.   These are
 * read-only to the user of the module.
 */
float angle = 0.0; //(Kalman)
float q_bias = 0.0; //(Kalman)
float rate = 0.0; //(Kalman)
float                   q_m = 0.0; //(Kalman)

int ax_m=0;
int ay_m=0;
int cnt = 0;            //Counter
unsigned long lastread=0;
unsigned long startmillis=0;

/*
 * R represents the measurement covariance noise.  In this case,
 * it is a 1x1 matrix that says that we expect 0.3 rad jitter
 * from the accelerometer.
 */
float R_angle = .3; //.3 deafault, but is adjusted externally (Kalman)


/*
 * Q is a 2x2 matrix that represents the process covariance noise.
 * In this case, it indicates how much we trust the acceleromter
 * relative to the gyros.
 */
static const float Q_angle = 0.001; //(Kalman)
static const float Q_gyro = 0.003; //(Kalman)
int ledPin = 13;

MD23 md23;
AXELCD lcd;

byte displayState = 0; // 0 clean,
float oldAngle = 0.0; //

float P = 0.0;
float I = 0.0;
float D = 0.0;

void setup()
{
  pinMode(ledPin, OUTPUT);
  pinMode(2, INPUT);     // declare pushbutton as input

  delay(500);
  Wire.begin(); // init i2c
  md23.setMode(0);  // set the mode (0 is default, so not really useful :P)
  md23.disableSpeedRegulation();
  lcd.clearDisplay(); // this always need 30 millis delay.
  delay(30);
  lcd.write("Chiappe!");
  delay(10);
  lcd.moveLine2(0);
  delay(10);
  lcd.write("Battery:"); // every other command needs 10 millis delay
  delay(10);

  byte b = md23.getBatteryVolts();
  lcd.write(b, DEC);
  delay(10);
  lcd.moveLine2(0);
  delay(10);


  Serial.begin(9600);
  for (int i = 0; i < NUMREADINGS; i++)
    readings[i] = 0;                      // initialize all the readings to 0 (gyro average filter)



  startmillis = millis();
  
}
float sqr(float a)
{
  return a*a;  
}
float sgn (float a) 
    {
    if (a > 0) 
        return +1;
    else 
        if (a < 0) 
            return -1;
    else 
        return 0;
}

long readMAX127(byte chan)
{
  byte control = 0x80 + (chan << 4);
  byte addr = 0x28;

  Wire.beginTransmission(addr);
  Wire.send(control); // send value
  Wire.endTransmission();

  Wire.requestFrom(addr, (byte)2);

  byte b1 = Wire.receive();
  byte b2 = Wire.receive() >> 4;
  long data = (b1 << 4) | (b2 & 0x0F);

  return data;
}


void loop()
{
  int delta = millis()-lastread;
  if( delta >= mydt) { // sample every dt ms -> 1000/dt hz.
    lastread = millis();
    
    total -= readings[index];               // subtract the last gyro reading
    readings[index] = analogRead(2); // read from the gyro sensor
    total += readings[index];               // add the reading to the total
    index = (index + 1);                    // advance to the next index

    if (index >= NUMREADINGS)               // if we're at the end of the array...
      index = 0;                            // ...wrap around to the beginning

    average = (total / NUMREADINGS)-500;    // calculate the average of gyro input

    dt = ((float)delta) / 1000.0;

    q_m= ((float)average)*(1500.0/1024.0)*PI/180 ;  // HAC remove 1.5 mult

    /* Unbias our gyro */
    const float q = q_m - q_bias; //(Kalman)

    const float Pdot[2 * 2] = {
      Q_angle - PP[0][1] - PP[1][0], /* 0,0 */ //(Kalman)
      - PP[1][1], /* 0,1 */
      - PP[1][1], /* 1,0 */
      Q_gyro /* 1,1 */
    };

    /* Store our unbias gyro estimate */
    rate = q; //(Kalman)

    /*
* Update our angle estimate
     * angle += angle_dot * dt
     *       += (gyro - gyro_bias) * dt
     *       += q * dt
     */
    angle += q * dt; //(Kalman)

    /* Update the covariance matrix */
    PP[0][0] += Pdot[0] * dt; //(Kalman)
    PP[0][1] += Pdot[1] * dt; //(Kalman)
    PP[1][0] += Pdot[2] * dt; //(Kalman)
    PP[1][1] += Pdot[3] * dt; //(Kalman)


    // read here!
    ax_m = analogRead(0)-510;
    ay_m = analogRead(1)-510;

    // non aggiusto R
    // R_angle= (joy_y_axis+1)*0.0098039; //external adjust jitter of accelerometer with nunchuck joystick

    const float angle_m = atan2( ay_m, ax_m ); //(Kalman)
    const float angle_err = angle_m - angle; //(Kalman)
    const float C_0 = 1; //(Kalman)
    const float PCt_0 = C_0 * PP[0][0];  //(Kalman)
    const float PCt_1 = C_0 * PP[1][0]; //(Kalman)
    const float E =R_angle+ C_0 * PCt_0; //(Kalman)
    const float K_0 = PCt_0 / E; //(Kalman)
    const float K_1 = PCt_1 / E; //(Kalman)
    const float t_0 = PCt_0; /* C_0 * P[0][0] + C_1 * P[1][0] (Kalman) */

    const float t_1 = C_0 * PP[0][1]; /* + C_1 * P[1][1]  = 0 (Kalman) */


    PP[0][0] -= K_0 * t_0; //(Kalman)
    PP[0][1] -= K_0 * t_1; //(Kalman)
    PP[1][0] -= K_1 * t_0; //(Kalman)
    PP[1][1] -= K_1 * t_1; //(Kalman)
    angle += K_0 * angle_err; //(Kalman)
    q_bias += K_1 * angle_err; //(Kalman)

  float calibration = -4.5; // read from a pot or something
  float myangle=(angle*57.2957795130823)-90.0 + calibration;

  if ((millis()-startmillis) > 6000 )
  {
    digitalWrite(ledPin, HIGH);

//    float integralAverage = (integral / NUMINTEGRALS);
//    integral = integral - integralAverage;

  int val = digitalRead(2);  // read input value
  if (val == HIGH) {
     I = 0.0;
    }
    

    float rate_deg = rate * 57.2957795130823;
    float pot1 = ((float)readMAX127(1))  ;
    float pot2 = ((float)readMAX127(2))  ;
    float pot3 = ((float)readMAX127(3))  ;
    
    // myangle is up at 0
    // motors is stopped at 128
    
    P = (myangle * (pot1 / 100.0)); // pot/10
    D = (myangle-oldAngle) * (pot2 / 10.0); // Kd = 50 (800 at pot)
    I = I + myangle * (pot3/100.0); // I * 1.0 + myangle * pot;  // 123 looks good
    
    float pid = P+I+D;
    float motors = 128.0 - (pid+(sgn(pid)*sqr(pid/18)));
    // cap the value
    if (motors>250.0) motors = 250.0;
    if (motors<5.0) motors = 5.0;
    
    byte motorsInput = (byte)motors;
    //Serial.println(myangle, DEC);
    Serial.println(motorsInput, DEC);
    if ((myangle > 80) || (myangle < -80))
    { 
      md23.setMotor1Speed(128);    
      md23.setMotor2Speed(128);
      I=0; // avoid accumulation of integration when stopped
    }
    else
    { 
      md23.setMotor1Speed(motorsInput);    
      md23.setMotor2Speed(motorsInput);
    }
    

    oldAngle = myangle;    
    

    
    // update the display
    displayState ++;
    if (displayState==7) displayState = 0;
    switch (displayState)
    {
      case 0:
      lcd.moveLine2(0);
      break;
      case 1:
      lcd.write(pot1, DEC);
      break;
      case 2:
      lcd.write(" ");
      break;
      case 3:
      lcd.write(pot2, DEC);
      break;
      case 4:
      lcd.write(" ");
      break;
      case 5:
      lcd.write(pot3, DEC);
      break;
      case 6:
      lcd.write("    ");
      break;
    }
    
  }
  else
  {
    digitalWrite(ledPin, LOW);
  }



//    Serial.print(" ");
    
//    Serial.println(int(angle_m*57.295779513082)+180, DEC); //Prints the accelometer
//    Serial.println(ay_m, DEC); //Prints the accelometer

//    Serial.print(" ");
     //Prints degrees Acceleromter+Gyros+KalmanFilters
//    Serial.print(" ");
  }

}

I was wondering if you were using the rotation sensor data from the motors? I was thinking of building something like this using large continuous rotation servos, but they don't output pulses with rotation like the motor assembly you are using.

no, i'm not reading the data from the encoders. i don't think they're really needed in this design..

What a great looking robot! I really like the use of laser-cut plastic.

I'm interested in how you make do without encoders. I've built a balancer and tried to make it work without encoders, and it balances fine, but on a smooth surface, it eventually builds a small bias in one direction or the other and starts to roll that way. Your robot seems to hold its ground very well on smooth surfaces.

Do you have any insight into how you keep bias from accumulating?

Thanks for posting all this detail. Your project is really inspiring.

• Nathan

The "looseness" you describe is a real problem with balancers. I used motors similar to yours at first, but was able to achieve much tighter and more responsive balancing by switching to zero-backlash servos (Dynamixel AX-12).

Here's a little sample movie of what a difference reduced backlash makes:
http://www.justin.tv/clip/aa7f9ae5ece

That demo is using the "encoder" (continuous rotation pot) integrated into the AX-12.

A friend of mine worked on a commercial balancing toy. He mentioned that it used cheap high-backlash motors, and they had to use sophisticated filtering and modeling to accommodate the backlash. It took an experienced control expert quite a while to get it working.

So I'll just stick with tighter motors.

• Nathan

Hi Nathan
Sorry to disappoint you, but those panels are not laser cutted I cutted them manually with a saw. It looks like they come out rather regular, but it's not that hard with regular rectangles.

About the buildup of speed: i implemented a PID controller, that is a kind of algorithm that is used for reaching a desired value in a controlled system like our.
I don't know if you already know it. In few words, it's a well know way to take into account three different parameters into the calculation of the final speed: the Proportional (that is the error, in our case, the current angle of the robot), the Derivative (that is, the angular velocity, or how fast it is falling/raising), and the Integrative, which is a sum of the errors in all previous iterations.

Well, the integrative is what avoids the buildup. It is a sum of all errors (that is, currentAngle - desiredAngle) from the beginning when you start the robot. Of course it should average to zero in the long term, bug as the robot keeps going in one direction as you say, the value increase costantly and so the "corrective" action (the value of I) increase until it stops the robot from "drifting".

The problem is finding right values for the three coefficients (P, I, D), that is why i have three potentiometers in the robot. The wikipedia article has some more info. Looks like hard math but in reality is quite simple, i never heard of it befor starting this project. In the code you can easily spot the section where it is implemented.

About the backlash, yeah, i think it kills the stability. The problem is that when the baricenter passes from one side to the other, the axle goes from one end of the backlash to the other (which i think could be about 3°, very approximatively). This is expecially visible when the robot is near the stability point, as the balance quickly shifts from forward to backward. You can almost feel the "click".

I think i should change the whole train.. But that's much money!
I can't see that video, it requires some plugin that doesn't work.. is there a youtube (or similar) version? i'm interested in taking a look

thanks for the feedback

That's very interesting. I use a PID but not a Kalman Filter. It may be that the bias is building up in my cheesy gyro filter and causing the integral to wind up incorrectly. Does your robot basically stay in place all day, or if you leave it a while will it be on the other side of the kitchen?

The pot idea is a great one. I have to try that -- that must have saved you a lot of time in development.

The video supposedly only requires Flash Player, like Youtube, although you might try going here to get the new version: http://www.adobe.com/products/flashplayer/

I will try to get something on Youtube when I'm back home later this evening.

• Nathan

Ah ok, than that's the reason. You cannot just sum up the readings from the gyro to get the absolute angle, it will drift quite a lot with time like any kind of dead reckoning.
The kalman filter is what helps in this situation: using data from accelerometers (that sense the gravitational acceleration) you can get an absolute value for the current angle. The problem is that while on the long time it averages to the right value, it will change badly as the robot swing, rotate or stop. The kalman filter is what makes an adeguate "combination" of the gyro and accelerometers to get a value that is correct both in the short time and in the long time.

Here's a serie of article that explain this far better than i did:

http://tom.pycke.be/mav/69/accelerometer-to-attitude
http://tom.pycke.be/mav/70/gyroscope-to-roll-pitch-and-yaw
http://tom.pycke.be/mav/71/kalman-filtering-of-imu-data

Well my robot is not perfectly stable and it swing a little, but it is not that it builds up a speed in one direction or the other. I've kept it running for about twenty minutes or so in my bedroom with about 2 meters of space

Yeah, the pots save huge time i was stopping and reflashing the robot at each "test" iteration, but it was soooo slow. Btw I had to buy an analog to digital conversion board as i finished up inputs pin

Ok no problem for the video, i can see it from my work pc.

Very nice job!!! Wow! this is exactly what I am working on!!! Well, when I have time to do it...

You can see this post of mine a few weeks ago in a similar thread:
http://www.arduino.cc/cgi-bin/yabb2/YaBB.pl?num=1219065902/3#3
So, I guess you have answered my prayers, I can study your code to see what I need to do to make my robot work. Right now I am struggling to make some encoders for the motors I have, although they have backlash too. I am thinking of using steppers, with them there is no need to use encoders, you will know the speed they will spin, but are they fast enough? What is the rated RPM (and voltage) of your motors?

Keep it up, you're the man!

Thanks Ro-Bot-X
I'm using EMG30 motors, they're 170 RPM and 12V.
Probably quite overkill for the job, in fact between them and the battery my robot is very big and heavy.
About the stepper, i never used one, but i'd say they're not up to the task. Maybe some good motors with little backlash are better.
If you have any question on the code, feel free to ask. Keep in mind that many values there are empirically tuned

can you post your MD23 library files?

Sure, i've uploaded it here. Contains all the code and examples, and it's released in the public domain.
The code should work good, but it's not 100% tested.

http://www.lugato.net/MD23.zip

Seems good enough! Great job!

You can try to move the heavy battery to the bottom of the robot. This may reduce the swing effect somewhat.

BTW, could you supply some details on the LCD you use and as well the AXELCD library files?

Thanks zhengrob Are you using it in a project?

About the battery, is doesn't work that way, becouse the robot is an inversed pendulum and so it actually works better when the weight is high rather than down (it increase intertial and give more reaction time).

The LCD i'm using is the Picaxe LCD display, this one, datasheet here. Basically it's a standard lcd with an additional board attached. It can work serially or with I2C. As my robot was almost only I2C, i used that mode. The library is very simple, it just sends the correct i2c messages with a syntax similar to the Serial.println method.

I'll post sources this evening.

Hello msx! thanks for your message!

I am just learning Kalman filter algorithm and your project is a good example instruction to me. Arduino is new to me therefore any details will help me to better understand the entire routine.

sorry, a little late, here's the picaxe lcd i2c display library

http://www.lugato.net/AXELCD.zip