极客工坊

 找回密码
 注册

QQ登录

只需一步,快速开始

查看: 188796|回复: 72

我的自平衡小车D3——滤波算法

  [复制链接]
发表于 2012-3-23 15:22:59 | 显示全部楼层 |阅读模式
先放代码
  1. #include <Wire.h>
  2. #define Acc 0x1D
  3. #define Gyr 0x69
  4. #define Mag 0x1E
  5. #define Gry_offset -13    // 陀螺仪偏移量
  6. #define Gyr_Gain 0.07     // 满量程2000dps时灵敏度(dps/digital)
  7. #define pi 3.14159

  8. float Com_angle;
  9. float y1, Com2_angle;
  10. float Klm_angle;

  11. #define Q_angle 0.01      // 角度数据置信度
  12. #define Q_omega 0.0003    // 角速度数据置信度
  13. #define R_angle 0.01      // 方差噪声
  14. float bias = 0;
  15. float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
  16. float angleG;
  17. long timer = 0;  // 采样时间
  18. void setup() {
  19.     sensor_init();        // 配置传感器
  20.     Serial.begin(19200);  // 开启串口以便监视数据
  21.     delay(1000);
  22.   }

  23. void loop() {
  24.     long o_timer = timer;                   // 上一次采样时间(ms)
  25.     float Y_Accelerometer = gDat(Acc, 1);   // 获取向前的加速度
  26.     float Z_Accelerometer = gDat(Acc, 2);   // 获取向下的加速度
  27.     float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;
  28.                                             // 根据加速度分量得到的角度(degree)
  29.     timer = millis();                       // 当前时间(ms)
  30.     float omega =  Gyr_Gain * (gDat(Gyr, 0) +  Gry_offset);
  31.     float dt = (timer - o_timer) / 1000.0;  // 微分时间(s)
  32.     angleG = angleG + omega * dt;           // 对角速度积分得到的角度(degree)
  33. // 一阶互补算法
  34.     float K;
  35.     K = 0.075;                              // 对加速度计取值的权重
  36.     float A = K / (K + dt);
  37.     Com_angle = A * (Com_angle + omega * dt) + (1-A) * angleA;
  38. // 二阶互补算法
  39.     K = 0.5;
  40.     float x1 = (angleA - Com2_angle) * K * K;
  41.     y1 = y1 + x1 * dt;
  42.     float x2 = y1 + 2 * K *(angleA - Com2_angle) + omega;
  43.     Com2_angle = Com2_angle + x2 * dt;
  44. // 卡尔曼滤波
  45.     Klm_angle += (omega - bias) * dt;          // 先验估计
  46.     P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
  47.     P_01 += -P_11 * dt;
  48.     P_10 += -P_11 * dt;
  49.     P_11 += +Q_omega * dt;                     // 先验估计误差协方差
  50.    
  51.     float K_0 = P_00 / (P_00 + R_angle);
  52.     float K_1 = P_10 / (P_00 + R_angle);
  53.    
  54.     bias += K_1 * (angleA - Klm_angle);
  55.     Klm_angle += K_0 * (angleA - Klm_angle);   // 后验估计
  56.     P_00 -= K_0 * P_00;
  57.     P_01 -= K_0 * P_01;
  58.     P_10 -= K_1 * P_00;
  59.     P_11 -= K_1 * P_01;                        // 后验估计误差协方差

  60.     Serial.print(timer);
  61.     Serial.print(",");
  62.     Serial.print(angleA, 6);
  63.     Serial.print(",");
  64.     Serial.print(angleG, 6);
  65.     Serial.print(",");
  66.     Serial.print(Com_angle, 6);
  67.     Serial.print(",");
  68.     Serial.print(Com2_angle, 6);
  69.     Serial.print(",");
  70.     Serial.print(Klm_angle, 6);
  71.     Serial.print(";");                      // 输出数据
  72.     delay(50);
  73.   }

  74. int gDat(int device, int axis) {

  75. // 读九轴姿态传感器寄存器函数
  76. // For Arduino, by 黑马
  77. // 调用参数表
  78. //   type    device      axis
  79. //                    0   1   2
  80. // ADXL345     Acc    x   y   z
  81. // L3G4200D    Gyr    x   y   z
  82. // HMC5883L    Mag    x   z   y
  83. // Example
  84. // 00 #include <Wire.h>
  85. // 01 #define Acc 0x1D;
  86. // 02 #define Gyr 0x69;
  87. // 03 #define Mag 0x1E;
  88. // 04
  89. // 05  void setup() {
  90. // 06    sensor_init();
  91. // 07    delay(1000);
  92. // 08  }
  93. // 09
  94. // 10  void loop() {
  95. // 11    int Z-Gyroscope;
  96. // 12    Z-Gyroscope = gDat(Gyr, 2);
  97. // 13    delay(50);
  98. // 14  }

  99.     int v;
  100.     byte vL, vH, address;               // 存放byte数值
  101.     if (device == Acc) address = 0x32;  // ADXL345的读数地址
  102.     if (device == Gyr) address = 0xA8;  // L3G4200D的读数地址
  103.     if (device == Mag) address = 0x03;  // HMC5883L的读数地址
  104.     address = address + axis * 2;       // 数据偏移-坐标轴
  105.     Wire.beginTransmission(device);     // 开始传输数据
  106.     Wire.send(address);                 // 发送指针
  107.     Wire.requestFrom(device, 2);        // 请求2 byte数据
  108.     while(Wire.available() < 2);        // 成功获取前等待
  109.     vL = Wire.receive();
  110.     vH = Wire.receive();                // 读取数据
  111.     Wire.endTransmission();             // 结束传输
  112.     if (device == Mag) v = (vL << 8) | vH;
  113.     else v = (vH << 8) | vL;            // 将byte数据合并为Int
  114.     return v;                           // 返回读书值
  115. }

  116. void sensor_init() {                         // 配置九轴姿态传感器
  117.     writeRegister(Acc, 0x2D, 0b00001000);    // 测量模式
  118.                             // 配置ADXL345
  119.     writeRegister(Gyr, 0x20, 0b00001111);    // 设置睡眠模式、x, y, z轴使能
  120.     writeRegister(Gyr, 0x21, 0b00000000);    // 选择高通滤波模式和高通截止频率
  121.     writeRegister(Gyr, 0x22, 0b00000000);    // 设置中断模式
  122.     writeRegister(Gyr, 0x23, 0b00110000);    // 设置量程(2000dps)、自检状态、SPI模式
  123.     writeRegister(Gyr, 0x24, 0b00000000);    // FIFO & 高通滤波
  124.                             // 配置L3G4200D(2000 deg/sec)
  125.     writeRegister(Mag, 0x02, 0x00);          // 连续测量
  126.                             // 配置HMC5883L
  127. }

  128. void writeRegister(int device, byte address, byte val) {    // 写寄存器
  129.     Wire.beginTransmission(device);
  130.     Wire.send(address);
  131.     Wire.send(val);
  132.     Wire.endTransmission();
  133. }
