L3G4200D + ADXL345 卡爾曼濾波

      兩輪自平衡小車的底座基本弄好了，用了個簡單的塑料盒子加上兩個直流電機和輪胎組成的，比較簡陋，但湊合能用。

 

  

 

 

 

 

 

    小車下面就是 L3G4200D + ADXL345 兩個模塊，加速度模塊沒固定好，板子太小了沒地方打孔，有時間將兩個模塊焊到一個萬能板上應該會容易固定一些。

 

    加速度模塊角度計算：

     如果傳感器 x 軸朝下， y 軸朝前

     那豎直方向弧度計算公式為： angle = atan2(y, z)   //結果以弧度表示並介於 -pi 到 pi 之間（不包括 -pi）

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

 

    陀螺儀角度計算：

     式中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;  
         

}  

 

 

    波形顯示 

       測試說明——單片機采集加速度和陀螺儀的信號，並使用上面的kalman濾波，計算出最優傾角，通過串口發送到pc機，pc機運行的串口示波器將相關波形顯示出來。 

       1、藍色為加速度換算后的角度。 

       2、黃色為陀螺儀直接積分后的角度。 

       3、紅色為kalman濾波后的角度。 

 

     

 

    用手指敲小車可以看到加速度模塊計算獲取的角度震動比較厲害，經過卡爾曼濾波后的波形相對平緩一些。 

 

     

 

    局部放大看一下曲線還是很優美的哦，哈。。

        

 

     波形顯示用了園子里xf_z1988的開源波形控件，他的主頁是：http://www.cnblogs.com/xf_z1988/