L3G4200D + ADXL345 卡尔曼滤波

blueice
发布于 2020-11-19 16:16
浏览
0收藏

   两轮自平衡小车的底座基本弄好了,用了个简单的塑料盒子加上两个直流电机和轮胎组成的,比较简陋,但凑合能用。

 

小车下面就是 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滤波后的角度。 L3G4200D + ADXL345 卡尔曼滤波-鸿蒙开发者社区

用手指敲小车可以看到加速度模块计算获取的角度震动比较厉害,经过卡尔曼滤波后的波形相对平缓一些。 L3G4200D + ADXL345 卡尔曼滤波-鸿蒙开发者社区

 局部放大看一下曲线还是很优美的哦,哈。。

 

分类
已于2020-11-19 16:16:02修改
收藏
回复
举报
回复
    相关推荐