New Concepts in Robotic Navigation & Mapping

New Concepts in Robotic Navigation & Mapping Martin Adams Dept. Electrical Engineering, AMTC University of Chile (martin@ing.uchile.cl)

Presentation Outline 1. Autonomous Robotics in Society. 2. What s in a Measurement: Landmark Existence and Spatial Uncertainty Why Radar? 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. RFS versus Vector Based SLAM Results. 4. Future Work in Robotics.

Autonomy in Urban Environments DARPA Urban Challenge. Sensor based road lane & pedestrian detection. Perform complex manoeuvres merging, passing, parking, interaction with manned vehicles [Thrun 2007, Leonard 2007]. Singapore: TechX Challenge Localisation, Path Planning, Object Recognition & Interaction [Adams 2009].

Autonomy in Mines/Rugged Terrains Rio Tinto + ACFR developing autonomous mining technologies [Durrant-Whyte 2007] Codelco developing unmanned haulage system FrontRunner at Gaby copper mine, Chile [Komatsu 2007] Singapore: TechX Challenge Sensing Issues: Millimetre Wave Radar in Surface Mining [Brooker 2006]. Localisation, Path Planning, Object Recognition & Interaction [Adams 2009].

Autonomy in Mines/Rugged Terrains Shovel Loaders, El Teniente Mine, Chile

Intelligent Transportation Systems (ITS) Platooning and Vehicle Following. Optimisation and monitoring of traffic flow needed. [Martinet et. al. 2005, Broggi 2000, Parent 1996]. ITS Austria: Traffic monitoring & optimisation. [Corvette/Connect Projects]. INRIA (Schiphol/Heathrow Airport Passenger Pick up).

Service Robotics Autonomous straddle carriers ACFR port automation projects in Brisbane & Singapore, [Durrant-Whyte 2007]. Hospital vehicles for delivery of food/laundry in Singapore [Swisslog]. Rehabilitation robotics [Tomatis BlueBotics 2008]

Security Robotics Surveillance and monitoring vehicles capable of indoor/outdoor motion. [Adams 2004]. KIRAS Security Research [Austrian FFG protection of infrastructure].

Security Robotics Surveillance and monitoring vehicles capable of indoor/outdoor motion. [Adams 2004]. KIRAS Security Research [Austrian FFG protection of infrastructure]. Remaining issues: Sensor modelling & fusion. Robust localisation/object representation SLAM in complex environments. System Flexibility: Path replanning/unexpected situations.

3D Laser Detection & Ranging (LADAR) Continuous 360 scanning unit. 200m max range. Time of Flight pulsed Riegl LADAR Accuracy (1 Sigma) 2cm.

A*Star: Fusing 3D LADAR & Vision [Sok, Adams 2009] Approx. calibrate 3D ladar + camera data -> Coloured 3D point cloud. OpenGL2

PCA: Dominant Data Directions, Extracted Planes Section of Original Image Segmented Image

Sensing the Environment Clearpath Robotic Skid Steer Platform Acumine Radar 360 deg. scanning unit, 94GHz FMCW Sick LD-LRS1000 Scanning LRF Microsoft Kinect camera system Video El Teniente

Sensing the Environment: Detection Errors The random nature of detections Radar LRF + Line RANSAC Visual SURF Features

Sensing the Environment: Detection Statistics Feature absence & presence statistics Radar LRF + Line RANSAC Visual SURF Features

Sensing the Environment: Radar

Radar Based Projects: A*Star - Radar vs. Ladar Wider beam width Foliage penetration

Importance of P fa False Alarms Radar detections registered to ground truth location.

Radar Based Projects: A*Star - Radar vs. Ladar Video: Raw_Data_Display.avi

Radar vs. Ladar Sample of NTU University Campus Dataset

Radar vs. Ladar Laser Map Radar Map Note the rich data output due to the multi-target detection capabilities of the radar, to those of the laser.

SLAM Fundamentals In an unknown environment robot & feature positions must be estimated simultaneously - SLAM. SLAM is a probabilistic algorithm p( xt, m z1: t, u1: t) xt = State of the robot at time t m = Map of the environment z u 1: 1: t t = Sensor inputs from time 1 to t = Control inputs from time 1 to t Update distribution estimate with Bayes theorem.

