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/