Optimization-based legged odometry and sensor fusion for legged robot continuous localization

Abstract In this manuscript, we tackle the problem of a continuous localization of a legged robot. We propose a novel, optimization-based procedure for the state estimation of the robot using measurements from internal sensors (legged odometry). Then, we propose the optimization-based integration of the legged odometry and the visual SLAM output. The proposed multi-modal localization system can continuously estimate the pose of the robot in various conditions despite fast motions of the robot, slippages or image motion blur. We provide the results of the real-time implementation of the proposed method on a multi-legged walking robot. We compare the proposed localization method to other state of the art localization systems and provide the dataset for future comparisons.

[1]  Stefan Roth,et al.  Covariance Matrix Adaptation for Multi-objective Optimization , 2007, Evolutionary Computation.

[2]  Michal R. Nowicki,et al.  Improving accuracy of feature-based RGB-D SLAM by modeling spatial uncertainty of point features , 2016, 2016 IEEE International Conference on Robotics and Automation (ICRA).

[3]  Andrew Howard,et al.  Real-time stereo visual odometry for autonomous ground vehicles , 2008, 2008 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[4]  Roland Siegwart,et al.  State estimation for legged robots on unstable and slippery terrain , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[5]  Daniel Cremers,et al.  LSD-SLAM: Large-Scale Direct Monocular SLAM , 2014, ECCV.

[6]  Stergios I. Roumeliotis,et al.  A Square Root Inverse Filter for Efficient Vision-aided Inertial Navigation on Mobile Devices , 2015, Robotics: Science and Systems.

[7]  Simona Nobili,et al.  Direct visual SLAM fusing proprioception for a humanoid robot , 2017, 2017 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[8]  Hauke Strasdat,et al.  Scale Drift-Aware Large Scale Monocular SLAM , 2010, Robotics: Science and Systems.

[9]  Daniel E. Koditschek,et al.  Design Principles for a Family of Direct-Drive Legged Robots , 2016, IEEE Robotics and Automation Letters.

[10]  Davide Scaramuzza,et al.  Ultimate SLAM? Combining Events, Images, and IMU for Robust Visual SLAM in HDR and High-Speed Scenarios , 2017, IEEE Robotics and Automation Letters.

[11]  Mike Stilman,et al.  State Estimation for Legged Robots - Consistent Fusion of Leg Kinematics and IMU , 2012, RSS 2012.

[12]  G. Klein,et al.  Parallel Tracking and Mapping for Small AR Workspaces , 2007, 2007 6th IEEE and ACM International Symposium on Mixed and Augmented Reality.

[13]  Jan Faigl,et al.  Stereo vision-based localization for hexapod walking robots operating in rough terrains , 2016, 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS).

[14]  Michael Bosse,et al.  Keyframe-based visual–inertial odometry using nonlinear optimization , 2015, Int. J. Robotics Res..

[15]  R. Siegwart,et al.  ROBOT-CENTRIC ELEVATION MAPPING WITH UNCERTAINTY ESTIMATES , 2014 .

[16]  Dominik Belter.  EFFICIENT REACTIVE BEHAVIOR FOR SIX-LEGGED WALKING ON ROUGH TERRAIN WITH PROPRIOCEPTIVE SENSING , 2017 .

[17]  James Kennedy,et al.  Particle swarm optimization , 2002, Proceedings of ICNN'95 - International Conference on Neural Networks.

[18]  Piotr Skrzypczynski,et al.  Adaptive Motion Planning for Autonomous Rough Terrain Traversal with a Walking Robot , 2016, J. Field Robotics.

[19]  Peter Fankhauser,et al.  ANYmal - a highly mobile and dynamic quadrupedal robot , 2016, IROS 2016.

[20]  Rüdiger Dillmann,et al.  Reactive posture behaviors for stable legged locomotion over steep inclines and large obstacles , 2014, 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[21]  Piotr Skrzypczynski,et al.  Precise self-localization of a walking robot on rough terrain using parallel tracking and mapping , 2013, Ind. Robot.

[22]  Stefan Leutenegger,et al.  ElasticFusion: Dense SLAM Without A Pose Graph , 2015, Robotics: Science and Systems.

[23]  Jan Faigl,et al.  An experimental study on feature-based SLAM for multi-legged robots with RGB-D sensors , 2017, Ind. Robot.

