智能车制作

标题: 卡尔曼角度波型求指导谢谢 [打印本页]

作者: Eternalve 时间: 2013-10-4 17:49
标题: 卡尔曼角度波型求指导谢谢

蓝色的是加速度计算出的角度
红色是卡尔曼滤出来的
但是红色的那个尖角一直都消不掉
也不知道什么样的波形算可以使用
希望知道的学长给说说谢谢啦
[attach]50976[/attach]

作者: glacier0 时间: 2013-10-4 19:36
这卡尔曼搞的跟没有有啥区别

作者: rubick 时间: 2013-10-4 19:37
你这明显有问题啊，滤波后的为什么尖峰比滤波前还要大，是不是哪里写错了。

作者: Eternalve 时间: 2013-10-4 22:22
本帖最后由 Eternalve 于 2013-10-4 22:31 编辑

还想问下原始数据处理有什么比较好的方法吗我是加速度计的两个轴算一个角度
陀螺仪读出角加速度

现在的一个数据
紫色的是陀螺仪的数据
[attach]50978[/attach]

感觉还是很粗糙
没有官方教程和大家的那种光滑的感觉

作者: Eternalve 时间: 2013-10-4 22:24

rubick 发表于 2013-10-4 19:37
你这明显有问题啊，滤波后的为什么尖峰比滤波前还要大，是不是哪里写错了。

额晚上找出问题了
陀螺仪极性反了
现在调了一下有一个大范围的平衡
谢谢

一直都没注意这个问题调多了才慢慢反应过来
还是应该吧加速度计和陀螺仪的数据都用曲线画出来
然后把卡尔曼的参数向想要的的方向调整

作者: Eternalve 时间: 2013-10-4 22:34

glacier0 发表于 2013-10-4 19:36
这卡尔曼搞的跟没有有啥区别

学长请帮忙看下这个图
感觉很粗糙是我原始数据处理的不好还是什么问题呀
[attach]50979[/attach]

作者: glacier0 时间: 2013-10-5 19:51
你确定你这是卡尔曼滤波？
连代码都不愿意传上来谁知道你这是啥问题

作者: Eternalve 时间: 2013-10-5 20:35

glacier0 发表于 2013-10-5 19:51
你确定你这是卡尔曼滤波？
连代码都不愿意传上来谁知道你这是啥问题

额没发过贴呀=。。=、

谢谢啦
用的是这种输入角度（0~180）和角加速度

float kf_again(float accelerometer_angle,float gyroscope_rate)
{
Q = 1,R = 6;

NowData = RealData + gyroscope_rate*0.005;
      //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);
      //2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R));
      //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
RealData = NowData + Kg*(accelerometer_angle - NowData);
      //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);
      //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)

return RealData;
}

作者: ちl小丑. 时间: 2013-10-7 23:33
NowData = RealData + gyroscope_rate*0.005; gyroscope_rate前面好像还要乘一个系统参数B可以调的，我之前也是有那个尖角，然后把B调成互补滤波里面的那个陀螺仪的比例系数就解决了
后面的图好像也是控制量没给够，所以导致跟踪缓慢了
看你第一张图好像是对加速度计的信任程度太大了.我的Q和R都差好几个数量级...
不知道说的对不对
现在抖动频率低的时候能跟踪的很好，抖动频率高了就会偏移不知道为啥，还望调出来以后求教！！！

作者: Eternalve 时间: 2013-10-11 08:08
本帖最后由 Eternalve 于 2013-10-11 08:12 编辑

ちl小丑. 发表于 2013-10-7 23:33
NowData = RealData + gyroscope_rate*0.005; gyroscope_rate前面好像还要乘一个系统参数B可以调的，我 ...

谢谢回复

这几天重新弄了一下滤波之前的原始数据

给加速度计加了一些限制  比如x轴受全部重力（静态）时  读回来的值是4100
就if（X_axis>4100）X_axis=4100;
完全不受重力也这样限制一下
因为看数据的时候发现有一些太大的值  已经超过1g了

