[arduino|module] Kalman Filter on ADXL345, ITG3200(视频)

Photobucket

在之前《ADXL345,HMC5883L,ITG3200读数的物理意义》一文中,大致介绍过加速度计(Accelerometer)ADXL345,陀螺仪(Gyroscope)ITG3200以及电子罗盘(Compass)HMC5883L大致的用法。在现实的应用中,真正要用到这些传感器数据,最好还是要对其进行一定的处理,其中最主要的一种,就是 Kalman Filter,也就是传说中的“卡尔曼滤波”。

ADXL345,作为加速度计,常作为倾斜指示计。因为地球上的东西都受到重力,有重力就有重力加速度,而根据重力加速度在正交三轴上的分量,就可以计算出当前传感器的倾斜角度。而现实的情况是,加速度计不仅仅只是受到重力的影响,各种外力同样也会产生对应方向上的加速度(比方说视频中老是把它翻来覆去的手)。在这样的条件下,如果依然认为加速度计X/Y/Z三轴上的分加速度都来源于重力加速度,并以此为基础进行倾斜角度的计算,就会发生严重的偏差。

如图所示,其中红色的点为单纯从 ADXL345 计算出的X轴向倾角,而绿色的点为经过卡尔曼滤波后计算出的倾角。

Photobucket

所以,需要引入陀螺仪(角速度计),配合加速度计来计算倾角。而卡尔曼滤波,就将这两种数据结合在一起,一齐作为输入,而以计算出的倾角作为最后的输出,可以进入下一步的计算(比方说PID)。

Kalman Filter 主要的代码来自这里,有兴趣的朋友不妨看看还是很有意思。因为代码已经手工优化过,所以读起来还是比较费劲,好在注释比较详尽。我在此基础上将起转化为C++的一个类库(library),视频中出现的 Arduino 程序代码,和 Processing 上位机代码都作为示例包含在其中。

