Informed sampling techniques accelerate sampling-based motion planners by focusing the search on promising regions of the state space, yet most existing methods rely on Euclidean heuristics that can become inadmissible under configuration-dependent Riemannian metrics. While scalar eigenvalue bounds restore admissibility by uniformly scaling the Euclidean distance, they discard the directional structure of the metric, producing overly conservative informed sets. We propose a matrix-valued admissible heuristic that exploits the Loewner order on symmetric positive definite matrices to compute the tightest constant lower bound on the metric tensor while retaining directional information discarded by scalar bounds. The Cholesky factorization of this constant bound maps the lower-bound informed set to a standard prolate hyperspheroid in Euclidean space, enabling direct, rejection-free sampling using existing algorithms. We then present an incremental algorithm for computing such a constant lower bound on the metric tensor from a sequence of pointwise evaluations. Experiments on manipulation tasks with a 6-DoF UR5, 7-DoF Franka, and 14-DoF PR2 under three distinct Riemannian metrics show that our heuristic produces consistently tighter informed sets than those produced by the Euclidean and scalar-eigenvalue heuristics, accelerating convergence across multiple state-of-the-art asymptotically optimal planners. Our implementation is available in the open-source geodex framework at https://github.com/utiasSTARS/geodex.
In this paper, we present a robotics-tailored exposition of Galilean symmetry that leverages the community's familiarity with and understanding of rigid-body transformations and pose representations. Our approach contrasts with common treatments in the physics literature that introduce Galilean symmetry as a stepping stone to Einstein's relativity. We show that the classical homogeneous matrix algebra known well to roboticist translates directly into the Galilean modeling framework and provides a powerful tool for tackling robotics problems involving inertial measurements and spatio-temporal variables. We demonstrate the potential of the approach by developing a powerful model for studying motion of vehicles in an Earth fixed rotating reference frame, and developing a fusion algorithm for measurements with uncertain time-stamps.
Abhishek Goudar, Jonathan Kelly, Timothy D. Barfoot
原文摘要
Multi-sensor state estimation requires accurate spatial and temporal calibration between heterogeneous sensors. Existing spatiotemporal calibration methods typically represent sensor extrinsic parameters as a rigid-body transformation together with a scalar temporal offset, causing the spatial and temporal parameters to enter the estimation problem through separate mechanisms. While the spatial transformation composes directly with the robot pose, accounting for the temporal offset requires evaluating the trajectory at a shifted time, typically through motion compensation, interpolation, or a continuous-time trajectory representation. In this work, we formulate joint trajectory estimation and spatiotemporal calibration using the special Galilean group for the robot trajectory and the Aristotelian subgroup for sensor extrinsics. This representation incorporates the coupling between motion and temporal offset directly through the group structure, avoiding the need for a separately parameterized continuous-time trajectory. We develop twist- and IMU-driven motion priors, accommodate both ideal and non-ideal clocks, and evaluate the framework with camera, position, and range measurements. Simulated and real experiments demonstrate calibration and trajectory-estimation performance comparable to established methods across heterogeneous sensor configurations. Our simplified framework therefore offers a quality-of-life improvement over existing methods without compromising performance.