3D relative pose estimation from distance-only measurements

In this paper, we develop an algorithm for determining the relative position and attitude of two robots moving in 3D, using only dead-reckoning and inter-robot distance measurements. From the mechanical analogue of parallel manipulators, it is known that this problem has 40 solutions when six distance measurements are available. These general solutions are not known in closed-form, and existing closed-form solutions require additional bearing sensors, or impose strong constraints on the geometric structure of the manipulator (in our case, the robots’ trajectories). This paper presents, for the first time, an efficient, algebraic algorithm to solve the relative pose using 10 distance measurements, without imposing any constraints on the robots’ motion. We further present a weighted leastsquares refinement step, and validate our algorithm in various simulations, demonstrating its efficiency and accuracy.