新手平衡小车卡尔曼滤波算法总结

2013的弹子球

回复【100楼】电子音符:
---------------------------------
回复【98楼】魂淡:
---------------------------------
检查程序对不对，如果可能，把波形绘在上位机上。

电子音符

回复【101楼】2013的弹子球:
---------------------------------
谢谢楼主，问题解决了，原因是卡尔曼滤波出现角度延迟太大造成的，修改一下参数就好了

Kowalski

感谢楼主分享~~~

魂淡

[mw_shl_code=c,true]//(1) PID_Control.OUT+=PID_Speed.I*Position; //（2） void TIM4_IRQHandler(void) //TIM4中断 { if (TIM_GetITStatus(TIM4, TIM_IT_Update) != RESET) //检查指定的TIM中断发生与否:TIM 中断源 { static u8 ms1=0,ms2=0,ms3=0; ms1++; ms2++; ms3++; if(ms1==2) { ms1=0; MPU6050_Pose(); } if(ms2==4) { ms2=0; Calculate_Pwm(Pitch); } if(ms3==20) { Lowpass_Filter();//20ms 计算一次积分的值 } TIM_ClearITPendingBit(TIM4, TIM_IT_Update ); //清除TIMx的中断待处理位:TIM 中断源 }[/mw_shl_code]

	目前我的小车已经能只用角度环站起来了（挺稳的 几乎不动） 还没加速度环。因为对速度环的理论还没完全搞清楚。所以想在论坛上讨教清楚。



	看了很多的PID资料，里面都会提到KI（积分系数），Ti（积分时间），T（采样周期）。那么问题来了？



	PS：代码附在上面 1我是每个20ms  采一次速度的积分值。许多人说这就是积分时间。我也是这么觉得的。那采样周期T在哪呢？（在代码（2）中） 

[/mw_shl_code]

	2代码中的I是指什么呢？ 是积分时间Ti呢，还是积分系数KI呢？



	（在代码（1）中） (PS:这两个问题困扰我好久了，不知道你是怎么理解的呢) 

[/mw_shl_code]

魂淡

。、、

chopininoctober

楼主，

上面公式用的是Angle。 而你的代码用的是Q_angle 。能不能解释一下？角度误差是个什么东西？

2013的弹子球

回复【106楼】chopininoctober:
---------------------------------
角度的协方差

半城夜

楼主大大 。。你的解释帮了我很大的忙谢谢 。。
但是我太笨 还有点弄不懂

五，最后一步对矩阵P进行更新，因为下一次滤波时要用到

PP[0][0] -= K_0 * t_0;

PP[0][1] -= K_0 * t_1;

PP[1][0] -= K_1 * t_0;

PP[1][1] -= K_1 * t_1;

P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟预测方差阵

这一块的PP[0][0] -= K_0 * t_0;右边式子是怎么换算到左边的啊

半城夜

已经算出来啦不过还是谢谢提供这么好的解析 没看之前完全弄不懂那些死板文章写的是什么

半城夜

 请问各位大大
#define dt                  0.02//卡尔曼滤波采样频率 
#define R_angle          0.5 //测量噪声的协方差（即是测量偏差）
#define Q_angle          0.0001//过程噪声的协方差 
#define Q_gyro           0.0003 //过程噪声的协方差  
这几个值怎么确定？是都要调吗？

中北的秋雨

回复【5楼】login_FAE:
---------------------------------
你的头像真搞笑

a435262767

谢谢楼主分享

352308347@qq.co

有东西分享不错,但是解释里面写的不明不白,看的好纠结.......希望楼主能纠正一下哪里写的不明白的地方.谢谢啦

352308347@qq.co

Pdot[0]=Q_angle - P[0][1] - P[1][0]; 
Pdot[1]= - P[1][1];
Pdot[2]= - P[1][1];/
Pdot[3]=Q_gyro;  //这里为什么等于Q_gyro?

//以下几步为什么用到叠加?
PP[0][0] += dot[0] * dt;   
PP[0][1] += dot[1] * dt; 
PP[1][0] += dot[2] * dt;
PP[1][1] += dot[3] * dt;

和谐年份

谢谢分享~~~

2013的弹子球

回复【104楼】魂淡:
---------------------------------
自平衡小车用双输入单输出这种控制策略才能真正稳定，闭环是两个闭环

2013的弹子球

回复【104楼】魂淡:
---------------------------------
有换算关系。T和积分时间，网上查一下

352308347@qq.co

回复【118楼】2013的弹子球:
------------------------
请问楼主,
五，最后一步对矩阵P进行更新，因为下一次滤波时要用到
PP[0][0] -= K_0 * t_0;
PP[0][1] -= K_0 * t_1;
PP[1][0] -= K_1 * t_0;
PP[1][1] -= K_1 * t_1;
P(k|k)=（I-Kg(k) H）P(k|k-1) ……… (5)//跟预测方差阵

这里的I是不是为1 的二阶矩阵?我算出的结果跟上面的不一样,我算的结果如下,不知道哪里出错
        P[0][0] -= K_0 * t_0+PP[1][0];
