ROS下使用MPU6050惯性测量模块

无论是开发飞行器还是移动机器人，都需要时刻知道它的位姿，知道它当前的运动状态，这里就需要借助，即惯性测量单元，它可以测量物体在三轴上的加速度以及角速度。

MPU6050

手头有一款廉价的IMU模块，芯片内部包含加速度计和陀螺仪，它可以输出三轴加速度以及三轴角速度，通过接口控制和输出，体积很小，非常适合嵌入式应用场合。在实际使用时需要将模块输出的三轴加速度以及三轴角速度进行滤波，并通过姿态解算转换成实际需要的角度值，比如用于飞行器的俯仰角、 横滚角、 偏航角。

由于三轴耦合性，三轴姿态解算不能简单的根据角速度去积分，积分是有累积误差的，角度很小时误差不明显，当角度很大时这个误差就会很大。随着时间的加长这个误差会变得越来越大。所以用角速率积分的方法去解算姿态是不可行的。有一种方法是借助重力加速度在三个轴上的分量来计算横滚角和俯仰角，乍一看似乎没毛病，但实际上这种方法是无法在物体动态情况下使用的。如果模块本身有加速度，是无法分辨出哪些是重力加速度的分量，哪些是模块本身加速度的分量。而且模块本身的加速度精度不高，所以使用这种方法也是不可行的。

相对于加速度，陀螺仪的精度会高一些，但是时间长了它会漂移；加速度虽然精度不高，但是没有明显的随着时间增加的漂移。卡尔曼滤波可以综合互补这两方面的情况得到较为精确的输出值。当然也可以借助互补滤波，或者使用模块内部的直接输出，测试时我分别使用了、一阶互补滤波、卡尔曼滤波，并进行了比较发现卡尔曼的效果较好。需要注意的是，使用时需要校正，它的角是不准确的，如果要输出准确的需要借助磁力计来实现。

卡尔曼滤波

简单来讲卡尔曼滤波是一种求平均的优化方法。比如，当我们判断一个电子秤或者温度计的输出往往是多次测量取其平均值；假如此时有两个电子秤，你想得到自己的真实体重，一个测得值是，一个测得的值是，那么该相信哪一个电子秤的输出值呢？假如你知道电子秤精度更高一些，更准确一些，电子秤由于使用时间长而老化精度更低一些，这个时候你会更相信多一些。这个时候你的体重就不是简单的求平均了:

y=(A+B)/2

因为更相信多一些，因此的权重会大于：

y=H∗A+(1−H)∗B

这个就是给电子秤的权重,注意这里的是小于的数值；卡尔曼滤波器会根据观测值和预测值不断的迭代运算，最终逼近真实值。非常适用于估计动态系统的最优状态，即便是系统观测值含有大量噪声，也能够通过逐步的迭代计算使得输出值更逼近真实值。关于，大家可以参考下面的专业文章：

http://en.wikipedia.org/wiki/Kalman_filter

http://www.cs.unc.edu/~welch/media/pdf/kalman_intro.pdf

http://academic.csuohio.edu/simond/courses/eec644/kalman.pdf

另外这里推荐几篇文章，对于从原理到使用都讲的很详细：

http://blog.tkjelectronics.dk/2012/09/a-practical-approach-to-kalman-filter-and-how-to-implement-it/

https://blog.csdn.net/heyijia0327/article/details/17487467

https://blog.csdn.net/xiahouzuoxin/article/details/39582483

https://segmentfault.com/a/1190000000514987

Processing平台测试

手里有一块的飞控板子，上面集成了模块，所以先在平台下进行了测试，上位机选择了,这块源码可以参考:

https://github.com/mjs513/FreeIMU-Updates/tree/master/processing,

的驱动参考官方，

可以参考:

https://github.com/TKJElectronics/Example-Sketch-for-IMU-including-Kalman-filter

借助进行跟随演示：

ROS平台测试

首先安装imu_tools，可以源码安装

安装依赖：

安装mpu6050_serial_to_imu

编译：

这里需要注意，修改串口波特率和自己的底层模块匹配，另外修改下的串口为自己系统下的串口，打开目录下的文件将端口修改为系统现有的串口，我的为，如果提示串口打不开,需要修改一下权限

另外串口的数据包格式要符合的格式，它提供了一个的底层测试文件，里面有详细的数据包格式说明。

测试时运行一下命令：

此时可以看到3D图形，可以查看一下此时都有哪些话题：

输出如下：

/clicked_point

/imu/data

/imu/temperature

/initialpose

/move_base_simple/goal

/rosout

/rosout_agg

/tf

/tf_static

选择一下IMU话题输出：

系统输出看下图：