我的自平衡小车D3——滤波算法
先放代码#include <Wire.h>#define Acc 0x1D
#define Gyr 0x69
#define Mag 0x1E
#define Gry_offset -13 // 陀螺仪偏移量
#define Gyr_Gain 0.07 // 满量程2000dps时灵敏度(dps/digital)
#define pi 3.14159
float Com_angle;
float y1, Com2_angle;
float Klm_angle;
#define Q_angle 0.01 // 角度数据置信度
#define Q_omega 0.0003 // 角速度数据置信度
#define R_angle 0.01 // 方差噪声
float bias = 0;
float P_00 = 0, P_01 = 0, P_10 = 0, P_11 = 0;
float angleG;
long timer = 0;// 采样时间
void setup() {
sensor_init(); // 配置传感器
Serial.begin(19200);// 开启串口以便监视数据
delay(1000);
}
void loop() {
long o_timer = timer; // 上一次采样时间(ms)
float Y_Accelerometer = gDat(Acc, 1); // 获取向前的加速度
float Z_Accelerometer = gDat(Acc, 2); // 获取向下的加速度
float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;
// 根据加速度分量得到的角度(degree)
timer = millis(); // 当前时间(ms)
float omega =Gyr_Gain * (gDat(Gyr, 0) +Gry_offset);
float dt = (timer - o_timer) / 1000.0;// 微分时间(s)
angleG = angleG + omega * dt; // 对角速度积分得到的角度(degree)
// 一阶互补算法
float K;
K = 0.075; // 对加速度计取值的权重
float A = K / (K + dt);
Com_angle = A * (Com_angle + omega * dt) + (1-A) * angleA;
// 二阶互补算法
K = 0.5;
float x1 = (angleA - Com2_angle) * K * K;
y1 = y1 + x1 * dt;
float x2 = y1 + 2 * K *(angleA - Com2_angle) + omega;
Com2_angle = Com2_angle + x2 * dt;
// 卡尔曼滤波
Klm_angle += (omega - bias) * dt; // 先验估计
P_00 += -(P_10 + P_01) * dt + Q_angle *dt;
P_01 += -P_11 * dt;
P_10 += -P_11 * dt;
P_11 += +Q_omega * dt; // 先验估计误差协方差
float K_0 = P_00 / (P_00 + R_angle);
float K_1 = P_10 / (P_00 + R_angle);
bias += K_1 * (angleA - Klm_angle);
Klm_angle += K_0 * (angleA - Klm_angle); // 后验估计
P_00 -= K_0 * P_00;
P_01 -= K_0 * P_01;
P_10 -= K_1 * P_00;
P_11 -= K_1 * P_01; // 后验估计误差协方差
Serial.print(timer);
Serial.print(",");
Serial.print(angleA, 6);
Serial.print(",");
Serial.print(angleG, 6);
Serial.print(",");
Serial.print(Com_angle, 6);
Serial.print(",");
Serial.print(Com2_angle, 6);
Serial.print(",");
Serial.print(Klm_angle, 6);
Serial.print(";"); // 输出数据
delay(50);
}
int gDat(int device, int axis) {
// 读九轴姿态传感器寄存器函数
// For Arduino, by 黑马
// 调用参数表
// type device axis
// 0 1 2
// ADXL345 Acc x y z
// L3G4200D Gyr x y z
// HMC5883L Mag x z y
// Example
// 00 #include <Wire.h>
// 01 #define Acc 0x1D;
// 02 #define Gyr 0x69;
// 03 #define Mag 0x1E;
// 04
// 05void setup() {
// 06 sensor_init();
// 07 delay(1000);
// 08}
// 09
// 10void loop() {
// 11 int Z-Gyroscope;
// 12 Z-Gyroscope = gDat(Gyr, 2);
// 13 delay(50);
// 14}
int v;
byte vL, vH, address; // 存放byte数值
if (device == Acc) address = 0x32;// ADXL345的读数地址
if (device == Gyr) address = 0xA8;// L3G4200D的读数地址
if (device == Mag) address = 0x03;// HMC5883L的读数地址
address = address + axis * 2; // 数据偏移-坐标轴
Wire.beginTransmission(device); // 开始传输数据
Wire.send(address); // 发送指针
Wire.requestFrom(device, 2); // 请求2 byte数据
while(Wire.available() < 2); // 成功获取前等待
vL = Wire.receive();
vH = Wire.receive(); // 读取数据
Wire.endTransmission(); // 结束传输
if (device == Mag) v = (vL << 8) | vH;
else v = (vH << 8) | vL; // 将byte数据合并为Int
return v; // 返回读书值
}
void sensor_init() { // 配置九轴姿态传感器
writeRegister(Acc, 0x2D, 0b00001000); // 测量模式
// 配置ADXL345
writeRegister(Gyr, 0x20, 0b00001111); // 设置睡眠模式、x, y, z轴使能
writeRegister(Gyr, 0x21, 0b00000000); // 选择高通滤波模式和高通截止频率
writeRegister(Gyr, 0x22, 0b00000000); // 设置中断模式
writeRegister(Gyr, 0x23, 0b00110000); // 设置量程(2000dps)、自检状态、SPI模式
writeRegister(Gyr, 0x24, 0b00000000); // FIFO & 高通滤波
// 配置L3G4200D(2000 deg/sec)
writeRegister(Mag, 0x02, 0x00); // 连续测量
// 配置HMC5883L
}
void writeRegister(int device, byte address, byte val) { // 写寄存器
Wire.beginTransmission(device);
Wire.send(address);
Wire.send(val);
Wire.endTransmission();
}几种滤波算法相比，卡尔曼滤波明显复杂的多，计算量增大不少，现在的程序运算+取样部分已经达到30ms左右，不知道会不会受到限制，这个先不讨论了。
采样曲线：
再来个局部的：
从曲线上看，平滑效果最好的是二阶互补滤波，但是由于K取值较小，收敛速度比较差；
卡尔曼滤波不负众望，收敛速度和滤波效果平衡得很好，或许Q_omega还可以尝试更小的值；
一阶互补滤波效果最差，但是响应还是很灵敏，K值应该还有减小的空间，而且它的运算非常简单，对采样时间几乎不构成什么影响。
三种滤波算法都可以通过调整参数得到更均衡的滤波效果，我想请教的是在这样一个没有理想曲线的情况下，有没有一个定量的方法来判定滤波特性的好坏？还是只能根据实际要求来尝试得到一个平衡值？又或对于自平衡小车和四轴飞行器，有各自的经验曲线？
补充：各种算法的耗时差异(取128次平均/微秒)
加速度计采样及换算：521.4
陀螺仪采样及积分：214.2
一阶互补算法：80.9
二阶互补算法：80.5(奇怪，比一阶还快:o)
卡尔曼滤波算法：298.4 能不能认为陀螺仪的漂移是均匀的，把陀螺仪的减去，差值做线性拟合，然后求Err值…… 把传感器装舵机上试试~
1、2、3自由度都试试~{:soso_e113:} Malc 发表于 2012-3-23 20:56 static/image/common/back.gif
把传感器装舵机上试试~
1、2、3自由度都试试~
嗯是个办法，明天试试
其他两个自由度的影响怎么修正捏？毕竟不会和传感器的轴完全重合吧。直接取个经验常数行不？ 我假定陀螺仪仅存在线性的漂移，即：陀螺仪的数据=信号+kt
用滤波后的数据减掉陀螺仪的数据，即：噪声-kt
再做一次线性回归，得到标准差如下：
加速度计：2.56393835
一阶互补滤波：1.321999511
二阶互补滤波：0.461729098
卡尔曼滤波：0.794898496
不知道这样评估是否有意义，我总觉得收敛速度比振动噪声的后果要严重些，但是用线性回归只是把两者都当成简单的误差来处理了。 以下是关于一阶互补滤波参数的分析
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
一阶互补滤波效果似乎不太理想，振动噪声和收敛速度较难取得一个满意的平衡 {:soso_e179:}强烈加精~~真详细~~偶得仔细学习 请教一下了LZ，曲线图是什么软件做的？ I-robofan 发表于 2012-3-24 11:26 static/image/common/back.gif
请教一下了LZ，曲线图是什么软件做的？
导出数据在excel里画的啊，这种计算量用excel就够了 之前的取样有些问题，重新调了一下，发现互补算法也可以调出很漂亮的滤波曲线{:soso_e128:}
K取值0.6 #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不久，对程序还算是门外汉。近来也在研究这个，能请教一下如上像“陀螺仪偏移量”等这些参数是如何确定的呢？有什么其他的参考资料吗？谢谢！ 不错啊，在版版这里找到了Kalman滤波实例，注释很详尽，哈哈，先收下了，谢谢！ 我看l3g4200d数据手册上量程选择+-2000时，sensitivity为70 mdps/digit，想请教一下这个单位代表的是什么意思，是不是意味着l3g4200输出的值需要乘以0.07才能够得到degree per sencond 杜骁释 发表于 2012-5-9 10:40 static/image/common/back.gif
我看l3g4200d数据手册上量程选择+-2000时，sensitivity为70 mdps/digit，想请教一下这个单位代表的是什么意 ...
对头，70mdps/digit的意思就是单位数字量表示 70 毫 度 每 秒