卡尔曼滤波和角度测定

小车下面就是 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 ; //R倾角

static const float Q_angle = 0.001; //Q倾角

static const float Q_gyro = 0.003; //Q陀螺仪

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的开源波形控件，他的主页是：/xf_z1988/

Relax Blog

第二个Arduino小车两轮自平衡

自从做了第一个Arduino小车兴趣大增，于是开始制作第二个Arduino小车，这次我想做得相对复杂一点。一直对SEGWAY非常着迷，查了些技术资料发现自平衡小车的原理也比较简单：利用陀螺仪和加速度模块获得小车角度，Arduino对获取的数据进行处理，然后控制电机运转纠正倾斜，从而达到平衡的效果。

先来个视频：

需要准备的硬件有：

1、陀螺仪我选用的是L3G4200D三轴陀螺仪，其实自平衡小车只用到其中的一轴

2、加速度计我用的是ADXL345三轴加速度计，自平衡小车也只用到其中两轴

3、Arduino板子我用的是我使用的是arduino duemilanove 2009 arduino硬件区别请看这里：/en/Main/Hardware

4、L298N电机驱动模块一个需要带光耦

5、直流减速电机两个小车轮胎两个塑料盒子一个

还需要一些杜邦线、电池、螺丝等辅助的东西

有朋友问我这些东西哪里能买到，其实这些材料拜一下淘宝大神就能找到的。

组装过程比较简单，在塑料盒合适的位置打孔，然后用螺丝固定住电路板和电机即可：