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

我们使用
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>