我之前的输出角度是0~180
后面直接用0~4096来衡量
我想这样加速度计  和陀螺仪的数据处理以后量纲可能不一样
乘以一个系数把它们调到合理范围比较好
这个可能和系统参数B是一个意思  不知道我这样理解的对不对

目前波形还好
Q=1，R=100这样
我的也是高频有延迟
不知道小车的系统允不允许这样一个滞后
类似的算法是不是都有滞后呀

在调pid的时候发现
人不扶着车的话  就算给较大的P 也会直接向一边倒下
可能是我们的机械结构有问题
所以在弄机械结构

再谢谢回复
加油^_^

波形还是一般般
但是比之前好了些
没有微调
蓝色acc 紫色gyro 红色kalman

作者: ccs5433 时间: 2013-12-8 23:23
用的什么上位机可以看波形啊？谢谢～

作者: 为丞而琳 时间: 2013-12-9 00:01
你陀螺仪的波形是不对的啊

作者: Eternalve 时间: 2013-12-9 20:49

ccs5433 发表于 2013-12-8 23:23
用的什么上位机可以看波形啊？谢谢～

我用的是 Serial_Digital_Scope V2串口猎人也可以

作者: 李辉427527731 时间: 2014-1-23 19:38

ちl小丑. 发表于 2013-10-7 23:33
NowData = RealData + gyroscope_rate*0.005; gyroscope_rate前面好像还要乘一个系统参数B可以调的，我 ...

麻烦看看我这个波形尖角是怎么回事

