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.
Contributions
- Stretch-ICP, a novel registration and deskewing algorithm that yields a continuous trajectory under aggressive motions, together with the HRMC dataset, which enables high-frequency trajectory and velocity error analysis.
- An extended experimental evaluation that compares SAAVE against Point-LIO, a lidar-inertial method explicitly designed to remain robust under gyroscope saturation, providing a stronger state-of-the-art baseline.
Results in Images
The top figure shows a toy example comparing our method, Stretch-ICP, with the classical ICP algorithm. The light blue points represent a map, while the yellow points represent a scan acquired by a moving lidar. The orange dot and blue square represent the start and end positions of two consecutive intra-scan trajectories. The gray line represents the previous intra-scan trajectory, and the black line the current one. The green arrows indicate the matches used by the registration algorithms. Subfigure (a) shows three iterations of ICP, and subfigure (b) shows three iterations of Stretch-ICP. In both subfigures, the registration is complete at the end of the three iterations. As can be seen, when motion prediction is imperfect, ICP applies a corrective rigid transformation to align the scan with the map. This correction repositions the intra-scan trajectory estimated with the IMU preintegration, introducing a discontinuity at the scan’s start, as illustrated by the gap between the blue square and the orange x in the upper right trajectory. In contrast, Stretch-ICP treats registration as a continuous-time trajectory deformation problem. Rather than correcting the scan with a rigid transformation, it distributes the registration correction over the intra-scan trajectory by stretching it in a manner consistent with inertial constraints and scan-boundary continuity. We can see that, unlike ICP, Stretch-ICP keeps the trajectory continuous and deskews the scan while registering it to the map.
The bottom figure shows an example of reconstructed trajectories obtained with a SLAM framework using ICP (purple) and Stretch-ICP (green) as registration algorithms. For clarity, the 3D trajectories are projected onto the X-Z plane, and the dotted line indicates the ground-truth trajectory. The black arrows indicate the direction of motion through time. The zoomed-in views on the left show a scan boundary, where ‘End’ marks the end of one intra-scan trajectory and ‘Start’ marks the beginning of the next. With classical ICP, rigid scan-to-map corrections can produce visible jumps at these boundaries, which translate into inaccurate velocity estimates and reduced SLAM robustness. By enforcing continuity while registering and deskewing the scan, Stretch-ICP produces smoother reconstructed motion. In our experiments, it reduced the linear and angular velocity errors at scan boundaries by 95.2% and 94.8%, respectively.
Reference
Deschênes, S.-P., Vannini, V., Giguère, P., & Pomerleau, F. (2026). Stretch-ICP: A Continuous-Trajectory Registration and Deskewing Algorithm in Scenarios of Aggressive Motions. Sensors, 26(8). https://doi.org/10.3390/s26082567