💻 computer science

State and Trajectory Estimation of Tensegrity Robots via Factor Graphs and Chebyshev Polynomials

This paper proposes a novel two-stage framework for robust state and trajectory estimation of cable-driven tensegrity robots that combines factor graphs for sensor fusion with Chebyshev polynomials for continuous-time velocity and state reconstruction, demonstrating superior performance over conventional ICP-based methods in handling the system's nonlinear and underconstrained dynamics.

Original authors: Edgar Granados, Patrick Meng, Charles Tang, Shrimed Sangani, William R. Johnson III, Rebecca Kramer-Bottiglio, Kostas Bekris

Published 2026-04-10

📖 5 min read🧠 Deep dive

CC BY 4.0

Original authors: Edgar Granados, Patrick Meng, Charles Tang, Shrimed Sangani, William R. Johnson III, Rebecca Kramer-Bottiglio, Kostas Bekris

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 looks like a floating sculpture made of sticks and bouncy strings. This is a tensegrity robot. Unlike a standard robot with rigid metal joints and motors, this one is flexible, squishy, and can roll, bounce, and deform to get through tight spaces. It's like a living jellyfish made of rigid bars held together by tension.

But here's the problem: because it's so wobbly and strange, it's incredibly hard to know exactly where every part of it is at any given moment. If you want to control it, you need to know its "state" (where it is and how fast it's moving). Traditional robot brains struggle with this because the robot's movements are chaotic and non-linear.

This paper introduces a new "brain" for these robots that uses two clever tricks: Factor Graphs and Chebyshev Polynomials. Here is how it works, broken down into simple concepts:

1. The Problem: The "Blurry" Robot

Imagine trying to take a photo of a spinning, wobbly jellyfish in a dark room.

The Camera: You have a camera (RGB-D) trying to see the robot, but the robot blocks its own view (self-occlusion), and the colors of the sticks might look like background objects. It's a messy, noisy picture.
The Sensors: The robot also has "feeling" sensors (cables) that tell it how stretched its strings are, but they don't tell you where the robot is in the room, only how its shape is changing.
The Challenge: You need to combine the blurry camera photos with the cable feelings to figure out exactly where the robot is and how fast it's moving.

2. Trick #1: The Factor Graph (The "Puzzle Solver")

The authors use a method called a Factor Graph. Think of this as a giant, smart puzzle solver.

The Pieces (Variables): The puzzle pieces are the positions of the robot's rigid sticks (the bars).
The Clues (Factors): The clues are the data from the camera and the cables.
How it works: Instead of guessing the whole picture at once, the Factor Graph connects the clues to the pieces. It asks: "If the camera sees a red dot here, and the cable says it's stretched this much, does that fit with the stick being in this position?"
The Filter: Before solving the puzzle, the robot uses a "Mahalanobis distance" filter. Imagine you are looking for a red ball in a pile of red apples. The filter is like a smart sieve that says, "That red dot is too far away from the other red dots to be part of the robot; throw it out as noise." This cleans up the messy camera data before the puzzle solver even starts.

The Result: The Factor Graph fuses the camera and cable data to give a very accurate, real-time guess of where the robot is, even without knowing exactly how the motors are pushing it.

3. Trick #2: Chebyshev Polynomials (The "Smoothie Blender")

The Factor Graph gives you a snapshot of the robot's position at specific moments (like taking a photo every second). But what if you want to know exactly how it moved between those seconds? Or what its speed was?

The Problem: If you just connect the dots (snapshots) with straight lines, the movement looks jerky and fake.
The Solution: The authors use Chebyshev Polynomials. Think of this as a "smoothie blender" for data.
- Imagine you have a few scattered points of a curve. A Chebyshev polynomial is a special mathematical recipe that takes those scattered points and blends them into a perfectly smooth, continuous curve.
- Unlike other methods that might get messy or require complex math to find the speed, Chebyshev polynomials are like a magic ruler: they let you instantly calculate the exact position and speed at any point in time, even between your snapshots.

4. The Big Picture: Why This Matters

The researchers tested this on a real robot and in a computer simulation. They compared their new method to an old method (ICP) that was like trying to solve a puzzle by hand, requiring you to manually set the starting position every time.

The Old Way: Slow, requires manual setup, and gets confused easily.
The New Way:
1. Robust: It handles noise and missing data (like when the robot hides its own face from the camera) very well.
2. Automatic: It doesn't need a human to tell it where to start.
3. Smooth: It creates a high-definition, continuous movie of the robot's movement, not just a slideshow of snapshots.

The Takeaway

This paper is like giving a wobbly, flexible robot a pair of glasses and a super-smart memory.

The Factor Graph acts as the glasses, cleaning up the blurry vision and combining it with the robot's sense of touch to see clearly.
The Chebyshev Polynomial acts as the memory, turning a series of disjointed memories into a smooth, continuous story of where the robot has been and how fast it was going.

This allows engineers to finally control these tricky robots with precision and use the data to teach them new tricks via machine learning, opening the door for robots that can explore disaster zones or space in ways rigid robots never could.

1. Problem Statement

Tensegrity robots (structures composed of rigid rods in compression and cables in tension) offer high compliance and adaptability but present significant challenges for state estimation due to their:

Nonlinear and underconstrained dynamics: The relationship between actuation and motion is complex and difficult to model explicitly.
Sensor limitations: Reliable continuous-time estimation of all rigid links is crucial for closed-loop control and system identification, yet conventional methods often fail.
Data noise and occlusion: Visual data (RGB-D) suffers from self-occlusions and noise, while onboard cable sensors provide shape information but not absolute world position.
Discrete vs. Continuous needs: Most existing methods use discrete-time estimation (estimating state at specific time steps), which is insufficient for high-frequency control, fine-grained system identification, and machine learning tasks requiring smooth, continuous trajectories and velocity derivatives.

