Robot Localisation in 3D Environments Using Sparse Range Measurements

This paper presents an algorithm for mobile robot localisation given a map of a 3D environment and a sparse set of range-bearing measurements. The environment is represented using a spline approximation of its vector distance function (VDF). For a given location in the environment, VDF encodes the distance to the nearest occupied region along three orthogonal axes. VDF is first obtained from an occupancy voxel map and its three components are then approximated in the least-square sense using a set of three dimensional cubic b-splines, providing a rich and continuous representation of the environment. First and second order derivatives of the VDF are also computed and stored. The difference between an observed range measurement in a given direction and its expected value is formulated as a function of the robot location and the spline coefficients representing the VDF. This leads to a non-linear least-squares optimization problem that can be solved to localise the robot given a set of such measurements. It is demonstrated that a sparse set of range-bearing measurements, an order of magnitude smaller than what is typically available from 3D range sensor is adequate to achieve accurate localisation. The algorithm presented is illustrated using a number of examples including a single point range sensor mounted on a pan-tilt head to localise a robot moving in an indoor environment.