Source-linked AI summary
Estimating Uncertain Spatial Relationships in Robotics
Randall Smith, Matthew Self, Peter Cheeseman
TL;DR
Robotics needs a way to represent and reason about uncertain spatial relationships, especially when information from multiple sensors and reference frames must be combined. The paper introduces a stochastic map with mean, covariance, and dependency estimates, together with procedures for extracting and incrementally revising those estimates. The resulting framework provides a general probabilistic solution for relative spatial estimation and supports planning decisions involving accumulated uncertainty.
Problem
Robotics applications require spatial-uncertainty representations and methods for combining uncertain relative information across reference frames.
Method
The paper represents spatial relationships with a stochastic map containing estimates, uncertainties, and inter-dependencies, and updates it incrementally using estimation and filtering procedures.
Results
The framework provides a general solution for estimating uncertain relative spatial relationships and supports decisions about accumulated uncertainty and sensor sufficiency.
Takeaways & Limitations
Probabilistic spatial estimates offer a less conservative alternative to worst-case methods and can support efficient robot planning and uncertainty-aware operation decisions.
Takeaways & Limitations
The representation assumes that estimating only the first two moments is adequate for decision making, motivated by sensor models and approximately Gaussian combined information.
Abstract
from arXiv · showhide
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.
1 Introduction
Robotics applications need explicit representations of spatial uncertainty, but combining uncertain relative information across reference frames is difficult. The paper introduces a stochastic-map framework for representing, combining, transforming, and incrementally updating these relationships.
- Combining multiple overlapping lower-resolution sensors can provide sufficient accuracy while avoiding the cost of precision engineering, highly accurate sensors, fixtures, and calibration points.The approach integrates spatial information and uncertainty from all available sources.
- Explicit uncertainty estimation can help determine whether planned operations may fail from accumulated uncertainty and whether proposed sensor information can reduce uncertainty to tolerable levels.This is particularly relevant for inexpensive mobile robots, where combining uncertain information from many sensors may be necessary.
- The paper presents a general solution for estimating uncertain spatial relationships regardless of the frames in which information is given or answers are required.The difficulty is especially acute when uncertain relative information must be propagated among multiple reference frames.
- Earlier min-max methods are conservative because they combine worst-case error bounds, whereas this paper uses a probabilistic representation.The paper also extends related probabilistic work within formal estimation theory without using graph transformations.
- The stochastic map explicitly represents each spatial relationship, its uncertainty, and the dependencies among estimates.Its matrix procedures support combining information, transforming relationships between frames, and incrementally building the best estimate as new information arrives.
2 The Stochastic Map
The stochastic map represents uncertain spatial relationships with probabilistic estimates, covariances, and dependencies, then supports extracting, combining, and incrementally revising map information. A mobile-robot example illustrates how new observations reduce uncertainty and support spatial decisions.
- 2.3 Example: The map is built incrementally as new spatial information is obtained, with procedures for describing its structure, extracting information, and revising estimates.The paper illustrates this process through a mobile robot that observes its environment at different times and places.
- 2.1 Representation: Each uncertain relationship is modeled using its probability distribution's mean and covariance rather than its full distribution.The covariance diagonal contains variances, and off-diagonal terms contain covariances between spatial variables.
- 2.1 Representation: The stochastic map stores nominal spatial relationships together with their uncertainties and inter-dependencies in a system mean and covariance representation.The system state vector collects variables from multiple uncertain relationships, while covariance submatrices encode dependencies between them.
- 2.2 Interpretation: The paper uses normal-distribution contours to visualize mean-and-covariance estimates, while explicitly stating that the underlying relationships are not assumed to be normal.The plotted ellipses represent 99.9% confidence regions for the relationships.
- 2.3 Example: The mobile-robot example consists of sensing object #1, moving, sensing a distinct object #2, and sensing object #1 again to improve localization.The example uses a two-dimensional robot with three degrees of freedom.
- 2.3 Example: A repeated observation of object #1 acts as a constraint that reduces uncertainty in the estimated locations of the robot, object #1, and object #2.The example also uses the map to distinguish a newly sensed object #2 from object #1 and to assess collision likelihood for a planned motion.
3.1 Uncertain Relationships
The paper represents uncertain spatial relationships using means and covariances, propagating these quantities through linear and nonlinear transformations. Linear transformations are exact for the first two moments, whereas nonlinear transformations use first-order Taylor approximations and Jacobians.
- 3.1 Uncertain Relationships: The representation estimates each relationship’s mean and covariance rather than its complete probability distribution.The paper uses a normal distribution only when specific probability contours are needed, not as a general assumption about the underlying relationships.
- 3.1.1 Linear Relationships: For a linear transformation y = Mx + b, the first two moments and cross-covariances are computed exactly from those of x.The covariance relationships also propagate dependence between y and another relationship z.
- 3.1.2 Non-Linear Relationships: Nonlinear transformations are approximated by a Taylor expansion around the estimated mean, with the Jacobian providing the first-order estimate.The Jacobians are evaluated at the estimated mean of the input variables.
- 3.1.2 Non-Linear Relationships: The paper uses first-order estimates throughout because exact nonlinear moments require the complete probability density function, which is generally unavailable.Second-order terms can improve the mean estimate but are not used in the remainder of the paper.
3.2 Spatial Relationships
The paper develops compounding and reversal operations for uncertain spatial relationships, allowing relative relationships to be propagated through chains and expressed between arbitrary frames. Means and covariances are estimated with first-order formulas, with computational shortcuts based on Jacobians.
- 3.2.1 Compounding: Compounding combines head-to-tail relationships to estimate the resultant relationship, while accounting for rotations rather than treating transformations as vector addition.The operation supports estimating a robot’s location after a sequence of relative motions.
- 3.2.2 The Inverse Relationship: Reversal expresses a relationship from the opposite direction, but its uncertainty is re-expressed rather than inverted.The reversal operation supplies the direction changes needed to construct relationships across differently oriented frames.
- 3.2.3 Composite Relationships: Compounding is associative but not commutative, and reversal enables head-to-head and tail-to-tail combinations.Tail-to-tail relationships can be computed recursively or with precomputed composite Jacobians.
- 3.2.3 Composite Relationships: Recursive composition of a relationship chain produces the same first-order result as estimating the composite relationship directly.This avoids the increasingly conservative stepwise bounds produced by min-max methods.
- 3.2.4 Extracting Relationships: The resulting machinery extracts implicit relationships between map objects by applying the appropriate combination operation to their world-referenced estimates.For relative locations between object frames, the tail-to-tail operation is substituted for the general relationship function.
4 Building the Map
The map is incrementally maintained by updating its state mean and covariance when objects move or new spatial information arrives. New information either adds an object or constrains existing objects, with updates weighted by prior and measurement uncertainty.
- 4 Building the Map: Map changes update the state mean and covariance when an object moves or new spatial information is obtained.The map’s state includes object locations and their uncertainty dependencies.
- 4.1 Extrapolation: Object motion is propagated between discrete sensing states through a process model driven by the prior state and noisy control inputs.When process noise is uncorrelated with the state, the covariance update simplifies because the off-diagonal process-noise terms are zero.
- 4.2.1 Adding Objects: Adding a new object expands the state vector and covariance matrix, with separate cases for estimates independent of or dependent on existing map estimates.A directly specified world-coordinate estimate has zero cross-covariance with prior estimates, whereas a relation measured from an existing object creates dependence.
- 4.2.2 Adding Constraints: Adding a constraint between existing objects changes their estimates without increasing the dimensions of the state vector or covariance matrix.Constraints may come from noisy landmark measurements or geometric relationships such as colinearity and coplanarity.
- 4.2.2 Adding Constraints: The update combines the map’s current relationship estimate with new information using a Kalman-filter weighting matrix.Large measurement covariance reduces the measurement’s influence, while large prior covariance makes the update approach the measurement correction.
5 Developed Example
The developed example incrementally builds and updates a stochastic map as a mobile robot senses objects, moves, changes reference frames, and applies geometric constraints. The procedures generalize across networks with the example’s topology and support automated offline planning.
- Map initialization: The stochastic map represents zero covariance as no uncertainty, with the corresponding normal distribution becoming a limiting Dirac delta.
- Mobile robot example: The robot incrementally adds object measurements, motion, frame transformations, and repeat observations to update its spatial map.The example begins with the robot as the world-frame identity, senses object #1, moves, transforms the map, and incorporates a new relative measurement.
- Generalization: The resulting formulae apply to any relationship network with the example’s topology and can be automated for offline robot planning.
- Geometric constraint: A rectangle constraint is modeled like a noisy sensor, using a function that measures departures from rectangularity when shape uncertainty is allowed.For an exact constraint, the function need only be zero when the four points form a rectangle; a loose constraint requires a useful residual measure.
6 Discussion and Conclusions
The discussion frames the theory as a general state-estimation approach for uncertain spatial relationships in robotic networks. It supports planning and sensor decisions, while relying on small angular errors, moment-based representations, and assumptions that may extend beyond spatial variables.
- Discussion and conclusions: The theory estimates uncertain relative relationships throughout a network and transforms them into any frame of interest.It applies state-estimation theory to entire spatial configurations represented by sensed relationships, prior constraints, and relative motions.
- Discussion and conclusions: The estimates can assess accumulated action uncertainty, predict whether sensors will make uncertainty tolerable, and identify implausible sensor results.
- Assumptions: The method assumes small angular errors because nonlinear relationships are linearized; simulations reported estimates within 1% for angular standard deviations up to 5°.
- Assumptions: The method models only means and covariances, relying on the claim that combined relationships become rapidly Gaussian and are therefore adequately represented by two moments.
- Scope: Although demonstrated with spatial information, the theory can also represent forces, velocities, time intervals, and other quantities when their relationships are specified.
Appendix A
The appendix extends the spatial relationship machinery from planar transformations to three-dimensional representations. It describes six degrees of freedom and rotation-matrix-based orientation handling.
- Appendix A: Three-dimensional spatial relationships have six degrees of freedom: translations in x, y, and z plus three orientation angles.
- Appendix A: For computational purposes, orientation is represented with rotation matrices whose orthogonal columns correspond to Cartesian axes.
- Appendix A: Primitive rotation matrices can be multiplied to obtain a final orientation, with Euler angles and other interpretations used to describe rotations.
Relationships Using Euler Angles
The Euler-angle formulation derives inverse and head-to-tail spatial relationships and their Jacobians for uncertainty propagation. It uses compound rotation matrices and atan2-based angle recovery, but covariance accuracy degrades near singular angle configurations.
- Relationships Using Euler Angles: Euler angles are defined through the displayed trigonometric transformation relationships used to parameterize orientation.
- Relationships Using Euler Angles: The head-to-tail relationship x3 = x1 ⊕ x2 is computed from the component transformations, with compound rotation R3 = R1R2.
- Relationships Using Euler Angles: The Euler-angle solution uses atan2 over its full input range and solves φ3 first before using it to determine the remaining angles.
- Relationships Using Euler Angles: The Jacobian is simplified using final terms already computed for the mean relationship, but its covariance estimates lose accuracy near singular combinations of mean angles.Methods for avoiding the singularity were still being explored.
- Relationships Using Euler Angles: The inverse relationship and its Jacobian express a transformation’s inverse spatial variables using elements of its associated rotation matrix.
Relationships Using Roll, Pitch and Yaw Angles
The section expresses spatial relationships using roll, pitch, and yaw angles, including inverse conversion and head-to-tail composition. Jacobians are provided for both relationships to support their transformation and propagation.
- The rotation matrix for the RPY angles is explicitly defined and used in the head-to-tail relationship.The matrix R1 is identified as the rotation matrix corresponding to the angles in x1.
- The head-to-tail relationship x3 = x1 ⊕ x2 combines two spatial states represented with roll, pitch, and yaw angles.The formulation defines associated transformation matrices and angle relationships for the composed state.
- The composed roll, pitch, and yaw angles are specified through atan2 expressions involving the elements of the resulting rotation matrix.These expressions define the angle representation of x3 from rotation-matrix elements nx3, ny3, nz3, ax3, ay3, and ox3.
- The inverse relationship converts the elements of a rotation matrix into roll, pitch, and yaw angles using atan2 expressions.The rotation-matrix elements nx and related terms are used in the inverse angle formulas.
- Jacobians are given for the inverse angle relationship and for the head-to-tail composition.The inverse Jacobian is defined from rotation-matrix elements, while the composition Jacobian uses the component states and their RPY angles.