The paper addresses the need for a robust method to estimate the 6-DoF poses of rigid rods and their velocities in continuous time, fusing noisy RGB-D camera data with onboard cable length sensors, without relying on a pre-defined actuation model.

2. Methodology

The authors propose a two-stage approach: an online state estimation stage using Factor Graphs, and an offline trajectory refinement stage using Chebyshev Polynomials.

A. Preprocessing: Mahalanobis Distance-Based Clustering

To handle the high volume and noise of RGB-D point cloud data:

Color-based Segmentation: Points are classified by color (Red, Green, Blue for endcaps; Black for bars).
Incremental Clustering: A clustering algorithm based on Mahalanobis distance is used to group points into clusters representing endcaps and bars.
Outlier Rejection: The algorithm computes the error between a prior estimate and a new measurement. If the error exceeds a threshold (based on a Chi-squared distribution), the measurement is rejected. This reduces spurious detections (e.g., environmental objects with similar colors) before optimization.

B. Online State Estimation: Factor Graphs

A factor graph framework is employed to fuse sensor data in a unified probabilistic framework.

Variables: Represent the 6-DoF poses ($SE(3)$) of the robot's three rigid bars.
Factors (Constraints):
- Endcap Factors: Relate observed endcap points to bar poses, handling the "correspondence problem" (identifying which endcap is which) via symmetry constraints ( $f_{R0}, f_{R1}, f_{R2}$ ).
- Cable Factors: Enforce the measured distances between specific endcaps based on onboard cable sensors.
- Between Factors: Enforce temporal consistency between consecutive time steps, penalizing unrealistic jumps in state. The covariance of this factor scales with the time delta ($dt$) to handle variable observation frequencies.
- Bar Factors: Relate clustered black points (bars) to the estimated bar poses, minimizing the distance to the closest bar.
Optimization: The system solves for the Maximum A Posteriori (MAP) estimate, effectively performing nonlinear least squares optimization to find the most probable configuration given the noisy observations.

C. Offline Trajectory Estimation: Chebyshev Polynomials

To generate continuous-time trajectories and estimate velocities (which the discrete factor graph does not provide directly):

Parameterization: The discrete state estimates are fitted using Chebyshev polynomials.
Advantages: Unlike splines, Chebyshev polynomials are easily defined over Lie groups (manifolds like $SE(3)$) and allow for easy computation of derivatives (velocities).
Process:
1. A pseudo-spectral parametrization fits the endcap trajectories.
2. A search algorithm determines the optimal polynomial degree to balance fit accuracy and overfitting.
3. Bar trajectories are derived by querying the endcap polynomials at Chebyshev points.
4. This yields a smooth, high-fidelity representation of the robot's motion, including velocities, suitable for system identification.

3. Key Contributions

First Application of Factor Graphs to Tensegrity: This is the first work to apply factor graphs to tensegrity robots, leveraging the robot's structural constraints to create sparse, real-time representations.
Robust Sensor Fusion: The integration of RGB-D vision and onboard cable sensors within a probabilistic framework that handles occlusions and noise effectively via Mahalanobis clustering.
Actuation-Model-Free Estimation: The method estimates state and trajectory without requiring an explicit, difficult-to-define actuation model (mapping motor inputs to structural deformation).
Continuous-Time Trajectory Reconstruction: The novel use of Chebyshev polynomials to convert discrete estimates into continuous-time trajectories with accessible velocity derivatives, essential for data-driven modeling.
Comprehensive Evaluation: Validation on both simulated data (Mujoco) and real-world datasets, comparing against state-of-the-art ICP-based methods.

4. Experimental Results

The method was evaluated on three datasets: two existing datasets (Original Long/Short) and a new dataset collected with a different robot/environment.

Comparison with ICP: The proposed method was compared against a previously proposed ICP-based algorithm [4].
- Initialization: The ICP method requires manual initialization from a stable configuration and struggles with synchronization gaps. The proposed method requires no manual initialization.
- Accuracy:
  - In terms of translation and center-of-mass error, the ICP method performed slightly better on some datasets.
  - However, the proposed Factor Graph method showed lower rotation error on the new dataset (0.365 vs 0.523 rad).
  - The Chebyshev method had slightly higher position errors than the raw Factor Graph output but provided the necessary continuous-time representation.
Ablation Study: Simulations showed that Chebyshev polynomials maintain high fidelity even with varying observation frequencies (5Hz to 30Hz) and significant noise, outperforming linear interpolation for intermediate state estimation.
Robustness: The clustering algorithm successfully filtered out noise and occlusions, allowing the factor graph to converge to accurate states even with missing data.

5. Significance and Future Work

Impact: This work bridges the gap between the complex dynamics of soft/tensegrity robots and the need for high-fidelity state estimation required for modern control and machine learning.
System Identification: The continuous-time, velocity-rich output enables accurate system identification and the training of data-driven models (e.g., Graph Neural Networks) which previously lacked high-quality labels.
Future Directions: The authors suggest extending this framework to simultaneous estimation and control, and adapting the factor graph for other tensegrity topologies (e.g., 6-bar platforms) and additional sensor modalities.

In summary, the paper presents a robust, model-free pipeline that transforms noisy, discrete sensor data into high-fidelity, continuous-time state and trajectory estimates, significantly advancing the capability to control and learn from tensegrity robots.