Continuum Robot Localization using Distributed Time-of-Flight Sensors

TL;DR

This work tackles onboard localization of deformable continuum robots using distributed low-resolution ToF sensors. It introduces a continuous-time factor-graph-based MAP estimator that fuses sparse ToF measurements with a robot shape prior, enabling full-body localization despite frequent geometric degeneracies. The approach is validated through simulations and real-world experiments, demonstrating robustness to moderate prior-map mismatch and showing practical localization accuracy (average around $2.5$ cm in position and $7.2^{\circ}$ in rotation on a 53 cm robot). The results highlight the potential of compact body-mounted depth sensing for autonomous inspection and manipulation with CRs, while also identifying limitations related to map dependency and local minima that guide future work.

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.

Figures (9)

• Figure 1: A continuum robot equipped with distributed sensors is shown localizing in a cluttered environment. The 53cm long, 7.6cm diameter extensible soft, continuum robot is equipped with three distributed sensor rings, each with three low-resolution ToF sensors and one gyroscope. Using only these sparse measurements, the robot is able to localize itself with high accuracy (1.9cm average distance error in the shown environment). Noisy ToF sensor measurements are shown as green points alongside the ToF sensor fields of view. At many points in time, individual sensor rings do not see enough features to fully constrain their location, yet by fusing the distributed measurements with a robot shape prior, accurate localization is still possible.
• Figure 2: Simulated jet engine environment with CR in the MuJoCo physics engine. The robot is 1.5m long and contains distributed ToF, IMU, and strain sensors. The environment mesh is displayed in blue while the collision mesh is shown in green. Time-of-flight sensor rays are visualized in yellow.
• Figure 3: Experimental setup including the extensible continuum robot, the OptiTrack camera system, and a closeup of one of the sensorized rings carrying the markers, ToF sensors, and IMUs. The scene depicted contains all four objects present (scene R4).
• Figure 4: Side-by-side comparison of the simulated environment (left) and the estimated robot (right). The estimated robot shape and trajectory closely match the ground truth, with an average position error at the marker locations of less than 1cm and rotational error of less than 1. Anomalies in the environment are marked---they appear to have little effect on localization performance in simulation.
• Figure 5: Simulated scene S8 (left), prior map S0 (middle), and the reconstructed scene (right) via ToF measurements. A depth-based color gradient is applied to the point clouds for visual clarity. The small objects added and removed between the two scenes appear to have little effect on localization performance in simulation.