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

东东bh

楼主，你的卡尔曼滤波代码中选择H为[1,0]，这样应该是不好的，最终你的Q_bias没有得到真正意义上的迭代，你想想，你先验估计得到的X(k)里的Q_bias最终在后验估计中永远乘以零，这导致下一次的估计和上一次输出的Q_bias完全没有关系，事实上Z=H*X（k）,H的选取应该根据比例来的，也就是由实际域到测量域的转换，这里通常应该选择H为[1，1]，本人拙见，求批判~

东东bh

额 楼上回复的有点有点问题，H确实应该选择[1，0]，但依然存在零飘更新错误问题，实际上，因为零飘和角度线性无关是的，所以必须要有两个测量值才能正确预估，既H是2*2的，Z是2*1的，否则零飘没有基准，会出现更新错误，实际上更新的零偏是无意义的，是不是这样呢，自己琢磨的

2013的弹子球

回复【52楼】东东bh:
---------------------------------
那个卡尔曼增益最后算出来其实是个定值的，其实就是对误差的积分，类比于PID控制里面的i,可以消除静差。

东东bh

回复【53楼】2013的弹子球:
---------------------------------
如果Q,R是常数，K会收敛，也即P也会收敛到常量，这是卡尔曼的必然，后验更新公式X（k）= x(k-1) +K*(Z-H*X(k-1)),这里Z-H*X(k-1)实际上是Accel减去上一次的后验估计结果，可以认为是角度的误差，K为常数，对于角度来说最终是对角度误差的积分，对于Q_bias来说意思是误差是由零偏引起的，所以对Q_bias修正误差的K倍

扬帆起航

平衡小车单纯平衡，只调P 不调I和D可以么？

2013的弹子球

回复【55楼】扬帆起航:
---------------------------------
以实践为准。。。。。我调的时候只有P是能平衡的。。。最好加上D

2013的弹子球

回复【32楼】yw455801125:
---------------------------------
。。编码器模式。。应该直接就出了吧，正反转通过数的增加减少判断，速度通过差值判断

扬帆起航

回复【56楼】2013的弹子球:
---------------------------------
那P的范围是怎么确定的？从0开始吗？

雪梦my

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

  [0][0] += dot[0] * dt;
  [0][1] += dot[1] * dt;
  [1][0] += Pdot[2] * dt;
  P[1][1] += Pdot[3] * dt;
这个是如何根据第二个卡尔曼的房产写出来的？回复【2楼】2013的弹子球:
---------------------------------

2013的弹子球

回复【59楼】雪梦my:
---------------------------------
文中有说哦~

雪梦my

回复【60楼】2013的弹子球:
---------------------------------
我下载下来看了的，我觉得有点问题啊，就是
Pdot[0]  =Q_angle - [0][1] - [1][0];
P[0][0] += dot[0] * dt;那么就是
P[0][0] = [0][0]+（Q_angle - [0][1] - [1][0]） * dt  这样这个Q_angle 后面会有*dt这个运算，和文章中的a-c*dt-b*dt+D(Angle)有出入啊，能不能解释一下啊，大神？

2013的弹子球

回复【61楼】雪梦my:
-----------------------------
你说的对。。。你看的好仔细。。由于dt也是常数。。。直接给出了，就不算了。。。

雪梦my

回复【62楼】2013的弹子球:
---------------------------------
我还是没明白，能不能qq交流一下，把你qq发给我，我们好好交流一下，可以吗？

2013的弹子球

回复【63楼】雪梦my:
---------------------------------
Q=Q_angle*dt。。

雪梦my

回复【64楼】2013的弹子球:
---------------------------------
如果我想修改这个响应速度，修改程序中的 Q_angle = 0.002, Q_gyro = 0.002, R_angle = 0.5, dt = 0.01，可以做到吗？

2013的弹子球

回复【65楼】雪梦my:
---------------------------------
可以，我写的有问题，这几个值得取值非常重要，如果这几个值不能准确获得就不要使用。

雪梦my

回复【66楼】2013的弹子球:
---------------------------------
我昨天一直在看你发的那个程序，我总是有点不明白，就是那个A*P（K|k-1）A' +Q  这个矩阵的运算 应该都是加，怎么还会出来减呢？

雪梦my

回复【67楼】雪梦my:
---------------------------------
可以qq联系不  我的qq  595144878

雪梦my

回复【66楼】2013的弹子球:
---------------------------------
可以qq联系不  我的qq 3045218020

雪梦my

回复【64楼】2013的弹子球:
---------------------------------
这个Q是一个角度值，Q_angle*dt 就是在dt时间内的一个积分的角度值，是这样的吗？

2013的弹子球

回复【70楼】雪梦my:
---------------------------------
。你慢慢看吧，相关的书，这么我也不好回答你。

雪梦my

回复【71楼】2013的弹子球:
---------------------------------
恩 我今天看的差不多了。谢谢啊...

MJM

需要这样的资料。谢谢楼主这贡献！

嗨儿

挺好的资料，谢谢楼主！

