Estimating Uncertain Spatial Relationships in Robotics

Randall Smith, Matthew Self, Peter Cheeseman

Seminal March 2013

TL;DR Stack the poses of the robot and every mapped object into one state vector with a full covariance matrix (the stochastic map), and build and update it with pose compounding, inversion and an extended Kalman filter. The paper cites Monte Carlo tests in which these linearized estimates stayed within 1% of the true means and variances for angular errors up to 5 degrees.

What they do

Robots get spatial information as uncertain relative measurements in many frames. Earlier methods kept worst-case min-max error bounds, which become very conservative when many pieces are combined. The stochastic map instead stacks the world poses of the robot and all objects into one state vector and estimates its mean and full covariance, including cross-covariances between objects. The paper gives procedures to read any relationship out of the map and to update it as the robot moves and senses.

How it works

Spatial operations. Compounding (⊕) chains two relationships head-to-tail; reversal (⊖) inverts one. Means pass through these functions and covariances through their Jacobians (first-order). 3-DoF formulas in the text, 6-DoF Jacobians for Euler and roll-pitch-yaw angles in an appendix.
Motion. A move sets the robot pose to x_R ⊕ y_R; only the robot’s row and column of the covariance matrix change.
New objects. An object sensed relative to the robot adds a row and column, with its cross-covariance to the robot filled in.
Constraints. Re-observing a mapped object, or a hypothesized relation such as four points forming a rectangle, is a measurement z = h(x) + v fused with the (iterated) extended Kalman filter.

Results

A theory paper; the example is a simulated planar robot with 3 DoF.

The robot senses object 1 and, before moving, the map shows that the chance of colliding with it during the planned motion is vanishingly small.
After moving, it senses an object and decides from the map that it cannot be object 1. Sensing object 1 again reduces the uncertainty of the robot, object 1 and object 2 together.
A rectangle constraint on four uncertain points tightens their estimates.
Cited Monte Carlo simulations: with angular errors up to 5 degrees standard deviation, the first-order means and variances were within 1% of the correct values.

Limitations

Linearization assumes small angular errors, and only the first two moments are kept, which the authors argue but do not show is adequate for decisions. The 6-DoF Jacobians have singularities near which the covariance estimates lose accuracy. In the example the sensor is assumed to recognize object 1, so matching observations to mapped objects is left open.

Figures

From Figure 1: The robot senses object 1, then moves, shown in the world frame (left) and from the robot's new frame ... — From Figure 1: The robot senses object 1, then moves, shown in the world frame (left) and from the robot's new frame (right). Ellipses are 99.9% confidence regions; seen from the robot, object 1 is more uncertain because its location is compounded with the motion uncertainty.

From Figure 3: After adding object 2 to the map, the robot senses object 1 again from its new location.

From Figure 4: The updated estimates after the second sighting of object 1 is applied as a constraint; the robot, obj... — From Figure 4: The updated estimates after the second sighting of object 1 is applied as a constraint; the robot, object 1 and object 2 all become less uncertain.

From Figure 6: Four uncertain points (left) and their estimates after imposing that they form a rectangle (right), ov... — From Figure 6: Four uncertain points (left) and their estimates after imposing that they form a rectangle (right), overlaid on the original ellipses.

Abstract

In this paper, we describe a representation for spatial information, called the stochastic map, and associated procedures for building it, reading information from it, and revising it incrementally as new information is obtained. The map contains the estimates of relationships among objects in the map, and their uncertainties, given all the available information. The procedures provide a general solution to the problem of estimating uncertain relative spatial relationships. The estimates are probabilistic in nature, an advance over the previous, very conservative, worst-case approaches to the problem. Finally, the procedures are developed in the context of state-estimation and filtering theory, which provides a solid basis for numerous extensions.

Summaries are written automatically from each paper's text. Figures and videos come from the paper and its project page; all credit goes to the authors.

Matt Strong