加速度计和陀螺仪的使用指南

风色幻想

传感器直接采样的结果
#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

// 05  void setup() {

// 06    sensor_init();

// 07    delay(1000);

// 08  }

// 09

// 10  void 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端得到的数据曲线


红色曲线是AngleA，绿色是AngleG，范围±10°，整个测量时间为20s

可以看到最前面静止区域，两条曲线噪声都不算严重，至少可以说明系统噪声不会是太严重的问题。

A部分是敲击桌子产生的，由于加速度计是通过测量两个方向的加速度来计算偏转角的。理想情况我们想要得到的是这两个方向的重力加速度分量。但是任何机械系统不可避免存在的平动振动也会产生相当大的加速度，而加速度计完全无法区分这两种加速度，所以振动加速度在我们的测量中引入了强烈的噪声。

B部分是用手偏移一个角度的结果，而C部分是系统晃动时采集的数据，此时不可避免的引入振动，加速度计过分敏感的神经再次受到强烈的刺激 。

而陀螺仪对振动不敏感，所以基本不会受到什么影响。我认为最前面加速度计测量得到的噪声也主要来源于桌面的震动，毕竟我桌子上的电脑风扇也是一个很大的噪声源。陀螺仪积分过程本身还有一个好处，时间积分为0的随机噪声也在此过程中平滑掉了。

短时间来看，陀螺仪已经可以非常好的对姿态进行判定，但是对于较长的时间，误差也随积分的过程不断累积，尽管有做过修正量Gry_offset来调校，随着时间累积，这个调校总归是不可靠的。

下面是100s的静态测试


互补滤波，我的理解是这种滤波方式就是按一定的权重对两个值进行叠加
很直观的融合方式。

以下是代码：
#include <Wire.h>

#define Acc 0x1D;

#define Gyr 0x69;

#define Mag 0x1E;



void setup() {

    sensor_init();

    Serial.begin(115200);

    delay(1000);

  }



  void loop() {

    Z-Gyroscope = gDat(Gyr, 2);

    delay(50);

  }



#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 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;

   

    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(";");                      // 输出数据

    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

// 05  void setup() {

// 06    sensor_init();

// 07    delay(1000);

// 08  }

// 09

// 10  void 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();

}

红-加速度计
绿-陀螺仪
白-一阶互补滤波
黄-二阶互补滤波
从曲线上看互补滤波效果还是不错滴，二阶滤波K取0.5时曲线看起来已经很漂亮了，而且并没有明显的迟滞。

371278638

看楼主的波形效果好漂亮......帮顶咯!

kmani

好东西，帮顶！极客工坊。

371278638

请问楼主你的波形是如何显示的？

xiong57785

                              

hanxi6mm

多谢分享

leicai05

对自己有用

wcm_e

xuexi mark

bfk2003

mark！！

我惯的女人

有用啊

123bac

波形使用设么做的

paul_guo

371278638 发表于 2012-12-12 17:33
请问楼主你的波形是如何显示的？

谢谢LZ的分享,很漂亮..

我估计LZ是把数据贴出来,在电脑上做的波形,反正我是以前是这样干的.哈.

阿发

学习了..

济南电子爱好者

这个只适合单轴吧

德玛西亚

楼主 问下 关于 互补滤波 一阶二阶  这些理论知识 是属于哪个学科的？

elmagnificoam

我也来学习一下啊 看看 好像还可以

六月峰

不明觉厉

dengchengmi

学习，讲的很好！

dengchengmi

学习学习！

chinamaken

大大的顶下