我的自平衡小车D2——加速度计与陀螺仪获取姿态参数的差异
滤波的算法还没有研究明白，先放上一段从传感器直接采样的结果。
#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 angleG;
unsigned 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)
int dt = timer - o_timer; // 微分时间
angleG = angleG + Gyr_Gain * (gDat(Gyr, 0) +Gry_offset) * dt / 1000;
// 对角速度积分得到的角度(degree)
Serial.print(timer);
Serial.print(",");
Serial.print(angleA, 6);
Serial.print(",");
Serial.print(angleG, 6);
Serial.print(";"); // 输出数据
delay(10);
}
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();
}复制代码通过串口上传3个数据：采样时间、通过加速度计算得到的角度AngleA、通过角速度积分得到的角度AngleG。
在pc端得到的数据曲线
2012-3-23 10:18 上传(12.19 KB)
红色曲线是AngleA，绿色是AngleG，范围±10°，整个测量时间为20s
可以看到最前面静止区域，两条曲线噪声都不算严重，至少可以说明系统噪声不会是太严重的问题。
A部分是敲击桌子产生的，由于加速度计是通过测量两个方向的加速度来计算偏转角的。理想情况我们想要得到的是这两个方向的重力加速度分量。但是任何机械系统不可避免存在的平动振动也会产生相当大的加速度，而加速度计完全无法区分这两种加速度，所以振动加速度在我们的测量中引入了强烈的噪声。
B部分是用手偏移一个角度的结果，而C部分是系统晃动时采集的数据，此时不可避免的引入振动，加速度计过分敏感的神经再次受到强烈的刺激。
而陀螺仪对振动不敏感，所以基本不会受到什么影响。我认为最前面加速度计测量得到的噪声也主要来源于桌面的震动，毕竟我桌子上的电脑风扇也是一个很大的噪声源。陀螺仪积分过程本身还有一个好处，时间积分为0的随机噪声也在此过程中平滑掉了。
短时间来看，陀螺仪已经可以非常好的对姿态进行判定，但是对于较长的时间，误差也随积分的过程不断累积，尽管有做过修正量Gry_offset来调校，随着时间累积，这个调校总归是不可靠的。
下面是100s的静态测试
2012-3-23 10:50 上传(5.04 KB)
如果系统是动态的，受到系统反应时间的影响，误差会更大，所以用加速度计进行修正是必要的。
卡尔曼滤波应该是比较"简单"而成熟的算法，但是要完全看懂对于我这样的初鸟来说也不太容易。D3应该就是关于卡尔曼滤波的一些心得了。其实这些对于论坛里各位DX来说可能是相当浅显的内容，不过毕竟也是我个人从0到1的过程，希望DX们不吝指教，见笑了。
via - 极客工坊