L3G4200D + ADXL345 卡尔曼滤波

小车下面就是 L3G4200D + ADXL345 两个模块，加速度模块没固定好，板子太小了没地方打孔； 

    加速度模块角度计算：

    如果传感器 x 轴朝下， y 轴朝前

    那竖直方向弧度计算公式为： angle = atan2(y, z)   //结果以弧度表示并介于 -pi 到 pi 之间（不包括 -pi）

    如果要换算成具体角度：     angle = atan2(y, z) * (180/3.14) 

    如： actan(1,sqrt(3)) * 180 / PI = 30°

    陀螺仪角度计算：

    式中angle(n)为陀螺仪采样到第n次的角度值；

    angle(n-1)为陀螺仪第n-1次采样时的角度值；

    gyron为陀螺仪的第n次采样得到的瞬时角速率值；

    dt为运行一遍所用时间；

    angle_n += gyro(n) * dt//积分计算

卡尔曼滤波： 

     网上找的kalman滤波，具体代码如下 ：

static const float dt = 0.02; 

static float P[2][2] = {{ 1, 0 }, { 0, 1 }}; 

float angle; 
float q_bias; 
float rate; 

static const float R_angle = 0.5 ; 
static const float Q_angle = 0.001; 
static const float Q_gyro  = 0.003; 

float stateUpdate(const float gyro_m){ 

float q; 
float Pdot[4]; 
q = gyro_m - q_bias; 
Pdot[0] = Q_angle - P[0][1] - P[1][0];    /* 0,0 */ 
Pdot[1] = - P[1][1];             /* 0,1 */ 
Pdot[2] = - P[1][1];                 /* 1,0 */ 
Pdot[3] = Q_gyro;     /* 1,1 */ 

rate = q; 


angle += q * dt; 

P[0][0] += Pdot[0] * dt; 
P[0][1] += Pdot[1] * dt; 
P[1][0] += Pdot[2] * dt; 
P[1][1] += Pdot[3] * dt; 

return angle; 
} 

float kalmanUpdate(const float incAngle) 
{ 

float angle_m = incAngle; 
float angle_err = angle_m - angle; 


float h_0 = 1; 

const float PHt_0 = h_0*P[0][0]; /* + h_1*P[0][1] = 0*/ 
const float PHt_1 = h_0*P[1][0]; /* + h_1*P[1][1] = 0*/ 

float E = R_angle +(h_0 * PHt_0); 

float K_0 = PHt_0 / E; 
float K_1 = PHt_1 / E; 

float Y_0 = PHt_0;  /*h_0 * P[0][0]*/ 
float Y_1 = h_0 * P[0][1]; 
  
P[0][0] -= K_0 * Y_0; 
P[0][1] -= K_0 * Y_1; 
P[1][0] -= K_1 * Y_0; 
P[1][1] -= K_1 * Y_1; 


angle += K_0 * angle_err; 
q_bias += K_1 * angle_err; 

return angle; 

http://www.cnblogs.com/relax/archive/2012/01/14/2322290.html

制作之前我们需要对陀螺仪 + 加速度计 进行测试，看我们获取的角度数据是否满足要求。网上常用的方法是使用卡尔曼滤波将陀螺仪和加速度计的数据进行融合而得到一个相对稳定正确的角度值，具体方法在我前面的文章中提到过：L3G4200D + ADXL345 卡尔曼滤波

     

     获取到角度以后需要找到小车的平衡点，也就是无外力作用的时候小车能够立在地面上的角度： 角度差 = 小车角度 - 平衡点角度。

     用小车角度数据结合当前的倾斜目标值，通过PID运算，得出电机PWM脉宽数据，指挥电机运行即可。

     PID算法相对比较简单，而且arduino有现成的PID libraries : http://arduino.cc/playground/Code/PIDLibrary

PID::PID(double* Input, double* Output, double* Setpoint,
        double Kp, double Ki, double Kd, int ControllerDirection)
{
    PID::SetOutputLimits(0, 255);                //default output limit corresponds to 
                                                //the arduino pwm limits

    SampleTime = 100;                            //default Controller Sample Time is 0.1 seconds

    PID::SetControllerDirection(ControllerDirection);
    PID::SetTunings(Kp, Ki, Kd);

    lastTime = millis()-SampleTime;                
    inAuto = false;
    myOutput = Output;
    myInput = Input;
    mySetpoint = Setpoint;
        
}

     PID LIB的参数分别是这样的： 

           Input        输入值(这里输入卡尔曼融合获取的角度值)

           Output      PID计算的结果，供电机驱动的PWM使用

           Setpoint    期望值（这里输入小车平衡点的角度值）

           Kp、Ki、Kd 这是KPI的三个重要参数 

    这三个参数的详细说明我从网上摘录了一段： 

    比例（P）控制

    比例控制是一种最简单的控制方式。其控制器的输出与输入误差信号成比例关系。当仅有比例控制时系统输出存在稳态误差（Steady-state error）。

 

    积分（I）控制

    在积分控制中，控制器的输出与输入误差信号的积分成正比关系。对一个自动控制系统，如果在进入稳态后存在稳态误差，则称这个控制系统是有稳态误差的或简称有差系统（System with Steady-state Error）。为了消除稳态误差，在控制器中必须引入“积分项”。积分项对误差取决于时间的积分，随着时间的增加，积分项会增大。这样，即便误差很小，积分项也会随着时间的增加而加大，它推动控制器的输出增大使稳态误差进一步减小，直到等于零。因此，比例+积分(PI)控制器，可以使系统在进入稳态后无稳态误差。

 

    微分（D）控制

    在微分控制中，控制器的输出与输入误差信号的微分（即误差的变化率）成正比关系。自动控制系统在克服误差的调节过程中可能会出现振荡甚至失稳。其原因是由于存在有较大惯性组件（环节）或有滞后(delay)组件，具有抑制误差的作用，其变化总是落后于误差的变化。解决的办法是使抑制误差的作用的变化“超前”，即在误差接近零时，抑制误差的作用就应该是零。这就是说，在控制器中仅引入 “比例”项往往是不够的，比例项的作用仅是放大误差的幅值，而目前需要增加的是“微分项”，它能预测误差变化的趋势，这样，具有比例+微分的控制器，就能够提前使抑制误差的控制作用等于零，甚至为负值，从而避免了被控量的严重超调。所以对有较大惯性或滞后的被控对象，比例+微分(PD)控制器能改善系统在调节过程中的动态特性。 

PID myPID(&Input, &Output, &Setpoint,2,5,1, DIRECT); //PID对象声明


setupPID();  //PID初始化

....
Kalman_Filter(Adxl_angle, Gyro_sensor); //卡尔曼融合获取angle
Input = angle;
myPID.Compute();  //PID计算获取 Output
Drive(Output);   //根据Output驱动电机

void setupPID(){
  Input = 0;
  Setpoint = 17;  //我的小车自平衡角度为17
  myPID.SetSampleTime(100);  //控制器的采样时间100ms
  //myPID.SetOutputLimits(0, 2000); 
  myPID.SetMode(AUTOMATIC);
}

  如果你做完这些小车也能成功站起来了，我的小车抖动得比较厉害，是因为我的直流电机减速太多了（减速比1：220的单轴电机），而且PID的kp,ki,kd三个参数没调整好。等有时间换个电机再仔细调整一下参数，最好能做成可以控制前景、后退、转弯的小车。 弄个体积大点的就能骑着上下班了，哈。

http://www.cnblogs.com/relax/archive/2012/01/06/2313935.html

#define DEBUG 0    // set to 1 to print to serial monitor, 0 to disable
#include <Servo.h>

Servo headservo;  // 头部舵机对象

// Constants
const int EchoPin = 2; //超声波信号输入
const int TrigPin = 3; //超声波控制信号输出

const int leftmotorpin1 = 4; // 直流电机信号输出
const int leftmotorpin2 = 5;
const int rightmotorpin1 = 6;
const int rightmotorpin2 = 7;

const int HeadServopin = 9; // 舵机信号输出 只有9或10接口可利用
const int Sharppin = 11; // 红外输入 

const int maxStart = 800;  //run dec time

// Variables
int isStart = maxStart;    //启动
int currDist = 0;    // 距离
boolean running = false; 

void setup() {

  Serial.begin(9600); // 开始串行监测

  //信号输入接口
  pinMode(EchoPin, INPUT);
  pinMode(Sharppin, INPUT);

  //信号输出接口
  for (int pinindex = 3; pinindex < 11; pinindex++) {
    pinMode(pinindex, OUTPUT); // set pins 3 to 10 as outputs
  }

  //舵机接口
  headservo.attach(HeadServopin);

  //启动缓冲活动头部
  headservo.write(70);
  delay(2000);
  headservo.write(20);
  delay(2000);
}

void loop() {

  if(DEBUG){
    Serial.print("running:");
    if(running){
      Serial.println("true"); 
    }
    else{
      Serial.println("false"); 
    }
  }

  if (isStart <= 0) {
    if(running){
      totalhalt();    // stop!
    }
    int findsomebody = digitalRead(Sharppin);
    if(DEBUG){
      Serial.print("findsomebody:");   
      Serial.println(findsomebody);   
    }    
    if(findsomebody > 0) {
      isStart = maxStart; 
    }
    delay(4000);
    return;
  }
  isStart--;
  delay(100);

  if(DEBUG){
    Serial.print("isStart: ");
    Serial.println(isStart);  
  }

  currDist = MeasuringDistance(); //读取前端距离

  if(DEBUG){
    Serial.print("Current Distance: ");
    Serial.println(currDist);  
  }  

  if(currDist > 30) {
    nodanger();
  }
  else if(currDist < 15){
    backup();
    randTrun();
  }
  else {
    //whichway();
    randTrun();
  }
}

//测量距离 单位厘米
long MeasuringDistance() {
  long duration;
  //pinMode(TrigPin, OUTPUT);
  digitalWrite(TrigPin, LOW);
  delayMicroseconds(2);
  digitalWrite(TrigPin, HIGH);
  delayMicroseconds(5);
  digitalWrite(TrigPin, LOW);

  //pinMode(EchoPin, INPUT);
  duration = pulseIn(EchoPin, HIGH);

  return duration / 29 / 2;
}

// 前进
void nodanger() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, LOW);
  digitalWrite(rightmotorpin2, HIGH);
  return;
}  

//后退
void backup() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  digitalWrite(leftmotorpin2, LOW);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, LOW);
  delay(1000);
}

//选择路线
void whichway() {
  running = true;
  totalhalt();    // first stop!

  headservo.write(20);
  delay(1000);
  int lDist = MeasuringDistance();   // check left distance
  totalhalt();      // 恢复探测头

  headservo.write(120);  // turn the servo right
  delay(1000);
  int rDist = MeasuringDistance();   // check right distance
  totalhalt();      // 恢复探测头

  if(lDist < rDist) {
    body_lturn();
  }
  else{
    body_rturn();
  }
  return;
}

//重新机械调整到初始状态
void totalhalt() {
  digitalWrite(leftmotorpin1, HIGH);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, HIGH);
  headservo.write(70);  //  set servo to face forward
  running = false; 
  return;
  delay(1000);
}  

//左转
void body_lturn() {
  running = true;
  digitalWrite(leftmotorpin1, LOW);
  digitalWrite(leftmotorpin2, HIGH);
  digitalWrite(rightmotorpin1, HIGH);
  digitalWrite(rightmotorpin2, LOW);
  delay(1500);
  totalhalt();
}  

//右转
void body_rturn() {
  running = true;
  digitalWrite(leftmotorpin1, HIGH);
  digitalWrite(leftmotorpin2, LOW);
  digitalWrite(rightmotorpin1, LOW);
  digitalWrite(rightmotorpin2, HIGH);
  delay(1500);
  totalhalt();
}  

void randTrun(){
  long randNumber;
  randomSeed(analogRead(0));
  randNumber = random(0, 10);
  if(randNumber > 5) {
    body_rturn();
  }
  else
  {
    body_lturn();
  } 
}