A Matrix Information-Geometric Method for Change-Point Detection of Rigid Body Motion

A matrix information-geometric method was developed to detect the change-points of rigid body motions. Note that the set of all rigid body motions is the special Euclidean group SE(3), so the Riemannian mean based on the Lie group structures of SE(3) reflects the characteristics of change-points. Once a change-point occurs, the distance between the current point and the Riemannian mean of its neighbor points should be a local maximum. A gradient descent algorithm is proposed to calculate the Riemannian mean. Using the Baker–Campbell–Hausdorff formula, the first-order approximation of the Riemannian mean is taken as the initial value of the iterative procedure. The performance of our method was evaluated by numerical examples and manipulator experiments.

Classical information geometry is developed from the research of the statistical manifolds consisting of probability density functions, where the Fisher information matrix was first taken as a Riemannian metric by Rao [17]. Chentsov [18] introduced a family of affine connections and Efron [19] found the relationship between curvatures and statistical inference. Moreover, the theory of dual connections was investigated by Amari [20]. Thus, classical information geometry covers a range of random cases. In order to study non-random cases, Barbaresco, Nielsen, and Bhatia et al. introduced the concept of matrix information geometry, which is mainly used to study radar signal processing, manifold learning, optimization, system stability and optimization, image processing, and so on [21,22]. Especially, matrix Lie groups (including orthogonal group, unitary group, special symplectic group, special Euclidean group, etc.) and submanifolds of the general linear group (including positive definite matrix manifold, Stiefel manifold and Grassman manifold, etc.) have important applications in many fields. Here, the left-invariant (or right-invariant) metric is adopted as the Riemannian metric and the geodetic distance is used to define the distance function. Importantly, the geodesic can be expressed explicitly by exponential map and logarithmic map on manifold, which is very convenient for solving practical problems.
In this paper, a matrix information-geometric method is presented to detect change points, based on the differential-geometric structure of SE (3) and not restricted by the above-mentioned condition in [14]. Note that the SE(3) group is unclosed under linear combination, so we first define the Riemannian mean in view of the Lie group structure of SE(3). Then, a gradient descent algorithm is proposed to calculate the Riemannian mean. Based on the Baker-Campbell-Hausdorff formula, the first-order approximation of the Riemannian mean is taken as the initial value of the iterative procedure so that the algorithm converges rapidly. Simulations and experiments were run to show the computational behavior of the proposed method.
The paper is organized as follows: Section 1 briefly introduces the Riemannian framework of Riemannian manifolds and matrix Lie groups. In Section 2, our proposed change-point detection method is described based on the Lie group structures of SE(3). Section 3 reports the experimental results to demonstrate the effectiveness of our method.

A Survey of Some Geometrical Concepts
In this section, we give a brief overview of Riemannian manifolds and matrix Lie groups [23][24][25][26][27][28][29], which forms the foundation of our change-point detection method on SE(3).

Riemannian Manifolds
Let us denote a Riemannian manifold by M. Its tangent space at point X ∈ M is denoted by T X M. Given any pair of points U, V ∈ T Y M, an inner product U, V X ∈ R can be defined. Let γ : [0, 1] → M be a sufficiently smooth curve on M, then the length of γ(t) is defined by where tr denotes the trace of a matrix. The geodesic distance between two matrices X and Y on M is the infimum of the lengths of all curves connecting them which can be achieved by the length of geodesics connecting them.
Given a regular function f : M → R, its Riemannian gradient ∇ X f in the direction of the vector V ∈ T X M measures the change rate of the function f in the direction V. Namely, for any smooth curve φ : [0, 1] → M satisfying φ(0) = X ∈ M andφ(0) = V, the Riemannian gradient ∇ X f is the unique vector in T X M such that

