Robotic Navigation - Experience Gained with RADAR

Transcription

1 Robotic Navigation - Experience Gained with RADAR Martin Adams Dept. Electrical Engineering, AMTC Universidad de Chile (martin@ing.uchile.cl)

2 Presentation Outline 1. Mapping, Tracking & SLAM at U. Chile. 2. Sensor Measurement & Detections: Landmark Existence and Spatial Uncertainty. 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. Set Based Likelihood Formulation. RFS SLAM A generalization of Random Vector SLAM. RFS versus Vector Based SLAM Results. 4. Future Work in RFS based Mapping/SLAM.

3 Presentation Outline 1. Mapping, Tracking & SLAM at U. Chile. 2. Sensor Measurement & Detections: Landmark Existence and Spatial Uncertainty. 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. Set Based Likelihood Formulation. RFS SLAM A generalization of Random Vector SLAM. RFS versus Vector Based SLAM Results. 4. Future Work in RFS based Mapping/SLAM.

4 Mapping in Mines/Rugged Terrains Mapping El Teniente s Esmeralda Mine. - VIDEO

5 Mapping in Mines/Rugged Terrains Individual LADAR scans Registered LADAR scans from 45 individual scans

6 Space Debris Tracking Detecting & Tracking space debris between 1 and 10 cubic cm based on optical & radio telescopic data Vicuña (Chile) NEO Space telescope (U. Chile + U. Serena) Chilbolton 3GHz range/doppler radar for SSA

7 Space Debris Tracking Goals: Provide tracking SW framework robust to significant false alarms & radar/visual blind zones

8 SLAM in outdoor/mining environments

9 SLAM in outdoor/mining environments Improved estimation and incorporation of detection statistics & measurement likelihoods in RFS based SLAM

10 Presentation Outline 1. Mapping, Tracking & SLAM at U. Chile. 2. Sensor Measurement & Detections: Landmark Existence and Spatial Uncertainty. 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. Set Based Likelihood Formulation. RFS SLAM A generalization of Random Vector SLAM. RFS versus Vector Based SLAM Results. 4. Future Work in RFS based Mapping/SLAM.

11 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

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

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

14 What s in a Measurement?

15 What s in a Measurement?

16 What s in a Measurement? Robotic Interpretation:

17 What s in a Measurement? Robotic Interpretation:

18 What s in a Measurement? Robotic Interpretation: Radar Interpretation:

19 What s in a Measurement? Robotic Interpretation: Radar Interpretation:

20 What s in a Measurement? In reality Probability of Detection less than unity, but may not be known.

21 What s in a Measurement? In reality Probability of Detection less than unity, but may not be known. However, landmark/feature measurements in SLAM result from a feature detection algorithm.

22 What s in a Measurement? In reality Probability of Detection less than unity, but may not be known. However, landmark/feature measurements in SLAM result from a feature detection algorithm. Principled algorithms provide estimates of be estimated a-priori (e.g. RANSAC). P D and P fa, or they can

23 What s in a Measurement? In reality Probability of Detection less than unity, but may not be known. However, landmark/feature measurements in SLAM result from a feature detection algorithm. Principled algorithms provide estimates of be estimated a-priori (e.g. RANSAC). P D and P fa, or they can Ideal scenario: Represent all detection hypotheses in terms of their: r ( i 2 k σ, k ) i, P D ( r i k ) and P fa (i.e. range, spatial uncertainty, detection uncertainty and false alarm probability).

24 The Importance of Missed Detections Wider beam width Foliage penetration

25 The Importance of False Alarms Radar detections registered to ground truth location.

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

27 Radar vs. Ladar Sample of NTU University Campus Dataset

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

29 Presentation Outline 1. Mapping, Tracking & SLAM at U. Chile. 2. Sensor Measurement & Detections: Landmark Existence and Spatial Uncertainty. 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. Set Based Likelihood Formulation. RFS SLAM A generalization of Random Vector SLAM. RFS versus Vector Based SLAM Results. 4. Future Work in RFS based Mapping/SLAM.

30 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.

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

32 A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] M =[m,m,m,m,m,m,m ]?

33 A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] M =[m,m,m,m,m,m,m ]

34 A Random Finite Set (RFS) Approach Untangle: Z=[z,z,z,z,z,z,z ] M =[m,m,m,m,m,m,m ] 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.

35 What is a RFS Measurement?

36 Representing Maps Q: Why do we even care about error in the number of landmarks? A: Catastrophic consequences in applications such as search & rescue, obstacle avoidance, UAV missions

37 RFSs versus Vectors for SLAM Vector Based Mapping and SLAM

38 RFSs versus Vectors for SLAM RFS Based Mapping and SLAM

39 SLAM Formulations System model: Robot pose Vector-based formulations: Bayes filter Motion input Noise parameters Landmark position Data association Batch estimation

