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

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

原文地址

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

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

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

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

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文件

接着打开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
        rospy.Subscriber('input', PoseWithCovarianceStamped, self.pub_ekf_odom)
        
        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这个文件里