robot_pose_ekf配置和使用

最近重新搭建了一辆ros小车, 在校准里程计后，发现小车在建图和导航过程中雷达扫描和地图信息之间还是会存在时不时的偏移，在了解robot_pose_ekf这个包后，考虑着将里程计信息和IMU(惯性测量单元)融合，来实现更加精准的定位。但使用过程中遇到不少的坑，所以在这里记录记录吧。

准备工作

在对应的工作空间下下载对应的包，并编译

git clone https://github.com/ros-planning/robot_pose_ekf.git

配置相关

打开robot_pose_ekf.launch可以看到

<launch>
<node pkg=“robot_pose_ekf” type=“robot_pose_ekf” name=“robot_pose_ekf”/>
<param name=“output_frame” value=“odom_combined”/>
<param name=“base_footprint_frame” value=“base_footprint”/>
<param name=“freq” value=“30.0”/>
<param name=“sensor_timeout” value=“1.0”/>
<param name=“odom_used” value=“true”/>
<param name=“imu_used” value=“true”/>
<param name=“vo_used” value=“true”/>
<remap from=“odom” to=“pr2_base_odometry/odom” />
</node>
</launch>

这里可以看到robot_pose_ekf这个节点监听并融合 里程计，imu, 视觉这三个传感器的信息，分别对应odom_used, imu_used, vo_used 着三个参数，由于这里不使用视觉传感器，所以vo_used 这里的参数设置为false. 此外，该节点将融合后的位置信息，以名为odom_combined的名字发出，取代了原有的odom,为了过多的修改，这里把output_frame参数改为odom,由于tf_tree的父坐标系只能有一个，所以要更改底盘驱动所发送的坐标变化，这里使用的是ros_arduino_python作为底盘驱动。(由与这里话题不需要映射，所以把remap这一行注释掉）

在base_controller.py这个文件中，我们注释掉一下代码

   # Create the odometry transform frame broadcaster.
       self.odomBroadcaster.sendTransform(
           (self.x, self.y, 0),
           (quaternion.x, quaternion.y, quaternion.z, quaternion.w),
           rospy.Time.now(),
           self.base_frame,
           "odom"
           )

接着发布imu和base_footprint　之间的静态tf转变(看了很多教程都有这点，但我觉得imu获取到的信息与周围的环境之间的位置之间并无关系,应该不添加效果也是一样，当然如果imu与机器人之间存在坐标轴之间的旋转，那应该就要添加了)

<node name=“base_to_imu_link” pk=“tf” type=“static_transform_publisher” args=“0 0 0 0 0 0” base_footprint imu_link"/>

下面配置前的tf图

配置后

可以看到发布odom坐标系的节点名变为了robot_pose_ekf了

至此，robot_pose_efk的配置就完成了，but在运行时又出现新的问题了

[ERROR] [1587284286.565322789]: filter time older than odom message buffer
[ERROR] [1587284286.581064157]: Covariance specified for measurement on topic wheelodom is zero

经过查阅资料后发现robot_pose_ekf所用的扩展卡尔曼滤波需要分别提供对应的协方差，这里imu传感器对应协方差在相应的包中已经提供，而编码器所测得的的却没有提供，所以接下来还需要为编码器测得的里程计提供相应的协方差，这里先采用turtlebot里程计所提供的数据.在
turtlebot_create这个包/create_node/src/create_node目录下，可以找到covariances这个文件，里面是对应的协方差。打开turtlebot_node.py我们可以看到

if sensor_state.requested_right_velocity == 0 and
sensor_state.requested_left_velocity == 0 and
sensor_state.distance == 0:
odom.pose.covariance = ODOM_POSE_COVARIANCE2
odom.twist.covariance = ODOM_TWIST_COVARIANCE2
else:
odom.pose.covariance = ODOM_POSE_COVARIANCE
odom.twist.covariance = ODOM_TWIST_COVARIANCE

在运动和静止时分别提供了两组协方差，依葫芦画瓢，在我们的底盘驱动中，在发布里程计信息前加入

       if vxy!=0 and vth!=0:
           odom.pose.covariance = ODOM_POSE_COVARIANCE
           odom.twist.covariance = ODOM_TWIST_COVARIANCE
       else:
           odom.pose.covariance = ODOM_POSE_COVARIANCE2
           odom.twist.covariance = ODOM_TWIST_COVARIANCE2

vxy,vth分别对应小车的线速度和角速度

至此就能使用融合后的里程计数据了
说多了都是泪，不知道是不是融合上数据有问题，地图偏移程度比没融合大得多，有时间再调试看看吧

#＃# 参考
[1]: https://blog.csdn.net/shenghuaijing3314/article/details/78220151
[2]: https://blog.csdn.net/EAIBOT/article/details/51405152?locationNum=1