ROS下使用MPU6050惯性测量模块

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

MPU6050

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

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

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

卡尔曼滤波

简单来讲卡尔曼滤波是一种求平均的优化方法。比如，当我们判断一个电子秤或者温度计的输出往往是多次测量取其平均值；假如此时有两个电子秤，你想得到自己的真实体重，一个测得值是60kg，一个测得的值是65kg，那么该相信哪一个电子秤的输出值呢？假如你知道电子秤A精度更高一些，更准确一些，电子秤B由于使用时间长而老化精度更低一些，这个时候你会更相信A多一些。这个时候你的体重就不是简单的求平均了:
$y=(A+B)/2$
因为更相信A多一些，因此A的权重会大于50%：
$y=H*A +(1-H)*B$
这个H就是给电子秤A的权重,注意这里的H是小于1的数值；卡尔曼滤波器会根据观测值和预测值不断的迭代运算，最终逼近真实值。kalman filter 非常适用于估计动态系统的最优状态，即便是系统观测值含有大量噪声，kalman filter也能够通过逐步的迭代计算使得输出值更逼近真实值。关于 kalman Filter ，大家可以参考下面的专业文章：

• 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

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

• 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平台测试

由于我手里有一块STM32的飞控板子，上面集成了mpu6050模块，所以先在STM32平台下进行了测试，上位机选择了Processing,这块源码可以参考 FreeIMU,mpu6050的驱动参考官方 Invensense Motion Driver，kalman filter可以参考Example-Sketch-for-IMU-including-Kalman-filter。
借助Processing进行3D跟随演示：

ROS平台测试

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

$cd ~/catkin_ws/src/
$git clone https://github.com/ccny-ros-pkg/imu_tools.git

安装依赖：

$rosdep install imu_tools

安装 mpu6050_serial_to_imu

$cd ~/catkin_ws/src/
$git clone https://github.com/fsteinhardt/mpu6050_serial_to_imu.git

编译：

$cd ~/catkin_ws/
$ catkin_make

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

$sudo chmod 777 /dev/ttyUSB0

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

测试时运行一下命令：

$roscore &
$roslaunch mpu6050_serial_to_imu demo.launch

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

$ rostopic list

输出如下：

/clicked_point
/imu/data
/imu/temperature
/initialpose
/move_base_simple/goal
/rosout
/rosout_agg
/tf
/tf_static

选择一下IMU话题输出：

$ rostopic echo /imu/data

系统输出看下图：