A New Probability Distribution for Simultaneous Representation of Uncertain Position and Orientation
Total Page:16
File Type:pdf, Size:1020Kb
A New Probability Distribution for Simultaneous Representation of Uncertain Position and Orientation Igor Gilitschenski∗, Gerhard Kurz∗, Simon J. Juliery, and Uwe D. Hanebeck∗ ∗Intelligent Sensor-Actuator-Systems Laboratory (ISAS) Institute for Anthropomatics and Robotics Karlsruhe Institute of Technology (KIT), Germany [email protected], [email protected], [email protected] yVirtual Environments and Computer Graphics Group Department of Computer Science University College London (UCL), United Kingdom [email protected] Abstract—This work proposes a novel way to represent uncer- These adaptions are usually based on a more intelligent tainty on the Lie group of rigid-body motions in the plane. This is propagation technique (such as the unscented transform) and a achieved by using dual quaternions for representation of a planar sound state representation. Most of these approaches maintain rigid-body motion and proposing a probability distribution from a Gaussian assumption because of its convenient properties and the exponential family of distributions that inherently respects the natural appearance as a limit distribution. However, as the noises underlying structure of the representation. This is particularly become larger, the errors introduced by this assumption become beneficial in scenarios involving strong measurement noise. A relationship between the newly proposed distributional model non-negligible. The reasons for this involve the fact that the and the Bingham distribution is discussed. The presented results Gaussian distribution does not capture the nonlinear structure involve formulas for computation of the normalization constant, of the underlying manifold. Recently, a number of authors have the mode, parameter estimation techniques, and a closed-form developed recursive estimation algorithms which use directional Bayesian measurement fusion. probability distributions instead. The proposed algorithms address angular [2], [3] and orientation estimation [4], [5] Keywords—Pose estimation, dual quaternions, Bingham distri- problems. These approaches show benefits over estimation bution, directional statistics, SE(2), Lie groups, probability theory techniques which maintain a Gaussian assumption. Our earlier works on estimating angular and orientational quantities build upon some of the numerous probability dis- I. INTRODUCTION tributions defined on periodic manifolds such as the wrapped Estimation of a rigid-body motion, i.e., simultaneous estimation normal distribution, the von Mises distribution [6], or the of translation and rotation presents a major problem in many Bingham distribution [7]. One of the approaches using dual applications involving robotic perception, processing of sensor quaternions for representing rigid body motions was presented data for mixed and augmented reality applications, and naviga- in [8]. Dual quaternions are a convenient representation because tion. This problem is particularly challenging for two reasons. they generalize the idea of rotations represented by quaternions First, it has a nonlinear structure because the values are defined to a representation of the entire rigid-body transformation. on an inherently nonlinear domain. The cases of particular However, this approach is based on an iterated EKF in order to practical interest are the manifolds of rigid-body motions in the account for the fact that the Gaussian assumption is inherently plane SE(2) and in three-dimensional space SE(3). Second, wrong and might yield poor results. there is no canonical way to describe dependencies between position and orientation. Combination of angular and linear quantities is of particular interest in robotic perception and inertial navigation systems. These problems are negligible in scenarios involving strong However, only a very limited number of approaches considers prior knowledge or high-precision sensors. Here, one can easily these quantities simultaneously in the underlying probability dis- approximate arising nonlinearities involved in the estimation tribution. Two approaches are particularly notable for estimation problem by linear models making use of the locally linear of rigid transformations. First, the projected Gaussian approach structure of the considered manifolds. Thus, usually two proposed by Feiten et al. [9], [10], [11]. It is based on projecting strategies are used to address this estimation scenario. First, parts of a higher dimensional Gaussian random vector to the unit costly high-precision sensors are deployed to avoid strong sphere in an intelligent way. Furthermore, dual quaternions are measurement noise. Second, the classical Kalman filter is used for state space representation. Unfortunately, this approach modified to consider nonlinear system models. The most famous has the drawback of using an approximation in the Bayesian adaptations involve the extended Kalman filter (EKF) and the update step for a typical dynamic state estimation scenario. unscented Kalman filter (UKF) [1]. In [12], an approach related to the Bingham distribution was used for estimating a “mean” rigid-body transformation. It In this work, the skew-field of quaternions will be denoted by maintains some properties of the Bingham distribution and H. First, consider the quaternion q 2 H uses a combination of orthogonal matrices and vectors for representing uncertain orientation and translation respectively. q = q1 + q2i + q3j + q4k ; However, it is not clear whether this approach makes a closed- which has the typical basis elements i, j, k. q will be described form Bayes update possible. > using the vector representation (q1; q2; q3; q4) whenever it is more convenient. As described above, we are particularly A. Main Contribution interested in quaternion multiplication, because it can be used to apply a sequence of rotation operations. For two quaternions In this work, we propose a novel probability distribution a; b 2 , it is defined as which is capable of capturing the underlying structure of H the manifold of planar rigid transformations SE(2). This is 0a1b1 − a2b2 − a3b3 − a4b41 achieved through the use of dual quaternions (restricted to a b + a b + a b − a b a · b = B 1 2 2 1 3 4 4 3C : planar transformations). Thus, there is no need to maintain an @a1b3 − a2b4 + a3b1 + a4b2A entire matrix to describe the actual transformation. Furthermore, a1b4 + a2b3 + a3b2 − a4b1 choosing dual quaternions for our formulation simplifies a future generalization to the SE(3) case. Similar to the Bingham distri- The inverse of a unit quaternion is obtained by quaternion conjugation, i.e., by changing the sign in front of each imaginary bution and its natural use for representing uncertain orientations −1 ∗ > represented by quaternions, the proposed distribution involves basis element. Thus q = q = (q1; −q2; −q3; −q4) . antipodal symmetry and makes closed form Bayesian inference The second concept necessary to define dual quaternions in classical measurement update and fusion scenarios possible. are dual numbers, which can be seen as an extension of real Furthermore, we show that the Bingham distribution appears as numbers. Similarly to complex numbers (which introduce the a marginal distribution for the spherical part of our proposed imaginary unit i), dual numbers are defined by introducing a model and describe the relationship between the modes and new element. It is usually denoted by " and characterized by normalization constant of both distributions. its nilpotency property "2 = 0. Thus, a dual number is given The remainder of this paper is structured as follows. In the by next section, we introduce dual quaternions and discuss their a + "b (1) convenience for composing and representing rigid transforma- tions. Our new distributional model is presented in Sec. III, for a; b 2 R. The multiplication of two dual numbers is where a parameter estimation technique is proposed and the straightforward mode of the distribution is discussed. In Sec. IV, we show how the distribution can be applied to representing uncertainty of (a1 + "b1)(a2 + "b2) = a1a2 + "(b1a2 + a1b2) : (2) positions and orientations in the plane. Furthermore, a closed- Dual quaternions (denoted by HD) are a quaternion equivalent form Bayesian inference procedure is derived for a typical to dual numbers. Thus, they are defined by replacing real estimation and fusion scenario. The work is concluded in numbers a, b in (1) with quaternions q1; q2 2 H. That is, a Sec. V. dual quaternion dq can be written as II. DUAL QUATERNIONS FOR REPRESENTING dq = q1 + "q2 : RIGID BODY MOTIONS Multiplication of dual quaternions works in the same way as Unit quaternions offer a convenient way of representing and multiplication of dual numbers (2) except for the fact that combining orientations. The latter is done by quaternion multi- multiplication of dual quaternions is not commutative because plication. Other operations from the skew field of quaternions quaternion multiplication is not commutative. Thus, the product obtain in this context a natural geometrical interpretation, e.g., of two dual quaternions da; db 2 HD is given by multiplicative inversion represents the inverse of a rotation and da · db =(a + "a ) · (b + "b ) exponentiation is applied to derive an interpolation algorithm 1 2 1 2 [13]. =a1 · b1 + "(a1 · b2 + a2 · b1) : This concept is generalized by dual quaternions of unit Several different types of conjugation exists for dual quaternions. magnitude making the representation of translations possible We are particularly interested in conjugating