【转】arduino制作的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 = {{ 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;
q = gyro_m - q_bias;
Pdot = Q_angle - P - P; /* 0,0 */
Pdot = - P; /* 0,1 */
Pdot = - P; /* 1,0 */
Pdot = Q_gyro; /* 1,1 */
rate = q;
angle += q * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * dt;
P += Pdot * 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; /* + h_1*P = 0*/
const float PHt_1 = h_0*P; /* + h_1*P = 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*/
float Y_1 = h_0 * P;
P -= K_0 * Y_0;
P -= K_0 * Y_1;
P -= K_1 * Y_0;
P -= 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/
页:
[1]