函数内部中有不少“神秘数”,我至今依然不太明白有什么作用。这里分享一些心得:

  • 对于陀螺仪 ITG3200,在tile.c的注释中有提到,并不需要进行校准,在卡尔曼滤波中最自动补偿,虽然诡异,但是就相信作者咯。不过,需要注意的是输入到卡尔曼滤波中的参数单位,应该是标准的rad/s。ITG3200 输出分辨率为 14.375 LSB per degree/s。所以其度数需要先转化为弧度,并除以14.375。
  • 对于加速度计ADXL345,虽然注释里面提到需要校准,但是我个人也是认为校准的意义不大,或者说过早校准的意义不大。因为在未来的安装过程中还会引入误差。
  • 卡尔曼滤波的执行周期很重要,因为内部有用角速度乘以时间获得角位移的运算,所以有个固定的dt非常重要,而我在例程的loop()中加入了对执行时长的控制。而这样的控制必须知道控制是否有效,所以我就借助了板载的LED(UNO上13号引脚上的那个),如果loop()完整执行超时的话,则LED就没有机会亮起。视频中可以看到,这个LED一直是“常亮”的状态,可见在规定时长内足够完成既定程序。
  • 在原注释中有提到,输入陀螺仪数据的函数必须在每个dt间隔执行,而输入加速度数据的函数并不需要执行地那么勤快。 不过在我的例程中,二者都以dt间隔依次执行,这样一方面简化程序,也使得每次执行的时长保持一致。即使按照 dt=0.004s 来计算,采样频率达到了 250hz,即便这个超过了当前ADXL345、ITG3200的采样频率,但理论上无非就是数据没刷新而已,相信对精度也没什么影响。
  • 数据的表示一直是一个问题,尤其对于Arduino来说,如果通过显示器件,比方说1602,12864来表示,一方面效率不够,另一方面也不足以展示数据的变化,并进行比较。而串口文本输出也是有不少玄机。玩过arduino串口的朋友,会有个Arduino输出浮点数很容易的印象,只需要 Serial.print(val); 就可以输出一个带小数点的数字(其实这些都是文本)。转换(通过HardwareSerial的父类Print实现)需要时间不说,还损失了很多精度,哪怕输出10位小数,这依然不是完整的精度,损失不仅来自于有效值位数,也来自于二进制数到十进制的转换。所以,在示例代码中,我直接输出了float型数据的4个字节(在Arduino系统中,float类型占用4个字节)。这样就是直接发送了浮点数在系统内存中的实现。而在 processing 这边,就从这4个字节中还原出浮点数,再进行进一步的处理,把数字到文本之间的转换工作,交给性能强大几个量级的PC系统来实现。
  • 有点诡异的是,Arduino发送浮点数的时候,是从低字节到高字节(至少从简化程序的角度是这么实现的)。而processing进行转换的时候,会将先收到的字节当作高字节来处理。所以,在接收到足够数量的字节数之后,我还对整个字节缓存进行了一个倒序操作,调用了 org.apache.commons.lang3.ArrayUtils 类的函数。如果Procesing在编译时提示找不到库,请下载commons-lang3-3.1-bin.zip文件,并解压出commons-lang3-3.1.jar文件,存如Processing目录的lib文件夹内。
    正确的类库安装方法如下,在工作区的主目录(File/Preferences/Sketchbook Location)内的libraries文件夹中,新建文件夹,取名为”CommonsLang3″,在其中再建立一个 library 文件夹,将commons-lang3-3.1.jar文件保存其中,并重命名为“CommonsLang3.jar”。(参考官方wiki
  • 三角函数的运算,默认都是使用弧度值,适应一下咯,都按弧度制来,不用转换来转换去的。
  • processing这边,其实一开始一直希望实现屏幕的水平移动,后来用多种方法实现,效率都太低,无法及时更新,于是改成刷屏的显示方式了,有点小遗憾,也希望有高手支招。

视频介绍:

KalmanFilter library 下载地址:https://github.com/downloads/aguegu/Arduino/KalmanFilter.7z 含视频中用到的 Arduino 和 Processing 的例程。例程在ubuntu 12.04 + processing 2.0b3 + arduino 1.0.1 测试可用。

*下载的示例代码会与视频中细节有所不同,因为视频录制在先,随后又添加了一些注释并改良一下,不用见怪。

有用的链接:

特别感谢:圣源电子CSK_极客工坊

关于aGuegu

向着更高的逼格
此条目发表在模块分类目录,贴了, , , , , , , , , , 标签。将固定链接加入收藏夹。
  • demo

    您好,我看到你的卡尔曼滤波程序,我照葫芦画瓢弄到mpu6050的模块上了,可是arduino输出貌似有问题。对于您的ino有写地方不明白:
    1。acc[0],acc[1],acc[2]应该分别代表了x,y,z轴的加速度,对么?
    2。angular_rate[0],angular_rate[1],angular_rate[2]应该分别代表了x,y,z轴的角加速度,对么?
    3。如果是,那这些值的范围应该是多少?
    4。sendFloat()这个函数,在端口监视器上看到的应该是乱码么?
    5。Serial.write(0xff);是做什么用的。
    由于找不到相应库文件,processing没有运行成功,所以:
    import org.apache.commons.lang3.ArrayUtils是那个库里边的
    希望您能够解答,谢谢。
    我是刚刚接触arduino的菜鸟,之前也没有类似单片机的经验,问的问题可能有些过于菜而冒犯,希望您不介意,再一次谢谢。

    • 1.2. 对
      3. 取值范围根据数据手册来定,不用的传感器配置,对应的每LSB的意义会有所不同。角速度计的度数直接进行三角函数运算获取角度,用的是其比值,所以单位并不是很重要。
      4. 乱码是肯定的,因为发送的并不是文本(char),而是字节(byte),需要将每4个字节还原成浮点数(float),可以参考processing例程
      5. 用来发送一个尾字节,用来标记每次发送数据的结尾。
      6. 这个库的下载安装方法,在正文中有提到,我已经修正导入library的方法,相见正文。

      感谢支持,看得还是蛮认真的哈~

  • demo

    3。您的数值的取值范围是多少,我用的陀螺仪mpu6050取值范围(原始数据)是acc:-32768到32767,angular_rate:-32768到32767。
    processing的库装好了,修改了端口,可以看到显示了。但是结果很诡异,估计是传输的值还需要处理。

    • 这个不是陀螺仪取值范围的问题,需要把陀螺仪的度数转换为现实的 弧度/秒 的单位

  • demo

    最近瞎捣鼓您的东西,想问个小白问题:
    加速度计(Accelerometer)ADXL345
    陀螺仪(Gyroscope)ITG3200
    电子罗盘(Compass)HMC5883L

    怎么连起来的呢?

    • 都连在I2C总线上呀,只是软件访问的地址不同而已。

      • demo

        请原谅我太新手,i2c不明白,我如果有加速度计、陀螺仪、电子罗盘三个东西要怎么连接呢

  • demo

    谢谢你,我这个硬件搞定了,姿态识别有什么好的算法推荐么

    • Kalman 其实就是姿态识别的一部分呀,把其它几个轴也加上,就能做比较全面的姿态分析了呀

  • Pingback: WII控制机器人揭密 | LT()

  • have you code balanzing robot whit adxl 345 and gyro itg 3200 ?
    share whit me,i no found anywere new board adxl 345 code,only old version analog board adxl 203 but my country no found and can buy anywere thats,all have only adxl 345 and gyro LPR550 and dealextreme can orden good MPU-6050 combo board, or gy-80 combo whit L364200D/adxl345
    but how chANGE CODE WORKING WHIT OTHER SENSOR 🙁 ????
    better if have code whit ADXL345 acceletometer and L364200D gyro or gyro lpy510, 550, itg 3200
    send code to me mail if you have good working code,i need,i looking many manu day at net but no found anythink good. you have only best idea.

    • I am also trying to make a self-balance robot. However, swift-ting the motor backwards and forwards seems killing my power supply. So I have to put this project aside for a while. If I success, I would let you know. ADXL345 + ITG3200 could definitely do the job. There is no doubt about that.

  • 你好,我用的卡尔曼滤波程序和你的是一样的,不过我用STM32读取MPU6050,用lcd屏来显示,为什么显示结果不对呢,能不能帮我一下,我反复的看了好几天了,实在找不出问题所在,我把卡尔曼滤波用一个.c文件的形式,然后在主函数的文件中调用kalman_update和state_update进行计算,可是不知道为什么,不管我怎么弄,就是不对呀,谢谢您了

  • 好人不学坏

    楼主你好,我下载了你给的链接,在运行Arduino 后提示我没有卡曼的程序( KalmanFilter.h: No such file or directory) ,我是个初学者,我不知道该怎么弄了,求指点。

    • github关闭了下载功能,而且我也转移了文件,可以在这里找:https://github.com/aguegu/ardulibs/tree/master/kalmanfilter

  • king

    lz好,通过这个算出来的数据有动态漂移,就是在快速晃动的时候,角度值变化很大,怎么解决这个问题呢?还有就是从一个角度变到另一个角度,不是“啪”一下就到,而是慢慢的从这个角度变到另一个角度,怎么做到迅速到达?

  • cropse

    不好意思請問一下
    我目前在做加速度計的加速度二次積分得到距離
    但是目前第一次積分就已經遇到問題,做一次單軸短距離的移動(~20cm)
    就會有積分回來的速度會被誤判成等加速運動,距離也就跟著一直增加
    我試著對加速度用卡爾曼濾波也沒有實際的改善,同時也參考了河海大学室內定位的論文
    但是誤差情況還是相當嚴重
    不知道有沒有甚麼方法呢??

  • 飞鱼

    楼主,我修改了一下程序以适应我的硬件,但是得到的角度和加速度计的角度相差90度,感觉很诡异,请问会是什么原因造成的呢?望不吝赐教。

    • 研究一下芯片焊接的方位咯,实在不行就在软件这边转过来

  • YH

    你好,我看了你的卡尔曼滤波视频,觉得效果很好,尤其是在收到横向外力抖动的时候,都能消失影响,我依据Trammell Hudson的代码,利用matlab进行了仿真,发现一个问题,那就是:如果模块收到某个方向上,大小不变的持续力干扰(例如一阵持续的强风),那么加速度换算出的倾角就会持续错误,但没剧烈抖动,那么此时,卡尔曼滤波结果会相信加速度换算出的倾角,即卡尔曼滤波结果与加速度换算的错误结果一致。不知道实际情况是否这样?