我的自平衡小车D2——加速度计与陀螺仪获取姿态参数的差异

黑马

滤波的算法还没有研究明白，先放上一段从传感器直接采样的结果。

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. 
   
9. float angleG;
   
10. unsigned long timer = 0;  // 采样时间
    
11. void setup() {
    
12.     sensor_init();        // 配置传感器
    
13.     Serial.begin(19200);  // 开启串口以便监视数据
    
14.     delay(1000);
    
15.   }
    
16. 
    
17. void loop() {
    
18.     long o_timer = timer;                   // 上一次采样时间(ms)
    
19.     float Y_Accelerometer = gDat(Acc, 1);   // 获取向前的加速度
    
20.     float Z_Accelerometer = gDat(Acc, 2);   // 获取向下的加速度
    
21.     float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;
    
22.                                             // 根据加速度分量得到的角度(degree)
    
23.     timer = millis();                       // 当前时间(ms)
    
24.     int dt = timer - o_timer;               // 微分时间
    
25.     angleG = angleG + Gyr_Gain * (gDat(Gyr, 0) +  Gry_offset) * dt / 1000;
    
26.                                             // 对角速度积分得到的角度(degree)
    
27.     Serial.print(timer);
    
28.     Serial.print(",");
    
29.     Serial.print(angleA, 6);
    
30.     Serial.print(",");
    
31.     Serial.print(angleG, 6);
    
32.     Serial.print(";");                      // 输出数据
    
33.     delay(10);
    
34.   }
    
35. 
    
36. int gDat(int device, int axis) {
    
37. 
    
38. // 读九轴姿态传感器寄存器函数
    
39. // For Arduino, by 黑马
    
40. // 调用参数表
    
41. //   type    device      axis
    
42. //                    0   1   2
    
43. // ADXL345     Acc    x   y   z
    
44. // L3G4200D    Gyr    x   y   z
    
45. // HMC5883L    Mag    x   z   y
    
46. // Example
    
47. // 00 #include <Wire.h>
    
48. // 01 #define Acc 0x1D;
    
49. // 02 #define Gyr 0x69;
    
50. // 03 #define Mag 0x1E;
    
51. // 04
    
52. // 05  void setup() {
    
53. // 06    sensor_init();
    
54. // 07    delay(1000);
    
55. // 08  }
    
56. // 09
    
57. // 10  void loop() {
    
58. // 11    int Z-Gyroscope;
    
59. // 12    Z-Gyroscope = gDat(Gyr, 2);
    
60. // 13    delay(50);
    
61. // 14  }
    
62. 
    
63.     int v;
    
64.     byte vL, vH, address;               // 存放byte数值
    
65.     if (device == Acc) address = 0x32;  // ADXL345的读数地址
    
66.     if (device == Gyr) address = 0xA8;  // L3G4200D的读数地址
    
67.     if (device == Mag) address = 0x03;  // HMC5883L的读数地址
    
68.     address = address + axis * 2;       // 数据偏移-坐标轴
    
69.     Wire.beginTransmission(device);     // 开始传输数据
    
70.     Wire.send(address);                 // 发送指针
    
71.     Wire.requestFrom(device, 2);        // 请求2 byte数据
    
72.     while(Wire.available() < 2);        // 成功获取前等待
    
73.     vL = Wire.receive();
    
74.     vH = Wire.receive();                // 读取数据
    
75.     Wire.endTransmission();             // 结束传输
    
76.     if (device == Mag) v = (vL << 8) | vH;
    
77.     else v = (vH << 8) | vL;            // 将byte数据合并为Int
    
78.     return v;                           // 返回读书值
    
79. }
    
80. 
    
81. void sensor_init() {                         // 配置九轴姿态传感器
    
82.     writeRegister(Acc, 0x2D, 0b00001000);    // 测量模式
    
83.                             // 配置ADXL345
    
84.     writeRegister(Gyr, 0x20, 0b00001111);    // 设置睡眠模式、x, y, z轴使能
    
85.     writeRegister(Gyr, 0x21, 0b00000000);    // 选择高通滤波模式和高通截止频率
    
86.     writeRegister(Gyr, 0x22, 0b00000000);    // 设置中断模式
    
87.     writeRegister(Gyr, 0x23, 0b00110000);    // 设置量程(2000dps)、自检状态、SPI模式
    
88.     writeRegister(Gyr, 0x24, 0b00000000);    // FIFO & 高通滤波
    
89.                             // 配置L3G4200D(2000 deg/sec)
    
90.     writeRegister(Mag, 0x02, 0x00);          // 连续测量
    
91.                             // 配置HMC5883L
    
92. }
    
93. 
    
94. void writeRegister(int device, byte address, byte val) {    // 写寄存器
    
95.     Wire.beginTransmission(device);
    
96.     Wire.send(address);
    
97.     Wire.send(val);
    
98.     Wire.endTransmission();
    
99. }

复制代码

通过串口上传3个数据：采样时间、通过加速度计算得到的角度AngleA、通过角速度积分得到的角度AngleG。

在pc端得到的数据曲线

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

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

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

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

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

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

下面是100s的静态测试


