Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization?...
-
Upload
nguyenkiet -
Category
Documents
-
view
252 -
download
4
Transcript of Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization?...
![Page 1: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/1.jpg)
Working with the robot_localization PackageTom Moore
Clearpath Robotics
![Page 2: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/2.jpg)
What is robot_localization?
• General purpose state estimation package• No limit on the number of input data sources
• 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
• State vector:
€
x y z α β γ ˙ x ˙ y ˙ z ˙ α ˙ β ˙ γ ˙ ̇ x ˙ ̇ y ˙ ̇ z [ ]
• Supported message types for state estimation nodes• nav_msgs/Odometry• geometry_msgs/PoseWithCovarianceStamped• geometry_msgs/TwistWithCovarianceStamped• sensor_msgs/Imu
![Page 3: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/3.jpg)
NodesState estimation nodes
Sensor preprocessing nodes
• ekf_localization_node – Implementation of an extended Kalman filter (EKF)• ukf_localization_node – Implementation of an unscented Kalman filter (UKF)
• navsat_transform_node – Allows users to easily transform geographic coordinates (latitude and longitude) into the robot’s world frame (typically map or odom)
![Page 4: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/4.jpg)
robot_localization and the ROS Navigation Stack
Source: http://wiki.ros.org/move_base
*kf_localiza,on_node
*kf_localiza,on_node
IMU
amcl
Wheel Encoders
Visual Odometry Barometer
And so much more!
gmapping navsat_transform_node
GPS
![Page 5: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/5.jpg)
Preparing Your Sensor Data: REPs
Fact: you can’t spell “prepare” without REP.
Two important REPs• REP-103: http://www.ros.org/reps/rep-0103.html
• Covers standards for units and basic coordinate frame conventions
• REP-105: http://www.ros.org/reps/rep-0105.html• Covers naming and semantics of the “principal” coordinate frames in ROS
![Page 6: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/6.jpg)
Preparing Your Sensor Data: Transforms
map
odom
base_link
pose data
twist data*
ekf_localization_node
ekf_localization_node
* and IMU data
![Page 7: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/7.jpg)
Preparing Your Sensor Data: Frame IDs
Requires a base_link ! imu_link transformNo transforms required
![Page 8: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/8.jpg)
Preparing Your Sensor Data: Covariance
€
ˆ x k|k−1 = f ˆ x k−1|k−1( )Pk|k−1 = JPk−1|k−1J
T + Q
€
K k = Pk|k−1HkT Hk
TPk|k−1HkT + Rk( )−1
ˆ x k|k = ˆ x k|k−1 + K k zk −Hk ˆ x k|k−1( )Pk|k = I −K kHk( )Pk|k−1 I −K kHk( )T + K kRkK k
T
initial_estimate_covariance (P0) process_noise_covariance
nav_msgs/Odometrygeometry_msgs/PoseWithCovarianceStampedgeometry_msgs/TwistWithCovarianceStampedsensor_msgs/Imu
![Page 9: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/9.jpg)
Configuring *kf_localization_node
<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"/>
Coordinate frame specification
![Page 10: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/10.jpg)
Configuring *kf_localization_node
<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"/>
Input specification
![Page 11: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/11.jpg)
<rosparam param="odom1_config"> [true, true, true, false, false, false, false, false, false, false, false, false, false, false, false]</rosparam>
Configuring *kf_localization_node
<rosparam param="odom0_config"> [true, true, false, false, false, false, false, false, false, false, false, true, false, false, false]</rosparam>
Basic input configuration
x, y, z roll, pitch, yaw y velocity, y velocity, z velocity roll velocity, pitch velocity, yaw velocity x accel., y accel., z accel.
<rosparam param="imu0_config"> [false, false, false, true, true, true, false, false, false, true, true, true, true, true, true]</rosparam>
<param name="odom1_differential" value="true”>
![Page 12: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/12.jpg)
Covariance specification (P0 and Q)
Configuring *kf_localization_node
<rosparam param="initial_estimate_covariance">[0.8, 0, ..., 1e-9]
</rosparam>
<rosparam param="process_noise_covariance">[0.04, 0, ..., 0.02]
</rosparam>
![Page 13: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/13.jpg)
Live Demo
![Page 14: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/14.jpg)
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)
![Page 15: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/15.jpg)
Using navsat_transform_nodeRelevant settings
<param name="magnetic_declination_radians" value="0"/> <param name="yaw_offset" value="0"/> <param name="zero_altitude" value="true"/> <param name="broadcast_utm_transform" value="true"/> <param name="publish_filtered_gps" value="true"/>
![Page 16: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/16.jpg)
Using navsat_transform_nodeTypical Setup
navsat_transform_node
*kf_localization_node (map)
/odometry/filtered (/nav_msgs/Odometry)
/odometry/gps (/nav_msgs/Odometry)/imu/data
(sensor_msgs/Imu)/gps/fix (sensor_msgs/NavSatFix)
(Sensor data)
*kf_localization_node (odom)(Sensor data)
![Page 17: Working with the robot localization Package - ROSCon · PDF fileWhat is robot_localization? • General purpose state estimation package • No limit on the number of input data sources](https://reader034.fdocuments.in/reader034/viewer/2022052207/5a7944427f8b9af91c8c2e5c/html5/thumbnails/17.jpg)
robot_localization in the Wild
From this… (ayrbot)
…to this (OTTO)
robot_localization works on a broad range of robots!