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

2026-05-17

AI Summary

This work addresses the failure of laser-inertial state estimation under extreme motion caused by gyroscope saturation. To overcome this challenge, we propose two novel methods: SAAVE (Saturation-Aware Angular Velocity Estimation), which accurately recovers angular velocity during gyroscope saturation and reduces estimation error by 83.4%, and Stretch-ICP, which leverages continuous 6-DoF trajectory registration with tightly coupled de-warping to decrease linear and angular velocity errors at scan boundaries by 95.2% and 94.8%, respectively. We further introduce the first high-dynamic TIGS dataset and present the first systematic solution to robust SLAM under gyroscope saturation, substantially enhancing system stability and accuracy during aggressive motion.

Abstract

Robust robotic autonomy remains challenging in complex environments, where loss of stability on uneven or slippery terrain can induce extreme accelerations and angular velocities. Such motions corrupt sensor measurements and degrade state estimation, motivating the need for improved algorithmic robustness. To investigate this issue, we introduce the Tumbling-Induced Gyroscope Saturation (TIGS) dataset, which consists of recordings from a mechanical lidar and an Inertial Measurement Unit (IMU) tumbling down a hill. The dataset contains angular speeds up to four times higher than those in similar datasets and is publicly available. We then propose two complementary methods to improve Simultaneous Localization And Mapping (SLAM) robustness and evaluate them on TIGS. First, Saturation-Aware Angular Velocity Estimation (SAAVE) estimates angular velocities when gyroscope measurements become saturated during aggressive motions, reducing angular speed estimation error by 83.4%. Second, Stretch-ICP, a novel registration and deskewing algorithm, enables reconstruction of smoother 6-Degrees Of Freedom (DOF) trajectories under aggressive motions compared to classical Iterative Closest Point (ICP). Stretch-ICP reduces linear and angular velocity errors by 95.2% and 94.8%, respectively, at scan boundaries. Together, these contributions improve the robustness and consistency of lidar-inertial state estimation under aggressive motions.

Problem

• aggressive motions
• gyroscope saturation
• state estimation
• sensor corruption
• robotic autonomy

Innovation

• Stretch-ICP
• gyroscope saturation
• aggressive motion
• lidar-inertial SLAM
• deskewing