如果系统是动态的，受到系统反应时间的影响，误差会更大，所以用加速度计进行修正是必要的。
卡尔曼滤波应该是比较"简单"而成熟的算法，但是要完全看懂对于我这样的初鸟来说也不太容易。D3应该就是关于卡尔曼滤波的一些心得了。其实这些对于论坛里各位DX来说可能是相当浅显的内容，不过毕竟也是我个人从0到1的过程，希望DX们不吝指教，见笑了。

开心就好

不错的文章,值得一看

Malc

滤波方法有很多额，光卡尔曼滤波就分了三种
卡尔曼用在平衡车上不一定是最合适的
楼主可以试试别的，比如互补，MK飞控的滤波方法

黑马

Malc 发表于 2012-3-23 12:13
滤波方法有很多额，光卡尔曼滤波就分了三种
卡尔曼用在平衡车上不一定是最合适的
楼主可以试试别的，比如 ...

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

以下是代码：

1. #include <Wire.h>
   
2. #define Acc 0x1D;
   
3. #define Gyr 0x69;
   
4. #define Mag 0x1E;
   
5. 
   
6. void setup() {
   
7.     sensor_init();
   
8.     Serial.begin(115200);
   
9.     delay(1000);
   
10.   }
    
11. 
    
12.   void loop() {
    
13.     Z-Gyroscope = gDat(Gyr, 2);
    
14.     delay(50);
    
15.   }
    
16. 
    
17. #include <Wire.h>
    
18. #define Acc 0x1D
    
19. #define Gyr 0x69
    
20. #define Mag 0x1E
    
21. #define Gry_offset -13    // 陀螺仪偏移量
    
22. #define Gyr_Gain 0.07     // 满量程2000dps时灵敏度(dps/digital)
    
23. #define pi 3.14159
    
24. 
    
25. float Com_angle;
    
26. float y1, Com2_angle;
    
27. float angleG;
    
28. long timer = 0;  // 采样时间
    
29. void setup() {
    
30.     sensor_init();        // 配置传感器
    
31.     Serial.begin(19200);  // 开启串口以便监视数据
    
32.     delay(1000);
    
33.   }
    
34. 
    
35. void loop() {
    
36.     long o_timer = timer;                   // 上一次采样时间(ms)
    
37.     float Y_Accelerometer = gDat(Acc, 1);   // 获取向前的加速度
    
38.     float Z_Accelerometer = gDat(Acc, 2);   // 获取向下的加速度
    
39.     float angleA = atan(Y_Accelerometer / Z_Accelerometer) * 180 / pi;
    
40.                                             // 根据加速度分量得到的角度(degree)
    
41.     timer = millis();                       // 当前时间(ms)
    
42.     float omega =  Gyr_Gain * (gDat(Gyr, 0) +  Gry_offset);
    
43.     float dt = (timer - o_timer) / 1000.0;  // 微分时间(s)
    
44.     angleG = angleG + omega * dt;           // 对角速度积分得到的角度(degree)
    
45. // 一阶互补算法
    
46.     float K;
    
47.     K = 0.075;                              // 对加速度计取值的权重
    
48.     float A = K / (K + dt);
    
49.     Com_angle = A * (Com_angle + omega * dt) + (1-A) * angleA;
    
50. // 二阶互补算法
    
51.     K = 0.5;
    
52.     float x1 = (angleA - Com2_angle) * K * K;
    
53.     y1 = y1 + x1 * dt;
    
54.     float x2 = y1 + 2 * K *(angleA - Com2_angle) + omega;
    
55.     Com2_angle = Com2_angle + x2 * dt;
    
56.    
    
57.     Serial.print(timer);
    
58.     Serial.print(",");
    
59.     Serial.print(angleA, 6);
    
60.     Serial.print(",");
    
61.     Serial.print(angleG, 6);
    
62.     Serial.print(",");
    
63.     Serial.print(Com_angle, 6);
    
64.     Serial.print(",");
    
65.     Serial.print(Com2_angle, 6);
    
66.     Serial.print(";");                      // 输出数据
    
67.     delay(50);
    
68.   }
    
69. 
    
