Stretch-ICP: A Continuous-Trajectory Registration and Deskewing Algorithm in Scenarios of Aggressive Motions

Technical Summary: Stretch-ICP: A Continuous-Trajectory Registration and Deskewing Algorithm in Scenarios of Aggressive Motions

Problem Statement
Robotic autonomy in complex environments is frequently compromised by "aggressive motions"—regimes characterized by abrupt changes, impacts, and loss of predictability, such as a robot tumbling down a hill. These conditions induce extreme accelerations and angular velocities that corrupt sensor measurements. Specifically, they cause two critical failures in Simultaneous Localization and Mapping (SLAM) systems:

1. Gyroscope Saturation: During high-speed tumbling, angular velocities often exceed the measurement range of Micro-Electro-Mechanical Systems (MEMS) gyroscopes. This leads to saturated readings where the sensor reports a constant maximum value, rendering standard inertial integration useless for motion estimation.
2. Trajectory Discontinuity: Classical registration methods, such as Iterative Closest Point (ICP), correct motion distortion by applying rigid transformations to entire scans. While this improves global pose consistency, it introduces discontinuities at scan boundaries. These jumps result in physically implausible spikes in estimated linear and angular velocities, degrading the performance of downstream control algorithms and state estimation.

Existing datasets lack the necessary intensity to test these failure modes, and current SLAM algorithms often rely on IMU priors that fail completely when gyroscopes saturate or assume smooth motion that does not hold during collisions.

Methodology
The authors propose a two-part solution to address these challenges, supported by the introduction of two new datasets: the Tumbling-Induced Gyroscope Saturation (TIGS) dataset and the High-Resolution Motion Capture (HRMC) dataset.

1. Saturation-Aware Angular Velocity Estimation (SAAVE):
   To recover angular velocity when gyroscopes saturate, the authors utilize accelerometer data under the assumption of free-fall. By modeling the IMU's position relative to the robot's Center of Mass (COM) and the rotation axis, they derive the angular velocity from the measured centripetal acceleration.

   • Key Assumptions: The IMU is offset from the rotation axis; the robot is in free-fall (net acceleration at COM is zero); the rotation axis remains stable between measurements; and only one gyroscope axis is saturated at a time.
   • Implementation: The method estimates angular speed using the magnitude of the lever arm and the centripetal acceleration component. To handle noise and the brief periods where the free-fall assumption is invalid (collisions), the estimates are smoothed using a Gaussian Process (GP) with a white-noise-on-jerk motion prior.

2. Stretch-ICP:
   To address trajectory discontinuities, the authors introduce Stretch-ICP, a registration and deskewing algorithm that treats scan alignment as a continuous-time trajectory deformation problem rather than a rigid scan correction.

   • Mechanism: Instead of applying a single rigid transformation to a scan, Stretch-ICP optimizes a factor graph to "stretch" the intra-scan trajectory. This factor graph incorporates three constraints:
     • Trajectory Beginning Factor: Forces the start of the current intra-scan trajectory to match the end of the previous one, ensuring continuity.
     • IMU Factors: Constrains consecutive states using IMU preintegration (utilizing the covariance outputs from SAAVE).
     • Stretching Factor: Distributes the registration correction (derived from point-to-plane matching errors) across the entire intra-scan trajectory, maintaining consistency with inertial constraints.

Key Contributions

• TIGS Dataset: A new dataset comprising 32 runs of a perception rig tumbling down a steep hill, featuring angular speeds up to 18.6 rad/s (four times higher than similar datasets) and linear accelerations up to 157.8 m/s². It includes ground-truth angular velocity measurements from a high-range gyroscope.
• HRMC Dataset: A dataset recorded indoors with a Vicon Motion Capture system, providing high-frequency (200 Hz) 6-DOF ground truth for evaluating trajectory continuity and velocity estimation errors.
• SAAVE: A method to estimate angular velocities during gyroscope saturation periods, reducing estimation error by 83.4% compared to saturated sensor readings.
• Stretch-ICP: A novel registration algorithm that enforces trajectory continuity across scan boundaries, reducing linear and angular velocity errors at scan boundaries by 95.2% and 94.8%, respectively.

Results
The authors evaluated their methods on the TIGS and HRMC datasets, comparing them against standard ICP-based SLAM and Point-LIO (a state-of-the-art method robust to saturation).

• Angular Velocity Recovery: SAAVE significantly reduced the median angular speed error during saturation periods by 83.4%.
• SLAM Robustness: Integrating SAAVE into a SLAM framework (SAAVE-ICP-SLAM) improved median translational localization error by 71.5% and rotational error by 65.5% compared to using raw saturated IMU data. It also reduced mapping failures (defined as >15% outliers) from 37.5% (12/32 runs) to 0%.
• Trajectory Continuity: When comparing SAAVE-ICP-SLAM against SAAVE-Stretch-SLAM on the HRMC dataset, Stretch-ICP reduced the median linear speed estimation error at scan boundaries from 4.51 m/s to 0.24 m/s (95.2% improvement) and angular speed error from 1.80 rad/s to 0.10 rad/s (94.8% improvement).
• Trade-off: The use of Stretch-ICP resulted in a modest increase in final pose localization error compared to standard ICP, but it yielded significantly smoother, physically consistent trajectories and velocity estimates.

Significance and Claims
The paper claims that robust robotic autonomy requires addressing both sensor saturation and the physical consistency of reconstructed trajectories. The authors assert that while tumbling is an extreme stress test, the proposed methods improve robustness for practical scenarios involving impacts, slips, hard landings, and aggressive traversal of uneven terrain.

The primary significance lies in the combination of SAAVE and Stretch-ICP:

1. SAAVE allows the system to recover motion priors when standard gyroscopes fail, preventing total SLAM divergence.
2. Stretch-ICP ensures that the resulting trajectory is continuous and physically plausible, preventing the velocity spikes that typically arise from classical rigid registration.

The authors note that their current implementation is an offline research prototype. SAAVE is computationally dominated by Gaussian Process smoothing, and Stretch-ICP adds optimization overhead per registration iteration. Consequently, the work highlights a trade-off where modest increases in final pose error are accepted to achieve significant gains in trajectory continuity and motion estimation reliability under aggressive dynamics. Future work is identified as necessary to optimize these methods for real-time performance and to conduct sensitivity analyses for tuning.