复制代码
几种滤波算法相比,卡尔曼滤波明显复杂的多,计算量增大不少,现在的程序运算+取样部分已经达到30ms左右,不知道会不会受到限制,这个先不讨论了。

采样曲线:


再来个局部的:


从曲线上看,平滑效果最好的是二阶互补滤波,但是由于K取值较小,收敛速度比较差;
卡尔曼滤波不负众望,收敛速度和滤波效果平衡得很好,或许Q_omega还可以尝试更小的值;
一阶互补滤波效果最差,但是响应还是很灵敏,K值应该还有减小的空间,而且它的运算非常简单,对采样时间几乎不构成什么影响。

三种滤波算法都可以通过调整参数得到更均衡的滤波效果,我想请教的是在这样一个没有理想曲线的情况下,有没有一个定量的方法来判定滤波特性的好坏?还是只能根据实际要求来尝试得到一个平衡值?又或对于自平衡小车和四轴飞行器,有各自的经验曲线?

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复

使用道具 举报

 楼主| 发表于 2012-3-23 15:42:40 | 显示全部楼层
补充:各种算法的耗时差异(取128次平均/微秒)

加速度计采样及换算:521.4
陀螺仪采样及积分:214.2
一阶互补算法:80.9
二阶互补算法:80.5(奇怪,比一阶还快)
卡尔曼滤波算法:298.4
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-23 20:37:16 | 显示全部楼层
能不能认为陀螺仪的漂移是均匀的,把陀螺仪的减去,差值做线性拟合,然后求Err值……
回复 支持 反对

使用道具 举报

