Continuum Robot Localization using Distributed Time-of-Flight Sensors

Localization and mapping of an environment are crucial tasks for any robot operating in unstructured environments. Time-of-flight (ToF) sensors (e.g.,~lidar) have proven useful in mobile robotics, where high-resolution sensors can be used for simultaneous localization and mapping. In soft and continuum robotics, however, these high-resolution sensors are too large for practical use. This, combined with the deformable nature of such robots, has resulted in continuum robot (CR) localization and mapping in unstructured environments being a largely untouched area. In this work, we present a localization technique for CRs that relies on small, low-resolution ToF sensors distributed along the length of the robot. By fusing measurement information with a robot shape prior, we show that accurate localization is possible despite each sensor experiencing frequent degenerate scenarios. We achieve an average localization error of 2.5cm in position and 7.2° in rotation across all experimental conditions with a 53cm long robot. We demonstrate that the results are repeated across multiple environments, in both simulation and real-world experiments, and study robustness in the estimation to deviations in the prior map.

💡 Research Summary

The paper tackles the challenging problem of localizing a deformable continuum robot (CR) in unstructured environments using only onboard, low‑resolution time‑of‑flight (ToF) sensors. Conventional mobile‑robot SLAM relies on high‑resolution lidar, which is too bulky for the small diameters and compliance of CRs. To overcome this, the authors distribute several miniature ToF sensors and gyroscopes along the robot’s body and fuse their sparse measurements with a shape prior in a continuous‑time factor‑graph based maximum a posteriori (MAP) estimator.

The state vector x(s,t) comprises an SE(3) pose T_ib(s,t), generalized strain ε(s,t), and generalized velocity ϖ(s,t) defined over arc length s and time t. A sliding‑window Gauss‑Newton optimizer minimizes a sum of squared error factors, each weighted by appropriate covariances. For ToF data, a point‑to‑plane error e_j(x)=α n_jᵀ D(p_j−T_ib q_bj) is constructed, where p_j and n_j are a matched map point and its normal, and q_bj is the sensor‑frame measurement. Distance‑dependent noise models (Table I) and a Cauchy robust loss implemented via iteratively re‑weighted least squares (IRLS) mitigate outliers and measurement degeneracies. Gyroscope factors follow prior work, assuming constant bias estimated from a short stationary period. In simulation, a virtual strain sensor provides bending angle and curvature, linked to the robot’s internal strain state through a simple linear mapping.

A prior map of the environment is pre‑scanned as a dense point cloud with normals and planarity weights, stored in a hashed voxel structure for fast nearest‑neighbor queries. The authors deliberately introduce map mismatches—unmodeled objects and missing features—to evaluate robustness. The estimator can still converge because the shape prior supplies global geometric constraints, while distributed sensors supply local observations that together resolve ambiguities.

Experiments are conducted both in MuJoCo simulation and on a physical 53 cm long, 7.6 cm diameter soft robot equipped with three sensor rings (each ring: three VL53L5CX ToF sensors and one gyroscope) plus a tip‑mounted ToF. Simulated environments emulate a scaled aircraft engine compressor section; ten variants are created with different anomaly combinations. Real‑world tests use a Creality Raptor 3D scanner to build a high‑accuracy prior map (≈0.1 mm accuracy). Across all conditions, the system achieves an average translational error of 2.5 cm and rotational error of 7.2°, comparable to results obtained with high‑resolution lidar on rigid platforms. Notably, individual sensors frequently experience degenerate measurements (e.g., insufficient features within the field of view), yet the fused estimator remains stable.

Key contributions are: (1) a continuous‑time factor‑graph framework that integrates sparse ToF data with a robot shape prior, enabling full‑body localization of a deformable CR under frequent geometric degeneracies and partial map mismatch; (2) the first real‑world demonstration of onboard‑only localization for a soft, extensible CR in cluttered settings, validated in both simulation and hardware; (3) a systematic robustness analysis showing graceful degradation when the prior map deviates from reality. The work highlights that accurate localization does not require bulky high‑resolution sensors; instead, strategic sensor distribution combined with strong priors can achieve SLAM‑level performance for soft robots.

Limitations include reliance on a pre‑existing static map and lack of dynamic obstacle handling. Future work suggested by the authors involves integrating online map updating (full SLAM), optimizing sensor placement for minimal hardware, and extending the approach to medical or aerospace inspection tasks where soft robots must operate autonomously for extended periods.