使用robot_pose_ekf对传感器信息融合

　robot_pose_ekf是ROS Navigation stack中的一个包，通过扩展卡尔曼滤波器对imu、里程计odom、视觉里程计vo的数据进行融合，来估计平面移动机器人的真实位置姿态，输出odom_combined消息。robot_pose_ekf只适用于平面上的轮式移动机器人，因此odom信息中的z，pitch和roll分量可以被忽略。IMU可以提供车体坐标系相对于世界坐标系的姿态（RPY角），其中Roll和Pitch是绝对角度，因为有重力方向作为参考，而偏航角Yaw则是一个相对角度（如果IMU中没有集成电子罗盘测量地球磁场角作为参考）。IMU姿态的协方差矩阵代表了姿态测量的不确定度。

　　robot_pose_ekf默认监听的topic为：imu_data、odom和vo，因此要注意发布消息时topic的名称要对应，否则会起不到滤波作用（The robot_pose_ekf node does not require all three sensor sources to be available all the time. A source can appear and disappear over time, and the node will automatically detect and use the available sensors）。不想使用默认名称的话可以用remap元素进行名称重映射。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="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>
    </node>
 </launch>

　　参数说明如下：

freq：滤波器更新和发布频率。注意频率高仅仅意味着一段时间可以获得更多机器人位姿信息，但是并不表示可以提高位姿估计的精度。
sensor_timeout：当传感器停止向滤波器发送信息时，滤波器在没有传感器的情况下等待多长时间才重新开始工作。
odom_used, imu_used, vo_used：确认是否输入。
output_frame, base_footprint_frame：用于指定输出tf变换中坐标系的名字，默认为odom_combined和base_footprint。

Published Topics

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)： The output of the filter (the estimated 3D robot pose)

　　rostopic list命令可以查看ros中的topic，下图中白色的/robot_pose_ekf/odom_combined话题就是robot_pose_ekf节点发布的：

Provided tf Transforms

odom_combined → base_footprint

　　robot_pose_ekf在输出odom_combined信息同时还会发布相关的坐标变换，输入下面指令查看tf变换关系：

rosrun rqt_tf_tree rqt_tf_tree

　　可以看出robot_pose_ekf节点会发布base_footprint坐标系相对于odom_combined坐标系的变换：

　　参数output_frame和base_footprint_frame默认为odom_combined和base_footprint，也可以根据需要在launch文件中进行更改，比如分别改为odom和base_link，再次用rqt_tf_tree命令查看，如下图所示：

robot_pose_ekf节点默认会从odom、imu_data、vo这三个topic上订阅消息，可以使用remap将其映射到新名称的topic上。重映射是基于替换的思想，每个重映射包含一个原始名称和一个新名称。每当节点使用重映射中的原始名称时，ROS客户端库就会将它默默地替换成其对应的新名称。

    <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
        <param name="output_frame" value="odom"/>
        <param name="base_footprint_frame" value="base_link"/>
        <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="false"/>
        <param name="debug" value="false"/>
        <param name="self_diagnose" value="false"/>

        <remap from="imu_data" to="imu" />      <!-- 将节点订阅的imu_data话题改名为imu -->
    </node>
 </launch>

　　下面在VREP中进行仿真测试：将移动机器人目录中的lumibot模型拖入场景中，并将加速度计和陀螺仪安装在机器人上，并在lumibot_body上添加一个脚本，发布里程计和imu信息（可以在发布的信息上添加一些噪声）。注意使用robot_pose_ekf进行滤波时传感器的协方差矩阵信息不能空着，否则可能会出现错误，因此要设置合理的值。

　　imu数据的协方差矩阵设置可以参考：https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/gyro.py

self.imu_data.orientation_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]
self.imu_data.angular_velocity_covariance = [1e6, 0, 0, 0, 1e6, 0, 0, 0, 1e-6]

　　底盘运动时odom的协方差矩阵如下，参考：https://github.com/Arkapravo/turtlebot/blob/master/turtlebot_node/src/turtlebot_node/covariances.py

ODOM_POSE_COVARIANCE = [1e-3, 0, 0, 0, 0, 0,

                        0, 1e-3, 0, 0, 0, 0,
                        0, 0, 1e6, 0, 0, 0,
                        0, 0, 0, 1e6, 0, 0,
                        0, 0, 0, 0, 1e6, 0,
                        0, 0, 0, 0, 0, 1e3]

ODOM_TWIST_COVARIANCE = [1e-3, 0, 0, 0, 0, 0, 
                         0, 1e-3, 0, 0, 0, 0,
                         0, 0, 1e6, 0, 0, 0,
                         0, 0, 0, 1e6, 0, 0,
                         0, 0, 0, 0, 1e6, 0,
                         0, 0, 0, 0, 0, 1e3]

注意imu信息的协方差矩阵中代表机器人航向角的分量方差为1e-6，而里程计信息的协方差矩阵中机器人姿态分量的协方差为1e3，两个值相差很大。在进行EKF融合时，会更“相信”imu提供的姿态信息，因为其方差更小。比如机器人在转动过程中轮子发生了打滑，用编码器推算出的姿态一直在旋转，而实际姿态（主要由IMU测量得到）却没发生太大变化，这种情况就需要使用信息融合方法来减小误差。协方差矩阵中的参数设置非常重要，要根据传感器手册或者实际使用测量来确定。

　　VREP脚本中可以订阅odom_combined消息，在回调函数中设置odomCombined相对于odom的位置，然后通过Graph记录下来，绘制出轨迹曲线。

function subscriber_callback(msg)

    -- This is the subscriber callback function
    simSetObjectPosition(odomCombinedHandle,odomHandle,{msg.pose.pose.position.x, msg.pose.pose.position.y, msg.pose.pose.position.z})
end


if (sim_call_type==sim_childscriptcall_initialization) then
　　 
　　subscriber = simExtRosInterface_subscribe('/robot_pose_ekf/odom_combined','geometry_msgs/PoseWithCovarianceStamped','subscriber_callback')
   simExtRosInterface_subscriberTreatUInt8ArrayAsString(subscriber)

end

运行robot_pose_ekf.launch文件，然后开始VREP仿真。下图中蓝色的曲线是使用robot_pose_ekf融合后的机器人运动轨，红色为原始的带噪声的轨迹曲线（这只是一个例子，实际效果怎么样还要调整各种参数）：

　　另外需要注意的是，robot_pose_ekf会发布base_link到odom的tf变换，因此我们自己的程序中就不用发布了，否则会出现冲突（在tf树中是不能构成回路的，只能有一个父坐标系，但是可以有很多子坐标系）。下图是仿真过程中rivz显示的原始odom（黄色箭头）和融合后的odom_combined（红色箭头）信息，以及base_link坐标系和odom坐标系间的变换关系。