智能车制作

标题: 问一下各位，加速度计和陀螺仪融合后的波形怎样才算最好，我用了卡尔曼（贴代码贴数据 [打印本页]

作者: zelight 时间: 2012-2-9 20:31
标题: 问一下各位，加速度计和陀螺仪融合后的波形怎样才算最好，我用了卡尔曼（贴代码贴数据
本帖最后由 zelight 于 2012-2-9 20:43 编辑

我用了卡尔曼融合，代码是zlstone的。static float P[2][2] = {
   { 1, 0 },
   { 0, 1 }
   };

static float Pdot[4] ={0,0,0,0};
static const char C_0 = 1;
static float q_bias, angle_err, PCt_0, PCt_1, E, K_0, K_1, t_0, t_1;
  int test_angel=0,test_angel_dot=0;
//-------------------------------------------------------
void Kalman_Filter(float angle_m,float gyro_m) //gyro_m:gyro_measure
{                                                 //加速度器，陀螺仪
//float angle=0, angle_dot=0;
  int k;

angle+=(gyro_m-q_bias) * dt;                // 对角速度进行积分

Pdot[0]=Q_angle - P[0][1] - P[1][0]; //=
Pdot[1]=- P[1][1];                //=-1
Pdot[2]=- P[1][1];                //=-1
Pdot[3]=Q_gyro;

P[0][0] += Pdot[0] * dt;
P[0][1] += Pdot[1] * dt;
P[1][0] += Pdot[2] * dt;
P[1][1] += Pdot[3] * dt;

angle_err = angle_m - angle;       //计算陀螺仪积分后的值和加速度器计算的角度之差。

PCt_0 = C_0 * P[0][0];
PCt_1 = C_0 * P[1][0];

E = R_angle + C_0 * PCt_0;

K_0 = PCt_0 / E;
K_1 = PCt_1 / E;

t_0 = PCt_0;
t_1 = C_0 * P[0][1];
P[0][0] -= K_0 * t_0;
P[0][1] -= K_0 * t_1;
P[1][0] -= K_1 * t_0;
P[1][1] -= K_1 * t_1;

angle += K_0 * angle_err;       //角度
q_bias += K_1 * angle_err;
angle_dot = gyro_m-q_bias;       //角速度

我发现响应有点慢，比如从0到19，它会先到21再到19。。。。虽然修改Q和R可以纠正，但感觉响应太快又不好，到底怎样的滤波，才算好的波形？？急求？？

从下面的图片可以发现，难以去掉的尖峰（可以考虑增加Q，过程噪声的值），还有响应时间的缓慢（可以通过增加Q和R），这些都是通过实践所得，但不能太大，我也是试验所得，其实据说只有R可以改，Q最好不要随意修改。

补充内容 (2012-2-10 16:40):
http://www.megasociety.org/noesis/138/kalman.html  主要是这张表格

补充内容 (2012-2-10 16:41):
这张表格很有用

作者: wo520 时间: 2012-2-9 20:44
这个，本人也是试过，卡尔曼相对来说是比较慢，互补相对会快点，但融合效果没有卡尔曼好！只能看你自己选了！

作者: zelight 时间: 2012-2-9 21:11

wo520 发表于 2012-2-9 20:44
这个，本人也是试过，卡尔曼相对来说是比较慢，互补相对会快点，但融合效果没有卡尔曼好！只能看你自己选了 ...

谢谢。

作者: yaozhifeng 时间: 2012-2-9 22:00

作者: 晨景的风 时间: 2012-2-9 22:40
厉害，上面有个博士？来参赛单位？

作者: 晨景的风 时间: 2012-2-9 22:40
不怎么会阿，大侠们教教他

作者: 二硫碘化钾 时间: 2012-2-10 09:40
楼主是用什么仿真的？？？

作者: zl6977 时间: 2012-2-10 10:42
我怕也有这个问题，”我发现响应有点慢，比如从0到19，它会先到21再到19。。。“
而且我的是陀螺仪采回的原始数据就有一个先大再小的过程。不知大家怎么解决的啊？

作者: zelight 时间: 2012-2-10 16:39

二硫碘化钾发表于 2012-2-10 09:40
楼主是用什么仿真的？？？

上位机

作者: zelight 时间: 2012-2-10 16:41

zl6977 发表于 2012-2-10 10:42
我怕也有这个问题，”我发现响应有点慢，比如从0到19，它会先到21再到19。。。“
而且我的是陀螺仪采回的原 ...

看一下这张表格http://www.megasociety.org/noesis/138/kalman.html

作者: zl6977 时间: 2012-2-10 16:46
本帖最后由 zl6977 于 2012-2-10 16:48 编辑

zelight 发表于 2012-2-10 16:41
看一下这张表格http://www.megasociety.org/noesis/138/kalman.html

谢谢你哥们，关于我说的那个问题，由于陀螺仪直接采回的数据也有这个问题，所以我怀疑是硬件问题，我查了一下，ENC-03的典型用法电路真的有这样的问题。你看：http://www.ourdev.cn/bbs/bbs_content_all.jsp?bbs_sn=1512964 加个QQ吧？

作者: wo520 时间: 2012-2-10 17:14
我想问下楼主仿真用的硬件有用到是RS-232吗？

作者: zelight 时间: 2012-2-10 18:10

wo520 发表于 2012-2-10 17:14
我想问下楼主仿真用的硬件有用到是RS-232吗？

嗯，是的

作者: wanstrive 时间: 2012-2-10 19:05
我上位机那块搞不定额。。

作者: zelight 时间: 2012-2-10 19:10

zl6977 发表于 2012-2-10 16:46
谢谢你哥们，关于我说的那个问题，由于陀螺仪直接采回的数据也有这个问题，所以我怀疑是硬件问题，我查了 ...

谢谢，我试过他的方法1效果确实还好，虽然有很大的温漂

作者: 大李钢刀 时间: 2012-2-10 19:13
楼主研究得透彻，顶！！

作者: 断翅at雄鹰 时间: 2012-2-11 16:31
羡慕

作者: lgupen 时间: 2012-2-13 12:32

zelight 发表于 2012-2-10 16:41
看一下这张表格http://www.megasociety.org/noesis/138/kalman.html

反应的慢是你的dt参数太小了吧。。。试着放大试试

作者: zelight 时间: 2012-2-21 00:18

lgupen 发表于 2012-2-13 12:32
反应的慢是你的dt参数太小了吧。。。试着放大试试

谢谢，现在能立了

作者: 涛涛 时间: 2012-2-29 16:17
羡慕啊！！！！

作者: 狼仔 时间: 2012-3-1 22:12

作者: 骑鱼士 时间: 2012-3-4 13:39
这就是卡尔曼啊

作者: hanlinson 时间: 2013-7-31 10:27
响应比较慢的问题你解决没有？？

作者: wmslecz 时间: 2014-3-16 16:31
看不懂卡尔曼呀...

欢迎光临智能车制作 (http://dns.znczz.com/)