Real-time depth and inertial fusion for local SLAM on dynamic legged robots

We present a real-time SLAM system that combines an improved version of the Iterative Closest Point (ICP) and inertial dead reckoning to localize our dynamic quadrupedal machine in a local map. Despite the strong and fast motions induced by our 80 kg hydraulic legged robot, the SLAM system is robust enough to keep the position error below 5% within the local map that surrounds the robot. The 3D map of the terrain, computed at the camera frame rate is suitable for vision based planned locomotion. The inertial measurements are used before and after the ICP registration, to provide a good initial guess, to correct the output and to detect registration failures which can potentially corrupt the map. The performance in terms of time and accuracy are also doubled by preprocessing the point clouds with a background subtraction prior to performing the ICP alignment. Our local mapping approach, in spite of having a global frame of reference fixed onto the ground, aligns the current map to the body frame, and allows us to push the drift away from the most recent camera scan. The system has been tested on our robot by performing a trot around obstacles and validated against a motion capture system.