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

▶
a general framework for mobile robot pose tracking and
SMART_READER_LITE
LIVE PREVIEW

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

Feb 20, 2023 150 likes •493 views

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

slide-1
SLIDE 1

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

slide-2
SLIDE 2

ICRA 2013, May 6 – SDIR VIII workshop

Outline

  • Motivations
  • The ROAMFREE Project
  • Target Platforms
  • The Logical Sensor paradigma
  • Middlewares & Software Architecture
  • Sensor fusion techniques
  • Extended Kalman Filter
  • Gauss-Newton
  • Conclusions & Future work

2

ROAMFREE ROA ROAMFREE FREE

slide-3
SLIDE 3

ICRA 2013, May 6 – SDIR VIII workshop

Motivations

  • Position tracking module is a fundamental component in

autonomous robotics architectures

  • 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

3

Where am I ???

slide-4
SLIDE 4

ICRA 2013, May 6 – SDIR VIII workshop

The ROAMFREE1 project aims at developing:

  • 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

Framework description

4

1 Italian Ministry of University and Research (MIUR) through the PRIN 2009 grant “ROAMFREE: Robust Odometry

Applying Multi-sensor Fusion to Reduce Estimation Errors”.

ROAMFREE ROA ROAMF MFREE REE

slide-5
SLIDE 5

ICRA 2013, May 6 – SDIR VIII workshop

Target Platforms: “One Size Fits Them All”

  • Complex autonomous robotic applications
  • Multiple, different sensors available
  • Robust and accurate pose tracking needed
  • Cumbersome/impractical parameter calibration

5 IMU Laser Rangefinders Wheel encoders Cameras GPS Steering angle encoders Wheel encoders

ROAMFREE ROA ROAMF MFREE REE

slide-6
SLIDE 6

ICRA 2013, May 6 – SDIR VIII workshop

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

6

ROAMFREE

Sensor a Sensor b Sensor c Node 1 Node 2 Sensor d Sensor e Actuator a Actuator b

MW Application

Sensor readings Pose estimate

slide-7
SLIDE 7

ICRA 2013, May 6 – SDIR VIII workshop

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

7

M W

measurement

ROAMFREE

Sensor a Sensor b params

physical sensors logical sensor calibration feedback

slide-8
SLIDE 8

ICRA 2013, May 6 – SDIR VIII workshop

Logical Sensors - Examples

8 Wheel radius, distance, ...

par Wheel encoders FW kinematic

ROAMFREE Sensor displacement wrt robot reference frame

par GPS

ROAMFREE Camera calibration, scale factors, ...

Camera b Camera a par Visual

  • dometry

Algorithm

ROAMFREE

slide-9
SLIDE 9

ICRA 2013, May 6 – SDIR VIII workshop

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

9

slide-10
SLIDE 10

ICRA 2013, May 6 – SDIR VIII workshop

ROAMFREE Library: Kinematics More predefined logical sensors to handle common kinematics

  • Differential drive
  • Ackermann
  • Omnidirectional

The user can easily add custom logical sensors

10

slide-11
SLIDE 11

ICRA 2013, May 6 – SDIR VIII workshop

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)
  • ...

11

slide-12
SLIDE 12

ICRA 2013, May 6 – SDIR VIII workshop

Framework Architecture

GUI

Calibration suite g2o BFL

12

  • User can develop in Python or C++
  • Uses g2o and BFL external libraries

Sensor Fusion

Sensor library

SE(3) EKF Gauss Newton Roscpp node

wrappers

Rospy node

Tracking module

slide-13
SLIDE 13

ICRA 2013, May 6 – SDIR VIII workshop

Framework Architecture

GUI

Calibration suite g2o

Roscpp node

wrappers

Rospy node

BFL

13

Sensor Fusion

Sensor library

SE(3) EKF Gauss Newton

Tracking module

Online:

  • Pose tracking
  • Very flexible interface!
slide-14
SLIDE 14

ICRA 2013, May 6 – SDIR VIII workshop

Framework Architecture

GUI

Calibration suite g2o

Roscpp node

wrappers

Rospy node

BFL

14

Sensor Fusion

Sensor library

SE(3) EKF Gauss Newton

Tracking module

Offline:

  • Parameters Calibration
slide-15
SLIDE 15

ICRA 2013, May 6 – SDIR VIII workshop

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

15

sensor_msgs/ Imu gps_common/ GPSFix

rosbag Accelerometer Gyro GPS

slide-16
SLIDE 16

ICRA 2013, May 6 – SDIR VIII workshop

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

sensor_msgs/Imu gps_common/GPSFix geometry_msgs/Pose

