State estimation techniques for continuum robots (CRs) typically involve using computationally complex dynamic models, simplistic shape approximations, or are limited to quasi-static methods. These approaches can be sensitive to unmodeled disturbances acting on the robot. Inspired by a factor-graph optimization paradigm, this work introduces a continuous-time stochastic state estimation framework for continuum robots. We introduce factors based on continuous-time kinematics that are corrupted by a white noise Gaussian process (GP). By using a simple robot model paired with high-rate sensing, we show adaptability to unmodeled external forces and data dropout. The result contains an estimate of the mean and covariance for the robot's pose, velocity, and strain, each of which can be interpolated continuously in both space and time. This same interpolation scheme can be used during estimation, allowing for inclusion of measurements that are asynchronous to the estimated states. Our method's inherent sparsity leads to a linear solve complexity with respect to time and interpolation queries in constant time. We demonstrate our method on a CR with gyroscope and pose sensors, highlighting its versatility in real-world systems.
Figures & tables
Fig. 1: A CR moving through time, taking asynchronous and noisy measurements of its state. This situation arises in practical scenarios where multiple high-rate sensors are used on the robot. In such scenarios, data from multiple high-rate sensors produce update rates much faster than a CR shape can reasonably be solved for. In this work, we propose addressing this issue by using a physically motivated Gaussian process prior defined over both space and time that is capable of continuous-time interpolation in constant time. This efficient interpolation can be used to fuse high-rate measurements from multiple sensors at slower time steps while maintaining accurate estimates in between time steps.
Method
Estimates Uncertainty
Dynamic
Continuous-Time
Robot Model
Feliu-Talegon et al. [ 21 ]
×
✓
✓
Physical
Zheng et al. [ 18 ]
×
✓
✓
Physical
Lobaton et al. [ 32 ]
×
✓
✓
Physical
Ataka et al. [ 26 ]
×
✓
×
Geometric
Borgstadt et al. [ 25 ]
✓
✓
×
Geometric
Lilge et al. [ 36 ]
✓
×
×
Physical
TABLE I: Comparison of Continuum Robot State Estimation Approaches
Symbol
Description
SE(3)
Special Euclidean group
se(3)
Lie algebra associated with SE(3)
(⋅)∧
Linear operator mapping R6 to se(3)
(⋅)∨
Inverse operator of (⋅)∧
(⋅)⋏
Spatial cross product operator for R6
J
Lie algebra left Jacobian
TABLE II: Nomenclature
Fig. 2: Factor-graph representation of the prior for the continuous-time estimation framework. Unary factors are depicted using green dots, binary space factors are represented with blue dots, and binary time factors are depicted using yellow dots. Each triangle represents a node at a specific point in arclength and time that is estimated directly in our solver. Each factor is connected to one or two nodes, and never connects two nodes that vary in both arclength and time.
Fig. 3: Factor-graph representation of the full continuous-time estimation framework. Unary factors are depicted using green dots, binary space factors with blue dots, and binary time factors with yellow dots. Each triangle represents a node at a specific point in arclength and time that is estimated directly in our solver. Measurements are added sparsely either directly to an estimation node or as an interpolated factor between two estimation nodes and are depicted as red dots. In practice, measurements will solely be added as interpolated factors in time, as the spatial estimation points included in the graph will be selected to align with the physical placement of each sensor on the robot.
Fig. 4: Schematic showing the spatial-temporal interpolation scheme proposed in this work. Given a query node at time tk≤t<tk+1 and arclength sn≤s<sn+1 , we first use spatial interpolation to find the nodes at (s,tk+1) and (s,tk) . Then, temporal interpolation is applied at each of the previously interpolated nodes, resulting in the estimate at the query point (s,t) .
Fig. 5: Sparsity structure of the problem’s inverse kernel matrix Pˇ−1 (left) and its associated Cholesky factor Lˇ (right) for an estimation problem with N=5,K=4 . The sparsity structure follows from the factor graph in Fig. 2 . Each row and column of the matrix corresponds to a node in the factor graph, with each non-zero entry corresponding to a correlation between two nodes. The Cholesky factor is lower triangular, with non-zero entries bounded about the diagonal. The band scales with N , the number of spatial nodes in our state, leading to a solve complexity of O(N3K) , where K is the number of time steps in our estimation window. Given M measurements, the total system time complexity, including matrix construction, becomes O(N3K+M) .
Variable
Size
Description
Q0
18×18
Base state covariance
Q1
6×6
Acceleration process noise covariance. Encodes system dynamics
Q2
6×6
Strain rate process noise covariance. Encodes system statics
Q3
6×6
Strain time derivative process noise covariance
Q4
6×6
Velocity spatial derivative process noise covariance
Rpose
6×6
Pose measurement noise covariance
TABLE III: Estimator Parameters
Physical Property / Assumption →
Parameter Setting
Kirchhoff rod approximation (minimal shear/elongation) →
Q07:9,Q21:3 small, Q24:5 large
Extensible simulated robot →
Q09 large for full-robot extension, Q23 large for segment-wise extension
Fixed base →
Q01:6,Q013:18 small
Stiff (flexible) robot →
Q24:5 small (large), Q4 small (large)
Heavy (light) robot →
Q11:6 small (large), Q3 small (large)
Known sensor noise →
R≈measured noise
TABLE IV: Tuning Heuristics
Fig. 6: Picture of the robot prototype described in Section III-B during data collection. Sensor locations are labeled in red with other relevant parts labeled in black.
Fig. 7: Schematic of relevant coordinate frames for extrinsic calibration. Measured pose values from both the Vicon and Aurora sensing systems are depicted as transforms between two frames. Calibration is performed to estimate the static transforms Tmb , Tte , and Tag . During estimation, all data is transformed into the Vicon world frame to enable comparison with ground-truth motion capture readings.
Fig. 8: Sensitivity analysis of the NEES metric for the estimator on the ‘Fast Contact’ trajectory using the modified Morris method [ 51 ] . Each point represents that parameter’s elementary effect standard deviation ( σ ) with respect to the mean absolute elementary effect ( μ∗ ). The sensitivity is found in the information space of each parameter. Elements towards the right indicate stronger influence on consistency, whereas elements towards the top are more nonlinear and dependent on others. In this work, we see that the angular components of Q1 and Q2 have the strongest influence on consistency while also having the most variability in the Morris sense.
Fig. 9: Comparison of the estimator (bottom) and the simulation (top) showing an extensible robot during the ‘Simulation Dropout’ trajectory using two pose sensors at five different time steps. 3 σ confidence bounds are shown. The estimator demonstrates its ability to generalize to an extensible robot configuration here, highlighting the method’s versatility across multiple robot types. The demo shown has an average tip error of 2.0 mm. Time nodes are placed at a rate of 30 Hz with spatial estimation performed after the fact for visualization.
Trial Data Type
MAE (Tip)
MAE (Body)
RMSE (Tip)
RMSE (Body)
Average NEES
Trajectory description
Pos
Rot
Pos
Rot
Pos
Rot
Pos
Rot
Simulation
7.44
A sporadic trajectory with random motions and extensions
Pose (mm, rad)
1.555
0.005
1.205
0.021
1.935
0.005
1.773
0.029
Velocity (mm/s, rad/s)
28.511
0.044
15.661
0.030
45.714
0.064
30.978
0.054
Simulation High Noise
7.55
‘Simulation’ with sensor noise multiplied by a factor of 2
Pose (mm, rad)
1.989
0.007
1.380
0.021
2.461
0.007
1.998
0.030
TABLE V: Performance Metrics and Trajectory Description for Simulated Trajectories.
Fig. 10: Comparison of the estimator (bottom) and a video (top) recording of the robot during the ‘Fast Contact’ trajectory using both pose and gyroscope sensors at eight different time steps. 3 σ confidence bounds are shown. The estimator demonstrates its ability to maintain reasonable estimates of the robot’s state when unmodeled external forces are applied. The demo shown has an average tip error of 1.169%. Time nodes are placed at a rate of 30 Hz with no interpolated states added after the fact.
Fig. 11: Robot tip position estimate during the ‘Fast Contact’ fused data trajectory. In this demo, external forces are applied to the robot and noted by the shaded regions. Ground-truth values are shown as points, while the estimated mean and 3- σ bounds are shown as solid lines and shaded regions, respectively. Interpolation in time is performed after the fact, showing 9 interpolated nodes between each estimated one. The estimator is able to maintain a reasonable estimate of the robot’s state despite the unmodeled external forces. We note that during aggressive external forces (bottom zoomed region), the estimator may experience overconfidence in its estimate.
Trial Configuration
MAE (Tip)
MAE (Body)
RMSE (Tip)
RMSE (Body)
Average NEES
Trajectory description
Pos (%)
Rot (rad)
Pos (%)
Rot (rad)
Pos (%)
Rot (rad)
Pos (%)
Rot (rad)
Out-of-Bounds
A dynamic trajectory powered by tendon actuation. Some pose data is lost as the robot leaves the sensor workspace
Pose-only
1.565
0.043
6.566
0.132
2.318
0.059
8.290
0.177
6.662
Gyro-only
16.735
0.745
11.712
0.158
17.939
0.762
12.988
0.201
7.778
Fused
1.508
0.038
6.251
0.115
2.091
0.041
7.897
0.151
7.933
Fast Contact
A dynamic trajectory achieved through applying sustained external contacts. Fast oscillations present
TABLE VI: Performance Metrics and Trajectory Description for Experimental Trajectories.
Fig. 12: χ2 coverage plot for the estimator applied over each of the five experimental trajectories. Coverage for the pose-only, gyro-only, and fused configurations are provided. Callouts are provided for the 1-, 2-, and 3- σ coverage points. The estimator behavior shows a conservative estimate near the mean with some overconfidence at higher uncertainties, an effect that is exacerbated when the gyroscope sensor is added.
Fig. 13: Runtime comparison for the ‘Fast Contact’ fused data estimate. The runtime shown is for estimating only the mean of the state. The proposed implementation runtime scales linearly with respect to the number of time nodes K , a result predicted from the sparsity of the problem. Cubic scaling is also observed with respect to the number of spatial nodes N , as predicted.
Fig. 14: Comparison to the quasi-static benchmark [ 36 ] on the ‘Out-of-Bounds’ trajectory using end-effector pose measurements. The leftmost column shows the robot tip position over time for both the proposed method and the quasi-static estimator. The right two columns show a comparison between the two methods at two different time steps, 5 s and 9 s. When data at a particular time step is available (e.g., middle column), the two methods produce similar mean estimates, with our new method having higher amounts of certainty despite using the same noise characteristics in the arclength WNOA model. The additional information provided in time allows the proposed method to produce a more confident estimate, making it less sensitive to high-frequency noise. When data is not available (e.g., right column), the quasi-static benchmark does not have well-defined behavior and is unable to produce a reasonable estimate.
Fig. 15: A snapshot of the ‘Fast Contact’ fused data estimate at time 2.833 s with three different selections of N . For each of the N=5,9,17 cases, interpolation is performed so that 17 total nodes are displayed. As N increases, the covariance estimates slightly decrease. The change also results in small deviations in the mean estimate.
Stochastic state estimation methods for continuum robots (CRs) often struggle to balance accuracy and computational efficiency. While several recent works have explored sliding-window formulations for CRs, these methods are limited to simplified, discrete-time approximations and do not provide stochastic representations. In contrast, current stochastic filter methods must run at the speed of measurements, limiting their full potential. Recent works in continuous-time estimation techniques for CRs show a principled approach to addressing this runtime constraint, but are currently restricted to offline operation. In this work, we present a sliding-window filter (SWF) for continuous-time state estimation of CRs that improves upon the accuracy of a filter approach while enabling continuous-time methods to operate online, all while running at faster-than-real-time speeds. This represents the first stochastic SWF specifically designed for CRs, providing a promising direction for future research in this area.
Spencer Teetaert, Sven Lilge, Jessica Burgner-Kahrs +1
University of Toronto Robotics Institute, Toronto, ON, Canada
Recent factor graph approaches to continuum robot state estimation have been successful for quasi-static applications and spatiotemporal estimation using white-noise kinematic motion priors. However, when inertial effects are significant, these approximations may fail to capture the underlying physics, limiting accuracy during dynamic motions. In contrast, our approach approximates the Cosserat rod dynamics of continuum robots. We write inertia and damping as equivalent applied loads, so that the dynamic balance retains the algebraic form of the static one from prior work with quasi-static robots. Without backbone observations, the framework reduces to a stochastic forward simulation of the robot's motion. Given observations, it jointly refines kinematic and dynamic states and infers external loads, among other states. We validate the approach through simulation and experiments, demonstrating stochastic forward simulation as well as state estimation on tendon-driven continuum robots.
James M. Ferguson, Tucker Hermans, Alan Kuntz
Department of Electrical and Computer Engineering and Department of Computer Science, Vanderbilt University, TN, USA. · Kahlert School of Computing and Robotics Center, University of Utah, UT, USA.
In this paper, we present a fully discrete approach for the accurate and numerically efficient dynamical modeling and state estimation of continuum robots. The model is based on geometrically exact beams in a minimal, strain-based formulation and derived in the framework of Lie group variational integrators, allowing to preserve important geometric properties that we exploit to achieve high accuracy and numerical efficiency. We then propose a disturbance observer based on an extended Kalman filter formulation that reliably estimates system states as well as model uncertainties and external disturbances. Experiments on a real system validate the accuracy and efficiency of the proposed model and observer.
Maximilian Herrmann, Leander Pfeiffer, Paul Kotyczka
Technical University of Munich, TUM School of Engineering and Design, Munich Institute of Robotics and Machine Intelligence (MIRMI), Chair of Automatic Control.