IMU-Laser Scanner Localization: Observability Analysis

Transcription

1 IMU-Laser Scanner Localization: Observability Analysis Faraz M. Mirzaei and Stergios I. Roumeliotis {faraz Dept. of Computer Science & Engineering University of Minnesota Minneapolis, MN Multiple Autonomous Robotic Systems Laboratory Technical Report Number January 2009 Repository Revision: 578 Last Update: :44:22Z Dept. of Computer Science & Engineering University of Minnesota EE/CS Building 200 Union St. S.E. Minneapolis, MN Tel: (612) Fax: (612) URL: faraz

2

3 IMU-Laser Scanner Localization: Observability Analysis Faraz M. Mirzaei and Stergios I. Roumeliotis {faraz Dept. of Computer Science & Engineering University of Minnesota Minneapolis, MN Multiple Autonomous Robotic Systems Laboratory, TR January Nonlinear Observability Consider the state-space representation of the following infinitely-smooth nonlinear system: { ẋ = f(x, u) y = h(x) (1) where x R n is the state vector, u = [u 1,..., u l ] T R l is the vector of control inputs, and y = [y 1,..., y m ] T R m is the measurement vector, with y k = h k (x), k = 1... m. If the process function, f, is input-linear [1], it can be separated into a summation of independent functions, each one corresponding to a different component of the control input vector. In this case, (1) can be written as: { ẋ = f0 (x) + f 1 (x)u f l (x)u l (2) y = h(x) where f 0 is the zero-input function of the process model. The zeroth-order Lie derivative of any (scalar) function is the function itself, i.e., L 0 h k (x) = h k (x). The first-order Lie derivative of function h k (x) with respect to f i is defined as: L 1 f i h k (x) = h k(x) f i1 (x) + + h k(x) f in (x) x 1 x n = h k (x) f i (x) (3) where f i (x) = [f i1 (x),..., f in (x)] T, represents the gradient operator, and denotes the vector inner product. Considering that L 1 f i h k (x) is a scalar function itself, the second-order Lie derivative of h k (x) with respect to f i is: L 2 f i h k (x) = L 1 f i ( L 1 fi h k (x) ) = L 1 f i h k (x) f i (x) (4) Higher-order Lie derivatives are computed similarly. Additionally, it is possible to define mixed Lie derivatives, i.e., with respect to different functions of the process model. For example, the second-order Lie derivative of h k with respect to f j and f i, given its first derivative with respect to f i, is: L 2 f jf i h k (x) = L 1 f j ( L 1 fi h k (x) ) = L 1 f i h k (x) f j (x) (5) Based on the preceding expressions for the Lie derivatives, the observability matrix is defined as the matrix with rows: O { L l f i f j h k (x) i, j = 0,..., l; k = 1,..., m; l N} (6) 2

4 The important role of this matrix in the observability analysis of a nonlinear system is captured by the following proposition [2]: Proposition 1 (Observability Rank Condition). If the observability matrix (cf. (6)) of the nonlinear system defined in (1) is full rank, then the system is locally weakly observable. Remark 1. Since the process and measurement functions (cf. (1)) are infinitely-smooth, the observability matrix O can have infinite number of rows. However, to prove that O is full rank, it suffices to show that a subset of its rows are linearly independent. In general, there exists no systematic method for selecting the suitable Lie derivatives and corresponding rows of O when examining the observability of a system. Instead, this selection is performed by sequentially considering the directions of the state space along which the gradient of each of the candidate Lie derivatives provides information. 2 Observability Analysis of IMU-Laser Scanner Localization In this section, we prove that the system describing the IMU-laser scanner localization is observable when three planes, whose normal vectors are linearly independent, are concurrently observed by the laser scanner. Under this condition, and since the IMU and the laser scanner are rigidly connected and their relative transformation is known, we can employ the pose estimation method described in [3] to estimate ( G p I, I q G ). For the purpose of observability analysis, we introduce two new inferred measurements 1 h 1 and that replace the laser scan measurements [3]: I q G = h 1(x) = ξ 1 ( I l 1, I l 2, I l 3 ) (7) G p I = (x) = ξ 2 ( I l 1, I l 2, I l 3 ). (8) The two functions ξ 1 and ξ 2 in (7) and (8) need not to be known explicitly; only their functional relation with the random variables, I q G and G p I, is required for the observability analysis. Our approach uses the Lie derivatives [2] of the above inferred measurements (7) and (8) for the system in [3], to show that the corresponding observability matrix is full rank. For this purpose, we first rearrange the nonlinear kinematic equations (see [3]) in a suitable form for computing the Lie derivatives: 2 I q G ḃg G v I = 6 4 ḃ 7 a 5 Gṗ I q Ξ(I G )b g G g C T ( I q G )b a G v I } {{ } f 0 where ω m and a m are considered the control inputs, and [ ] q4 I Ξ( q) = 3 + q q T q Ξ(I G ) } {{ } f 1 ω m+ with q = C T ( I q G ) a m, (9) } {{ } f 2 [ q. (10) q4] Note also that f 0 is a 16 1 vector, while f 1 and f 2 are matrices of dimensions In order to prove that the system is locally weakly observable, it suffices to show that the observability matrix, whose rows comprise the gradients of the Lie derivatives of the measurements h 1 and with respect to f 0, f 1, and f 2 [cf. (9)], is full rank. Since the measurement and kinematic equations describing the IMU-laser scanner localization are infinitely smooth, the observability matrix has an infinite number of rows. However, to prove it is full rank, it suffices to show that a subset of its rows are linearly independent. The following matrix contains one such subset of rows whose linear independence can be easily shown using block Gaussian elimination: L 0 h 1 I L 0 O = L 1 f 0 h 1 L 1 f 0 h = I 3 X Ξ(I q G ) I 3 L 2 f 0 X 2 C T ( I q G ) 1 The observability of the system can also be shown using only the inferred position measurement (8). For details refer to Section 2.1. TR RepRev578 3

5 where L i f 0 h j (x) denotes the i-th order Lie derivative of h j (x) with respect to f 0. The matrices X 1 and X 2 which have dimensions 4 4 and 3 4, respectively, do not need to be computed explicitly since they will be eliminated by the block element (1, 1) of the matrix, i.e., the identity matrix I 4. The resulting matrix after this step is: I I 3 O = Ξ(I q G ) I C T ( I q G ) The determinant of the rotation matrix C( q) is always one for any unit quaternion q, therefore it is always full rank. In the next step we show that Ξ( q) cannot be rank deficient 2. If Ξ( q) is rank deficient, all its square submatrices of dimension 3 3 will be rank deficient and their determinants will be zero. Matrix Ξ( q) has 4 such submatrices, indicated by removing one row of its rows. Using MATLAB notations, the determinant of these submatrices are: p 1 (q 1, q 2, q 3, q 4 ) = det (Ξ([1, 2, 3], :)) = q q 4 q q 2 3q 4 + q 2 2q 4 (11) p 1 (q 1, q 2, q 3, q 4 ) = det (Ξ([1, 2, 4], :)) = q 3 q 2 4 q 3 3 q 3 q 2 2 q 3 q 2 1 (12) p 1 (q 1, q 2, q 3, q 4 ) = det (Ξ([1, 3, 4], :)) = q 2 q q 2 q q q 2 q 2 1 (13) p 1 (q 1, q 2, q 3, q 4 ) = det (Ξ([2, 3, 4], :)) = q 2 3q 1 q 2 2q 1 q 1 q 2 4 q 3 1 (14) For Ξ( q) to be full rank, p i, i = 1,..., 4 should be simultaneously equal to zero. Additionally, the solution q = [q 1, q 2, q 3, q 4 ] T that satisfies p i ( q) = 0, i = 1,..., 4, must also satisfy the unique quaternion constraint, p 0 = q T q 1 = 0. However, a Groebner-basis analysis [4] of these five equations in Maple reveals that their only basis is 1, which means that the variety defined by them is empty [4]. In other words, there is no q that simultaneously satisfies p i ( q) = 0, i = 0,..., 4, thus Ξ( q) cannot be rank deficient for any q 3. Considering that Ξ( q) and C( q) are always full rank for any unit quaternion q, it is clear that all the rows of the matrix O are linearly independent. Hence, we conclude the observability analysis by the following lemma: Lemma 1. Given line measurements corresponding to three planes with linearly independent normal vectors, the system describing the IMU-laser scanner localization is locally weakly observable. 2.1 Alternative proof of observability It is possible to prove that the system defined in (9) is observable when only position measurements [cf. (8)] are available. In this case, as shown below, additional constraints on the motion of the IMU is needed to guarantee observability. We begin by using the following alternative observability matrix (among the infinite number of choices) that only exploits the measurement function (x): O = L 0 L 1 f 0 L 2 f 2 f 0 L 3 f 0f 2 f 0 L 2 f I 3 = I 3 A( I q G ) X 1 B( I q G ) , A( q) = X 2 C T ( I q G ) c 1( q) q c 2( q) q c 3( q) q, B( q) = A( q)ξ( q) where c l ( q), l = 1, 2, 3 are the columns of the rotation matrix C( q), i.e., C( q) = [c 1 ( q) c 2 ( q) c 3 ( q)]. With a Groebner-basis analysis of the determinant of square submatrices of A( q) (similar to the previous section), it can be easily shown that under the unit norm quaternion constraint, the matrix A( q) is full rank for any q. In a similar way it can be shown that B( q) = A( q)ξ( q) is also full rank for any q. Considering that A( q) and B( q) are full rank for any q, it is straight-forward to show that O is full rank by performing block Gaussian elimination similar to the previous section. However, it should be noticed that since in forming O we used Lie derivatives with respect to f 2, the acceleration a m should not be identically zero at least in two direction 4. Therefore, this alternative proof is more 2 An easier way for showing that Ξ( q) is full rank is to notice that under the unit quaternion constraint Ξ T ( q)ξ( q) = I 3 for any q. However, we follow this more complicated approach, since a similar approach is used in the next section to provide an alternative proof of observability 3 This is equivalent to proving that the multivariate resultant of the five equations p i ( q) = 0, i = 0,..., 4 is always nonzero, indicating that these equations do not have a common solution. 4 This is due to the fact that the determinants of submatrices of A( q) and B( q) involving only the derivatives of two columns of C( q) are sufficient to prove that A( q) and B( q) are full rank, respectively. TR RepRev578 4

6 restrictive than the proof in the previous section where measurements of both position and orientation were available, but no assumption on the motion profile was made. References [1] H. Nijmeijer and A. van der Schaft, Nonlinear Dynamical Control Systems. New York: Springer-Verlag, [2] R. Hermann and A. Krener, Nonlinear controlability and observability, IEEE Transactions on Automatic Control, vol. AC-22, no. 5, pp , Oct [3] J. A. Hesch, F. M. Mirzaei, G. L. Mariottini,, and S. I. Roumeliotis, A 3D pose tracker for the visually impaired, in Proc. of the IEEE/RSJ International Conference on Intelligent Robots and Systems (submitted), [4] D. Cox, J. Little, and D. O Shea, Using Algebraic Geometry. Springer, TR RepRev578 5