PP[0][1] -= K_0 * t_1+PP[1][1];
PP[1][0] -= K_1 * t_0+PP[0][0];
PP[1][1] -= K_1 * t_1+PP[0][1];
还有,
 dot[0]=Q_angle - P[0][1] - P[1][0]; 
Pdot[1]= - P[1][1];
Pdot[2]= - P[1][1];/
Pdot[3]=Q_gyro;

PP[0][0] += dot[0] * dt;   
PP[0][1] += dot[1] * dt; 
PP[1][0] += dot[2] * dt;
PP[1][1] += dot[3] * dt;  
这几步我还是没看明白,楼主能不能再说明一下?谢谢啦

352308347@qq.co

回复【118楼】2013的弹子球:
---------------------------------
这里的H是不是[1 1]啊?

葱花

留个脚印，做收藏

别说话、吻我

请问程序中的C_0是什么

坡坡

楼主，你好，想向你问个问题，我在用串口调试助手显示mpu6050的输出角度是不一会就会不出数据了，好像卡死了一样，不知什么情况？不知你遇到过吗，谢谢

坡坡

坡坡 发表于 2016-1-3 22:12
楼主，你好，想向你问个问题，我在用串口调试助手显示mpu6050的输出角度是不一会就会不出数据了，好像卡死 ...

@2013的弹子球

qq942266575

谢谢分享。

小李飞刀

   很厉害啊

随风飞

好贴 不错

yzz

2013的弹子球 发表于 2015-1-3 17:56
楼上看一下这个。。MIT的超经典无数人看过的互补滤波器

        &nbsp ...

谢谢啊。。

靳洪都

mark收藏一下，最近正想了解这方便的知识，谢谢分享。

max2016

感谢分享

望着你发槑

66666666666666

DancingPulses

谢谢分享

机械师1990

MARK，学习了，我也想研究研究

lzh4774

楼主关于卡尔曼的是哪本参考书？

jikun_zhang

很好,很多产品都要用到,谢谢.

Aminoacid

谢谢楼主分享

q1049675423

谢谢楼主分享~~

wszdxmh

http://www.openedv.com/thread-79275-1-1.html
大神，能帮我看一下这个是什么情况吗？

Crystal子午123

可以的

Roderick

我想请教一下：为什么用陀螺仪X轴的数据？而不用y轴或者z轴的呢？

xjlzdh

谢谢楼主的分享。有个问题想请教下子：如果小车在水平面上运动，这个偏转角度是 否也可以用加速度计计算的角度和陀螺仪的角度来进行卡尔曼滤波，因为我从别的网站上看到说对于飞行器偏转角由于重力加速度和水平面垂直，次偏转角无法有加速度计测的，即加速度的角度和陀螺仪的角度不是同一个角度，这样的话是否做卡尔曼滤波就没有意义？  谢谢

华丽与creazy

我用printf("%0.2f    %0.2f    %0.2f\r\n",Angle,Angle_ax,Gyro_y);函数分别读取的加速度，角速度和倾角，我发现当我快速的改变板子的倾角的时候，比如快速变化10度，Angle（卡尔曼滤波后的倾角）瞬时变化非常快，可能会瞬间变化几十度然后回到正常角度，而当我缓慢变化10度的时候，Angle变化是正常线性变化到10度，在这两种变化中，Angle_ax（从MPU6050读取的值经过处理后的陀螺仪的Y轴数据）的变化一直都是线性正常的，并且Angle的值特别接近Angle_ax的值
问题：1，我快速改变板子倾角时Angle的变化正常吗？
      2，Angle正常变化的时候是应该与Angle_ax的值相近吗？

现在情况就是，就算我是在减小倾角，只要我快速地改变，它显示的倾角都会先增大再减小，而如果我慢速改变的话，倾角就会缓慢减小而不会出现中间的角度增大


*************读取数据********************
//定义MPU6050内部地址
#define        SMPLRT_DIV                0x19        //陀螺仪采样率 典型值 0X07 125Hz
#define        CONFIG                          0x1A        //低通滤波频率 典型值 0x00
#define        GYRO_CONFIG                0x1B        //陀螺仪自检及测量范围                 典型值 0x18 不自检 2000deg/s
#define        ACCEL_CONFIG        0x1C        //加速度计自检及测量范围及高通滤波频率 典型值 0x01 不自检 2G 5Hz
#define INT_PIN_CFG     0x37
#define INT_ENABLE      0x38
#define INT_STATUS      0x3A    //只读
#define        ACCEL_XOUT_H        0x3B
#define        ACCEL_XOUT_L        0x3C
#define        ACCEL_YOUT_H        0x3D
#define        ACCEL_YOUT_L        0x3E
#define        ACCEL_ZOUT_H        0x3F
#define        ACCEL_ZOUT_L        0x40
#define        TEMP_OUT_H                0x41
#define        TEMP_OUT_L                0x42
#define        GYRO_XOUT_H    0x43
#define        GYRO_XOUT_L                0x44       
#define        GYRO_YOUT_H        0x45
#define        GYRO_YOUT_L                0x46
#define        GYRO_ZOUT_H        0x47
#define        GYRO_ZOUT_L                0x48

