Pages: [1] 2 3 ... 5   Go Down
Author Topic: Self Balancing Robot  (Read 19886 times)
0 Members and 2 Guests are viewing this topic.
0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley



You can check some video here:




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

« Last Edit: October 29, 2008, 07:30:13 am by msx » Logged


London
Offline Offline
Faraday Member
**
Karma: 8
Posts: 6240
Have fun!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

Nice job!

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

0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

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


London
Offline Offline
Faraday Member
**
Karma: 8
Posts: 6240
Have fun!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

It can't be all that bad, it seems to be working a treat  smiley-wink
Logged

0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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

Code:
#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(" ");
  }

}


Logged


London
Offline Offline
Faraday Member
**
Karma: 8
Posts: 6240
Have fun!
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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

0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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


0
Offline Offline
Newbie
*
Karma: 0
Posts: 3
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

0
Offline Offline
Newbie
*
Karma: 0
Posts: 3
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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.  smiley-wink

- Nathan
Logged

0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Hi Nathan smiley
Sorry to disappoint you, but those panels are not laser cutted smiley 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 smiley

thanks for the feedback smiley
Logged


0
Offline Offline
Newbie
*
Karma: 0
Posts: 3
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

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
Logged

0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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 smiley

Yeah, the pots save huge time smiley 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 smiley-razz

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


Toronto, Canada
Offline Offline
Full Member
***
Karma: 0
Posts: 144
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

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


0
Offline Offline
Full Member
***
Karma: 0
Posts: 109
Arduino rocks
View Profile
WWW
 Bigger Bigger  Smaller Smaller  Reset Reset

Thanks Ro-Bot-X smiley
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 smiley
Logged


0
Offline Offline
Newbie
*
Karma: 0
Posts: 11
Arduino rocks
View Profile
 Bigger Bigger  Smaller Smaller  Reset Reset

can you post your MD23 library files?
Logged

Pages: [1] 2 3 ... 5   Go Up
Jump to: