Optimal Initialization Strategies for Range-Only Trajectory Estimation

Abhishek Goudar, Frederike Dümbgen, Timothy D. Barfoot, Angela P. Schoellig

Abstract: Range-only (RO) pose estimation involves determining a robot's pose over time by measuring the distance between multiple devices on the robot, known as tags, and devices installed in the environment, known as anchors. The non-convex nature of the range measurement model results in a cost function with possible local minima. In the absence of a good initial guess, commonly used iterative solvers can get stuck in these local minima resulting in poor trajectory estimation accuracy. In this letter, we propose convex relaxations to the original non-convex problem based on semidefinite programs (SDPs). Specifically, we formulate computationally tractable SDP relaxations to obtain accurate initial pose and trajectory estimates for RO trajectory estimation under static and dynamic (i.e., constant-velocity motion) conditions. Through simulation and hardware experiments, we demonstrate that our proposed approaches estimate the initial pose and initial trajectories accurately compared to iterative local solvers. Additionally, the proposed relaxations recover global minima under moderate range measurement noise levels.