Robot Body Schema Learning from Full-body Extero/Proprioception Sensors

TL;DR

The paper addresses autonomous robot body topology discovery from on-board exteroceptive and proprioceptive data by introducing a binary Heterogeneous Dependency Matrix $\mathbf{D}$ that is provably equivalent to a Heterogeneous Out-tree representation of the robot. A per-sensor SE(3) neural network estimates global sensor poses, whose Jacobians produce dependency features that form $\mathbf{D}$; theoretical results (tree-matrix equivalence and observability conditions) underpin exact topology recovery, while data-driven remedies handle noise and partial observability. Practical components include a Transform Invariant Jacobian, dependency-feature extraction, DP-GMM clustering for row reduction, and a matrix-completion pipeline combining MILP-based permutation alignment with a Trellis expansion. Experimental validation on six simulated open-chain robots and a UR5e real robot demonstrates accurate topology recovery without topology priors, highlighting potential for unknown forward kinematics, damaged body monitoring, and applicability to reconfigurable or soft robots. The work provides a rigorous framework for autonomous self-awareness in robotics, enabling robust body-schema learning from multi-modal sensor data with concrete algorithms for correction when observations are incomplete or noisy.

Abstract

For a robot, its body structure is an a-prior knowledge when it is designed. However, when such information is not available, can a robot recognize it by itself? In this paper, we aim to grant a robot such ability to learn its body structure from exteroception and proprioception data collected from on-body sensors. By a novel machine learning method, the robot can learn a binary Heterogeneous Dependency Matrix from its sensor readings. We showed such matrix is equivalent to a Heterogeneous out-tree structure which can uniquely represent the robot body topology. We explored the properties of such matrix and the out-tree, and proposed a remedy to fix them when they are contaminated by partial observability or data noise. We ran our algorithm on 6 different robots with different body structures in simulation and 1 real robot. Our algorithm correctly recognized their body structures with only on-body sensor readings but no topology prior knowledge.

Figures (28)

• Figure 1: System flowchart: we cannot directly observe robot topology (represented by heterogeneous out-tree), but we can infer it by extracting a heterogeneous dependency matrix from exteroception and proprioception data and use the matrix-tree equivalence to infer the unobserved tree structure.
• Figure 2: Neural network structure of $f_{\phi }\left ( \theta \right )$. The roll-pitch-yaw angles to rotation matrix transformation can be found in Equation \ref{['eq_18']}. The result will be reshaped to a homogeneous transformation matrix of 4 $\times$ 4 by padding zeros and ones as Equation \ref{['eq_8']}.
• Figure 3: Pipeline to extract robot body structure (represented by a heterogeneous out-tree). (left) Each IMU is linked to a SE(3) neural network as shown in Figure \ref{['fig_35']} to approximate its global pose given the joint angles. (bottom) each neural network $f_{\phi }\left ( \theta \right )$ outputs the current homogeneous transformation $\mathbf{T}$; The 'Jacobian' step extracts Jacobian matrix $\mathbf{J}$ of size (4, 4, N) from $\mathbf{T}$; normalize by each column to get Transform Invariant Jacobian $\overline{\mathbf{J}}$; extract Dependency Feature $d$ from $\overline{\mathbf{J}}$ by Equation \ref{['eq_10']}. (right) stack all Dependency Features from all sensors and merge the duplicates generates a heterogeneous dependency matrix; running algorithm \ref{['algo_3']} to generate the out-tree from the matrix.
• Figure 4: Two matrices transformed by switching rows [b,c] and columns [d,f]; we assume they are equivalent.
• Figure 5: (a) Topology of open-chain robot. (b) Topology simplification of a robot. The first arrow is because different sensors on the same link provide redundant joint-dependency information. The second arrow is because link and sensor are one-one pair, so it is equivalent to discuss either. (c) Examples of heterogeneous out-tree.

Theorems & Definitions (26)

• Definition 1
• Proposition 1
• Definition 2
• Definition 3
• Definition 4
• Definition 5
• Lemma 1
• Definition 6
• Lemma 2
• Proposition 2