智能车制作

标题: 卡尔曼滤波后的图请老师指点 [打印本页]

作者: silence1221 时间: 2012-5-12 13:11
标题: 卡尔曼滤波后的图请老师指点
卡尔曼参数Q_angle=0.001, Q_gyro=0.003, R_angle=0.067, dt=0.005;
现在车子能直立，但是很不稳定先是直立，然后来回抖动，越摆越大，然后倒下。。。。现在怎么调这个波形呢
[attach]24424[/attach]

作者: silence1221 时间: 2012-5-12 13:12
波形是在桌面上用手来回摆动车子测得的

作者: 吉他手 时间: 2012-5-12 15:33
（1）首先你需要清楚，滤波后的角度信号需要逼近加速度计中没有带有运动噪声的角度；从你的波形显示的滤波后的角度与加速度信号大体上呈现反相特点；逻辑上应该是同相；这一点通过调整后面的角度控制的PI参数中的极性可以修改；
（2）之所以出现震荡，另外一个问题就是控制的“极性”。角度的P，I都应该是负反馈，如果某一项极性反了，就会造成控制输出发散，车模震荡倒下。请检查一下。
（3）如果你对于卡尔曼滤波不是太清楚，可以先实现参考方案中的方法，它简单，物理概念清楚。获得调试的经验之后再研究卡尔曼滤波。以免在此耽搁太多的时间。毕竟后面还有更多的参数需要调整呢。
从本质上讲，论坛上所给出的卡尔曼滤波的算法和参考方案方法所得到的结果是一样的。

作者: silence1221 时间: 2012-5-12 19:11

吉他手发表于 2012-5-12 15:33
（1）首先你需要清楚，滤波后的角度信号需要逼近加速度计中没有带有运动噪声的角度；从你的波形显示的滤波后 ...

谢谢老师的指点我会继续研究下的

作者: 滑民航 时间: 2012-5-13 10:24
想问下  你的卡尔曼是不是用这个
void Kalman_Filter(float angle_m,float gyro_m)                      //gyro_m:gyro_measure
{
      angle+=(gyro_m-q_bias) * dt;//先验估计

      Pdot[0]=Q_angle - P[0][1] - P[1][0];// Pk-' 先验估计误差协方差的微分
      Pdot[1]=- P[1][1];
      Pdot[2]=- P[1][1];
      Pdot[3]=Q_gyro;
      //更新协方差矩阵  其中
*
* state_update is called every dt with a biased gyro measurement
* by the user of the module.  It updates the current angle and
* rate estimate.
*
* The pitch gyro measurement should be scaled into real units, but
* does not need any bias removal.  The filter will track the bias.
*
*    A = [ 0 -1 ]
*       [ 0  0 ]
*/
*
   * Compute the derivative of the covariance matrix
   * (equation 22-1)
   * Pdot = A*P + P*A' + Q
   *
   */
      P[0][0] += Pdot[0] * dt;// Pk- 先验估计误差协方差微分的积分 = 先验估计误差协方差
      P[0][1] += Pdot[1] * dt;
      P[1][0] += Pdot[2] * dt;
      P[1][1] += Pdot[3] * dt;


      angle_err = angle_m - angle;//zk-先验估计


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

      //误差估计  算出卡尔曼滤波
      E = R_angle + C_0 * PCt_0; //R_angle为测量噪声（white gaussian noise）

   //更新卡尔曼滤波增益
      K_0 = PCt_0 / E;//Kk
      K_1 = PCt_1 / E;
      //t_0,t_1为测量系统参数，为下面的后验协方差提供。

      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;//输出值（后验估计）的微分 = 角速度
}

作者: silence1221 时间: 2012-5-13 11:29

滑民航发表于 2012-5-13 10:24
想问下你的卡尔曼是不是用这个
void Kalman_Filter(float angle_m,float gyro_m) ...

恩是的怎么了

作者: silence1221 时间: 2012-5-13 13:29
本帖最后由 silence1221 于 2012-5-13 13:31 编辑

吉他手发表于 2012-5-12 15:33
（1）首先你需要清楚，滤波后的角度信号需要逼近加速度计中没有带有运动噪声的角度；从你的波形显示的滤波后 ...

吉他手老师，昨天我又重新调了下滤波，这是现在的波形图了，你看看哪里还不对。

[attach]24467[/attach]

直接用手摆动小车测试的波形红色是加速度计黄色是滤波之后

作者: 滑民航 时间: 2012-5-13 16:29

silence1221 发表于 2012-5-13 11:29
恩是的怎么了

那个gyr-m是不是陀螺仪的测量值吗还是陀螺仪积分后的角度值

作者: silence1221 时间: 2012-5-13 17:55

滑民航发表于 2012-5-13 16:29
那个gyr-m是不是陀螺仪的测量值吗还是陀螺仪积分后的角度值

//是根据陀螺仪计算的角速度

作者: 滑民航 时间: 2012-5-13 20:05

silence1221 发表于 2012-5-13 17:55
//是根据陀螺仪计算的角速度

你们的陀螺仪过冲大吗

作者: silence1221 时间: 2012-5-14 10:11

滑民航发表于 2012-5-13 20:05
你们的陀螺仪过冲大吗

还行吧自己焊的官网的图

作者: luoy 时间: 2012-5-16 20:21
楼主你卡尔曼滤波输入角度值是加速度传感器计算的角度还是陀螺仪经过积分得到的角度

作者: yssdsz 时间: 2012-5-17 17:03
LZ用弧度计算的还是化成角度计算的？

作者: xiongcaifei 时间: 2013-1-22 17:00
楼主，你的加速度计得出来的角度是度数？还是弧度？还是经过3.14后的数值呢？？？

作者: xiongcaifei 时间: 2013-1-22 17:02

silence1221 发表于 2012-5-13 11:29
恩是的怎么了

楼主，你的加速度计得出来的角度是度数？还是弧度？还是经过3.14后的数值呢？？？

作者: huangjianwu 时间: 2013-4-17 21:26

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