Continuum Robot Localization using Distributed Time-of-Flight Sensors

AI summary

Overview

Research area: Soft and continuum robotics, specifically onboard state estimation and localization for deformable robots in unstructured environments.

Technical level: Advanced. The paper builds on continuous-time factor-graph maximum a posteriori (MAP) estimation with Lie-group (SE(3)) state representations, point-to-plane measurement factors, and iteratively reweighted least squares. The experimental results section, however, is readable with a general robotics background.

Scope (one sentence): The paper shows that a 53 cm soft, extensible continuum robot can localize itself using only sparse, low-resolution time-of-flight and gyroscope measurements distributed along its body, fused with a robot shape prior.

What This Paper Is About

Continuum robots are deformable, often very small, and operating in cluttered spaces, which means the high-resolution lidar that mobile robots rely on for localization is too large to mount on them. Individual small time-of-flight (ToF) sensors frequently see too little of the environment to pin down their own position, a problem the authors call a degenerate measurement.

The goal is to achieve accurate, full-body, onboard-only localization of a deformable continuum robot despite these degeneracies, and to study how robust that localization is when the prior map of the environment does not match the real scene.

Key Contributions

1. A continuous-time, factor-graph-based MAP estimation framework that fuses sparse ToF measurements with a robot shape prior, enabling stable full-body localization of deformable continuum robots under frequent geometric degeneracies and partial prior map mismatch.

2. The first real-world demonstration of onboard-only localization of a soft, extensible continuum robot in cluttered environments using distributed, low-resolution ToF and inertial sensing, validated on both simulated and physical platforms.

3. An open-source code release at https://github.com/utiasASRL/space_time_continuum.

4. A robustness study against prior map deviations (added objects and removed features) plus a demonstration application of anomaly detection by comparing the reconstructed scene against the prior map.

Main Findings

• Headline accuracy: An average localization error of 2.5 cm in position and 7.2° in rotation across all experimental conditions with a 53 cm long robot.

• Simulation results are near-identical across scene deviations: Using scene S0 as the prior map for all 30 trials across scenes S0–S9, translation MAE ranged from 0.61 cm to 0.78 cm and rotation MAE stayed at 0.96°. Adding or removing geometries from the environment caused no noticeable degradation in simulation.

• Real-world accuracy in known environments: Between 1.2 cm and 2.5 cm average position error and 3.7° to 10.3° rotational error when the prior map matched the real scene.

• Added objects hurt more than removed ones: Adding an object increased error by an average of 1.7 cm and 1.0°; removing an object increased it by an average of 0.5 cm and 1.5°.

• The large wet-floor sign was the toughest anomaly: Adding it raised average position error to 5.1 cm and rotation error to 6.5°; removing it produced 3.6 cm position error and 14.6° rotation error.

• The tip localizes worst: Pooled across all conditions, the top ring achieved 1.7 ± 1.3 cm and 4.8 ± 4.1°, the middle ring 2.3 ± 1.2 cm and 7.0 ± 3.4°, and the bottom (tip) ring 3.6 ± 1.9 cm and 9.9 ± 4.9°. Error accumulates along the robot's length.

• Scene reconstruction tracks localization quality: Per-point RMSE was 0.8 cm for R4–R4, 1.2 cm for R3–R3, and 1.4 cm when large anomalies degraded localization.

• Sensors are complementary in simulation: Strain sensing alone produces "spatial drift" that grows from base to tip, while ToF alone frequently converges to local minima, largely because of the radial symmetry of the environment. The strain sensor acts as a restoring force against jumps to local minima.

• Failures are driven by map deviation plus sudden motion: The worst-performing trials all involved a prior map with the large wet-floor sign removed, combined with poor initialization or sudden changes in robot shape.

Methodology in Plain English

The estimator is a sliding-window, continuous-time factor graph based on prior work by the same group. Rather than estimating the robot pose at only a few instants, it represents the robot's pose, generalized strains, and generalized velocities as continuous functions of time and arc length along the body, then optimizes over discrete samples while interpolating between them. The full state is solved as a MAP problem with Gauss-Newton, and uncertainties come from a Laplace approximation. The interpolation scheme makes it natural to fuse sensors that sample at different rates.

