A General Framework for Mobile Robot Pose Tracking and Multi Sensors - PowerPoint PPT Presentation

A General Framework for Mobile Robot Pose Tracking and Multi Sensors Self-Calibration Davide Cucci, Matteo Matteucci {cucci, matteucci}@elet.polimi.it Dipartimento di Elettronica, Informazione e Bioingegneria, Politecnico di Milano, Italy

2 Outline ROAMFREE ROA FREE ROAMFREE • Motivations • The ROAMFREE Project  Target Platforms  The Logical Sensor paradigma  Middlewares & Software Architecture • Sensor fusion techniques  Extended Kalman Filter  Gauss-Newton • Conclusions & Future work ICRA 2013, May 6 – SDIR VIII workshop

3 Motivations • Position tracking module is a fundamental component in autonomous robotics architectures Where am I ??? • Too often ad-hoc methods are employed  Platform dependent  Cumbersome calibration procedures  Limited reusability • Sensor fusion is a mature field  Build frameworks upon established techniques for robust sensor fusion and calibration ICRA 2013, May 6 – SDIR VIII workshop

4 Framework description The ROAMFREE 1 project aims at developing: ROA ROAMF MFREE REE ROAMFREE  Ready to use library of sensors and kinematics  6-DOF accurate and robust pose tracking module  Calibration suite for intrinsic sensor parameters (e.g.: sensors gains, biases, displacements, misalignments) Core concepts:  Independence from physical hardware and robotic platform  Turn-on-and go but flexible and extensible  Optimized C++ core libraries / Python bindings  Interfaces to 1 Italian Ministry of University and Research (MIUR) through the PRIN 2009 grant “ROAMFREE: Robust Odometry Applying Multi-sensor Fusion to Reduce Estimation Errors”. ICRA 2013, May 6 – SDIR VIII workshop

5 Target Platforms: “One Size Fits Them All” GPS Laser IMU Rangefinders Cameras Steering angle encoders Wheel Wheel encoders encoders • Complex autonomous robotic applications ROA ROAMF MFREE REE ROAMFREE • Multiple, different sensors available • Robust and accurate pose tracking needed • Cumbersome/impractical parameter calibration ICRA 2013, May 6 – SDIR VIII workshop

6 Reference Architecture & Middleware • Complex robotic applications often employ a distributed architecture with a middleware connecting multiple nodes transparently • The ROAMFREE toolsuite integrates without changes to existing software architecture Sensor a Sensor b Sensor c Sensor d Sensor e Node 1 Node 2 Actuator a MW Actuator b Sensor readings ROAMFREE Application Pose estimate ICRA 2013, May 6 – SDIR VIII workshop

7 Logical Sensors • To abstract from physical hardware we deal with logical sensors  Black-box odometry information sources  Defined by the type of measurement they provide (e.g.: angular velocity, absolute position, …) • The measurement is a function of a set of calibration parameters estimated by ROAMFREE logical sensor measurement Sensor a ROAMFREE M W Sensor b params calibration feedback physical sensors ICRA 2013, May 6 – SDIR VIII workshop

8 Logical Sensors - Examples Camera calibration, Wheel radius, scale factors, ... distance, ... Sensor displacement wrt robot reference frame Wheel Camera a Camera b encoders GPS Visual FW odometry kinematic Algorithm par par par ROAMFREE ROAMFREE ROAMFREE ICRA 2013, May 6 – SDIR VIII workshop

9 ROAMFREE Library: Sensors ROAMFREE provides default logical sensor wrappers to handle  Position and velocity in world frame: e.g., Global Positioning System  Linear and angular velocity in sensor frame: e.g., Visual odometry, Gyros  Acceleration in sensor frame: e.g., Accelerometers  Vector field in sensor frame: e.g., Earth Magnetic Field, Gravity Field ICRA 2013, May 6 – SDIR VIII workshop

10 ROAMFREE Library: Kinematics More predefined logical sensors to handle common kinematics  Differential drive  Ackermann  Omnidirectional The user can easily add custom logical sensors ICRA 2013, May 6 – SDIR VIII workshop

11 ROAMFREE Library - Parameters We consider all the common sources of distortion, bias and noise • For each logical sensor  Displacement and misalignment wrt Robot reference frame • For angular velocity, acceleration, vector field  Gains  Biases  Non-orthogonality of sensor axes • Kinematic models  Wheel radius, distance (Differential Drive)  Wheelbase (Ackermann)  ... ICRA 2013, May 6 – SDIR VIII workshop

12 Framework Architecture • User can develop in Python or C++ • Uses g 2 o and BFL external libraries Rospy node Roscpp GUI node wrappers Calibration Tracking suite module Sensor Fusion Gauss SE(3) Newton EKF Sensor library g 2 o BFL ICRA 2013, May 6 – SDIR VIII workshop

