[学习SLAM]ROS下robot_pose_ekf扩展卡尔曼融合包的使用案例（robot_pose_ekf） 篇章 (3)

       最近在使用imu进行数据融合，使用的是robot_pose_ekf的融合包，发现网上的教程太不详细，折腾了1个月，终于搞定了。于是乎，想写一个关于ekf包的使用教程。里程计和惯导模块imu的数据融合来得到一个不易丢失的机器人姿态。首先，你得有一个imu，而且得校准好，我使用的是razor 9dof的imu，通过ros包pub一个Imu的data类型。
这是我google的比较有用的一个ekf包的使用教程
首先，先给大家看下我的tf!

单里程计的tf
这张图是单使用里程计的tf图片，你可以通过

rosrun rqt_tf_tree rqt_tf_tree

来查看自己的tf，维护好自己的tf对我们来说至关重要，稍有不慎就会出现很多麻烦，别问我是怎么知道的，都是泪，从上面的图片可以看到odom–base_footprint是通过arduino转换的，这一部分一般都是在我们与底盘通信的node里发布的。
这里写图片描述

就是这里，这部分代码就是实现上面tf转换的源代码，为什么要把这部分提出来呢，我们之后再说。好了，下面我们开始说怎么使用ekf包了。 odom–base_footprint的tf转换，所以我们使用这个包第一步就是要将我们上面提到的那一段代码注释掉， 因为ekf包会为我们处理好这部分tf，所以不需要我们发布变换了，这一步很重要！ 之后我们需要加一个imu的link，我推荐直接使用静态tf发布吧。

  <node name="base_imu_link" pkg="tf" type="static_transform_publisher" args="0 0 0 0 3.1415926 0  /base_link /base_imu_link 50"/>

这里就OK了，之后我们就可以使用ekf包来融合了，首先开启底盘的launch，来发布odom的topic，然后开启imu的launch来发布imu的topic，确保这两个topic有数据，可以rostopic echo 来查看，之后运行robot_pose_ekf的launch文件，为了查看ekf包是否正常工作，可以用下面代码：

rosservice call /robot_pose_efk/get_status

查看Odometry sensor和IMU sensor 是否is used ，is active来确保ekf包的是否正确使用，下面贴出我的融合成功后的tf：

 

可以看到这里的odom–base_footprint的转换就是由/robot_pose_ekf发布的了，至于融合后的效果就是要看imu的校准度了，看效果的话可以用rviz或者导出rosbag包用matlab的plot函数绘出来，matlab可能更直观点， 至于如何使用matlab绘制里程计data，有时间我再贴出来吧。上面的内容是我自己总结出来的，有什么错误或不足大家可以指出来，一起学习，共同进步哈。
 

 

---

ROS机器人底盘-IMU和里程计融合

1.概述

实际使用中会出现轮子打滑和累计误差的情况，这里单单使用编码器得到里程计会出现一定的偏差，虽然激光雷达会纠正，但一个准确的里程对这个系统还是较为重要

2.IMU数据获取

IMU即为 惯性测量单元，一般包含了三个单轴的加速度计和三个单轴的陀螺仪，简单理解通过加速度二次积分就可以得到位移信息、通过角速度积分就可以得到三个角度，实时要比这个复杂许多

2.1 PIBOT IMU

PIBOT在嵌入式程序提供出原始的数据接口，通过配置可以输出原始raw_imu topic，

 

raw_imu

该topic类型为自定义具体如下，即为三轴加速度三轴陀螺仪和三轴磁力计的原始数据

 

raw_imu

 

 

raw_imu

 

通过对原始数据处理得到一个/imu/data_raw数据类型为sensor_msgs/Imu，

 

通过ROS提供的相关包imu_tools进行滤波
可以看到complementary_filter_gain_node会订阅该topic，即该topic作为输入滤波得到最终数据(发布/imu/data topic 类型同样为sensor_msgs/Imu)

 

输出该topic可以看到得到的值波动已经较小了，且静止的时候接近于0

 

/imu/data

 

3.两种融合的方法

3.1 一种简单的方法

从imu得到的数据为一个相对角度(主要使用yaw，roll和pitch 后面不会使用到)，使用该角度来替代由编码器计算得到的角度。
这个方法较为简单，出现打滑时候因yaw不会受到影响，即使你抬起机器人转动一定的角度，得到的里程也能正确反映出来

3.2 扩展的卡尔曼滤波

官方提供了个扩展的卡尔曼滤波的包robot_pose_ekf，robot_pose_ekf开启扩展卡尔曼滤波器生成机器人姿态，支持

• odom（编码器）
• imu_data（IMU）
• vo（视觉里程计）
  还可以支持GPS
  引用官方图片 

•  

  
  PR2从实际初始点(绿色)溜达一圈回到初始点(绿色)，编码器的里程(蓝色)发生了漂移，而使用robot_pose_ekf融合出来的里程(红色)则跟实际位置基本重合(后面我们会针对这个测试下效果)

中间的圆是小圆放大的展示效果

再回去看下该包的输出

 

 

• 发布一个topic， 类型需要注意下是PoseWithCovarianceStamped并非Odometry
  后面会用到这个作为显示，所以还需要一个转换

   

  
  查看该topic信息可以看到odom_ekf订阅了该topic
  再次查看该节点信息可以看到

   

  ，他会发出一个Odometry的topic

   

• 发出一个tf

在robot_pose_ekf配置时，做了些映射处理，这样可以保证导航层在使用和不用imu的时候无需修改就可以工作

 

bringup.lauch或者bringup_with_imu.launch 输出的tf都为odom → base_footprint ;发出的里程也都是odom
bringup_with_imu.launch轮子的里程topic 映射为wheel_odom

这里很重要，后面的对该包的验证会使用到

下2张图展示了未使用IMU和使用IMU时候的tf tree情况, 可以看到用了一致的frame

 

no imu

 

use imu

参考  作者：PIBOT导航机器人 链接：https://www.jianshu.com/p/ea936eaf76c9