Three kinds of information enter the graph. First, robot shape priors, inherited from the prior framework, which regularize the estimate using a continuum-robot model. Second, ToF measurements, each treated as a point-to-plane error against a matched point in a prior map. Because ToF returns are noisy and prone to outliers, a Cauchy loss is applied through an iteratively reweighted least squares scheme. The measurement noise model scales with distance (short readings below 0.025 m are discarded, and the standard deviation ranges from 0.014d down to 0.006d as distance grows). Third, gyroscope measurements, modeled with constant bias estimated from an initial stationary average. In simulation only, a fourth factor models strain sensing (bending angle and curvature mapped to strain), matching the representation used by fiber Bragg grating systems.

The prior map is a point cloud with precomputed normals and planarity weights, stored in a hashed voxel map for fast nearest-neighbor queries. Simulated maps come from Poisson sampling of CAD scenes; real maps were produced with a Creality Raptor 3D scanner at 0.1 mm accuracy and 0.1 mm to 2 mm resolution, with reflective markers for alignment to a motion capture system. Varying which prior map the estimator sees while holding the sensor data fixed is how the authors deliberately introduce map mismatch.

Simulation used MuJoCo with a jet-engine scene (outer diameter 3.1 m, cropped to the front compressor section) and a 1.5 m robot modeled as rigid links connected by spherical joints under the pseudo-rigid body approximation. ToF sensors were simulated as 8×8 grids of rangefinders matching the VL53L5CX, with 45° fields of view, 4 m maximum range, and at most 15 Hz output. Real-world experiments used a 70 cm × 70 cm × 60 cm cube, four OptiTrack cameras, and a 53 cm, three-module soft robot with TPU 82A flexures, three ToF sensors and an ISM330BX IMU per ring, sampled at 15 Hz and 120 Hz respectively. Estimator parameters were tuned on a single hold-out trial and then frozen for all other trials.

Finally, after estimation, each ToF measurement is projected into the inertial frame using the uncertainty-aware projection to reconstruct a point cloud of the environment, which is then compared against the prior map for anomaly detection using a squared Mahalanobis distance threshold.

Why This Matters

This work pushes continuum robot localization away from the assumptions that have dominated the field: dense sensing, rigid-body kinematics, or external tracking infrastructure such as motion capture or fluoroscopic imaging. It demonstrates that sparse, cheap, low-resolution onboard sensors plus a deformable-body shape prior are sufficient for full-body pose estimation, and it quantifies honestly how much that capability depends on prior map quality.

Real-world applications:

• Aircraft and jet engine inspection, the specific scenario the simulation environment was designed to emulate with a scaled model jet engine.
• Surgery and endoscopy, where continuum robots have been studied and external line-of-sight tracking is often unavailable.
• Search and rescue in confined or collapsed spaces.
• Industrial inspection and maintenance in constrained or convoluted environments, including the articulated borescope market that has already seen commercial uptake.

Industry relevance: Inspection robots that do not need external tracking hardware or line-of-sight camera setups are easier and cheaper to deploy in field conditions. The anomaly detection application points directly at automated inspection workflows, where a robot must both know where it is and notice what has changed. The open-source release lowers the barrier to adopting the estimator.

Future Directions

• Detecting and escaping local minima. The authors state that without a method for detecting and escaping these minima, the estimator can produce large errors, and they identify poor initialization, significant map departure, and sudden movements as triggers.

• Incorporating more sensor modalities. The onboard ISM330BX already provides accelerometer measurements that are not used; the authors leave adding them "to future work," and note that strain sensing helped in simulation.

• Reducing prior map dependency. Localization accuracy is currently tied to prior map accuracy, and the authors propose relaxing this requirement through live map updates or other robustness improvements.

• Making anomaly detection reliable. Real-world experiments showed false-positive detections near scene objects that corrupt sensor readings, so further noise reduction and localization improvement are required before the method is viable for inspection.

• Real strain sensor hardware. The strain factor exists as a simulation-only component with an FBG-compatible representation, enabling future hardware integration.

Target Audience

Researchers and graduate students working on soft and continuum robotics, continuous-time state estimation, and SLAM for non-rigid platforms. It will also interest robotics engineers building inspection or medical systems who need localization without external motion capture, and readers tracking how sparse, low-resolution depth sensing can substitute for larger sensors on size-constrained robots. Reader should be comfortable with factor graphs, SE(3) notation, and robotics terminology, though the experimental results tables are accessible to a broader audience.

Authors’ abstract

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.