979653421@qq.co

回复【61楼】雪梦my:
---------------------------------
我也发现多了一个dt，层主搞明白了吗

2013的弹子球

回复【75楼】979653421@qq.com:
---------------------------------
...由于dt是常数，Q_angle*dt 直接作为一个整体了。。。，32的浮点乘法运算差不多要1ms很慢，我靠这种问题就不要问了，一想想不就明白了

yangtkzc

回复【54楼】东东bh:
---------------------------------
回复【54楼】东东bh:
---------------------------------
Q_bias是啥意思？

yangtkzc

解释的好模糊，该解释的都没有解释

2013的弹子球

回复【78楼】yangtkzc:
---------------------------------
你如果相关的看都没看过，我当然解释的不清楚，如果你看过一点肯定知道那是什么意思，我不能全部写下来。

雪梦my

回复【75楼】979653421@qq.com:
---------------------------------
差不多吧，你哪里不明白？

魂淡

 Absolute_Speed = (Absolute_Cont2 + Absolute_Cont3) *0.5;

   为什么计算速度的时候    
像这样 把编码器的脉冲数加起来 乘以0.5就是 速度的值

2013的弹子球

回复【81楼】魂淡:
---------------------------------
这个得看你的编码器一圈是多少个脉冲

persevere

回复【82楼】2013的弹子球:
---------------------------------
 if(Position<-6000) osition = -6000; 
 if(Position> 6000) osition =  6000;
请问下6000怎么来的？

weiqian559

回复【2楼】2013的弹子球：
你好，我最近也在做加速度计与陀螺仪的卡曼滤波融合，但是我的Q和R选取遇到了很大问题，要么跟踪速度慢滤波效果好，要么跟踪速度快但是滤波效果差，你能说一下QR怎么取比较合适么？谢谢

persevere

回复【26楼】yw455801125:
---------------------------------
请问下你弄懂这些代码了吗？

2013的弹子球

回复【84楼】weiqian559:
---------------------------------
看Q和R的定义。

weiqian559

回复【86楼】2013的弹子球:
---------------------------------
那么Q和R是通过实验手段确定的么？

2013的弹子球

回复【87楼】weiqian559:
---------------------------------
可以根据定义实验手段测，但是效果不一定好，方差呗，很简单的。手算都行

weiqian559

回复【88楼】2013的弹子球:
---------------------------------
就是根据陀螺仪和加速度计的输出来算么？

电子音符

楼主，为什么我的6050原来测得特别准，现在就不准了（主要X轴数据），角度死在90度，加速度为0，并且，把6050去掉也不变为0.....这是为什么？

电子音符

//******************6050初始化函数*******************************
void Init_MPU6050(void)
{
GPIO_6050_I2C_Config();                  //初始化6050的I2C接口
if(Single_Read(MPU6050_Addr,WHO_AM_I)==0x68)
 {

Single_Write(MPU6050_Addr,PWR_MGMT_1, 0x80); //.decive 复位reset 解除休眠状态,关闭温度传感器
delay_ms(10);
Single_Write(MPU6050_Addr,PWR_MGMT_1, 0x00);  //复位，温度开，NO CYCLE
delay_ms(10);
Single_Write(MPU6050_Addr,SMPLRT_DIV, 0x07); //采样率1KHz/(6+1)7ms转换一次
delay_ms(10);
Single_Write(MPU6050_Addr,CONFIG, 0x06); //低通滤波器的值：0x01=188Hz 0x02=98Hz 0x03=42Hz 0x04=20Hz 0x05=10Hz 0x06=5Hz 
delay_ms(10);
Single_Write(MPU6050_Addr,GYRO_CONFIG, 0x10);   //XYZ均不自测，：0x18(不自检，范围1000deg/s)
    delay_ms(10);
Single_Write(MPU6050_Addr,ACCEL_CONFIG, 0x08); //加速计自检、测量范围及高通滤波频率，典型值：0x01(不自检，+-4G，5Hz)
    delay_ms(10);
  }
//****************读6050数据（滤波前的角度角速度）*****************************
 Data_6050_str Data_6050;  //6050数据结构定义结构体

 void  READ_MPU6050(void)
{  
unsigned char BUF[12];  //接收数据缓存区 

//**********角速度传感器测量**************
// BUF[0]=Single_Read(MPU6050_Addr,GYRO_XOUT_L); 
// BUF[1]=Single_Read(MPU6050_Addr,GYRO_XOUT_H);
// Data_6050.GX=((BUF[1]<<8)+BUF[0])+GRY_offset;
//  Data_6050.Gryo_X =((float)Data_6050.GX)*(0.030517);   //滤波前X的角速度
    /*-----------------------------------------------------------*/
BUF[2]=Single_Read(MPU6050_Addr,GYRO_YOUT_L);
BUF[3]=Single_Read(MPU6050_Addr,GYRO_YOUT_H);
Data_6050.GY=((BUF[3]<<8)+BUF[2])+GRY_offset;
    Data_6050.Gryo_Y =((float)Data_6050.GY)*(0.030517);  //滤波前Y的角速度
/*-----------------------------------------------------------*/
// BUF[4]=Single_Read(MPU6050_Addr,GYRO_ZOUT_L);
// BUF[5]=Single_Read(MPU6050_Addr,GYRO_ZOUT_H);
// Data_6050.GZ=((BUF[5]<<8)+BUF[4])+GRY_offset;
//  Data_6050.Gryo_Z =((float)Data_6050.GZ)*(0.030517);  //滤波前Z的角速度

//************加速度传感器测量*************
BUF[6]=Single_Read(MPU6050_Addr,ACCEL_XOUT_L); 
BUF[7]=Single_Read(MPU6050_Addr,ACCEL_XOUT_H);
Data_6050.AX= (BUF[7]<<8)+BUF[6];  //滤波前x的加速度

/*-----------------------------------------------------------*/
// BUF[8]=Single_Read(MPU6050_Addr,ACCEL_YOUT_L); 
// BUF[9]=Single_Read(MPU6050_Addr,ACCEL_YOUT_H);
// Data_6050.AY=    (BUF[9]<<8)+BUF[8];  //滤波前y的加速度
 
  /*-----------------------------------------------------------*/
BUF[10]=Single_Read(MPU6050_Addr,ACCEL_ZOUT_L); 
BUF[11]=Single_Read(MPU6050_Addr,ACCEL_ZOUT_H);
Data_6050.AZ= (BUF[11]<<8)+BUF[10];  //滤波前z的加速度

//    MPU6050_Adj();  //修正参数
 
 //*******************计算X轴倾斜角**************************
  if(Data_6050.AX>0)
{
Data_6050.Acc_X = atan2((float)Data_6050.AX,(float)Data_6050.AZ)*(180/3.14159265);     //反正切计算
}
else
{
   Data_6050.Acc_X = atan2((float)Data_6050.AZ,(float)Data_6050.AX)*(180/3.14159265)-90; //反正切计算
   Data_6050.Acc_X = -Data_6050.Acc_X;
}
}

2013的弹子球

回复【90楼】电子音符:
---------------------------------
不是很理解你的意思，90万向轴锁？

魂淡

平衡车怎么读取编码器的值  我的电机 编码器 转一圈有13340个脉冲   一直外部中断  感觉会 影响程序运行
而且不知道是不是 是不是我的编码器的值有问题。这是我测的数据

1、编码器的数值采集遇到了问题   尚不知是什么问题
PA.3是A相   外部中断PA.5是B相
PA.1是A相   外部中断PA.2是B相
当左轮接PA.3  A.5正转4s，编码器数值5091
当右轮接PA.2  A.1正转4s，编码器数值-220358
当左轮接PA.3  A.5倒转4s，编码器数值-218184
当右轮接PA.2  A.1倒转4s，编码器数值128242
当左轮接PA.2  A.1正转4s，编码器数值5931
当右轮接PA.3  A.5正转4s，编码器数值-220714
当左轮接PA.2  A.1倒转4s，编码器数值-217557
当右轮接PA.3  A.5倒转4s，编码器数值114909
我刚开始调PD  ， 车晃得很厉害  希望楼主指导

2013的弹子球

回复【93楼】魂淡:
---------------------------------
中断不会啊，先调P，耐心点

2013的弹子球

回复【93楼】魂淡:
---------------------------------
定时器中断不会影响程序吧。。。

魂淡

回复【95楼】2013的弹子球:
---------------------------------
恩  谢谢你的回答 。32的性能如此优秀 应该是不会。
那你觉得我的编码器有可能出问题了吗 我把编码器的值放在92楼了。
其次我想请教 一开始P的值设为多大好  我的占空比 满值是256  网上又说设占空比的10%  也有说60%的 我感觉都不准

2013的弹子球

回复【96楼】魂淡:
---------------------------------
我习惯是先设定一个初值，正反转加减这样，然后清零，频率高些，50hz完全没问题，还有占空比的值可以适量调大一点，然后控制的pwm频率给到2到10khz，调节p的时候，id为0，光一个p能够摇摇晃晃的站起来，然后再调D，先不用加速度闭环，基本能站稳后加速度闭环，注意你的6050是有安装误差的，找到车的大概平衡位置，然后6050求得的值中减去这个值。

魂淡

回复【97楼】2013的弹子球:
---------------------------------
谢谢指点   我现在只用P小车也几乎能站着不动   现在调D  想让小车有较强的抗干扰  但是很失败   加了D  反而效果不好

电子音符

我昨天回复的帖子怎么没有了...

电子音符

请问楼主，我的程序控制算法是：小车平衡位置向左偏时，小车向左加速，向右也一样。可是实际情况是，当小车平衡向左偏倒时，居然电机会向右有个加速！之后才是正常的向左加速。向右也一样，，不知道为什么，，依据这样，我就直接把控制电机的两路PWM互换，让其小角度调整。我的PWM满度500目前单调P值17，都没站起来，，