Mathematical Problems in Engineering

Volume 2017, Article ID 2696108, 13 pages

https://doi.org/10.1155/2017/2696108

## Orientation Uncertainty Characteristics of Some Pose Measuring Systems

National Institute of Standards and Technology, Gaithersburg, MD 20899, USA

Correspondence should be addressed to Marek Franaszek; vog.tsin@keram

Received 29 June 2017; Revised 16 November 2017; Accepted 20 November 2017; Published 31 December 2017

Academic Editor: Francesco Soldovieri

Copyright © 2017 Marek Franaszek and Geraldine S. Cheok. This is an open access article distributed under the Creative Commons Attribution License, which permits unrestricted use, distribution, and reproduction in any medium, provided the original work is properly cited.

#### Abstract

We investigate the performance of pose measuring systems which determine an object’s pose from measurement of a few fiducial markers attached to the object. Such systems use point-based, rigid body registration to get the orientation matrix. Uncertainty in the fiducials’ measurement propagates to the uncertainty of the orientation matrix. This orientation uncertainty then propagates to points on the object’s surface. This propagation is anisotropic, and the direction along which the uncertainty is the smallest is determined by the eigenvector associated with the largest eigenvalue of the orientation data’s covariance matrix. This eigenvector in the coordinate frame defined by the fiducials remains almost fixed for any rotation of the object. However, the remaining two eigenvectors vary widely and the direction along which the propagated uncertainty is the largest cannot be determined from the object’s pose. Conditions that result in such a behavior and practical consequences of it are presented.

#### 1. Introduction

The pose of a rigid object is defined by six degrees of freedom (6DOF): three angles describing the object’s orientation matrix and three components describing the object’s position (e.g., center of mass or center of bounding box). Accounting for the uncertainty of the measured pose is of great importance in many applications (e.g., propagating uncertainty along different joints in a robot arm or fusing measurements from multiple sensors) and it has been studied for a long time [1]. The methodology used in these studies is based on a 4 × 4 homogenous transformation matrix, related exponential mapping, and Lie algebra [2].

In this paper, our focus is on a different aspect of pose uncertainty. We are interested in how uncertainty of a single static measurement of a rigid body pose propagates to any Point of Interest (POI) associated with the object (e.g., a point on its surface). When the Computer Aided Design (CAD) model of an object is known, the location of any POI can be calculated using 6DOF data acquired by pose measuring systems [3]. In assembly applications where rigid parts need to be mated using autonomous robotic systems [4–8], uncertainty in pose has to be propagated to the POI. For example, in a peg-in-hole experiment (commonly used to test a robot’s performance [9–12]), uncertainty in the hole location directly affects the test outcome [13–17]. Thus, we acquire repeated measurements of a rigid object’s pose, obtained in the same experimental conditions, to investigate the uncertainty of a given POI.

In most practical applications, the six components of pose are not directly measured but are derived from other raw measurements. Many pose measuring systems report 6DOF data of an object based on the measurement of the 3D positions of a few points. These points, also known as fiducial markers, are rigidly attached to (or around) the measured object. Some systems may not require the use of markers as they may be trained to use some characteristic features of the measured object (e.g., well defined corner points). For systems which use 3D points, a homogenous transformation is found using point-based rigid body registration and minimizing the following error function called the Fiducials Registration Error (FRE) where is a set of fiducials measured in one coordinate frame (working frame) and is a set of corresponding fiducials measured in the second frame (destination frame). Pose measuring systems track the movement of the working frame and the transformation defines the object’s 6DOF pose relative to a starting reference frame (coordinate frame associated with the instrument). Point-based rigid body registration not only is implemented in pose measuring systems but is commonly used in many field applications where 3D points are measured in one frame but have to be accessed in another frame where they are required. These points (targets) need to be transformed from the working frame to the destination frame using the previously determined transformation .