13 Framework Architecture Online:  Pose tracking  Very flexible interface! Rospy node Roscpp GUI node wrappers Calibration Tracking suite module Sensor Fusion Gauss SE(3) Newton EKF Sensor library g 2 o BFL ICRA 2013, May 6 – SDIR VIII workshop

14 Framework Architecture Offline:  Parameters Calibration Rospy node Roscpp GUI node wrappers Calibration Tracking suite module Sensor Fusion Gauss SE(3) Newton EKF Sensor library g 2 o BFL ICRA 2013, May 6 – SDIR VIII workshop

15 Example: ROS integration – 1/3 • Sensor readings usually available through standard ROS msg (e.g.: sensor_msgs/Imu, gps_common/GPSFix) possibly logged with rosbags GPS rosbag gps_common/ GPSFix sensor_msgs/ Imu Gyro Accelerometer ICRA 2013, May 6 – SDIR VIII workshop

16 Example: ROS integration – 2/3 • ROAMFREE rospy node 1) subscribes to sensors topics and provides sensor parameters 2) drives the main sensor fusion library with sensor readings 3) publishes the resulting pose estimate Config: GPS rosbag sensor parameters, gps_common/ system architecture, ... GPSFix gps_common/GPSFix sensor_msgs/Imu Reading/6-DOF pose Rospy node geometry_msgs/Pose wrappers sensor_msgs/ Tracking Imu module Sensor Fusion Gyro Accelerometer ICRA 2013, May 6 – SDIR VIII workshop

17 Example: ROS integration – 3/3 • Offline calibration case:  Rosbags are employed to generate a ROAMFREE dataset  The dataset is feeded into the calibration GUI to estimate unknown sensor parameters ROAMFREE rosbag dataset Config gps_common/GPSFix sensor_msgs/Imu Rospy node GUI Calibration suite Sensor Fusion ICRA 2013, May 6 – SDIR VIII workshop

18 Sensor Fusion Techniques T wo modules available for sensor fusion • Graph-based Gauss-Newton optimization  Online tracking and calibration  Tracks the trajectory of the robot over a finite time window (instead of the single pose at time t )  Parameters calibration → long time window • Extended Kalman Filter in the SE(3) Lie Group  Online tracking  Work in progress ICRA 2013, May 6 – SDIR VIII workshop

19 Gauss-Newton Pose Tracking Maintains and optimizes online a measurements hyper-graph • Nodes: last robot poses or unknown parameters • Edges: measurement constraints Logical Sensor parameters Maximize the likelihood of the nodes given the sensor readings • Gauss-Newton (or Levenberg–Marquardt) optimization algorithm ICRA 2013, May 6 – SDIR VIII workshop

20 Graph construction: Example – 1/8 6-DOF Pose at time t 0 : ICRA 2013, May 6 – SDIR VIII workshop

21 Graph construction: Example – 2/8 Predict the next pose using measurement e.g.: New odometry measure at time t 1 , ICRA 2013, May 6 – SDIR VIII workshop

22 Graph construction: Example – 3/8 ODO(t 0 ,t 1 ) Add an odometry constraint between poses e.g.: Gauss-Newton tries to minimize these error functions. New odometry measure at time t 1 , ICRA 2013, May 6 – SDIR VIII workshop

23 Graph construction: Example – 4/8 ODO(t 0 ,t 1 ) ODO(t 1 ,t 2 ) New odometry measure at time t 2 , ICRA 2013, May 6 – SDIR VIII workshop

24 Graph construction: Example – 5/8 ODO(t 0 ,t 1 ) ODO(t 1 ,t 2 ) Absolute position constraint GPS(t 2 ) e.g. Sensor displacement wrt robot odometric center New GPS measure at time t 2 ICRA 2013, May 6 – SDIR VIII workshop

25 Graph construction: Example – 6/8 ODO(t 0 ,t 1 ) ODO(t 1 ,t 2 ) ODO(t 2 ,t 3 ) GPS(t 3 ) Another absolute position constraint GPS(t 2 ) Parameter vertices are shared by edges of the same type ICRA 2013, May 6 – SDIR VIII workshop

26 Graph construction: Example – 7/8 ODO(t 0 ,t 1 ) ODO(t 1 ,t 2 ) ODO(t 2 ,t 3 ) ACC(t 0 ,t 1 ,t 2 ) GPS(t 3 ) GPS(t 2 ) Edges in the past → out-of-oder measurements e.g.: acceleration measure at time t 2 ICRA 2013, May 6 – SDIR VIII workshop

27 Graph construction: Example – 8/8 ODO(t 0 ,t 1 ) ODO(t 1 ,t 2 ) ODO(t 2 ,t 3 ) ODO(t 3 ,t 4 ) ACC(t 0 ,t 1 ,t 2 ) GPS(t 3 ) GPS(t 2 ) Online case: Old poses and constraints are discarded as time passes ICRA 2013, May 6 – SDIR VIII workshop

Nice Video to Entertain Attendees ICRA 2013, May 6 – SDIR VIII workshop