SLAM Fundamentals Initial State and Uncertainty t=0 Using Range Measurements

SLAM Fundamentals t=1 Predict Robot Pose and Uncertainty at time 1

SLAM Fundamentals Correct pose and pose uncertainty t=1 Predict new feature positions and their uncertainties

SLAM Fundamentals Predict pose and uncertainty of pose at time 2 t=2 Predict feature measurements and their uncertainties

SLAM Fundamentals Correct pose and mapped features Update uncertainties for mapped features t=2 Estimate uncertainty of new features

SLAM: A Real Experiment SLAM using line intersection features in 80m by 35m office corridors. Vid 4, Vid 5

A Random Finite Set (RFS) Approach [Mullane, Vo, Adams 09] Given M = [m Given Given 1 X: 1 2 3 4 5 6 7 2 X : M = [m,m, m,m,m,m, m] M = [m,m,m, m 4 3 2 1 5 7 6 3 X :,m,m,m,m,m,m,m, m,m 6 7 5 4 3 2 1 Estimated map vector depends on vehicle trajectory? ] ] RFS makes more sense as order of features cannot/should not be significant [Mullane, Adams 2009].

A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] 1 2 3 4 5 6 7 M =[m,m,m,m,m,m,m ]? 1 2 3 4 5 6 7

A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] 1 2 3 4 5 6 7 M =[m,m,m,m,m,m,m ] 1 2 3 4 5 6 7

A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] 1 2 3 4 5 6 7 M =[m,m,m,m,m,m,m ] 1 2 3 4 5 6 7 Current vector formulations require data association (DA) prior to Bayesian update: Why? Features & measurements rigidly ordered in vector-valued map state. RFS approach does not require DA. Why? Features & measurements are finite valued sets. No distinct order assumed.

What is a RFS Measurement?

RFSs versus Vectors for SLAM Vector Based Mapping and SLAM

RFSs versus Vectors for SLAM RFS Based Mapping and SLAM

How to do RFS SLAM Intensity Function From Point Process Theory: A Random Finite Set can be approximated by its first order moment The Intensity function [Mahler 2003, Vo 2006]. v k

RFS SLAM Intensity Function From Point Process Theory: A Random Finite Set can be approximated by its first order moment The Intensity function [Mahler 2003, Vo 2006]. v k v k has the following properties:

RFS SLAM Intensity Function From Point Process Theory: A Random Finite Set can be approximated by its first order moment The Intensity function [Mahler 2003, Vo 2006]. v k v k has the following properties: 1. Its integral, over the set, gives the estimated number of elements within the set.

RFS SLAM Intensity Function From Point Process Theory: A Random Finite Set can be approximated by its first order moment The Intensity function [Mahler 2003, Vo 2006]. v k v k has the following properties: 1. Its integral, over the set, gives the estimated number of elements within the set. 2. The locations of its maxima correspond to the estimated values of the set members.

RFS SLAM Intensity Function From Point Process Theory: A Random Finite Set can be approximated by its first order moment The Intensity function [Mahler 2003, Vo 2006]. v k v k has the following properties: 1. Its integral, over the set, gives the estimated number of elements within the set. 2. The locations of its maxima correspond to the estimated values of the set members. Intensity function can be propagated through the Probability Hypothesis Density (PHD) filter.

Example: 1D Intensity Function (PHD) E.g. 2 Features located at x=1 and x=4 with spatial variance: i.e. Feature set {1, 4} Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 2σ = 2 2 2 Note: Maxima of PHD occur near x=1 and x=4 and PHD ( x) dx = 1 + 1 = 2 = No.of targets!

Example: 1D Intensity Function (PHD) E.g. 2 Features located at x=1 and x=4 with spatial variance: i.e. Feature set {1, 4} Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 2σ = 2 2 2 Important Point: A PHD is NOT a PDF, since in general it does not integrate to unity! Note: Maxima of PHD occur near x=1 and x=4 and PHD ( x) dx = 1 + 1 = 2 = No.of targets!

RFS SLAM Intensity Function Gaussian mixture representation of intensity function, showing peaks at feature locations at time k-1. Notice 2 features at (5, -8) represented by single, unresolved Gaussian with mass 2. Black crosses show true feature locations.

