Localization and Mapping in Confined Areas with a Hovering AUV - PowerPoint PPT Presentation

Marine Robot Localization and Navigation Workshop @ ICRA 2016 Localization and Mapping in Confined Areas with a Hovering AUV Michael Kaess Robotics Institute Carnegie Mellon University May 20, 2016

Inspecting Ships and Harbor Infrastructure SS Curtiss, San Diego Drift-free navigation + ensure full coverage Slow for covering large areas For non-complex areas we use imaging sonar instead 2 Michael Kaess

Why Sonar? Camera Sonar 3 Michael Kaess

Collaborators • MIT – John Leonard, Franz Hover, Pedro Teixeira, Josh Leighton • Univ. Michigan – Ryan Eustice, Matt Johnson-Roberson, Paul Ozog, Stephen Chaves, Jie Li • CMU – Tiffany Huang, Eric Westman 4 Michael Kaess

HAUV: Hovering Autonomous Underwater Vehicle Equipped with: – 5 Thrusters – Battery (1.5 kWh) – Ring laser gyro – Sonars: • Doppler Velocity Log (DVL) • Multi-beam sonar • Both are actuated – Cameras: • Stereo with LED lights • Periscope HULS3 – Fiber tether 5 Michael Kaess

Recent Experiments (Confined Area Search) Aug 13: USS Saratoga, Newport (324m) Aug 14: NS Savannah, Baltimore (180m) Mar+Jun 15: SS Curtiss, San Diego (180m) 6 Michael Kaess

Curtain Mission for Complex Areas 7 Michael Kaess

Close-up Inspection to Resolve Small Structures 1. De-noising sonar data 2. Eliminate drift using feature-based navigation (FBN) 8 Michael Kaess

De-noising • Difficult to extract range information because of artifacts • Several sources of error make this a difficult task – Cross-talk / side lobes – Reflections/multipath – Vehicle motion – Noise (ambient & electrical) – Target structure 9 Michael Kaess

DIDSON: Operating Principle • DIDSON operating mode: – 8 cycles to build a scan – 12 transducers fire simultaneously in each cycle – Interleave results to obtain the complete scan – 10 frames/s → 12ms/cycle • Although the motivation for this operating mode is to reduce cross talk by not firing adjacent transducers simultaneously, there is still significant cross talk: • Transducers have finite (non- zero) gain at the main lobes of other transducers in the same cycle! 10 Michael Kaess

Assembling a Point Spread Function • A 1-dimensional PSF can capture this angular dependence • Need beam pattern for transducers • Assuming invariance we can use a single beam’s beam pattern (e.g.) center beam 11 Michael Kaess

Experimental Validation Angular Radial (ignored) Sonar image (0.8mm target) 12 Michael Kaess

13 Michael Kaess

Filtering – Improved Resolution • Small object becomes visible 14 Michael Kaess

De-noising – Improved Resolution 15 Michael Kaess

FBN with Planar Surfaces • The map consists of Real-time with a handheld (infinite) planar RGB-D (Kinect) sensor surfaces Kaess, ICRA 2015 • Pool experiment: 16 Michael Kaess

Beyond Planes: FBN with Submaps • Sequential pings do not overlap • Accumulate submaps (low drift over tens of seconds) • Alignment produces pairwise pose-to-pose constraints • Integrated with vehicle navigation in factor graph • Online solution by iSAM [Kaess et al., IJRR 12] 17 Michael Kaess

Pool Experiment Dead reckoning FBN 18 Michael Kaess

FBN with Submaps • FBN eliminates long-term drift 19 Michael Kaess

Pier (PAX River 2015) Dead reckoning FBN 20 Michael Kaess

Non-Complex Area: Imaging Sonar Hull relative navigation, 1.5m standoff distance 21 Michael Kaess

Imaging Sonar Registration Frame A Frame B 22 Michael Kaess

Imaging Sonar and FBN • State-of-the-art requires planar assumption • Can we recover 3D geometry from forward-looking sonar images? • Also want to recover vehicle motion (feature-based navigation, FBN) 23 Michael Kaess

Sonar Geometry – Unknown Elevation Measured: Range r and bearing ψ Unknown: Elevation θ within opening angle of sonar, r min e.g. 28° for DIDSON r max 24 Michael Kaess

Camera Geometry – Unknown Range Measured: Image coordinates (u,v) related to bearing ψ and elevation θ Unknown: Range r 25 Michael Kaess

Multiple Views: Structure from Motion • Can recover 3D geometry from multiple views • Correspondence problem + Geometry recovery 26 Michael Kaess

Acoustic Structure from Motion (ASFM) Elevation of a feature can be recovered from multiple views! Ping i Ping 1 Ping 2 27 Michael Kaess

Factor Graph Representation Odometry measurement Robot pose Landmark measurement Landmark position Bipartite graph with variable nodes and factor nodes 28 Michael Kaess

Nonlinear Least-Squares poses landmarks argmax Θ � 𝑞 𝑗 ( Θ ) 𝑗 Gaussian noise 2 𝜖ℎ 𝑗 argmin Θ � ℎ 𝑗 Θ Ξ � ) 𝜖Θ� ℎ 𝑗 ( Θ 𝑗 � Θ Repeatedly solve linearized system A b argmin 𝜄 𝐵𝐵 − 𝑐 2 Efficient solution in online setting possible with iSAM (Kaess et al. 2008) and iSAM2 (Kaess et al. 2012) 29 Michael Kaess

Huang and Kaess, IROS 2015 Simulation – General Motion • Structure recovered with low uncertainty Side view! • Three views • Known data association • 100 Monte Carlo runs 30 Michael Kaess

Huang and Kaess, IROS 2015 Simulation – Forward Motion • Ambiguity: Cannot distinguish positive/negative elevation angle 31 Michael Kaess

Huang and Kaess, IROS 2015 Simulation – Yaw and Sideway Motion • Ambiguity: High uncertainty along arc 32 Michael Kaess

Huang and Kaess, IROS 2015 Simulation – Roll Motion • Lower uncertainty: Roll disambiguates sign of elevation 33 Michael Kaess

Huang and Kaess, IROS 2015 Simulation – Summary • Forward/pitch motion provides best constraints, followed by roll 34 Michael Kaess

Data Association is Difficult! Points are ordered by range! 35 Michael Kaess

Data Association is Difficult! (2) Moving sonar changes order of projections 36 Michael Kaess

Epipolar Geometry? Projecting samples over elevation range to find putative correspondences 37 Michael Kaess

Imaging Sonar – Boston Harbor 38 Michael Kaess

Imaging Sonar - Manually Selected Features 39 Michael Kaess

Imaging Sonar – Reprojection Error Feature points Projected estimated structure 40 Michael Kaess

Imaging Sonar – 3D Geometry • Before optimization - elevation of all features approx. equal • After optimization - elevation takes ladder structure 41 Michael Kaess

Imaging Sonar: Ladder • Front view before (left) and after (right) optimization: Elevation is clearly recovered (with some uncertainty) 42 Michael Kaess

Future Work • Further improvement of model fidelity from profiling • Point feature extraction for ASFM • More dense structure recovery • Multiple AUVs 43 Michael Kaess