16

wrappers Sensor Fusion Tracking module

Rospy node

Config: sensor parameters, system architecture, ...

Reading/6-DOF pose

sensor_msgs/ Imu gps_common/ GPSFix

rosbag Accelerometer Gyro GPS

slide-17
SLIDE 17

ICRA 2013, May 6 – SDIR VIII workshop

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

17

sensor_msgs/Imu gps_common/GPSFix

Rospy node

GUI Calibration suite

ROAMFREE dataset Config rosbag

Sensor Fusion

slide-18
SLIDE 18

ICRA 2013, May 6 – SDIR VIII workshop

Sensor Fusion Techniques

Two 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

18

slide-19
SLIDE 19

ICRA 2013, May 6 – SDIR VIII workshop

Gauss-Newton Pose Tracking Maintains and optimizes online a measurements hyper-graph

  • Nodes: last robot poses or unknown parameters
  • Edges: measurement constraints

Maximize the likelihood of the nodes given the sensor readings

  • Gauss-Newton (or Levenberg–Marquardt) optimization algorithm

19 Logical Sensor parameters

slide-20
SLIDE 20

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 1/8

6-DOF Pose at time t0:

20

slide-21
SLIDE 21

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 2/8

New odometry measure at time t1, Predict the next pose using measurement e.g.:

21

slide-22
SLIDE 22

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 3/8

New odometry measure at time t1, Add an odometry constraint between poses e.g.: Gauss-Newton tries to minimize these error functions.

ODO(t0,t1) 22

slide-23
SLIDE 23

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 4/8

New odometry measure at time t2,

ODO(t0,t1) ODO(t1,t2) 23

slide-24
SLIDE 24

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 5/8

New GPS measure at time t2 Absolute position constraint e.g. Sensor displacement wrt robot odometric center

ODO(t0,t1) ODO(t1,t2) GPS(t2) 24

slide-25
SLIDE 25

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 6/8

Parameter vertices are shared by edges of the same type

ODO(t0,t1) ODO(t1,t2) ODO(t2,t3) GPS(t3) GPS(t2) 25

Another absolute position constraint

slide-26
SLIDE 26

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 7/8

Edges in the past → out-of-oder measurements e.g.: acceleration measure at time t2

ODO(t0,t1) ODO(t1,t2) ODO(t2,t3) GPS(t3) GPS(t2) ACC(t0,t1,t2) 26

slide-27
SLIDE 27

ICRA 2013, May 6 – SDIR VIII workshop

Graph construction: Example – 8/8

ODO(t0,t1) ODO(t1,t2) ODO(t2,t3) ODO(t3,t4) GPS(t3) GPS(t2) ACC(t0,t1,t2) 27

Online case: Old poses and constraints are discarded as time passes

slide-28
SLIDE 28

ICRA 2013, May 6 – SDIR VIII workshop

Nice Video to Entertain Attendees

slide-29
SLIDE 29

ICRA 2013, May 6 – SDIR VIII workshop

Gauss-Newton Pose Tracking Lots of PROs

  • Iterative optimization algorithm (possibly) yields higher tracking

performances wrt other Bayesian approaches

  • Very flexible formulation thanks to the hyper-graph approach
  • Arbitrary number of sensors, possibly added at runtime
  • Asynchronous sensors, different sampling frequencies
  • Straightforward management of out-of-order measurements
  • Outliers handled robustifying error functions
  • Manifold optimization through encapsulation (Hertzberg et al., 2013)

With some CONs

  • Increased time complexity, lower operation frequency

29

slide-30
SLIDE 30

ICRA 2013, May 6 – SDIR VIII workshop

Further work

  • Run benchmarks on a variety of robotic platforms (in progress)
  • Extend the sensor library and the error models
  • Extend framework to handle multi-body platforms

(e.g.: vehicles with coachwork linked to body by suspensions)

  • Compare the EKF and the Gauss-Newton approaches

30

LURCH The Autonomous Wheelchair Quadrivio ATV Triskar 2

slide-31
SLIDE 31

ICRA 2013, May 6 – SDIR VIII workshop

Parameters Calibration Experiment

GPS displacement and IMU misalignment can be estimated employing only their noisy measurement (simulated dataset)

31

slide-32
SLIDE 32

ICRA 2013, May 6 – SDIR VIII workshop

Parameters Calibration Experiment

32

GPS displacement and IMU misalignment can be estimated employing only their noisy measurement (simulated dataset)

slide-33
SLIDE 33

ICRA 2013, May 6 – SDIR VIII workshop

Questions

33 33