State Estimation for Tensegrity Robots
Tensegrity robots are a class of compliant robots that have many desirable traits when designing mass efficient systems that must interact with uncertain environments. Various promising control approaches have been proposed for tensegrity systems in simulation. Unfortunately, state estimation methods for tensegrity robots have not yet been thoroughly studied. In this paper, we present the design and evaluation of a state estimator for tensegrity robots. This state estimator will enable existing and future control algorithms to transfer from simulation to hardware. Our approach is based on the unscented Kalman filter (UKF) and combines inertial measurements, ultra wideband time-of-flight ranging measurements, and actuator state information. We evaluate the effectiveness of our method on the SUPERball, a tensegrity based planetary exploration robotic prototype. In particular, we conduct tests for evaluating both the robot’s success in estimating global position in relation to fixed ranging base stations during rolling maneuvers as well as local behavior due to small-amplitude deformations induced by cable actuation.
Tensegrity robotics is a relatively young field of research wherein a robot is structured according to tensegrity principles. We define a tensegrity as a structure with discontinuous compression elements suspended within a web of tension elements. In this class of robots, motion is often achieved through actuation of the tensile elements within the structure.
Tensegrity robots have highly coupled dynamics due to their interconnected network of compliant tensile elements. As such, an external force exerted at a single point will cause a global displacement in all nodal positions of the system. This property is beneficial in that it allows the system to passively adapt to external forces and redistribute loads effectively through the tension network. However, it also causes difficulties in determining the state of the system when only limited sensor information is available.
Various control approaches have been proposed for tensegrity systems in simulation. However, these algorithms often rely on full state or trajectory information . In this paper, we present the design and evaluation of a state estimator for this class of robot which will allow the transfer of these existing and future control algorithms from simulation to hardware.
We focus on the use of low-cost ranging modules and inertial measurement units mounted to the rods of a tensegrity robot as the sensor inputs to an unscented Kalman filter (UKF). These ranging sensors can be purchased off-the-shelf and do not rely on any user-designed mechanical infrastructure to operate. We also use motor encoders to sense change in cable rest length as control inputs into the dynamic model utilized by the UKF. For testing our approach, we use the SUPERball prototype, a six strut tensegrity robot designed to explore tensegrity systems for planetary exploration . SUPERball is shown in Fig. 1.
This paper is organized as follows. We first present a detailed overview of the system’s sensors, ranging method and calibration routine. We then describe the implementation of the unscented Kalman filter and the dynamics model. This is followed by our experimental results. We end this paper with our conclusions and future outlook.
Ii SUPERball Overview
The Spherical Underactuated Planetary Exploration Robot (SUPERball) is a preliminary tensegrity robot prototype developed at the NASA Ames Research Center. The purpose the SUPERball project is to develop technologies for a new class of planetary exploration robot which is able to deploy from a compact launch volume, land at high speeds without the use of air-bags, and provide robust surface mobility. This broad set of functions can be enabled by utilizing the efficiency and structural compliance of tensegrity robots. Passive-structure drop tests have confirmed the analysis supporting the high-speed landing concept, and the current prototype of SUPERball is intended to develop the foundational engineering approaches required to support surface locomotion by tensegrity robots. A full system overview is out of the scope of this paper, but the relevant details of the system will be discussed. Please refer to Bruce et. al.  and Sabelhaus and Bruce  for system details.
Relevant to this work are the sensors, actuators, and the ROS network implemented on SUPERball. SUPERball consists of 6 identical rods held together by 24 cables in-line with springs. As described in , each rod of SUPERball is comprised of two modular tensegrity robotic platforms (end caps). Each platform is equipped with inertial measurement units, a motor with encoders, and a fully enabled Robot Operating System (ROS) node which communicates to our ROS network via WiFi. SUPERball is underactuated and only 12 out of the 24 cables can be actuated (shortened) by spooling cable around a spindle (one per end cap). This allows the robot to roll through deformation. Since each modular tensegrity platform can be outfitted with sensors, ranging sensors were added for this paper to enable positioning. The ranging sensors are discussed in detail in Section III.
Iii Ranging Setup and Calibration
This section introduces the hardware and software setup for a set of wireless ranging modules to enable position tracking of the robot both as an as internal distance measurements (end cap to end cap) an in an external (world) reference frame.
We equipped all end caps of SUPERball with a DWM1000 ranging module from DecaWave Ltd. By employing ultra wideband technology, the low-cost DWM1000 modules provide wireless data transfer and highly accurate timestamps of transmitted and received packets. This allows the distance between two DWM1000 modules to be estimated by computing the time-of-flight of exchanged messages without the need for synchronized clocks. We opted for this technology because it allows proprioceptive state estimation (distances between end caps), which cannot be easily tracked directly via motor encoders.  Furthermore, we placed eight more DWM1000 modules as ”fixed anchors” around our testing area to provide a world reference frame for ground truth and generation of a reward signal for the machine learning algorithms that we will use to develop locomotion controllers for the robot. Our intention is that the fixed anchors will not be required in the final deployed version of the robot, and are primarily for use during algorithm development.
We first introduce the basic sensor operation and our approach to efficiently estimate distances between a large number of sensor modules. This is followed by a discussion of our ranging software and hardware setup. Finally, we provide a calibration routine similar to a common motion capture system that allows for quick set up of the sensor network.
Iii-a Sensor Operation
Iii-A1 Bidirectional Ranging
We operate the DWM1000 in the so-called symmetric double-sided two-way ranging mode. In this mode, the modules exchange packets to estimate the time-of-flight between each other. While the time-of-flight of unsynchronized modules can be estimated with the exchange of only packets, the employed mode can significantly reduce measurement noise .
The basic ranging packet exchange is shown in Fig. 2. One module sends out a poll message containing an emission timestamp () using its local clock. A second module receives this message and timestamps the time of arrival using its local clock (). The second module then sends out a response packet at time (module 2’s clock). The first module receives this packet at time (module 1’s clock). Module 1 now sends out a final message containing and the emission time of the final message (, clock of module 1). Module 2 receives this information and timestamps it ().
Module 2 can now estimate the time-of-flight and the distance between itself and module 1 based on the 6 timestampes. The basic equations to estimate the distance between module and module (module initiates the ranging and module computes the distance) are given by:
The variables , , , and are also visualized in Fig. 2. The time-of-flight calculation between two modules and () is hindered by a fixed measurement offset (). This offset is due to antenna delays and other discrepancies between the timestamps and actual packet reception or emission. Whereas this offset is expected to be unique to each module, we found that it is necessary to estimate this offset pairwise for closely located modules. Our hypothesis is that the proximity of the robot’s motors and the sensor’s position near the end cap’s metal structure influence the antenna characteristics between pairs of modules.
Eq. 6 estimates the distances between the modules based on the time-of-flight calculation ( is the speed of light). We rewrite the time offset as a distance offset (with ). Here and refer to the positions of nodes and respectively (see Section IV). The variables represent the uncorrected distance estimates.
The DWM1000 requires careful configuration for optimal performance. We provide our main configuration settings in Table I. The ranging modules tend to measure non line-of-sight paths near reflective surfaces (e.g. floor, computer monitors), which may cause filter instability. Using the DWM1000’s built-in signal power estimator, we reject such suspicious packets. In practice, between and of packets are rejected in our indoor test environment.
Iii-A2 Broadcast Ranging
Due to the large number of exchanged packets (3 per pair) bidirectional ranging between pairs of modules quickly becomes inefficient when the number of modules grows. We propose a simple approach using timed broadcast messages that scales linearly in the number of modules (3 packets per module). In this setup one module periodically initiates a measurement sequence by sending out a poll message. When another module receives this message it emits its own poll message after a fixed delay based on its ID, followed by response and final messages after additional delays. Broadcast ranging is illustrated in Fig. 3.
One disadvantage of the broadcasting approach is that the total measurement time between a pair of modules takes longer (up to in our experimental setup) than a single pairwise bidirectional measurement (approx. ). However, broadcast ranging provides two measurements for each pair of modules per measurement iteration.
Note that each module now needs to keep track of the poll and final packet reception times of all other modules. The final packet becomes longer as each module needs to transmit the response reception time () of all other modules.
Iii-B Ranging Setup
Each end cap of SUPERball was fitted with a DWM1000 module located approximately from the end of the strut. To simplify the notation, we do not distinguish between end cap positions (ends of the struts) and the positions ranging sensor locations. In practice, we take this offset into account in the output function of our filter (see Section IV).
The broadcasting algorithm runs at and packet transmissions are spaced apart. This allows for over modules to range. After one ranging iteration, each end cap transmits its measurements over WiFi to the ROS network. A ROS node then combines measurements from all end caps into a single ROS message at .
The fixed anchors operate in a similar way to the end caps, but are not connected to a ROS node and can not directly transmit data to the ROS network. This means that we obtain two measurements (one in each direction) for each pair of modules on the robot, but only a single measurement between the fixed anchors and the modules on the robot.
One of the design goals of our state estimation method is quick deployment in new environments without significant manual calibration. To achieve this, we implemented an automatic calibration procedure to jointly estimate the constellation of fixed modules (anchors, defining an external reference frame) and the pairwise sensor offsets (). Calibration is performed - similar to common motion capture systems - by moving the robot around, while recording the uncorrected distance measurements ().
After recording a dataset, we minimize the reconstruction error by optimizing over the offsets ( rearranged as a vector), the estimated anchor locations , and the estimated moving module locations (i.e. the module on the robot’s end caps):
The brackets in indicate the moving module locations (end cap positions) at a specific timestep. For example contains the estimated end cap positions at timestep 5 in the recorded dataset. In Eq. 9, iterates over anchors, iterates over moving nodes and iterates over samples. The indicator variables are equal to when for sample there are at least valid measurements to the fixed module for moving module (i.e. the number of DOFs reduces).
In practice we also add constraints on the bar lengths, which take the same form as Eq. 8 with the offsets set to . We used BFGS to minimize Eq. 9 with a dataset containing approximately timesteps selected randomly from a few minutes of movement of the robot. Although the algorithm works without prior knowledge, we noticed that providing the relative positions of fixed nodes ( manual measurements) significantly improves the success rate as there are no guarantees on global convergence.
Once the external offsets (between the anchors and moving nodes) and the module positions are known, we can estimate the offsets between moving nodes in a straightforward way by computing the difference between the estimated internal distances and the uncorrected distance measurements.
Iv Filter Design
Tensegrity systems are nonlinear and exhibit hybrid dynamics due to cable slack conditions and interactions with the environment that involve collision and friction. This warrants a robust filter design to track the robot’s behavior.
The commonly used Extended Kalman Filter (EKF) does not perform well on highly nonlinear systems where first-order approximations offer poor representations of the propagation of uncertainties. Additionally the EKF requires computation of time-derivatives through system dynamics and output functions which is challenging for a model with complex hybrid dynamics.
The sigma point UKF does not require derivatives through the system dynamics and is third order accurate when propagating Gaussian Random Variables through nonlinear dynamics . The computational cost of the UKF is comparable to that of the EKF, but for tensegrity systems which commonly have a large range of stiffnesses and a high number of state variables the time-update of the sigma points dominates computational cost. As such we first describe the methods used to reduce computational cost of dynamic simulation, then in the following section we outline the specific implementation of the UKF for the SUPERball prototype.
Iv-a Dynamic Modelling
The UKF requires a dynamic model which balances model fidelity and computational efficiency since it requires a large number of simulations to be run in parallel. We model a tensegrity system as a spring-mass net and used the following incomplete list of simplifying assumptions:
Only point masses located at each node point
All internal and external forces are applied at nodes
Members exert only linear stiffness and damping
Unilateral forcing in cables
Flat ground at a known height with Coulomb friction
No bar or string collision modelling
For a tensegrity with nodes and members, the member force densities, , can be transformed into nodal forces, , by using the current Cartesian nodal positions, , and the connectivity matrix, , as described in . This operation is described by the equation:
where represents the creation of a diagonal matrix with the vector argument along its main diagonal. We first note that produces a matrix where each row corresponds to a vector that points between the th and th nodes spanned by each member. Therefore, this first matrix multiplication can be replaced with vector indexing as , where we use the notation to denote the th row of matrix . If we then compute with the same method as , we obtain a matrix of relative member velocities. The matrices and are used to calculate member lengths as and member velocities as
We can then use these values to calculate member force densities, , using Hooke’s law and viscous damping as:
Here and denote the th member’s stiffness and damping constants, respectively. Note that cables require some additional case handling to ensure unilateral forcing.
Scaling each by yields a matrix whose rows correspond to vector forces of the members. We denote this matrix as , and we note that . Thus this matrix of member forces can be easily applied to the nodes using:
We now have a method for computing nodal forces exerted by the members and need only compute ground interaction forces, which we will denote as . We computed ground interaction forces using the numerical approach in . The nodal accelerations can then be written as:
where is a diagonal matrix whose diagonal entries are the masses of each node and is matrix with identical rows equal to the vector acceleration due to gravity. It is then straightforward to simulate this second order ODE using traditional numerical methods.
Note also that it is possible to propagate many parallel simulations efficiently by concatenating multiple matrices column wise to produce for parallel simulations. The resultant vectorization of many of the operations yields significant gains in computational speed with some careful handling of matrix dimensions.
Iv-B UKF Implementation
We implement a traditional UKF as outlined in  with additive Gaussian noise for state variables and measurements.
Several parameters are defined for tuning the behavior of the UKF, namely , and , where determines the spread of the sigma points generated by the unscented transformation, is used to incorporate prior knowledge of distribution, and is a secondary scaling parameter. We hand tuned these parameters to the values , for Gaussian distributions and and found this to yield an adequately stable filter.
We define our state variables as and stacked in a vector where is the number of state variables. We assume independent state noise with variance thus with covariance .
For measurements we take estimated orientation data from our IMUs using a gradient descent AHRS algorithm based on , where is the number of bar angles available at the given time step and all ranging measures, , where is the number of ranging measures available at a given time step. We again assume independent noise with and the measurement covariance matrix is then defined as:
These user defined variables are then used within the framework of our UKF to forward propagate both the current expected value of the state as well as its covariance. Fig. 4 shows an overview of our complete state estimation setup.
V Filter Evaluation
V-a Experimental Setup
To evaluate the performance of the UKF, we used the eight ”fixed anchor” ranging base stations calibrated as detailed in Section III-C. Each end cap of SUPERball was then able to get a distance measurement to each base station. This information was sent over ROS along with IMU data (yaw,pitch,roll) and cable rest lengths to the UKF. The base stations were placed in a pattern to cover an area of approximately . Each base station’s relative location to each other may be seen in Fig. 5. SUPERball and the base stations were then used to show the UKF tracking a local trajectory of end caps and a global trajectory of the robotic system. In each of these experiments, the UKF was allowed time to settle from initial conditions upon starting the filter. This ensured that any erroneous states due to poor initial conditioning did not affect the filter’s overall performance.
V-B Local Trajectory Tracking
In order to track a local trajectory, SUPERball remained stationary while two of its actuators tracked phase shifted stepwise sinusoidal patterns. During the period of actuation, two end cap trajectories were tracked on SUPERball and compared to the trajectory outputs of the UKF. One end cap was directly connected to an actuated cable (end cap 2), while the other end cap had no actuated cables affixed to it (end cap 1). To obtain a ground truth for the position trajectory, a camera that measured the position of each end cap was positioned next to the robot. Both end caps started at the same relative height and the majority of movement of both fell within the plane parallel to the camera. Fig. 6 shows the measured and UKF global positions of the two end caps through time.
V-C Global Trajectory Tracking
For global trajectory tracking, SUPERball was actuated to induce a transition from one base triangle rolling through to another base triangle as presented in . Ground truth for this experiment was ascertained by marking and measuring the positions of each base triangle’s end caps before and after a face transition. We evaluate 4 settings of the state estimator. Full: The state estimator as described in Section IV with all IMU and ranging sensors. no IMU: Only the ranging sensors are enabled. full w. cst. offset: Same as full, but the offsets are set to a constant instead of optimized individually. 4 base station ranging sensors: 50% of the base station ranging sensors are disabled. The results of this experiment are presented in Fig. 7 and 8.
We have introduced a state estimation approach for tensegrity robots based on ultra wideband ranging sensors, inertial measurements, and actuator states. An unscented Kalman filter was used to combine these sensor and state observations. While this is a fairly common approach to state estimation, our algorithm is robust to measurement noise and significant amounts of missing data. To verify these statements, we evaluated our method on two tasks: rolling and stationary deformations of SUPERball.
Most of the current control approaches for tensegrity robots rely on position tracking for either motion planning or performance evaluation. Hence, our main contribution to the field of tensegrity robotics is that this work will finally allow various proposed control algorithms for tensegrity robots to be transferred from the simulation domain to hardware.
We believe that - in this context - our setup is a viable alternative to more established external solutions, such as motion capture systems. In particular, our approach is low-cost, self-calibrating and only relies on autonomous anchors. These last two qualities are particularly attractive for future space missions. We do not yet achieve the accuracy provided by commercial motion capture systems. However, this is not a priority at this point as we mainly are focused on large displacements by rolling of SUPERball.
The logical next step is to test our state estimator when SUPERball is moving around in a larger space. The RoverScape at NASA Ames - a football field sized outdoor rover testbed - is the ideal candidate test area for this.
A related future goal is to add support for an incremental number of ranging anchors. This will allow SUPERball to explore uncharted terrain by dropping beacons when the uncertainty of its current position increases.
We appreciate the support, ideas, and feedback from members of the Dynamic Tensegrity Robotics Lab. We are also grateful to Terry Fong and the NASA Ames Intelligent Robotics Group.
-  J. Rieffel, R. Stuk, F. Valero Cuevas, and H. Lipson, “Locomotion of a tensegrity robot via dynamically coupled modules,” Proceedings of the International Conference on Morphological Computation, 2007.
-  C. Paul, J. W. Roberts, H. Lipson, and F. J. Valero Cuevas, “Gait production in a tensegrity based robot,” International Conference on Advanced Robotics, pp. 216–222, 2005.
-  A. Graells Rovira and J. M. Mirats-Tur, “Control and simulation of a tensegrity-based mobile robot,” Robotics and Autonomous Systems, vol. 57, no. 5, pp. 526–535, May 2009.
-  C. Sultan, M. Corless, and R. Skelton, “Tensegrity flight simulator,” Journal of Guidance, Control, and Dynamics, vol. 23, no. 6, pp. 1055–1064, 2000.
-  J. Bruce, K. Caluwaerts, A. Iscen, A. P. Sabelhaus, and V. SunSpiral, “Design and evolution of a modular tensegrity robot platform,” in ICRA, May 2014, pp. 3483–3489.
-  A. P. Sabelhaus, J. Bruce, K. Caluwaerts, P. Manovi, R. F. Firoozi, S. Dobi, A. M. Agogino, and V. SunSpiral, “System design and locomotion of SUPERball, an untethered tensegrity robot,” in ICRA, 2015, pp. 2867–2873.
-  A. Ledergerber, M. Hamer, and R. D’Andrea, “A robot self-localization system using one-way ultra-wideband communication,” in IROS, 2015.
-  DecaWave, “APS011 application note: Sources of error in DW1000 based two-way ranging (TWR) schemes.”
-  E. Wan, R. Van Der Merwe, et al., “The unscented Kalman filter for nonlinear estimation,” in Adaptive Systems for Signal Processing, Communications, and Control Symposium 2000. AS-SPCC. The IEEE 2000. IEEE, 2000, pp. 153–158.
-  R. E. Skelton and M. C. Oliveira, Tensegrity systems. Springer, 2009.
-  K. Yamane and Y. Nakamura, “Stable penalty-based model of frictional contacts,” in ICRA, 2006, pp. 1904–1909.
-  S. O. Madgwick, A. J. Harrison, and R. Vaidyanathan, “Estimation of imu and marg orientation using a gradient descent algorithm,” in IEEE International Conference on Rehabilitation Robotics (ICORR2011), 2011, pp. 1–7.