Zupt, LLC's SLAM and Optimal Sensor fusion

1 de Mar de 2019
1 de 33

Zupt, LLC's SLAM and Optimal Sensor fusion

• 1. SLAM and Optimal Sensor Fusion For Subsea Model Generation and Precise Positioning w w w . Z u p t . c o m
• 2. SIMULTANEOUS LOCALIZATION AND MAPPING An Introduction with Minimal Equations Motivation? • We need a way to navigate accurately in GPS denied environments – SLAM lets us do this. • We may want to navigate where no external sensors are available. • SLAM lets us explore previously unknown environments autonomously. 2
• 3. APPLICATIONS Search and Rescue 3 Widely used today in autonomous vehicle applications – in air Simple versions used subsea (SLAM to calibrate a LBL beacon) Air Space Subsea
• 4. CURRENT EXAMPLES IN OUR INDUSTRY LBL SLAM Given: Inertial, USBL, LBL ranges, depth Find: ROV position, velocity, attitude, AND the locations of unknown transponders. Map = location of unknown transponder Localization = ROV position, velocity, and attitude 4 Stereo Inertial SLAM Given: Inertial, imagery (USBL, LBL ranges, depth also possible inputs) Find: ROV position, velocity, attitude, and the locations of millions of 3D triangulated features. Map = location of millions of unique environmental features detected Localization = ROV position, velocity, and attitude
• 5. Mapping LocalizationSLAM WHAT IS SLAM? SLAM provides the ability to position us while developing knowledge of the environment around us. Where is the world around me? Where am I? 5
• 6. PROBLEM STATEMENT What do we know (Given): Observations - z1,z2,…,zn (sensor information about the world) Control Input - u1,u2,…,un (information about trajectory changes) What do we want to know (Find): Vehicle state - position, velocity, attitude, sensor related error terms Map state - global feature locations. LBL beacon location, bathymetry or a high resolution model of our surroundings could be a map state.
• 7. SLAM PROCESS: INITIALIZATION • Choose a global frame • Small initial uncertainty • Sensor measurements initialize landmarks • Sensor could be range info, camera image, sonar or LiDAR) Airborne UAV as an example 7
• 8. SLAM PROCESS: PROPAGATION • UAV is moving • Dynamic models estimate the new location • But - uncertainty increases 8
• 9. SLAM PROCESS: U P D A T I N G A N D T H E N I N I T I A L I Z I N G N E W L A N D M A R K S • Data association matches previous landmarks • Uncertainty is decreased • New landmarks are added 9
• 10. EKF SLAM: THE CLASSICAL APPROACH Extended Kalman Filter - SLAM • Joint State Representation (vehicle and map states are estimated together) • Only suitable for small map sizes • Blue “map” states - get really big. • Taylor series linearizations introduce error • (if in prop or update phase if the equations are non linear – then represented by Taylor series (sqd, or sqrt) Single hypothesis data association – can introduce matching error mismatches 10
• 11. UKF SLAM • Unscented Kalman Filter SLAM • Propagate all points – new mean – new covariance • More accurate when motion and measurement models are nonlinear • Easy to implement • Still scales poorly for large maps • Still a single hypothesis 11
• 12. SPARSE EXTENDED INFORMATION FILTERING (SEIF) Uses inverse of the covariance matrix – so off diag’s get small and can be reduced to zero. Computational efficiency is increased at the expense of accuracy. Single hypothesis data association is still an issue. 12
• 13. SUB MAPPING Partition into small “sub-maps” – smaller covariance matrix – only uses features we are currently seeing. Sets of “sub-maps” are used to update a global map. Full map correlations are not maintained at every step EKF is used for “sub-maps” 13
• 14. FASTSLAM – THE PARTICLE FILTER APPROACH Vehicle state is represented by discrete samples Get rid of full covariance –each particle (vehicle state) believes it is good – but… Each sample has N Kalman filters attached to it (one for each landmark) only updated when we have a measurement for them. Map states independent of one another. Weights are generated according to an importance function – measurement innovation/innovation covariance = C-O used to generate weights Multi-hypothesis data association Allows for large Map sizes because features only need to be updated when they are seen 14
• 15. OPTIMIZATION BASED SLAM Maximize likelihood of joint state Graph SLAM - minimize a graph of constraints built upon vehicle motion and sensor data. Bundle Adjustment - minimize reprojection error from different viewpoints. • Can produce accurate results • Cost of performing optimization is high 15
• 16. REVIEW OF THESE VARIOUS SLAM ALGORITHMS Kalman Filter Based: • EKF and UKF do not work for large maps • Sub mapping and SEIF reduce computations at the expense of accuracy. FastSLAM: • Multi-hypothesis data association • Works well for large scale maps Optimization Based: • Can produce accurate results • Optimization is costly 16
• 17. VISUAL INERTIAL SLAM A stereo/multi camera pair, lights, acquisition/logging system and INS. Fiber to the surface for any more than 8 hours of data collection. Self contained data logging for up to 8 hours. 17
• 18. 18 3D RECON – THE BASICS • Calibration • Image Acquisition • Feature Detection • Feature Matching –feature descriptors – stereo depth estimation • Camera reference frame to earth reference frame – bundle adjustment – Fast SLAM – Sparse Reconstruction • Dense Reconstruction – disparity used to generate dense point cloud • 3D model in global frame
• 19. VISUAL-INERTIAL SLAM O U R I M P L E M E N T A T I O N O F A S U B S E A S L A M S O L U T I O N Inertial Propagation Global Feature Matching SLAM Updates and Feature Initialization 3D model in global frame Vehicle position, attitude and velocity Stereo Rectification Feature Detection and Description Local Matching Triangulation Lever Arm Adjustments Imaging Sensors IMU Dense Stereo Matching Sparse point cloud with descriptors Dense Point Cloud Dense Model Refinement
• 20. 3D RECON 3D recon is a self contained tool that delivers high resolution, geospatially correct, 3D models of what we image subsea. 3D Recon knows where it is with respect to the features it uses to build the model. 3D Recon is also a relative positioning system. 3D Recon navigates in real time while simultaneously building the 3D model. 3D Recon will not only build the model – but will allow you to very precisely navigate within the model - so you can navigate the last 3m, 2m, 5cm or even 1cm to the defined feature you want to go to on a structure, within your model. 20
• 21. 3D RECON - CALIBRATION We use a checkerboard calibration plate for our calibration process. Individual camera calibration technique is complete in both Matlab and OpenCV. A two component radial distortion model has the lowest re-projection error. Camera to camera calibration is also complete. The Camera to IMU calibration is complete within Matlab. We use a single Kalman filter that ends up with a 183 dimension state vector based on our checker board. 21 Calibration includes - individual camera, camera to camera, camera baseline to IMU
• 22. FEATURE DETECTION AND DESCRIPTION 22 We have included options for very standard machine vision feature detection techniques - ORB, SIFT, SURF. We have concluded our real time feature detection, feature matching, and triangulation software development.
• 23. FEATURE MATCHING 23
• 24. DISPARITY COMPUTATION D I F F E R E N C E I N X C O O R D I N A T E ( D E P T H ) I N B O T H I M A G E S 24 Disparity to XYZ Example Calculation:
• 25. POINT CLOUD GENERATION (SPARSE AND DENSE) 25
• 26. AUTOMATED DATA CLEANING 26
• 27. SIMULTANEOUS LOCALIZATION AND MAPPING 27 Sparse SLAM Map and Vehicle Poses Dense Point Cloud Projection using SLAM Poses Each feature point has XYZ, RBG+descriptor. Descriptor distance+XYZ distance are used for global matching for SLAM updates Each feature point has only XYZ,RBG. Down- sampling and refinements are made to further align projected point clouds.
• 28. DE-NOISING AND MESH GENERATION 28
• 29. ANALYZE STRUCTURE DEPTH 29
• 30. IMPROVING POSITIONING – SLAM AND BUNDLE ADJUSTMENT The navigation solution feeds the image processing and the knowledge gained from the feature matches improves the navigation – this is enabled within classic Simultaneous Localization and Mapping (SLAM)/Bundle Adjustment routines. This leads to very repeatable survey data. The yellow data incorrectly positioned in the left image is used to correctly combine and match (position) the data in the right image. The same feature now resides in the same geospatial location. 30
• 31. ACCURACY OF THIS DATA SET <+/-2MM 31
• 32. APPLICATIONS FOR 3D RECON Pipeline surveys – High resolution free span data, anode depletion volumes possible Metrology – Delivers metrology level accuracy, 30mm over 30m (1/1000) As Built – Delivers exactly what is on the seabed and exactly where it is – import into operator GIS Dimensional Control at Depth – Structure modeling and subsea offset determination Under Hull / SPAR Positioning – Tricky to position free moving targets in the water column. Asset Integrity Monitoring – Automated change detection, position and feature definition. Augmented Reality (valve ops / structure ID) – Identify a feature, automatically display metadata and automatically navigate to that specific feature. Precisely Navigate the last 3m to a known feature 32 Dimensions: 588mm x 100mm x 100mm
• 33. EMAIL kv@zupt.com Contact Info PHONE +1 832 295 7280 WEBSITE www.zupt.com