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();
  } 
}