使用GY-85校准iRobot create

iRobot作为turtlebot的基座之一，并没有内置的陀螺仪，只是依靠轮子上的编码器提供角度信息。由此造成运动一段时间后，角度信息出现偏差，进一步导致iRobot在/odom中的位置坐标与实际位置出现差异。针对这一问题，可以通过外部接入陀螺仪进行校准。

硬件设备：iRobot create2， GY-85芯片， Firefly-RK3288嵌入式开发板

开发板与GY-85芯片通过i2c接口通讯，从芯片中读取陀螺仪数据后，将数据以sensor_msgs/Imu的格式发送到主题/imu_data上。主题/imu_data上的数据与来自主题/odom上的数据都将被节点/robot_pose_ekf订阅，节点/robot_pose_ekf对两方数据进行融合处理。处理后的角度及坐标数据将以geometry_msgs/PoseWithCovarianceStamped的格式发送到主题/robot_pose_ekf/odom_combined上，并将odom→base_footprint的tf变换发送到/tf上（wiki上说是odom_combined→base_footprint，但实际应该是odom→base_footprint，待定）。

遇到的问题：

（1）：最初几次读取时，陀螺仪数据误差很大，需要去除。在iRobot静止时，陀螺仪中读取的原始数据并不为0，而是围绕某一值上下波动。如果不对数据进行处理，将产生极大的累计误差，使数据失去意义。

（2）：关于calibration，可以让iRobot转动一定圈数，对陀螺仪的角速度数据积分得到所转角度度数，用测量数据与实际数据的比值对陀螺仪进行校准。

（3）：因为主题/robot_pose_ekf/odom_combined上的数据格式为geometry_msgs/PoseWithCovarianceStamped，该格式在rviz上无法显示。可以写一个节点，将geometry_msgs/PoseWithCovarianceStampe格式转为geometry_msgs/PoseStamped格式，从而在rviz中显示；或者在rviz添加RobotModel，显示gyro_link，并将Show Trail及Show Axes选中，同样可以实现观测效果。

下面是两张运动轨迹的图片：

整个运动轨迹如下图所示：红线是齿轮编码器记录的轨迹，外加陀螺仪校准后的运动轨迹（我采用的是显示方式中的第二个方法，所以只能显示最后几秒的轨迹）



这是原点附近的细节图：图中灰实线的交点即为原点，可以看出，经陀螺仪数据校准后，精度有较大改善。



整个系统的节点图：其中/gyropx节点为陀螺仪节点，安卓手机客户端的相关的节点与本文关系不大，就没有显示。