Optimal Initialization Strategies for Range-Only Trajectory Estimation

TL;DR

This work tackles range-only localization by addressing the nonconvexity of RO measurements that complicates trajectory estimation. It develops convex semidefinite programming relaxations to obtain reliable initial estimates for static poses and short-horizon trajectories under constant-velocity motion, enabling bootstrap of local solvers. The approach uses lever-arm substitutions, a lifted matrix variable, and autosdp-generated redundant constraints to achieve (often) rank-1 SDP solutions, validated in both simulations and real hardware with moderate measurement noise. Practically, the method offers a fast, provably robust initialization tool for RO pose estimation that can improve online performance where diverse motion is limited or absent.

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 nonconvex nature of the range measurement model results in a cost function with possible local minima. In the absence of a good initialization, commonly used iterative solvers can get stuck in these local minima resulting in poor trajectory estimation accuracy. In this work, we propose convex relaxations to the original nonconvex 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 real experiments, we demonstrate that our proposed initialization strategies estimate the initial state accurately compared to iterative local solvers. Additionally, the proposed relaxations recover global minima under moderate range measurement noise levels.

Figures (13)

• Figure 1: Range-only trajectory estimation results from a hardware experiment. A quadrotor in motion, equipped with 2 range sensors, measures its distance to multiple anchors to estimate its 3D position, velocity, and yaw angle over a short time horizon (pitch and roll come from an IMU). We refer to this as 2.5D dynamic initialization. The trajectory is parameterized by a sequence of poses, represented here by orthogonal axes. In the absence of a good initialization, the iterative local solver gets stuck in a local minimum resulting in poor accuracy compared to the ground-truth trajectory. Our proposed approach leverages a semidefinite relaxation of the original problem to recover accurate trajectories as shown in the magnified image on the right. For each method, the robot pose at $t=0$ is indicated by a red dot at the origin of the orthogonal axes.
• Figure 2: Qualitative results showing the effect of redundant constraints on the eigenvalue spectrum of the SDP solution. The set $\mathcal{C}_{\textrm{rot}}$ represents the orthogonality and the determinant constraints, and the set $\mathcal{C}_{\textrm{lrn}}$ denotes the additional redundant constraints \ref{['eqn:redundant_constraint']}. In all cases, substitution constraints $\mathcal{C}_{\textrm{sub}}$ are included. The inclusion of additional redundant constraints results in a larger ratio of the dominant eigenvalues, and can lead to a rank-1 solution, as shown in the rightmost plot.
• Figure 3: Distribution of ${L}_2$ position and rotation errors from simulation for 2D static initialization (top) and 3D static initialization (bottom). The distributions are generated from 100 Monte Carlo trials across increasing range measurement noise, $\sigma_r$. The distribution of errors from the proposed method (SDP) is tighter compared to the baseline local solver (LS). The local solver accuracy is lower as it often gets stuck in local minima. The local solver typically converges to the global minimum when the robot is inside the convex hull of the anchors and performs poorly towards the boundary and outside the convex hull, whereas the proposed approach performs reliably even in such challenging scenarios.
• Figure 4: Distribution of ${L}_2$ position (top row) and rotation errors (bottom row) under increasing range measurement noise ($\sigma_r$) for 2D dynamic initialization from simulation. The position and rotation errors are computed over the full trajectory, which reflects any errors associated with the estimated velocity. The distribution of errors from the proposed method (SDP) is much tighter than the baseline local solver (LS) as the local solver gets stuck in local minima whereas the proposed method does not.
• Figure 5: Two simulation results from 2.5D dynamic initialization showing the trajectories estimated by the iterative local solver (LS) and our proposed method (SDP) along with the ground-truth trajectory (GT). Without a good initial point, the local solver estimates suboptimal trajectories, while our proposed approach is able to generate better trajectory estimates. For each method, the robot pose at $t=0$ is indicated by a red dot at the origin of the orthogonal axes.