40 SLAM Formulations RFS-based formulations: Bayes filter Measurement likelihood Batch estimation Pose / map distribution Normalizing factor Need to examine the relationship between the RFS and randomvector forms of the high-lighted distributions Measurement likelihood

41 SLAM Pose/Map Distribution Janossy density for RFS Pick 1 / m! possible map ordering Fix the map size to m, such that the cardinality distribution: RFS density Random-vector density

42 SLAM Normalizing Factor Set integral: Fix the map size: RFS integral Random-vector integral

43 SLAM Measurement Likelihood Multi-target likelihood RFS measurement likelihood

44 SLAM Formulations Select one data association hypothesis : RFS measurement likelihood Assume for all associated landmarks that In addition, for all non-associated measurements, assume Random-vector measurement likelihood

45 SLAM Ideal Detection Conditions The RFS formulation is equivalent to the vector-based formulation when: Map size is fixed / deterministic (with one ordering) Data association is assumed Probability of detection equals 1 for associated landmarks Probability of non-associated measurements being clutter equals 1 RFS-SLAM is a generalization of random-vector SLAM

46 How to do RFS SLAM PHD Approximation 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

47 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:

48 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.

49 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.

50 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.

51 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} [Mahler 2007]. Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 2σ = 2 2 2

52 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} [Mahler 2007]. Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 2σ = 2 2 2

53 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} [Mahler 2007]. Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 2σ = Note: Maxima of PHD occur near x=1 and x=4 and PHD ( x) dx = = 2 = No.of targets!

54 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} [Mahler 2007]. Suitable Gaussian Mixture PHD: PHD( x) 1 ( x 1) exp 2 2πσ 2σ σ 2 =1 + ( x 4) exp 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 = = 2 = No.of targets!

55 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.

56 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.

57 RFS Versus Vector Based SLAM Results based on single feature strategy: Comparative results for the proposed GM-PHD SLAM filter (black) and that of FastSLAM (red), compared to ground truth (green).

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

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

60 RFS Versus Vector Based SLAM Feature number estimates.

61 RFS Versus Vector Based SLAM Sample data registered from radar.

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

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

64 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

65 Singapore MIT Alliance: CENSAM Project

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

67 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

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

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

70 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.

71 RFS Versus Vector Based SLAM Results based on multi-feature strategy: Videos: Victoria park with added clutter (21,500 clutter detections) MH-FastSLAM with clutter RB-PHD-SLAM with clutter. Parque O Higgins with natural clutter MH-FastSLAM with clutter (joggers in park) RB-PHD-SLAM with clutter.

72 RFS Versus Vector Based SLAM Objective: Show that Random Finite Set (RFS) SLAM is a generalization of the random-vector based SLAM formulation Approach: Mathematically show that the RFS Bayes Filter can be reduced to the Bayes filter in its random-vector form, under a set of ideal detection conditions. This can also be shown for the batch estimation approach / formulation Ideal Detection Conditions: Perfect detection of landmarks No clutter / false alarms Map size is deterministic Assumed data association hypothesis Close to ideal detection conditions Non-ideal detection conditions

73 RFS Versus Vector Based SLAM Importance of findings: Understanding of why RFS-based SLAM algorithms perform better under non-ideal detection conditions Understanding of why vector-based SLAM algorithms perform better under ideal detection conditions Experimental Validation: Ideal vs. non-ideal detection conditions RB-PHD-SLAM vs. FastSLAM Simulations and real datasets

74 RFS Versus Vector Based SLAM Close to ideal conditions Non-ideal conditions FastSLAM RB-PHD-SLAM FastSLAM RB-PHD-SLAM

75 Presentation Outline 1. Mapping, Tracking & SLAM at U. Chile. 2. Sensor Measurement & Detections: Landmark Existence and Spatial Uncertainty. 3. Simultaneous Localisation & Map Building (SLAM). A Random Finite Set (RFS) Approach. Set Based Likelihood Formulation. RFS SLAM A generalization of Random Vector SLAM. RFS versus Vector Based SLAM Results. 4. Future Work in RFS based Mapping/SLAM.

76 Conclusions & Future Work 1. Improve robustness of autonomous sensing systems. 2. Probabilistic sensor modelling - improve estimates of detection statistics, e.g. - based on occlusions. 3. Other FISST SLAM techniques based on Labelled Multi-Bernoulli (MeMBer) Filter. 4. Metrics beyond OSPA for the intuitive evaluation and comparison of SLAM maps.

77 Acknowledgements The Research Teams: Javier Ruiz-del-Solar, Daniel Lühr, Felipe Inostroza, Keith Leung, 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..!

78 C++ Library for RFS SLAM Open source, with BSD-3 License Dependencies: Boost::math_c Boost::timer 1.48 Boost::system 1.48 Boost::thread 1.48 Eigen3 Tested on Ubuntu Template library Define your own process models Define your own measurement models Includes an implementation of the RB-PHD Filter Includes a 2-d SLAM example Well documented Will be updated with new published research Download at: