💻 computer science

Continuum Robot Localization using Distributed Time-of-Flight Sensors

This paper presents a localization technique for continuum robots that fuses data from distributed, low-resolution Time-of-Flight sensors with a shape prior to achieve accurate positioning and orientation in unstructured environments, overcoming the limitations of sensor size and robot deformability.

Original authors: Spencer Teetaert, Giammarco Caroleo, Marco Pontin, Sven Lilge, Jessica Burgner-Kahrs, Timothy D. Barfoot, Perla Maiolino

Published 2026-09-02

📖 4 min read☕ Coffee break read

CC BY 4.0

Original authors: Spencer Teetaert, Giammarco Caroleo, Marco Pontin, Sven Lilge, Jessica Burgner-Kahrs, Timothy D. Barfoot, Perla Maiolino

Original paper licensed under CC BY 4.0 (http://creativecommons.org/licenses/by/4.0/). ✨ This is an AI-generated explanation of the paper below. It is not written or endorsed by the authors. For technical accuracy, refer to the original paper. Read full disclaimer

Imagine a robot that does not move on wheels or legs, but instead bends and stretches like a living worm or a snake. These machines, known as continuum robots, are designed to slip into tight, twisting spaces where rigid machines cannot go, such as inside a human body for surgery or deep within the complex machinery of an aircraft engine. Because they are so flexible and often need to be very small, they cannot carry the large, heavy cameras or lasers that most robots use to see where they are. Without a way to know their exact position and shape, these robots are blind, unable to navigate safely or perform delicate tasks on their own. The challenge for engineers has been to give these soft, squishy machines a sense of direction using only tiny, lightweight sensors that fit on their bodies.

A team of researchers has developed a new way to solve this problem by giving the robot a distributed sense of touch and distance. Instead of relying on one powerful camera at the tip, they placed several small, low-resolution distance sensors along the entire length of the robot's body. These sensors, which work by measuring how long it takes for a pulse of light to bounce back from an object, are too weak on their own to map a complex room. A single sensor might see a wall but not know if it is facing a corner or a flat surface, leading to confusion. However, by placing many of these sensors along the robot and combining their weak, scattered signals with a computer model of how the robot is supposed to bend, the team created a system that can figure out exactly where the robot is and what shape it is taking.

The researchers tested this idea on a robot that is about 53 centimeters long, made of soft material that can stretch and compress. They equipped this robot with three rings of sensors, each holding three small distance detectors and a gyroscope to measure rotation. They then sent the robot into a cluttered environment filled with various objects, asking it to navigate while only using the data from its own body sensors. To help the robot understand its surroundings, the team first created a digital map of the room, but they also tested the system by changing the room—adding or removing objects—to see if the robot could still find its way when the map did not perfectly match reality.

The results showed that this method works surprisingly well, even when the sensors are confused or the map is slightly wrong. In their tests, the robot was able to determine its position with an average error of just 2.5 centimeters and its orientation with an error of about 7 degrees. This level of accuracy held true even when the robot encountered objects that were not in its original map, or when the environment looked different than expected. The system proved robust enough to handle the fact that individual sensors often failed to see enough details to make sense of the world on their own. By fusing the data from all the sensors along the robot's spine, the computer could fill in the gaps and maintain a clear picture of the robot's location.

The study also revealed how the robot's shape affects its ability to know where it is. The sensors near the base of the robot, which are anchored to a known starting point, were very accurate. However, as the robot stretched out, small errors in the measurements added up, making the tip of the robot slightly harder to track precisely. Despite this, the system remained stable and did not lose track of the robot entirely, even in difficult scenarios with many obstacles. The researchers found that the robot could also use its own movement data to help correct its position, acting as a backup when the distance sensors were uncertain.

This work demonstrates that it is possible to guide a soft, deformable robot through a messy, unstructured environment without needing external cameras or large tracking systems. The approach relies on the simple idea that many small, imperfect measurements, when combined with a good understanding of the robot's own structure, can create a reliable sense of place. While the system still depends on having a reasonably accurate starting map, the ability to tolerate changes in the environment suggests a path forward for robots that can inspect dangerous or inaccessible areas on their own. The researchers showed that by spreading sensors out along the body, a robot can overcome the limitations of small size and soft materials to navigate the real world with confidence.

Technical Summary: Continuum Robot Localization using Distributed Time-of-Flight Sensors

Problem Statement

Continuum robots (CRs) are highly effective for inspection and manipulation in constrained, unstructured environments due to their compliance and miniaturization capabilities. However, achieving accurate onboard localization for these robots remains a significant challenge. Traditional high-resolution sensors (e.g., standard LiDAR) are too large for integration into small-diameter CRs. Consequently, researchers must rely on small, low-resolution Time-of-Flight (ToF) sensors.

The core difficulty lies in the fact that these small sensors often produce degenerate measurements, where a single sensor cannot observe enough environmental features to fully constrain its position. Furthermore, existing localization approaches typically rely on dense sensing, rigid-body assumptions, or external reference systems (e.g., motion capture), which are unsuitable for deformable, onboard-only operation in cluttered environments. Even moderate deviations between a prior map and the true environment can cause standard estimators to fail.

Methodology

The authors propose a continuous-time, factor-graph-based Maximum A Posteriori (MAP) estimation framework designed to localize deformable continuum robots using only sparse, onboard sensors.

1. Sensor Fusion and State Estimation

The system fuses data from three sources distributed along the robot's body:

Distributed ToF Sensors: Low-resolution sensors (specifically VL53L5CX) mounted on rings along the robot.
Gyroscopes: Inertial measurements to aid in orientation.
Strain Sensors (Simulation only): Bending angle and curvature data (modeled after Fiber Bragg Grating systems) to provide a shape prior.

The state vector $x(s, t)$ includes continuous robot poses $T_{ib}(s, t) \in SE(3)$ , body-centric generalized strains $\epsilon(s, t)$ , and velocities $\varpi(s, t)$ at arc length $s$ and time $t$ .

2. Robust Measurement Factors

To handle the sparse and noisy nature of ToF data, the authors implement a point-to-plane measurement factor with a Cauchy loss function.

Point-to-Plane Error: The error is calculated by projecting measured ToF points onto a pre-computed prior map (point cloud with normals).
Robustness: A Cauchy loss is applied via an Iteratively Reweighted Least Squares (IRLS) scheme. This mitigates the impact of outlier measurements and geometric degeneracies, preventing the estimator from being overly influenced by incorrect matches in cluttered scenes.
Noise Modeling: The ToF noise is modeled as distance-dependent Gaussian noise, with standard deviation increasing as the measured distance increases (based on empirical data from the VL53L5CX).

3. Optimization

The framework utilizes a Gauss-Newton solver to minimize a weighted squared-error cost function. The continuous state is estimated at discrete time steps and arc lengths, with interpolation used to reconstruct the full continuous trajectory. The system operates in a sliding-window formulation.

4. Experimental Setup

Simulation: A 1.5 m long robot modeled in MuJoCo with 10 distinct environment scenarios (S0–S9) featuring added or removed geometric features to test map mismatch.
Real-World: A 53 cm long, extensible soft continuum robot equipped with three sensor rings (each with 3 ToF sensors and 1 gyroscope). Experiments were conducted in a 70 cm cube environment with varying object configurations (R0–R4) to simulate increasing complexity and map deviations.

Key Contributions

The paper claims two primary contributions:

A Robust Estimation Framework: A continuous-time, factor-graph-based MAP estimator that integrates sparse ToF measurements with a robot shape prior. This enables stable, full-body localization of deformable CRs even under frequent geometric degeneracies and partial prior map mismatches.
First Real-World Demonstration: The first onboard-only localization of a soft, extensible continuum robot in cluttered environments using distributed, low-resolution ToF and inertial sensing, validated across both simulated and physical platforms.

Results

Simulation Performance

In the simulated jet engine inspection scenario, the system demonstrated high robustness to map deviations.

Accuracy: The system achieved an average position error of approximately 0.78 cm (MAE) and rotation error of 0.96° (MAE) across all 10 scenes (including those with added/removed objects). The corresponding RMSE values were 0.99 cm and 0.98°, respectively.
Observation: The simulation results showed negligible degradation when the prior map deviated from the true environment, largely attributed to the robust cost function and the inclusion of strain data. However, the authors note that simulation results are significantly more accurate than real-world results due to idealized conditions and the absence of unmodeled errors.

Real-World Performance

In physical experiments, the system maintained localization capabilities despite environmental changes, though with higher error margins.

Baseline Accuracy: In environments where the prior map matched the scene, the system achieved an average position error between 1.2 cm and 2.5 cm and rotational error between 3.7° and 10.3°.
Impact of Anomalies:
- Adding an object increased average position error by ~1.7 cm and rotation error by ~1.0°.
- Removing an object increased position error by ~0.5 cm and rotation error by ~1.5°.
- The most significant degradation occurred with the removal of a large "wet floor sign," resulting in a position error of 3.6 cm and rotation error of 14.6°.
Overall Average: Across all experimental conditions, the system achieved an average localization error of 2.5 cm in position and 7.2° in rotation.
Error Distribution: Localization error accumulated along the robot's length, with the tip ring showing the highest error (3.6 cm translation, 9.9° rotation) compared to the base.

Scene Reconstruction

The authors demonstrated that the estimated robot trajectory and ToF measurements could be used to reconstruct the environment. When localization was accurate, the reconstructed scene closely matched the prior map (RMSE ~0.8–1.2 cm). When localization failed due to large anomalies, the reconstruction quality degraded significantly.

Significance and Limitations

The paper posits that this work demonstrates the feasibility of full-body, onboard-only localization for deformable continuum robots using only sparse, low-resolution depth sensors, eliminating the need for external tracking systems.

However, the authors remain modest regarding the current limitations:

Map Dependency: Accuracy is currently tied to the fidelity of the prior map. Large deviations between the map and the real scene can lead to estimator failure or convergence to local minima.
Local Minima: The system is susceptible to getting stuck in local minima, particularly with poor initialization, sudden robot movements, or significant map mismatches.
Future Work: The authors suggest future research should focus on relaxing the prior map dependency (e.g., live map updates), improving robustness to map inaccuracies, and integrating accelerometer data from the IMU to further aid in escaping local minima. They also note that while anomaly detection is a potential application, current noise levels in real-world experiments lead to false positives, requiring further improvements in localization accuracy before it becomes a viable inspection tool.