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 least-squares refinement step, and validate our algorithm in various simulations, demonstrating its efficiency and accuracy.

[1]  Peter I. Corke,et al.  Further Results with Localization and Mapping Using Range from Radio , 2005, FSR.

[2]  J. J. Caffery,et al.  A new approach to the geometry of TOA location , 2000, Vehicular Technology Conference Fall 2000. IEEE VTS Fall VTC2000. 52nd Vehicular Technology Conference (Cat. No.00CH37152).

[3]  Mohinder S. Grewal,et al.  Global Positioning Systems, Inertial Navigation, and Integration , 2000 .

[4]  Stergios I. Roumeliotis,et al.  Determining the Robot-to-Robot Relative Pose Using Range-only Measurements , 2007, Proceedings 2007 IEEE International Conference on Robotics and Automation.

[5]  I. Bonev,et al.  A new method for solving the direct kinematics of general 6-6 Stewart Platforms using three linear extra sensors , 2000 .

[6]  L. El Ghaoui,et al.  Convex position estimation in wireless sensor networks , 2001, Proceedings IEEE INFOCOM 2001. Conference on Computer Communications. Twentieth Annual Joint Conference of the IEEE Computer and Communications Society (Cat. No.01CH37213).

[7]  Ichiro Nakatani,et al.  Localization on Small Body Surface by Radio Assistance(WSANE2006) , 2006 .

[8]  Jan Verschelde,et al.  Algorithm 795: PHCpack: a general-purpose solver for polynomial systems by homotopy continuation , 1999, TOMS.

[9]  Uwe D. Hanebeck,et al.  Closed-form range-based posture estimation based on decoupling translation and orientation , 2005, Proceedings. (ICASSP '05). IEEE International Conference on Acoustics, Speech, and Signal Processing, 2005..

[10]  P. Dietmaier,et al.  The Stewart-Gough Platform of General Geometry can have 40 Real Postures , 1998 .

[11]  Sailes K. Sengijpta.  Fundamentals of Statistical Signal Processing: Estimation Theory , 1995 .

[12]  Paramvir Bahl,et al.  RADAR: an in-building RF-based user location and tracking system , 2000, Proceedings IEEE INFOCOM 2000. Conference on Computer Communications. Nineteenth Annual Joint Conference of the IEEE Computer and Communications Societies (Cat. No.00CH37064).

[13]  David A. Cox,et al.  Ideals, Varieties, and Algorithms , 1997 .

[14]  Sanjiv Singh,et al.  Preliminary results in range-only localization and mapping , 2002, Proceedings 2002 IEEE International Conference on Robotics and Automation (Cat. No.02CH37292).

[15]  D. Stewart.  A Platform with Six Degrees of Freedom , 1965 .

[16]  Charles W. Wampler.  FORWARD DISPLACEMENT ANALYSIS OF GENERAL SIX-IN-PARALLEL SPS (STEWART) PLATFORM MANIPULATORS USING SOMA COORDINATES , 1996 .

[17]  M. Shuster.  A survey of attitude representation , 1993 .

[18]  John J. Leonard,et al.  Pure range-only sub-sea SLAM , 2003, 2003 IEEE International Conference on Robotics and Automation (Cat. No.03CH37422).

[19]  Long Quan,et al.  Linear N-Point Camera Pose Determination , 1999, IEEE Trans. Pattern Anal. Mach. Intell..

[20]  M. Husty.  An algorithm for solving the direct kinematics of general Stewart-Gough platforms , 1996 .

[21]  Kostas Daniilidis,et al.  Linear Pose Estimation from Points or Lines , 2002, IEEE Trans. Pattern Anal. Mach. Intell..