Matrix Lie Groups
A group G is called a Lie group if it has differentiable structure, such that the group operators are differentiable, X, Y ∈ G. A matrix Lie group is a Lie group consisting of matrices. The tangent space of G at the identity is the Lie algebra g, where the Lie bracket is defined.
It is essential that the information between G and g is transmitted by exponential map and logarithmic map. The map from g to G is the exponential map, denoted by exp. In most problems, the exponential map has the characteristics of neither injective nor surjective projection, but it is a homeomorphic mapping from a neighborhood of the identity I ∈ G to a neighborhood of the identity 0 ∈ g. On the other hand, there is a map from G to g, called the logarithmic map, denoted by log, which is the local inverse of the exponential map.
It is well known that the exponential function has the property exp(a) exp(b) = exp(a + b) with real numbers a and b, but there is no such property in non-commutative Lie groups, such as the special orthogonal group. The product in logarithmic coordinates is defined as a mapping ν : where the Baker-Campbell-Hausdorff formula represents the Taylor series for ν(U, V) about the point (0, 0). The set of n × n real matrices is denoted by M n×n and the set consisting of all invertible n × n real matrices is called the general linear group, denoted by GL(n, R). Moreover, using the continuous map X → det(X), GL(n, R) is an open subset of M n×n and isomorphic to R n×n . Hence, GL(n, R) is a differentiable manifold.
On GL(n, R), the group multiplication is defined as the matrix multiplication. Its inverse map is a matrix X on GL(n, R) to its inverse matrix X −1 , and the identity element on GL(n, R) is the identity matrix I. In addition, the Lie algebra gl(n, R) of GL(n, R) is M n×n with the Lie bracket as follows: All other real matrix Lie groups are subgroups of GL(n, R). At the same time, their group operators are subgroup restriction of the one on GL(n, R). The Lie bracket on their Lie algebras is the matrix commutator. Let S and s represent a matrix Lie group and its Lie algebra, respectively. Then, the exponential map of s is the exponential of the matrix. That is to say, if an element V ∈ s is given, the exponential map is as follows: The definition of the logarithmic map is as follows: where X belongs to a neighborhood of the identity I on S.
A matrix Lie group also has the geometric structure of manifolds. Let X ∈ S, the tangent space of S at X is denoted by T X S. Then for any X, Y ∈ S and V ∈ T X S, we have the following maps where L and R respectively denote the left translation and the right translation, and (•) * represents the tangent mapping of •. The adjoint action Ad A : s → s is defined as From (9) and (10), the following formula is easy to obtain: Therefore, for X ∈ S and U, V ∈ T X S, we obtain the left invariant metric on S as follows:

Special Euclidean Group
Let SO(n) denote the special orthogonal group acting on R n , then the special Euclidean group SE(n) is represented as It is well-known that the action of SE(n) on R n is equivalent to a rotation with a translation, namely, the semi-direct product of SO(n) with R n [30], as follows: The Lie algebra se(n) of SE(n) can be expressed by The tangent space T X SE(n) and the normal space N X SE(n) associated with SE(n) can be characterized as follows: Notice that SE(n) is connected, which means that for any given pair X, Y, we can find a geodesic such that γ(0) = X and γ(1) = Y, by taking the initial velocity asγ(0) = log(X −1 Y) ∈ se(n).
When n = 3, a matrix of SE(3) represents a displacement of the rigid body, where A denotes the orientation or attitude and b corresponds to the translation. The skew-symmetric matrix Ω in se (3) can be uniquely written as with ω = (ω x , ω y , ω z ) ∈ R 3 . Let · F denote the Frobenius norm, then ω F expresses the amount of rotation corresponding to the identity vector along ω. Physically, ω stands for the angular velocity of the rigid body motion and v represents its linear velocity [31].

Design for Change-Point Detection Method
Let (X 1 , . . . , X n ) denote a time series. The detection procedure is formulated as follows: For each point X i under test, the Riemannian mean of the 2N + 1 points from X i−N to X i+N is calculated. Here N is the window size and its value can be selected experimentally. Then, the Riemannian distance d i from X i to its Riemannian mean of X i can be obtained. If this distance d i is a local maximum, then X i is possibly a change point.
Substituting (18) into (1), the Riemannian distance between X and Y on SE (3), which arises from the left-invariant metric (12), is expressed as Thus, for each point X i , the Riemannian mean M f (X i ) of 2N + 1 points from X i−N to X i+N is the point X minimizing Then, our principle can be redescribed as that X i is found to possibly be a change point if d(X i , M f (X i )) is a local maximum of the Riemannian distances (d(X i , M f (X i ))) i . As Figure 1 shows, whenever a change-point appears, the series (d(X i , M f (X i ))) i displays a local maximum point.

Method for Change-Point Detection
As previously discussed, this subsection proposes an algorithm to calculate the series (d(X i , M f (X i ))) i so that all local maximum points may be sought out. Actually, Riemannian means in Lie group have been discussed well in [27,32]. Here these ideas are extended to propose an algorithm for calculating the Riemannian mean of a set of 2N + 1 points on SE(3).
First, we need to compute the Riemannian gradient of the objective function f (X) to be optimized. In the definition of the Riemannian gradient (3), the generic smooth curve φ may be replaced with a geodesic, then we obtain the following theorem. Theorem 1. The Riemannian gradient of the objective function f (X) at point X is given by Proof. Let s(t) = X exp(tV) denote the geodesic emanating from X in the direction V =ṡ(0), then the real-valued function is written as Using Proposition 2.1 in [33] and some properties of the trace, we have the derivative of f s(t) with respect to t as follows: From (3), it is shown that Formula (22) is valid.
Let exp X denote the Riemannian exponential map about the point X, then the Riemannian gradient algorithm can be expressed as [23,24]: From (16), the Riemannian exponential map exp X on SE(n) is defined by exp X (XA) = X exp(A). By Theorem 1, the Riemannian gradient algorithm for calculating the Riemannian mean M f (X i ) of 2N + 1 points from X i−N to X i+N is rewritten as According to (22), the Riemannian mean is the stable point of Algorithm (26) if and only if As algorithm (26) converges rapidly to the Riemannian mean M f (X i ), the first-order approximate value of M f (X i ) is taken as the initial value of the iterative procedure. In fact, using the Baker-Campbell-Hausdorff formula (5) up to first-order terms, the Riemannian distance (20) between two points on SE(3) can be approximated as follows: Here the sum of squared approximate distances in (21) is minimized by the arithmetic mean of the logarithmic maps of the points in the neighborhood of X i , so the first-order approximation of the Riemannian mean M f (X i ) is given by To sum up, the algorithm for calculating the series (d(X i , M f (X i ))) i , in which the first-order approximation of the Riemannian mean is taken the initial value, is presented as follows: Algorithm 1 Algorithm for calculating the series of geodesic distances Inputs: Points X 1 , . . . , X n on SE(3) and window size N. Output: The approximation of the series (d(X i , M f (X i ))) i . For i = N + 1, . . . , n − N.

Otherwise continue looping. Endfor.
Geometrically, we left-multiply these points by X −1 i , so that X −1 i X j (j = i − N, . . . , i, . . . , i + N) are moved to the vicinity of the identity. Although this algorithm is proposed to detect the change-points of rigid body motion on SE(3), we can extend it to similar problems on other matrix Lie groups, especially the subgroups of the general linear group.

Simulations
In this section, we are devoted to illustrating the effectiveness of Algorithm 1. First, the simulation of a cylinder motion is provided, and the effects of the window size N are analyzed for the results of the change-point detection. Secondly, the change-point detection experiment is presented, which was carried out using a STAUBERT TX90XL manipulator (Staubli, Faverges, France). Change points of the manipulator's motion were identified in order to reduce its speed near the change points, so that the manipulator could move smoothly.

Change-Point Detection for the Motion of a Cylinder
Consider a cylinder's synthetic motion (X 1 , . . . , X 200 ), where six change points happen at moments 21, 51, 96, 121, 141, and 156. In Figure 2a, the blue cylinders represent the found change points, with a visual representation of this cylinder at every two poses of the corresponding position X i .
The proposed Algorithm 1 was applied to calculate the series (d(X i , M f (X i ))) i with different window sizes N = 1, 5, 10, 15. As Figure 2b shows, a visual representation of the series (d(X i , M f (X i ))) i is given by four-color simulation curves. Six local maxima were present at indices 21, 51, 96, 121, 141, and 156, so the simulation coincided with the practical motion of the cylinder extremity. Moreover, it can be observed that when the window size N = 15, the sixth change-point was missed. Therefore, the window size should not be greater than the difference between the corresponding moments of two adjacent change-points.

Change-Point Detection for the Motion of the STAUBERT TX90XL Manipulator
The STAUBLI manipulator has high motion accuracy and protection level, which is suitable for welding or ultrasonic non-destructive testing. In our experiment, a STAUBLI TX90XL manipulator is used to scan the surface of a rectangular workpiece with rounded corners, as shown in Figure 3a. The motion trajectory contained 2400 points with 1 mm equal arc length interval, and the tip-position of the flexible manipulator needed to scan back and forth across the rounded rectangular workpiece. Our provided method was used to find the change points, as shown in Figure 3b. After identifying those change points, it was convenient and necessary to reduce the speed of the manipulator at each change-point from 60 mm/s to 30 mm/s, so that a smooth movement could be maintained.
In the practical experiments, each joint velocity was measured based on the data given by each motor encoder. Figure 4 shows a comparison of the original velocity without change-point detection to the corrected velocity for each joint axis. It can be seen that the peak velocity after change-point detection and calibration was significantly reduced. The maximum velocities of joints 2-3 and 5-6 decreased by more than 40%. This application had good performance in smooth transition and stable operation for the manipulator's control.

Conclusions
In this paper, we propose a change-point detection method in the view of matrix information geometry. Using the Lie group structure of SE(3), a gradient descent algorithm is proposed to calculate the Riemannian mean. Based on the Baker-Campbell-Hausdorff formula, the first-order approximation of the Riemannian mean is taken as the initial value so that the iterative procedure can converge rapidly. The numerical example indicates that the provided method can identify these change points with significant posture changes in the rigid body motion. An experiment was also carried out on the motion control of the manipulator. By identifying the change points and reducing the speed of neighbor points, the sharp velocity change of each manipulator joint could be effectively improved, and a smooth transition could be maintained.