70. int gDat(int device, int axis) {
    
71. 
    
72. // 读九轴姿态传感器寄存器函数
    
73. // For Arduino, by 黑马
    
74. // 调用参数表
    
75. //   type    device      axis
    
76. //                    0   1   2
    
77. // ADXL345     Acc    x   y   z
    
78. // L3G4200D    Gyr    x   y   z
    
79. // HMC5883L    Mag    x   z   y
    
80. // Example
    
81. // 00 #include <Wire.h>
    
82. // 01 #define Acc 0x1D;
    
83. // 02 #define Gyr 0x69;
    
84. // 03 #define Mag 0x1E;
    
85. // 04
    
86. // 05  void setup() {
    
87. // 06    sensor_init();
    
88. // 07    delay(1000);
    
89. // 08  }
    
90. // 09
    
91. // 10  void loop() {
    
92. // 11    int Z-Gyroscope;
    
93. // 12    Z-Gyroscope = gDat(Gyr, 2);
    
94. // 13    delay(50);
    
95. // 14  }
    
96. 
    
97.     int v;
    
98.     byte vL, vH, address;               // 存放byte数值
    
99.     if (device == Acc) address = 0x32;  // ADXL345的读数地址
    
100.     if (device == Gyr) address = 0xA8;  // L3G4200D的读数地址
     
101.     if (device == Mag) address = 0x03;  // HMC5883L的读数地址
     
102.     address = address + axis * 2;       // 数据偏移-坐标轴
     
103.     Wire.beginTransmission(device);     // 开始传输数据
     
104.     Wire.send(address);                 // 发送指针
     
105.     Wire.requestFrom(device, 2);        // 请求2 byte数据
     
106.     while(Wire.available() < 2);        // 成功获取前等待
     
107.     vL = Wire.receive();
     
108.     vH = Wire.receive();                // 读取数据
     
109.     Wire.endTransmission();             // 结束传输
     
110.     if (device == Mag) v = (vL << 8) | vH;
     
111.     else v = (vH << 8) | vL;            // 将byte数据合并为Int
     
112.     return v;                           // 返回读书值
     
113. }
     
114. 
     
115. void sensor_init() {                         // 配置九轴姿态传感器
     
116.     writeRegister(Acc, 0x2D, 0b00001000);    // 测量模式
     
117.                             // 配置ADXL345
     
118.     writeRegister(Gyr, 0x20, 0b00001111);    // 设置睡眠模式、x, y, z轴使能
     
119.     writeRegister(Gyr, 0x21, 0b00000000);    // 选择高通滤波模式和高通截止频率
     
120.     writeRegister(Gyr, 0x22, 0b00000000);    // 设置中断模式
     
121.     writeRegister(Gyr, 0x23, 0b00110000);    // 设置量程(2000dps)、自检状态、SPI模式
     
122.     writeRegister(Gyr, 0x24, 0b00000000);    // FIFO & 高通滤波
     
123.                             // 配置L3G4200D(2000 deg/sec)
     
124.     writeRegister(Mag, 0x02, 0x00);          // 连续测量
     
125.                             // 配置HMC5883L
     
126. }
     
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. }

复制代码

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

Malc

呵呵 等过段时间有money了我也要开始研究平衡车了~

darkorigin

感谢分享，PID算法一直都是我想碰不敢碰的东西。分析的很精辟，呵呵~~PFPF

darkorigin

求教。 您的代码已经拜读多遍。
求教一个代码以外的东东
就是你最后贴出的图，是用什么方式做出来的呢？
经过坛子里这么多朋友的讲解，数据发送到串口上已经没问题了，但是后续如何做出图呢？？
我的本办法是导出到EXCEL，然后用图形工具，但是似乎您这里有更好的分析工具啊，呵呵！

黑马

darkorigin 发表于 2012-5-30 11:41
求教。 您的代码已经拜读多遍。
求教一个代码以外的东东
就是你最后贴出的图，是用什么方式做出来的呢？
...

呵呵我是在VB环境下读的，编译了一个小程序：

http://www.geek-workshop.com/forum.php?mod=viewthread&tid=708

比较简陋，不嫌弃的话试试合不合用吧

darkorigin

黑马 发表于 2012-5-30 13:55
呵呵我是在VB环境下读的，编译了一个小程序：

http://www.geek-workshop.com/forum.php?mod=viewthrea ...

郁闷，代码测试起来用你的监视器还是木有显示出来。。。
void setup()
{
    Serial.begin(19200);
}

void loop()
{
    Serial.print(analogRead(1));
    Serial.print(",");
    Serial.print(analogRead(2));   
    Serial.print(",");
    Serial.print(analogRead(3));
    Serial.print(",");
    delay(500);
}
出来的图就是空白的

COM口是COM3，设置是3,1024,3  
我是COM3口，数值理论最大1021,3组数据
不知哪里错鸟

天天向上/tp

我用的楼主的代码~~测试的。也显示不出来，不知道什么原因

天天向上/tp

第一个加速度和陀螺仪比较这个程序，用串口看不到数据，不知道怎么回事？

ladywent

请问，我只有一个UNO和一个三轴加速度传感器MMA7361，只用一个KP调节，根本站不起来，

是因为没有陀螺仪的问题么？

只用一个加速度传感器能否可以自平衡？谢！！！！

vellonj

darkorigin 发表于 2012-5-30 14:49
郁闷，代码测试起来用你的监视器还是木有显示出来。。。
void setup()
{

第一个数据为时间轴，后面分别为n组数据用并","分隔。最后的应该用“；”分隔数据包。

三组数据时：
    Serial.print(time);
    Serial.print(",");
    Serial.print(analogRead(1));
    Serial.print(",");
    Serial.print(analogRead(2));   
    Serial.print(",");
    Serial.print(analogRead(3));
    Serial.print("；");

lph1987

mark~~~学习

darkorigin

vellonj 发表于 2012-8-28 16:13
第一个数据为时间轴，后面分别为n组数据用并","分隔。最后的应该用“；”分隔数据包。

三组数据时：

谢谢~~~呵呵，我用了另外的一个开源的东西搞起来了，很强大。