[24]  Frank Dellaert,et al.  On-Manifold Preintegration for Real-Time Visual--Inertial Odometry , 2015, IEEE Transactions on Robotics.

[25]  Roland Siegwart,et al.  Unified temporal and spatial calibration for multi-sensor systems , 2013, 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[26]  Roland Siegwart,et al.  Navigation planning for legged robots in challenging terrain , 2016, IROS 2016.

[27]  S. Umeyama,et al.  Least-Squares Estimation of Transformation Parameters Between Two Point Patterns , 1991, IEEE Trans. Pattern Anal. Mach. Intell..

[28]  Roland Siegwart,et al.  RGB–D terrain perception and dense mapping for legged robots , 2016, Int. J. Appl. Math. Comput. Sci..

[29]  Pawel Wawrzynski,et al.  Robust estimation of walking robots velocity and tilt using proprioceptive sensors data fusion , 2015, Robotics Auton. Syst..

[30]  Joachim Clemens,et al.  Extended Kalman filter with manifold state representation for navigating a maneuverable melting probe , 2016, 2016 19th International Conference on Information Fusion (FUSION).

[31]  Wolfram Burgard,et al.  A benchmark for the evaluation of RGB-D SLAM systems , 2012, 2012 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[32]  Juan D. Tardós,et al.  ORB-SLAM2: An Open-Source SLAM System for Monocular, Stereo, and RGB-D Cameras , 2016, IEEE Transactions on Robotics.

[33]  Andrew Y. Ng,et al.  Stereo vision and terrain modeling for quadruped robots , 2009, 2009 IEEE International Conference on Robotics and Automation.

[34]  Karl A. Stol,et al.  On-board object tracking control of a quadcopter with monocular vision , 2014, 2014 International Conference on Unmanned Aircraft Systems (ICUAS).

[35]  Sven Behnke,et al.  Continuous mapping and localization for autonomous navigation in rough terrain using a 3D laser scanner , 2017, Robotics Auton. Syst..

[36]  Roland Siegwart,et al.  Robust visual inertial odometry using a direct EKF-based approach , 2015, IROS 2015.

[37]  Davide Scaramuzza,et al.  Event-Based, 6-DOF Camera Tracking from Photometric Depth Maps , 2016, IEEE Transactions on Pattern Analysis and Machine Intelligence.

[38]  Heiko Hirschmüller,et al.  Stereo camera based navigation of mobile robots on rough terrain , 2009, 2009 IEEE/RSJ International Conference on Intelligent Robots and Systems.

[39]  Jan Faigl,et al.  On localization and mapping with RGB-D sensor and hexapod walking robot in rough terrains , 2016, 2016 IEEE International Conference on Systems, Man, and Cybernetics (SMC).

[40]  Stefan Schaal,et al.  Learning, planning, and control for quadruped locomotion over challenging terrain , 2011, Int. J. Robotics Res..

[41]  J. M. M. Montiel,et al.  ORB-SLAM: A Versatile and Accurate Monocular SLAM System , 2015, IEEE Transactions on Robotics.

[42]  Heiko Hirschmüller,et al.  Stereo-vision-based navigation of a six-legged walking robot in unknown rough terrain , 2012, Int. J. Robotics Res..

[43]  John J. Leonard,et al.  Past, Present, and Future of Simultaneous Localization and Mapping: Toward the Robust-Perception Age , 2016, IEEE Transactions on Robotics.

[44]  Juan D. Tardós,et al.  Visual-Inertial Monocular SLAM With Map Reuse , 2016, IEEE Robotics and Automation Letters.

[45]  John J. Leonard,et al.  Kintinuous: Spatially Extended KinectFusion , 2012, AAAI 2012.

[46]  Krzysztof Walas,et al.  A Compact Walking Robot - Flexible Research and Development Platform , 2014, Recent Advances in Automation, Robotics and Measuring Techniques.

[47]  Alfred A. Rizzi,et al.  Autonomous navigation for BigDog , 2010, 2010 IEEE International Conference on Robotics and Automation.

[48]  Dong Jin Hyun,et al.  High speed trot-running: Implementation of a hierarchical controller using proprioceptive impedance control on the MIT Cheetah , 2014, Int. J. Robotics Res..