Parallel Continuous-Time Relative Localization with Augmented Clamped Non-Uniform B-Splines

TL;DR

This paper proposes CT-RIO, a novel Continuous-Time Relative-Inertial Odometry framework that consistently outperforms state-of-the-art methods, with improvements of up to 60% under high-speed motion.

Abstract

Accurate relative localization is critical for multi-robot cooperation. In robot swarms, measurements from different robots arrive asynchronously and with clock time-offsets. Although Continuous-Time (CT) formulations have proved effective for handling asynchronous measurements in single-robot SLAM and calibration, extending CT methods to multi-robot settings faces great challenges to achieve high-accuracy, low-latency, and high-frequency performance. Especially, existing CT methods suffer from the inherent query-time delay of unclamped B-splines and high computational cost. This paper proposes CT-RIO, a novel Continuous-Time Relative-Inertial Odometry framework. We employ Clamped Non-Uniform B-splines (C-NUBS) to represent robot states for the first time, eliminating the query-time delay. We further augment C-NUBS with closed-form extension and shrinkage operations that preserve the spline shape, making it suitable for online estimation and enabling flexible knot management. This flexibility leads to the concept of knot-keyknot strategy, which supports spline extension at high-frequency while retaining sparse keyknots for adaptive relative-motion modeling. We then formulate a sliding-window relative localization problem that operates purely on relative kinematics and inter-robot constraints. To meet the demanding computation required at swarm scale, we decompose the tightly-coupled optimization into robot-wise sub-problems and solve them in parallel using incremental asynchronous block coordinate descent. Extensive experiments show that CT-RIO converges from time-offsets as large as 263 ms to sub-millisecond within 3 s, and achieves RMSEs of 0.046 m and 1.8 °. It consistently outperforms state-of-the-art methods, with improvements of up to 60% under high-speed motion.

Figures (23)

• Figure 1: Comparison between typical indirect and direct relative localization. (a) Trajectories are estimated in separate reference frames and aligned to a global consensus. (b) Relative trajectories are estimated in a robocentric frame.
• Figure 2: Comparison between discrete-time and continuous-time relative localization methods. (a) Discrete-time methods estimate states $\{\mathcal{X}_0, \mathcal{X}_1, \ldots\}$ only at camera keyframes. Asynchronous measurements (e.g., UWB) are temporally interpolated to keyframe timestamps, IMU data are summarized via pre-integration, and intermediate frames and measurements are discarded. (b) Continuous-time methods parameterize the state $\mathcal{X}_{\kappa}$ with B-splines, allowing all measurements to directly constrain the trajectory at their native timestamps.
• Figure 3: Visualization of output frequency and latency using (a) unclamped B-spline, (b) clamped B-spline, and (c) clamped B-spline with knot-keyknot strategy and IA-BCD optimization.
• Figure 4: Three different spline parameterizations ($k=4$) with the earliest sensor time at $0s$ and the latest sensor time at $6s$. The unclamped B-splines benefit from the Extended Query Interval Property, which provides one additional evaluable segment. (a) Uniform B-spline. (b) Unclamped non-uniform B-spline. (c) Clamped non-uniform B-spline.
• Figure 5: Closed-form extension of a C-NUBS ($k=4$). The purple highlights the updated part. $(k-2)$ control points are adjusted to keep the shape. (a) Original spline. (b) Step 1 and 2 of \ref{['Pro: Closed-Form Extension']}. (c) Step 3 of \ref{['Pro: Closed-Form Extension']}.

Theorems & Definitions (18)

• Proposition 1: Closed-form extension of a C-NUBS
• proof
• Example 1
• Proposition 2: Closed-form shrinkage of a C-NUBS
• proof
• Example 2
• Remark 1
• Remark 2
• Remark 3
• Remark 4: Extended Query Interval Property