Turtlebot学习指导第四篇＿使用robot_pose_ekf包,EKF(扩展卡尔曼滤波器)对机器人位置进行校正

本文链接：https://blog.csdn.net/u010918541/article/details/51209666

我们使用 robot_pose_ekf 包对通过结合'odom'和'gyro data'信息对机器人位置进行校正.

1,配置这个包里有个默认的配置文件可以修改,大致如下

freq:滤波的频率,不会改变准确度

sensor_timeout:传感器停止向滤波器发送信息之后,等待多久接收下一个传感器的信息

odom_used, imu_used, vo_used: 确认是否输入

<span style="font-size:18px;"><launch>
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <param name="output_frame" value="odom"/>
    <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"/>
    <param name="debug" value="false"/>
    <param name="self_diagnose" value="false"/>
  </node>
 </launch></span>

2,节点

2.1 robot_pose_ekf应该订阅的话题

odom (nav_msgs/Odometry)

2D pose (used by wheel odometry):2DPose包含一机器人的位置以及转向信息,2D其实也代表了3D位置信息,只不过Z方向,roll和pitch方向被忽略

imu_data (sensor_msgs/Imu)

3D orientation (used by the IMU):3D旋转信息包含了roll,pitch,yaw方向的信息

vo (nav_msgs/Odometry)

3D pose (used by Visual Odometry):3D位置信息全方位表明了机器人的位置信息和旋转信息,比如GPS信息,轮子Odom只能提供2D位置信息.

一般的位置估计只需要前两个信息.

2.2robot_pose_ekf发布的话题

robot_pose_ekf/odom_combined (geometry_msgs/PoseWithCovarianceStamped)

发布出估计的3D位置信息

2.3提供的TF变换

odom_combined → base_footprint

示例演示

在ros by exampe一书中 7.8节 Out and Back Using Odometry中

$ roslaunch rbx1_bringup odom_ekf.launch

这个launch就是调用了robot_pose_ekf对机器人的位置进行估计,打开launch文件

<launch>
  <node pkg="rbx1_bringup" type="odom_ekf.py" name="odom_ekf" output="screen">
    <remap from="input" to="/odom_combined"/>
    <remap from="output" to="/odom_ekf"/>
  </node>
</launch>

接着打开odom_ekf.py

#!/usr/bin/env python

""" odom_ekf.py - Version 1.1 2013-12-20

    Republish the /robot_pose_ekf/odom_combined topic which is of type 
    geometry_msgs/PoseWithCovarianceStamped as an equivalent message of
    type nav_msgs/Odometry so we can view it in RViz.

    Created for the Pi Robot Project: http://www.pirobot.org
    Copyright (c) 2012 Patrick Goebel.  All rights reserved.

    This program is free software; you can redistribute it and/or modify
    it under the terms of the GNU General Public License as published by
    the Free Software Foundation; either version 2 of the License, or
    (at your option) any later version.5
    
    This program is distributed in the hope that it will be useful,
    but WITHOUT ANY WARRANTY; without even the implied warranty of
    MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE.  See the
    GNU General Public License for more details at:
    
    http://www.gnu.org/licenses/gpl.html
      
"""

import rospy
from geometry_msgs.msg import PoseWithCovarianceStamped
from nav_msgs.msg import Odometry

class OdomEKF():
    def __init__(self):
        # Give the node a name
        rospy.init_node('odom_ekf', anonymous=False)

        # Publisher of type nav_msgs/Odometry
        self.ekf_pub = rospy.Publisher('output', Odometry, queue_size=5)
        
        # Wait for the /odom_combined topic to become available
        rospy.wait_for_message('input', PoseWithCovarianceStamped)
        
        # Subscribe to the /odom_combined topic
      <span style="color:#FF0000;">  <span style="font-size:18px;">rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)</span></span>
        
        rospy.loginfo("Publishing combined odometry on /odom_ekf")
        
    def pub_ekf_odom(self, msg):
        odom = Odometry()
        odom.header = msg.header
        odom.child_frame_id = 'base_footprint'
        odom.pose = msg.pose
        
        self.ekf_pub.publish(odom)
        
if __name__ == '__main__':
    try:
        OdomEKF()
        rospy.spin()
    except:
        pass

这个节点就订阅/odom_combined下的位置估计信息然后发布的

robot_pose_ekf包的启动同样在

rbx1/rbx1_bringup/launch/includes/tb_create_mobile_base.launch.xml这个文件里

<launch>
  <!-- Turtlebot Driver -->
  <node pkg="create_node" type="turtlebot_node.py" name="turtlebot_node" respawn="true" args="--respawnable">
    <param name="bonus" value="false" />
    <param name="update_rate" value="30.0" />
    <param name="port" value="/dev/ttyUSB0" />
    <remap from="cmd_vel" to="mobile_base/commands/velocity" />
    <remap from="turtlebot_node/sensor_state" to="mobile_base/sensors/core" />
    <remap from="imu/data" to="mobile_base/sensors/imu_data" />
    <remap from="imu/raw" to="mobile_base/sensors/imu_data_raw" />
  </node>

  <!-- Enable breaker 1 for the kinect -->
  <node pkg="create_node" type="kinect_breaker_enabler.py" name="kinect_breaker_enabler"/>

  <span style="font-size:18px;color:#FF0000;"><!-- The odometry estimator -->
  <node pkg="robot_pose_ekf" type="robot_pose_ekf" name="robot_pose_ekf">
    <remap from="imu_data" to="mobile_base/sensors/imu_data"/>
    <remap from="robot_pose_ekf/odom_combined" to="odom_combined"/>
    <param name="freq" value="10.0"/>
    <param name="sensor_timeout" value="1.0"/>
    <param name="publish_tf" value="true"/>
    <param name="odom_used" value="true"/>
    <param name="imu_used" value="true"/>
    <param name="vo_used" value="false"/>
    <param name="output_frame" value="odom"/>
  </node></span>

  <!-- velocity commands multiplexer -->
  <node pkg="nodelet" type="nodelet" name="mobile_base_nodelet_manager" args="manager"/>
  <node pkg="nodelet" type="nodelet" name="cmd_vel_mux" args="load yocs_cmd_vel_mux/CmdVelMuxNodelet mobile_base_nodelet_manager">
    <param name="yaml_cfg_file" value="$(find turtlebot_bringup)/param/mux.yaml"/>
    <remap from="cmd_vel_mux/output" to="mobile_base/commands/velocity"/>
    <remap from="cmd_vel_mux/input/navi" to="cmd_vel"/>
  </node>
</launch>