//读取寄存器原生数据
           
        MPU6050_Raw_Data.Accel_X = (buf[0]<<8 | buf[1]);
        MPU6050_Raw_Data.Accel_Y = (buf[2]<<8 | buf[3]);
        MPU6050_Raw_Data.Accel_Z = (buf[4]<<8 | buf[5]);
        MPU6050_Raw_Data.Temp =    (buf[6]<<8 | buf[7]);  
        MPU6050_Raw_Data.Gyro_X = (buf[8]<<8 | buf[9]);
        MPU6050_Raw_Data.Gyro_Y = (buf[10]<<8 | buf[11]);
        MPU6050_Raw_Data.Gyro_Z = (buf[12]<<8 | buf[13]);
      
      
        //将原生数据转换为实际值，计算公式跟寄存器的配置有关
        MPU6050_Real_Data.Accel_X = -(float)(MPU6050_Raw_Data.Accel_X)/8192.0;
        MPU6050_Real_Data.Accel_Y = -(float)(MPU6050_Raw_Data.Accel_Y)/8192.0;
        MPU6050_Real_Data.Accel_Z = (float)(MPU6050_Raw_Data.Accel_Z)/8192.0;
              MPU6050_Real_Data.Gyro_X=-(float)(MPU6050_Raw_Data.Gyro_X - gyroADC_X_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Y=-(float)(MPU6050_Raw_Data.Gyro_Y - gyroADC_Y_offset)/65.5;   
        MPU6050_Real_Data.Gyro_Z=(float)(MPU6050_Raw_Data.Gyro_Z - gyroADC_Z_offset)/65.5;   
    }
   


//******卡尔曼参数************
               
const float Q_angle=0.001;  
const float Q_gyro=0.003;
const float R_angle=0.5;
const float dt=0.01;                          //dt为kalman滤波器采样时间;
const char  C_0 = 1;
float Q_bias, Angle_err;
float PCt_0, PCt_1, E;
float K_0, K_1, t_0, t_1;
float Pdot[4] ={0,0,0,0};
float PP[2][2] = { { 1, 0 },{ 0, 1 } };

/*****************卡尔曼滤波**************************************************/
void Kalman_Filter(float Accel,float Gyro)               
{
        Angle+=(Gyro - Q_bias) * dt; //先验估计
       
        Pdot[0]=Q_angle - PP[0][1] - PP[1][0]; // Pk-先验估计误差协方差的微分

        Pdot[1]= -PP[1][1];
        Pdot[2]= -PP[1][1];
        Pdot[3]=Q_gyro;
       
        PP[0][0] += Pdot[0] * dt;   // Pk-先验估计误差协方差微分的积分
        PP[0][1] += Pdot[1] * dt;   // =先验估计误差协方差
        PP[1][0] += Pdot[2] * dt;
        PP[1][1] += Pdot[3] * dt;
               
        Angle_err = Accel - Angle;        //zk-先验估计
       
        PCt_0 = C_0 * PP[0][0];
        PCt_1 = C_0 * PP[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 * PP[0][1];

        PP[0][0] -= K_0 * t_0;                 //后验估计误差协方差
        PP[0][1] -= K_0 * t_1;
        PP[1][0] -= K_1 * t_0;
        PP[1][1] -= K_1 * t_1;
               
        Angle        += K_0 * Angle_err;         //后验估计
        Q_bias        += K_1 * Angle_err;         //后验估计
        Gyro_y   = Gyro - Q_bias;         //输出值(后验估计)的微分=角速度

}

******************倾角计算*****************
void Angle_Calculate(void)
{

/****************************加速度****************************************/
       
        Accel_x  =  MPU6050_Real_Data.Accel_X;          //读取X轴加速度
        Angle_ax = Accel_x*1.2*180/3.14;     //弧度转换为度

/****************************角速度****************************************/
       
         Gyro_y = MPU6050_Real_Data.Gyro_Y;             
时间dt，所以此处不用积分
/***************************卡尔曼融合*************************************/
        Kalman_Filter(Angle_ax,Gyro_y);       //卡尔曼滤波计算倾角
       

星空旅行者

谢谢分享

心语心愿

果然很好的一个帖子，感谢分享

黄阿厉

谢谢分享

LuXing

mark、、、、

feng爸爸

谢谢分享

奔跑的蜗牛wk

你好，我这用了卡尔曼滤波后会滞后。当突然转移一个角度后，它的波形会向上一下，然后在向下，导致滞后了。请问怎么解决呢

丶来日方长丶

专门注册了账号来感谢楼主，刚好需要的时候看到了，帮助太大了

WangSing

请问楼主，6050不是有内置的dmp吗，经过它处理后得到姿态值（pitch roll yaw）之后 ，还需要卡尔曼滤波吗？