void AngleCalculate(void) // 角度计算
{
  float fDeltaValue;                                                                // GRAVITY_ANGLE_RATIO
  g_fGravityAngle=(float)0-0.216*(VOLTAGE_GRAVITY-GRAVITY_ZERO);
  g_fGyroscopeAngleSpeed=(float)0.02*(VOLTAGE_GYRO-GYRO_ZERO); //陀螺仪比例 0.055 GYROSCOPE_ANGLE_RATIO    GYRO_ZERO=1363 180/(1770-938)
  g_fCarAngle = g_fGyroscopeAngleIntegral+300;
  fDeltaValue =(g_fGravityAngle - g_fCarAngle)/3;//GRAVITY_ADJUST_TIME_CONSTANT; //Tg=3
  g_fGyroscopeAngleIntegral+=0.15*(g_fGyroscopeAngleSpeed+fDeltaValue);//GYROSCOPE_ANGLE_SIGMA_FREQUENCY;


OutData[1]=g_fGyroscopeAngleSpeed;
  OutData[2]=g_fGravityAngle;

[img]file:///E:/qq/834465537/Image/AT$R0V6%60L%60U4)]U48T[19CL.jpg[/img]

  OutPut_Data();

作者: 李辉427527731 时间: 2014-1-23 19:40

ちl小丑. 发表于 2013-10-7 23:33
NowData = RealData + gyroscope_rate*0.005; gyroscope_rate前面好像还要乘一个系统参数B可以调的，我 ...

[attach]55169[/attach]
就这个波形

作者: Eternalve 时间: 2014-1-25 00:26
本帖最后由 Eternalve 于 2014-1-25 00:29 编辑

1

作者: Eternalve 时间: 2014-1-25 00:26
本帖最后由 Eternalve 于 2014-1-25 00:32 编辑

李辉427527731 发表于 2014-1-23 19:40
就这个波形

蓝色的是加速度计的波形吗
加速度计动态都有过冲
黄色的已经滤的不错了呀

你的值在纵轴负半轴
我的范围在正半轴
所以过冲的地方不太一样吧

我觉得还行

最好再把陀螺仪的波形也显示出来
这样便于观察理解
一开始我也没弄出来看同学提醒以后才加上

不好意思不请自答请无视我看错回复了
我之前是陀螺仪正负号弄反了

作者: yebuyu 时间: 2014-1-25 10:01

作者: 李辉427527731 时间: 2014-1-26 18:28

Eternalve 发表于 2014-1-25 00:26
蓝色的是加速度计的波形吗
加速度计动态都有过冲
黄色的已经滤的不错了呀

黄色(融合的)的尖角怎么消除？

作者: Eternalve 时间: 2014-1-26 22:07

李辉427527731 发表于 2014-1-26 18:28
黄色(融合的)的尖角怎么消除？

[attach]55194[/attach]
这个是我校赛跑下来以后读出来的变化曲线

参数再调大一点吧
几条曲线都弄出来看看

我的理解是
从曲线上看
卡尔曼滤波就是 用陀螺仪的尖角和加速度计的过冲对冲 最后得出平滑的曲线
滞后一点没关系
前期看看曲线极性对了就好了
最后哪个参数合适不是人眼看曲线看出来的
还要放到车上去用
看看抖不抖
慢慢试参数调整

作者: Eternalve 时间: 2014-1-26 22:12

李辉427527731 发表于 2014-1-26 18:28
黄色(融合的)的尖角怎么消除？

如果参数调大还是一直消不掉
可以看一下角速度的值给反了吗

作者: 林JJ 时间: 2014-2-19 17:24
你好，我是一个菜鸟，现在也是在弄那个加速度器陀螺仪，我想问一下我用IIC读出他们给的数值后就可以直接进行卡尔曼了吗？还是说需要一些角度变化处理什么的？

作者: Eternalve 时间: 2014-2-19 17:41

林JJ 发表于 2014-2-19 17:24
你好，我是一个菜鸟，现在也是在弄那个加速度器陀螺仪，我想问一下我用IIC读出他们给的数值后就可以直接进行 ...

[attach]55956[/attach]
要经过一些处理的
还是把芯片手册过一遍吧
网上的参考一下

作者: 笑而不语 时间: 2014-2-19 22:26
哥们，我最近也在看这个，能不能给点经验啊，刚看完官方直立车的pdf，大概知道是个怎么样过程，我们是不是必须先用平滑滤波或者中值滤波把陀螺仪和加速度计的值采集出来，然后陀螺仪的值换成角速度，加速度计的值计算角度，官方上好像一个z轴就够了，我看你是通过 arcsin ( y /sqrt（x^2+y^2）) 这种形式来求角度的，还有你用哪个卡尔曼滤波是不是整合陀螺仪角速度和加速度的算出角度最后合成最终的我们需要的角度？没有用官网那个PD方案通过加速度计和陀螺仪角度偏差来求角度？可不可以给点建议哈

。还有你运算用的的反三角函数和平方根函数是是自己写的还是用的c语言的库？

作者: Eternalve 时间: 2014-2-20 13:16

笑而不语发表于 2014-2-19 22:26
哥们，我最近也在看这个，能不能给点经验啊，刚看完官方直立车的pdf，大概知道是个怎么样过程，我们是不是必 ...

我是直接：#include <math.h>
算角度是：arctan(按说明处理过的z轴数据 / 按说明处理过的x轴数据)；
反正切算角度
（这里得到的单位是rad 要自己转化成度或者其他数值来衡量大小）
具体哪两个轴的比值要看传感器怎么摆放对照手册看轴的方向来弄

作者: 笑而不语 时间: 2014-2-20 15:07

Eternalve 发表于 2014-2-20 13:16
我是直接：#include
算角度是：arctan(按说明处理过的z轴数据 / 按说明处理过的x轴数据)；
反正切算 ...

那你陀螺仪的值用在哪里了？是最后给计算的加速度值卡尔曼滤波时候用上的吗？

作者: 笑而不语 时间: 2014-2-20 17:08

Eternalve 发表于 2014-2-20 13:16
我是直接：#include
算角度是：arctan(按说明处理过的z轴数据 / 按说明处理过的x轴数据)；
反正切算 ...

对了，你直接用库函数？库函数都是浮点型的，你的单片机支持？反正我的单片机不支持

作者: 林JJ 时间: 2014-2-21 14:50
我想问一下，我用的是数字加速度器，放着不动的时候读出来的值会在5的范围内变动正常码？变换好角度之后就可以进行直接使用了吗？谢谢

作者: Eternalve 时间: 2014-2-21 21:50

笑而不语发表于 2014-2-20 17:08
对了，你直接用库函数？库函数都是浮点型的，你的单片机支持？反正我的单片机不支持

这个不清楚也用的是K60 STM32也可以呀都不用实际添加头文件只是在程序里面#include <math.h>

作者: Eternalve 时间: 2014-2-21 21:52

笑而不语发表于 2014-2-20 15:07
那你陀螺仪的值用在哪里了？是最后给计算的加速度值卡尔曼滤波时候用上的吗？

嗯就在这里用了NowData = RealData - use_gyro*0.005;

作者: Eternalve 时间: 2014-2-21 21:54

林JJ 发表于 2014-2-21 14:50
我想问一下，我用的是数字加速度器，放着不动的时候读出来的值会在5的范围内变动正常码？变换好角度之后就可 ...

最大值最小值是多少呢
5大概占百分之多少呀
如果 5/几千那应该可以吧

前期数据处理要仔细一点额

作者: 林JJ 时间: 2014-2-22 10:02

Eternalve 发表于 2014-2-21 21:54
最大值最小值是多少呢
5大概占百分之多少呀
如果 5/几千那应该可以吧

读出来最大最小是-128到127，比如放水平的话它就会在-1到3的范围波动，我还没有进行角度换算

作者: Eternalve 时间: 2014-2-22 18:25

林JJ 发表于 2014-2-22 10:02
读出来最大最小是-128到127，比如放水平的话它就会在-1到3的范围波动，我还没有进行角度换算

应该还行吧毕竟是数字的也改不了太多东西是几位精度的呀可以多读几位哦 8位感觉有点少 12位、14位比较好
可以直接用愿意的话也可以好好看看手册初始化的时候设置一下寄存器

作者: 林JJ 时间: 2014-2-22 18:55

Eternalve 发表于 2014-2-22 18:25
应该还行吧毕竟是数字的也改不了太多东西是几位精度的呀可以多读几位哦 8位感觉有点少 12位、 ...

谢谢

作者: 鸟人 时间: 2014-2-23 16:55
请问楼主你的卡尔曼得出来的值变化范围多大我读出来的值才有一点几是不时正常值？还有调的值是不是主要调R ??

作者: Eternalve 时间: 2014-2-23 16:58

鸟人发表于 2014-2-23 16:55
请问楼主你的卡尔曼得出来的值变化范围多大我读出来的值才有一点几是不时正常值？还有调的值是不是主要 ...

我是加速度计arctan算的角度出来的一点几是弧度（rad）  要自己转化成角度  或者其他度量
前面的那个卡尔曼程序  主要就是调Q R 的相对大小
可以固定Q=1；  调R
也可以固定R=1；调Q

作者: 鸟人 时间: 2014-2-24 09:54
那你的X轴的加速度零点值是陀螺仪水平时还是小车直立时的输出值？角速度和Z轴零点值是直立时输出值？？

作者: dingyongbin 时间: 2014-2-24 09:57
vccv

作者: Eternalve 时间: 2014-2-24 12:59

鸟人发表于 2014-2-24 09:54
那你的X轴的加速度零点值是陀螺仪水平时还是小车直立时的输出值？角速度和Z轴零点值是直立时输出值？？

我的“零点”就是小车处于平衡位置不动时加速度计的计算值
不动的时候陀螺仪处于零点有绕陀螺仪旋转的运动是采集到的陀螺仪ad电压值有变化
不知道说到点上了没有

作者: 鸟人 时间: 2014-2-24 20:02
我的三个零点值也是平衡时的输出值我滤出来波如下
黄色是加速度算出来的
红色是角速度  紫色是卡尔曼算出来得角度值
感觉滤出来的波形粗糙而且还有尖峰
你帮我看看是哪里出问题

程序是这个
Gyro  = g_ADvalue[0];
Acc_x = g_ADvalue[2];
Acc_z = g_ADvalue[1];
OutData[1]=Acc_z;

if(Gyro > 4090) Gyro += 1000;
else
if(Gyro > 4084) Gyro += 500;

if(Gyro < 55) Gyro -= 1000;
else
if(Gyro < 60) Gyro -= 500;

Acc_x = Acc_x - 1389.6;
Acc_z = Acc_z - 2450.0;
Gyro  = Gyro  - 2860.4;
Gyro_Data = Gyro;

OutData[0] = Gyro_Data;
accelerometer_angle =atan2f(Acc_z,Acc_x);
OutData[2] = accelerometer_angle*1000;

gyroscope_rate = Gyro*0.0023;       //参考电压3.3v 12位ADC 放大9.1倍 enc-03 0.67mv/deg./sec.

                                       //(3300/4096)/(0.67*9.1)*(3.14/180) =  0.0023

NowData = RealData + gyroscope_rate*0.04;                //1.预估计 X(k|k-1) = A(k,k-1)*X(k-1|k-1) + B(k)*u(k)
NowData_P = sqrt(Q*Q+RealData_P*RealData_P);             //2.计算预估计协方差矩阵 P(k|k-1) = A(k,k-1)*P(k-1|k-1)*A(k,k-1)'+Q(k)
Kg = sqrt(NowData_P*NowData_P/(NowData_P*NowData_P+R*R)); //3.计算卡尔曼增益矩阵 K(k) = P(k|k-1)*H(k)' / (H(k)*P(k|k-1)*H(k)' + R(k))
RealData = NowData + Kg*(accelerometer_angle - NowData);  //4.更新估计 X(k|k) = X(k|k-1)+K(k)*(Z(k)-H(k)*X(k|k-1))
RealData_P = sqrt((1-Kg)*NowData_P*NowData_P);          //5.计算更新后估计协防差矩阵 P(k|k) =（I-K(k)*H(k)）*P(k|k-1)

QingJiao =  RealData;

OutData[3] = QingJiao*1000;//*180/3.141592653;
// OutPut_Data();

作者: Eternalve 时间: 2014-2-25 23:31

鸟人发表于 2014-2-24 20:02
我的三个零点值也是平衡时的输出值我滤出来波如下
黄色是加速度算出来的
红色是角速度紫色是卡尔曼算出 ...

1.这里不太懂  但是看见一些帖子里面出过
if(Gyro > 4090) Gyro += 1000;
else
if(Gyro > 4084) Gyro += 500;

if(Gyro < 55) Gyro -= 1000;
else
if(Gyro < 60) Gyro -= 500;

2.这里
  NowData = RealData + gyroscope_rate*0.04;
  （1）0.04 就是40ms算一次 40ms有点长了
  （2）我在“+”  符号上出过错  有段时间弄反了
      现象是  滤波后的过冲比不滤的还大看你的好像没有

3.原始数据可以适当滤波高通低通什么的要根据器件实际情况来

4.Q  R值
估计还不够（我觉得是这个）滤出的波形有滞后也能用  满足要求就好

5.可以上pid调着看看  pid本身很好用    出不来效果就是前面的数据没处理好观察现象调Q R 和原始数据滤波

作者: 忍者1727 时间: 2014-3-29 18:22

Eternalve 发表于 2013-10-4 22:24
额晚上找出问题了
陀螺仪极性反了
现在调了一下有一个大范围的平衡

陀螺仪极性怎么调的？

作者: Eternalve 时间: 2014-3-29 20:31

忍者1727 发表于 2014-3-29 18:22
陀螺仪极性怎么调的？

和硬件的摆放有关系啦
极性就是式子里陀螺仪数值之前给个正负号
自己根据需求决定

作者: 超人不会飞 时间: 2015-12-14 02:16
你好，现在做直立车，卡在一个问题上了，你的加速度计算角度是只用了一路信号还是两路信号求反正切?

作者: 阿达的礼物 时间: 2016-11-26 22:32
很有帮助

欢迎光临智能车制作 (http://111.231.132.190/)