Occupancy voxel metric based iterative closest point for position tracking in 3D environments

Many applications for robotics require that the robot know its current position in the environment. While there exist several solutions for localizing a robot, even in a previously unknown environment, they often require an estimate of the robot's motion. However, in many situations, a robot may not have motion encoders, or its encoders may be highly inaccurate. We have developed an algorithm for tracking the position of a robot, based on a rangefinder device, that is robust to temporary errors in the range scan. By aligning each scan to an occupancy grid of prior scan data, we can find the robot's position more accurately than current techniques which only align to the previous scan. In addition, our solution can track the position of the robot based on three dimensional scan data, instead of requiring that the range sensor be fixed in a level plane.