发表于 2012-3-23 20:56:25 | 显示全部楼层
把传感器装舵机上试试~
1、2、3自由度都试试~{:soso_e113:}
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-23 21:00:10 | 显示全部楼层
Malc 发表于 2012-3-23 20:56
把传感器装舵机上试试~
1、2、3自由度都试试~

嗯是个办法,明天试试
其他两个自由度的影响怎么修正捏?毕竟不会和传感器的轴完全重合吧。直接取个经验常数行不?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-24 08:51:51 | 显示全部楼层
我假定陀螺仪仅存在线性的漂移,即:陀螺仪的数据=信号+kt
用滤波后的数据减掉陀螺仪的数据,即:噪声-kt
再做一次线性回归,得到标准差如下:

加速度计:2.56393835
一阶互补滤波:1.321999511
二阶互补滤波:0.461729098
卡尔曼滤波:0.794898496

不知道这样评估是否有意义,我总觉得收敛速度比振动噪声的后果要严重些,但是用线性回归只是把两者都当成简单的误差来处理了。
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-24 09:41:22 | 显示全部楼层
以下是关于一阶互补滤波参数的分析


K=0.5时 Err=1.54,响应速度明显太慢,迟滞严重


K=0.2时 Err=1.19,Err值挺小,但是响应还是跟不上节奏


K=0.15时 Err=1.21,噪声开始明显起来了


Err=100时 Err=1.31


Err=75时 Err=1.41


Err=60时 Err=1.51


Err=50时 Err=1.60

一阶互补滤波效果似乎不太理想,振动噪声和收敛速度较难取得一个满意的平衡

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2012-3-24 10:06:57 | 显示全部楼层
{:soso_e179:}强烈加精~~真详细~~偶得仔细学习
回复 支持 反对

使用道具 举报

发表于 2012-3-24 11:26:59 | 显示全部楼层
请教一下了LZ,曲线图是什么软件做的?
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-24 12:43:24 | 显示全部楼层
I-robofan 发表于 2012-3-24 11:26
请教一下了LZ,曲线图是什么软件做的?

导出数据在excel里画的啊,这种计算量用excel就够了
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-3-24 13:39:30 | 显示全部楼层
之前的取样有些问题,重新调了一下,发现互补算法也可以调出很漂亮的滤波曲线{:soso_e128:}

K取值0.6

本帖子中包含更多资源

您需要 登录 才可以下载或查看,没有帐号?注册

x
回复 支持 反对

使用道具 举报

发表于 2012-4-11 17:20:46 | 显示全部楼层
#define Gry_offset -13    // 陀螺仪偏移量
#define Gyr_Gain 0.07     // 满量程2000dps时灵敏度(dps/digital)
#define Q_angle 0.01      // 角度数据置信度
#define Q_omega 0.0003    // 角速度数据置信度
#define R_angle 0.01      // 方差噪声
楼主辛苦了,刚开始接触Arduino不久,对程序还算是门外汉。近来也在研究这个,能请教一下如上像“陀螺仪偏移量”等这些参数是如何确定的呢?有什么其他的参考资料吗?谢谢!
回复 支持 反对

使用道具 举报

发表于 2012-4-11 20:50:28 | 显示全部楼层
不错啊,在版版这里找到了Kalman滤波实例,注释很详尽,哈哈,先收下了,谢谢!
回复 支持 反对

使用道具 举报

发表于 2012-5-9 10:40:06 | 显示全部楼层
我看l3g4200d数据手册上量程选择+-2000时,sensitivity为70 mdps/digit,想请教一下这个单位代表的是什么意思,是不是意味着l3g4200输出的值需要乘以0.07才能够得到degree per sencond
回复 支持 反对

使用道具 举报

 楼主| 发表于 2012-5-9 10:57:05 | 显示全部楼层
杜骁释 发表于 2012-5-9 10:40
我看l3g4200d数据手册上量程选择+-2000时,sensitivity为70 mdps/digit,想请教一下这个单位代表的是什么意 ...

对头,70mdps/digit的意思就是单位数字量表示 70 毫 度 每 秒
回复 支持 反对

使用道具 举报

您需要登录后才可以回帖 登录 | 注册

本版积分规则 需要先绑定手机号

Archiver|联系我们|极客工坊

GMT+8, 2024-3-29 14:34 , Processed in 0.052823 second(s), 25 queries .

Powered by Discuz! X3.4 Licensed

Copyright © 2001-2021, Tencent Cloud.

快速回复 返回顶部 返回列表