Working With The Package: Robot - Localization
Working With The Package: Robot - Localization
Tom Moore
Clearpath Robotics
What is robot_localization?
• General purpose state estimation package
• No limit on the number of input data sources
• Supported message types for state estimation nodes
• nav_msgs/Odometry
• geometry_msgs/PoseWithCovarianceStamped
• geometry_msgs/TwistWithCovarianceStamped
• sensor_msgs/Imu
• State vector: [ x y z α β γ x˙ y˙ z˙ α˙ β˙ γ˙ ˙x˙ ]
˙y˙ ˙z˙
• Two typical use cases
• Fuse continuous sensor data (e.g., wheel encoder odometry and IMU) to
produce € locally accurate state estimate
• Fuse continuous data with global pose estimates (e.g., from SLAM) to
provide an accurate and complete global state estimate
Nodes
State estimation nodes
• ekf_localization_node – Implementation of an extended Kalman filter (EKF)
• ukf_localization_node – Implementation of an unscented Kalman filter (UKF)
GPS
*kf_localiza,on_node
IMU
Wheel
Encoders
*kf_localiza,on_node
Visual
Odometry
Barometer
And
so
much
more!
Source: http://wiki.ros.org/move_base
Preparing Your Sensor Data: REPs
Fact: you can’t spell “prepare” without REP.
• REP-105: http://www.ros.org/reps/rep-0105.html
• Covers naming and semantics of the “principal” coordinate frames in ROS
Preparing Your Sensor Data: Transforms
map
ekf_localization_node
Requires a
Nobase_link ! imu_link
transforms requiredtransform
Preparing Your Sensor Data: Covariance
−1
K k = Pk| k −1HTk (HTk Pk| k −1HTk + R k )
€
ˆ k| k −1 + K k ( z k − H k x
ˆ k| k = x
x ˆ k| k −1 )
T
Pk| k = (I − K k H k )Pk| k −1 (I − K k H k ) + K k R k K Tk
nav_msgs/Odometry
geometry_msgs/PoseWithCovarianceStamped
geometry_msgs/TwistWithCovarianceStamped
€
sensor_msgs/Imu
Configuring *kf_localization_node
Coordinate frame specification
<param name="map_frame" value="map"/>
<param name="odom_frame" value="odom"/>
<param name="base_link_frame" value="base_link"/>
<param name="world_frame" value="odom"/>
Configuring *kf_localization_node
Input specification
<param name="odom0" value="/controller/odom"/>
<param name="odom1" value="/some/other/odom"/>
<param name="pose0" value="/altitude"/>
<param name="pose1" value="/some/other/pose"/>
<param name="pose2" value="/yet/another/pose"/>
<param name="twist0" value="/optical_flow"/>
<param name="imu0" value="/imu/left"/>
<param name="imu1" value="/imu/right"/>
<param name="imu2" value="/imu/front"/>
<param name="imu3" value="/imu/back"/>
Configuring *kf_localization_node
Basic input configuration
<rosparam param="odom0_config">
[true, true, false, x, y, z
false, false, false, roll, pitch, yaw
false, false, false, y velocity, y velocity, z velocity
false, false, true, roll velocity, pitch velocity, yaw velocity
false, false, false] x accel., y accel., z accel.
</rosparam>
<rosparam param="imu0_config">
param="odom1_config">
[true, false,
[false, true, false,
true,
false, true,
true, false, true,
false,
false, false, false,
false, true,
true, false, true,
false,
false, true,
true, false, true]
false]
</rosparam>
<param name="odom1_differential" value="true”>
Configuring *kf_localization_node
Covariance specification (P0 and Q)
<rosparam param="initial_estimate_covariance">
[0.8, 0, ..., 1e-9]
</rosparam>
<rosparam param="process_noise_covariance">
[0.04, 0, ..., 0.02]
</rosparam>
Live Demo
Using navsat_transform_node
What does it do?
• Many robots operate outdoors and make use of GPS receivers
• Problem: getting the data into your robot’s world frame
• Solution:
• Convert GPS data to UTM coordinates
• Use initial UTM coordinate, EKF/UKF output, and IMU to generate a
(static) transform T from the UTM grid to your robot’s world frame
• Transform all future GPS measurements using T
• Feed output back into EKF/UKF
Required Inputs
• nav_msgs/Odometry (EKF output, needed for robot’s current pose)
• sensor_msgs/Imu (must have a compass, needed to determine global heading)
• sensor_msgs/NavSatFix (output from your navigation satellite device)
Using navsat_transform_node
Relevant settings
/odometry/filtered /odometry/gps
(/nav_msgs/Odometry) (/nav_msgs/Odometry)
/imu/data
(sensor_msgs/Imu)
/gps/fix
(sensor_msgs/NavSatFix) navsat_transform_node
Thank you!
http://wiki.ros.org/robot_localization
[email protected]