RFS SLAM Intensity Function Gaussian mixture representation of intensity function at time k. Peaks at feature locations (5, -8) now resolved - 2 Gaussians, mass 1. Note feature at (-5, -4) has reduced local mass, due to a small likelihood over all measurements.

RFS Versus Vector Based SLAM Comparative results for the proposed GM-PHD SLAM filter (black) and that of FastSLAM (red), compared to ground truth (green).

RFS Versus Vector Based SLAM -2 The raw dataset at a clutter density of 0.03 m.

RFS Versus Vector Based SLAM The estimated trajectories of the GM-PHD SLAM filter (black) and that of FastSLAM (red). Estimated feature locations (crosses) are also shown with the true features (green circles).

RFS Versus Vector Based SLAM Feature number estimates.

RFS Versus Vector Based SLAM Sample data registered from radar.

RFS Versus Vector Based SLAM SLAM input: Odometry path + radar data Extracted point feature measurements registered to odometry.

RFS Versus Vector Based SLAM NN-EKF FastSLAM PHD-SLAM EKF, FastSLAM and PHD-SLAM with Radar data.

Singapore MIT Alliance: CENSAM Project Environmental monitoring of coastal waters. Navigation and map info. necessary above/below water surface. Fusion of sea surface radar, sub-sea sonar data for combined surface/sub-sea mapping. Autonomous Kayak Surface Vehicle with Radar

RFS Versus Vector Based SLAM Singapore: Video: kayak+radar_10_1_11.avi

Singapore MIT Alliance: CENSAM Project

Singapore MIT Alliance: CENSAM Project Surface and sub-sea data. Verification of radar/sonar data with coastal satellite images.

Singapore MIT Alliance: CENSAM Project Coastal Mapping, Surveillance, HARTS / AIS verification Mobile platform can remove blind spots from land-based radar. Video: CoastalModelling.avi Video: CoastalandAIS.avi

RFS Versus Vector Based SLAM GPS Trajectory (Green Line), GPS point feature coordinates (Green Points), Point feature measurement history (Black dots).

RFS Versus Vector Based SLAM Top: Posterior MHT SLAM estimate (red). Bottom: Posterior RB-PHD SLAM estimate (blue). Ground truth (Green).

RFS Versus Vector Based SLAM (Red) MHT SLAM Feature Number estimate. (Blue) PRB-PHD SLAM Feature Number estimate. (Green) Actual Number to enter FoV at each time index.

Navigation in Complex Environments Navigation in the presence of clutter. on land. Victoria Park Data Set [ACFR, University Sydney].

Navigation in Complex Environments Navigation in the presence of clutter. underwater. Forward looking sonar data [John Leonard, MIT].

Navigation in Complex Environments Australian Institute of Marine Science & ACFR: AUV Sirius for underwater monitoring. [Stefan Williams 2007]. BHP Billiton: Sponsors Trials At Ningaloo Reef for testing suitability of robots for reef monitoring. [Stefan Williams 2007]. Forward looking sonar data [John Leonard, MIT].

Navigation in Complex Environments Australian Institute of Marine Science & ACFR: AUV Sirius for underwater monitoring. [Stefan Williams 2007]. Successful sensing & navigation required in environments with complex feature representations, disturbed by clutter. BHP Billiton: Sponsors Trials At Ningaloo Reef for testing suitability of robots for reef monitoring. [Stefan Williams 2007]. Forward looking sonar data [John Leonard, MIT].

Conclusions & Future Work 1. Improve robustness of autonomous sensing systems. 2. Probabilistic sensor modelling - improve robustness to sensor bias, random walk, clutter, false alarms and missed detections. 3. Pursue Radar technologies in mining environments for surface reconstruction and mapping. 4. Surface & sub-sea joint environmental/navigation estimation. 5. Pursue processing of radar for pedestrian/traffic monitoring in ITS. Improve traffic safety through communicating sensor technologies.

Acknowledgements The Research Team: John Mullane, Ba-Ngu Vo, Samuel Keller, Chhay Sok, Liu Bingbing, Ebi Jose, Tang Fan, Lochana Perera, Zhang Sen, Zen Koh, Bimas Winaju, Sardha Wijesoma, Andy Shacklock, Akshay Rao, Tan Chai Soon..!