Noise present in the measurement of the fiducials propagates to the transformation . Such noisy transformation (when applied to a target) yields random deviation of the transformed target from its nominal true location and is quantified by the Target Registration Error () defined as the root mean square of distances between these two points. The development of a closed form equation for has been the aim of extensive research for many years [18–22]. The main conclusions from these efforts can be summarized as follows: depends on the location of target relative to the three main axes of the moment of inertia derived from the spatial configuration of fiducials ; can be expressed as the sum of two components: one related to uncertainty in position and the second related to uncertainty in the orientation data ; both components of depend on the magnitude of the noise ( increases for noisier fiducial measurements); the orientation component is anisotropic; that is, it depends on the direction in space while the positional component is isotropic.

For the class of pose measuring systems which use point-based registration to track the pose of a rigid object, propagation of orientation uncertainty to a given POI is equivalent to the propagation of the fiducials’ uncertainty to a target point and, therefore, should inherit the above-mentioned characteristics of . The anisotropic distribution of the orientation uncertainty was reported for the pose measuring system using a stereo camera to track spherical, reflective markers attached to an object [23]. It was found that the distribution is bimodal on a unit sphere with the smallest uncertainty located at poles defined by the eigenvector associated with the largest eigenvalue of the covariance matrix of the orientation data. It was hypothesized that such a distribution offers an opportunity for better planning of robotic operations by ensuring that a given POI is in the region of small uncertainty. However, to take advantage of such a strategy, the direction of eigenvector must stay fixed in the CAD coordinate frame regardless of the object’s orientation.

In this paper, we expanded the study in [23] to determine the conditions for which the observed behavior (stability of ) holds by acquiring static measurements of several poses using a different pose measuring system (a large-scale tracking system iGPS). For each pose, the covariance matrix of the orientation data was determined. While the matrices were different for different poses, we found that the direction of the eigenvector exhibited very small variations compared to the directions of the other two eigenvectors, and , which showed larger variations. This behavior was reproduced in computer simulations and, to the best of our knowledge, it has not been reported in the literature. Analysis of existing theoretical expressions for in point-based registration reveals the reason for such unusual behavior of pose measuring systems which employ point-based registration to calculate 6DOF data. We found that misalignment between the directions of the anisotropic noise of fiducials and the directions of the axes of the moment of inertia characterizing the configuration of the fiducials is responsible for the observed phenomenon. It appears that the direction of is almost independent of the misalignment whereas the directions of and were dependent on the misalignment. Furthermore, our study shows that is well aligned with the eigenvector of the moment of inertia matrix corresponding to the smallest eigenvalue.

The location of any POI is fixed relative to the locations of fiducials. Therefore, for the class of pose measuring systems discussed in this paper, if a vector pointing to a POI is aligned with , this POI will be in the region of small propagated uncertainty, regardless of the object’s orientation. Prior knowledge of such behavior may be useful in robotic operations when tight tolerances are required. A procedure for determining the placement of fiducials so that the smallest uncertainty is propagated to a given POI is introduced. The optimal placement of fiducials has been studied earlier for rigid body registration. Two main applied approaches were use of theoretical models of [24] (some of them based on isotropic noise [25, 26]); numerical search for the optimal placement using covariance matrices of experimental noise, evaluating transformations and then corresponding to [22, 27]. While these studies showed implicit directional dependence of and its reduction, they did not alert practitioners that the uncertainty of a given POI on the rotated rigid object may depend on the object’s orientation nor provide clear guidance on how to ensure that this uncertainty will be close to the smallest possible value, regardless of object’s orientation. This paper attempts to provide this missing information.

In the next section, some background information and relevant equations are reviewed, followed by a brief description of the experimental setup and data postprocessing. This is followed by a presentation of the results, discussion, and conclusions.

#### 2. Previous Research

In this section a brief review of the theoretical work relevant to our experiments is presented. Section 2.1 presents a brief review of point-based rigid body registration. This is followed by a discussion of the propagation of noise from the fiducials used to register two sets of points and to the registration parameters and then to the transformed target point; an analytical formula for based on anisotropic, homogenous Gaussian noise perturbing the fiducials is provided. In Section 2.2, the propagation of orientation uncertainty of a 6DOF rigid object to an individual point on its surface is discussed.

##### 2.1. TRE in Point-Based Rigid Body Registration

Given two sets of* J* fiducials and measured in the working and destination frames, respectively, the rotation and translation which minimize the error function in (1) can be obtained in the following way. First, the origins of both frames are moved to the respective centroids and , that is, the locations of fiducials in the translated frames are and , . Then, the covariance matrix is determined as where is the transposed matrix. The rotation matrix can be calculated as in [28] where the matrices and are obtained from the Singular Value Decomposition (SVD) of the covariance matrixOnce the rotation matrix is determined, the related translation vector is calculated as This transformation minimizes in (1) in the least-squares sense, and this procedure is implemented in many commercial software packages.

However, noise in the measured fiducials and affects the registration transformation, and it needs to be propagated to the target transformed to the destination frame, namely, . Intuitively, it is obvious that the statistical properties of the target error will depend on the characteristics of the noise perturbing the fiducial locations as well as on the location of the target relative to the configuration of the fiducials. Based on the seminal papers by Sibson [29] and Fitzpatrick et al. [18], most theoretical studies and supporting computer simulations split the registration to two transformations: a “big” deterministic one and a small noisy one , that is, the two frames are first initially aligned using the big transformation and the fine tuning is done by the small rotation and translation. Thus, any point in the working frame is transformed to in the destination frame asThe rationale behind such an approach was put forward by Sibson who observed that the distribution of was completely determined by stochastic noise in the fiducials and not by the big transformation . This observation is an extension of the well-known property that a variance of a 3D point perturbed by Gaussian noise is the same in all coordinate frames related by any translation , that is, . As stated in [18],* “Neither this reorientation nor the special positioning of the origin above is necessary to effect a solution *[...]*, nor for any part of the derivation that follows. However, they do reduce the complexity considerably, and they can be easily undone at the end*.*”* The big rotation can be found from SVD of the covariance matrices and of fiducials and asand the big translation can by calculated by substituting in (5). Since both matrices and are symmetric and have positive diagonal elements, their SVD decomposition yieldsand similarly for **. **Matrix is diagonal matrixMatrix is closely related to the matrix of the moment of inertia as where is the identity matrix. Thus, defines the moments of inertia relative to the three major axes, and the orientation of the axes is determined by matrix in the working frame and in the destination frame. When a coordinate system is aligned with the axes of the moment of inertia (customarily done in theoretical analysis of in point-based rigid body registration) then matrix takes a simple diagonal formIt should be stressed that the moment of inertia characterizes the configuration of the fiducials in space, not the noise affecting the locations of the fiducials. In general, when the distance between fiducials is a few orders of magnitude larger than the noise, the moment of inertia relative to the major axes remains constant, that is, , and for this reason, we drop the subscript in .

While noise does not affect the moment of inertia, it has a great impact on the Target Registration Error (). Different forms for estimating were developed for different characteristics of fiducial noise, starting from the simplest isotropic, homogenous, Gaussian noise (the same for all fiducials) to the most complex, anisotropic, nonhomogenous Gaussian with nonzero mean (i.e., nonzero bias). No closed form solution has yet been developed for the most complex case. An analytical expression was provided for Gaussian, zero mean, homogenous, and anisotropic noise characterized by covariance matrix ; see equation in [30]. For such noise model, was evaluated from the variance where is the unit vector pointing towards the target , that is, and is the variance of the angular error (deviation of the directional vector from its nominal, noise free direction) and is parametrized by two spherical angles and asEquation (12a) contains two terms: the first is isotropic and is related to the uncertainty in translation in (5); the second term is anisotropic as it depends on angles and is related to uncertainty in the rotation in (3). The isotropic term is inversely proportional to the number of fiducials , and for most target locations which are not very close to the origin of the coordinate frame, the term related to orientation uncertainty in will be dominant.

We note that the orientation of the noise matrix (i.e., the coordinate frame formed by its eigenvectors) and the orientation of the moment of inertia matrix are completely unrelated and their relative orientation depends on the experimental conditions.

##### 2.2. Propagation of Orientation Uncertainty of Rigid Body to a POI

Let vector define the location of a POI in the CAD coordinate frame and let be a unit vector parallel to such that . If is the orientation matrix of a rigid object and its location obtained from the* j*th measurement, then is the location of the POI on the rotated object in the coordinate frame of the pose measuring instrument,where is a unit vector pointing to a rotated POI in the coordinate frame of the instrument and it can be parametrized by two spherical angles as in (13). We are interested in propagating the uncertainty of to the uncertainty of . We assume thatwhere is the averaged orientation obtained from repeated measurements acquired in the same experimental conditions, is a small random rotation (noise), and . In axis-angle representation , the smallness of the rotation is gauged by small values of angle and this leads to the following expression for in linear approximationwhere is the identity matrix, is a unit vector defining the axis of rotation, and A covariance matrix of the orientation data can be calculated as

Repeated measurements of the orientation matrix in (15) yield a corresponding set of vectors which are tightly distributed around the average direction . If denotes the angle between and , then its distribution can be described by the Fisher-Bingham-Kent (FBK) distribution [31–33] as where is the angular uncertainty and is the Kent correction to the Fisher distributionThis correction takes into account the nonzero eccentricity parameter which describes the shape of the elliptical contour of a constant probability on the plane ( for symmetric circle contour when ). Larger values of uncertainty correspond to larger deviations of vector from the mean direction . For pose measuring systems which use point-based rigid body registration, the angular uncertainty is equivalent to the angular uncertainty from (12a) and (12b) when homogenous, anisotropic model of Gaussian noise characterizes the experimental conditions. However, the analysis in this subsection and as discussed here, the angular uncertainty is more general than the uncertainty discussed in Section 2.1 because it is applicable to any sequence of noisy rotations , no matter what sensors and raw measurements were used to get the rotation matrices. Equations (12a) and (12b) are applicable only to the class of pose measuring systems which utilize point-based rigid body registration.

#### 3. Data Collection and Processing

##### 3.1. Experimental Setup

A commercially available, large-scale tracking system (iGPS) was used to collect 6DOF data [34]. The manufacturer specified positional uncertainty is 250 *μ*m. The system consists of a network of eight transmitters placed outside of the working volume (3 m × 3 m × 1.8 m) to track vector bars within the work volume. The transmitters were mounted on 3.05 m high steel columns anchored to the concrete floor. The columns were evenly distributed around the perimeter of the lab space [15 m × 16 m × 10 m (high)], and the working volume was in the center of the lab. Two vector bars were used in the experiment, and each vector bar contains two detectors which define a vector in space (the detectors in a vector bar were separated by 101.6 mm). The two vector bars rigidly mounted to an aluminum rail were used to create a local coordinate frame: in commercial applications, a rigid object remains fixed in the local frame which is tracked by the system.

Four different local frames were created and used to obtain measurements for four configurations of the vector bars; see Figure 1. Both vector bars were parallel to each other, and the distance between them was 375.7 mm for configurations (a–c) and 902.2 mm for configuration (d). The line connecting the two bars for configuration (a) is parallel to that for (b) and similarly for configurations (c) and (d); the line in (a) and (b) is perpendicular to the line in (c) and (d). Each local frame was used to measure* M* different static poses (*M* = 12, 27, 12, 20 for frames (a–d), respectively), and at each pose, repeated measurements in the same experimental conditions were acquired (*N* ≥ 50,000).