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

Abstract

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.

1. 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)$.

1.1. Riemannian Manifolds

Let us denote a Riemannian manifold by $\mathbb{M}$. Its tangent space at point $X\in \mathbb{M}$ is denoted by $T_X\mathbb{M}$. Given any pair of points $U,V\in T_Y\mathbb{M}$, an inner product ${⟨U,V⟩}_X\in \mathbb{R}$ can be defined. Let $\gamma :[0,1]\to \mathbb{M}$ be a sufficiently smooth curve on $\mathbb{M}$, then the length of $\gamma (t)$ is defined by

$$\ell (\gamma ):={\int }_0^1\sqrt{{⟨\dot{\gamma }(t),\dot{\gamma }(t)⟩}_{\gamma (t)}}dt={\int }_0^1\sqrt{\mathrm{tr}[{(\gamma {(t)}^{-1}\dot{\gamma }(t))}^T\gamma {(t)}^{-1}\dot{\gamma }(t)]}dt,$$

(1)

where tr denotes the trace of a matrix. The geodesic distance between two matrices X and Y on $\mathbb{M}$ is the infimum of the lengths of all curves connecting them

$$d(X,Y):=inf\{\ell (\gamma )\mid \gamma :[0,1]\to \mathbb{M}with\gamma (0)=X,\gamma (1)=Y\},$$

(2)

which can be achieved by the length of geodesics connecting them.

Given a regular function $f:\mathbb{M}\to \mathbb{R}$, its Riemannian gradient ${\nabla }_{X}f$ in the direction of the vector $V\in T_X\mathbb{M}$ measures the change rate of the function f in the direction V. Namely, for any smooth curve $\varphi :[0,1]\to \mathbb{M}$ satisfying $\varphi (0)=X\in \mathbb{M}$ and $\dot{\varphi }(0)=V$, the Riemannian gradient ${\nabla }_{X}f$ is the unique vector in $T_X\mathbb{M}$ such that

$${⟨V,{\nabla }_{X}f⟩}_X=\frac{d}{dt}f(\varphi (t)){|}_{t=0}.$$

(3)

1.2. Matrix Lie Groups

A group G is called a Lie group if it has differentiable structure, such that the group operators

$$\begin{array}{c}\mu :G\times G\to G,(X,Y)\mapsto X·Y\\ \iota :G\to G,X\mapsto X^{-1}\end{array}$$

(4)

are differentiable, $X,Y\in G$. A matrix Lie group is a Lie group consisting of matrices. The tangent space of G at the identity is the Lie algebra $\mathfrak{g}$, where the Lie bracket is defined.

It is essential that the information between G and $\mathfrak{g}$ is transmitted by exponential map and logarithmic map. The map from $\mathfrak{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\in G$ to a neighborhood of the identity $\mathbf{0}\in \mathfrak{g}$. On the other hand, there is a map from G to $\mathfrak{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 $\nu :\mathfrak{g}\times \mathfrak{g}\to \mathfrak{g}$ such that

$$exp(U)exp(V)=exp(\nu (U,V)),$$

where the Baker–Campbell–Hausdorff formula

$$\nu (U,V)=U+V+[U,V]+O(\mid (U,V){\mid }^3)$$

(5)

represents the Taylor series for $\nu (U,V)$ about the point $(\mathbf{0},\mathbf{0})$.

The set of $n\times n$ real matrices is denoted by $M_{n\times n}$ and the set consisting of all invertible $n\times n$ real matrices is called the general linear group, denoted by $GL(n,\mathbb{R})$. Moreover, using the continuous map $X\mapsto det(X)$, $GL(n,\mathbb{R})$ is an open subset of $M_{n\times n}$ and isomorphic to ${\mathbb{R}}^{n\times n}$. Hence, $GL(n,\mathbb{R})$ is a differentiable manifold.

On $GL(n,\mathbb{R})$, the group multiplication is defined as the matrix multiplication. Its inverse map is a matrix X on $GL(n,\mathbb{R})$ to its inverse matrix $X^{-1}$, and the identity element on $GL(n,\mathbb{R})$ is the identity matrix I. In addition, the Lie algebra $\mathfrak{gl}(n,\mathbb{R})$ of $GL(n,\mathbb{R})$ is $M_{n\times n}$ with the Lie bracket as follows:

$$[U,V]=UV-VU,\forall U,V\in \mathfrak{gl}(n,\mathbb{R}).$$

(6)

All other real matrix Lie groups are subgroups of $GL(n,\mathbb{R})$. At the same time, their group operators are subgroup restriction of the one on $GL(n,\mathbb{R})$. The Lie bracket on their Lie algebras is the matrix commutator.

Let S and $\mathfrak{s}$ represent a matrix Lie group and its Lie algebra, respectively. Then, the exponential map of $\mathfrak{s}$ is the exponential of the matrix. That is to say, if an element $V\in \mathfrak{s}$ is given, the exponential map is as follows:

$$exp(V)={\displaystyle \sum _{m=0}^{\infty }}\frac{V^m}{m!}.$$

(7)

The definition of the logarithmic map is as follows:

$$log(X)={\displaystyle \sum _{m=1}^{\infty }}{(-1)}^{m+1}\frac{{(X-I)}^m}{m},$$

(8)

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\in S$, the tangent space of S at X is denoted by $T_{X}S$. Then for any $X,Y\in S$ and $V\in T_{X}S$, we have the following maps

$$\begin{array}{c}{L}_{X}Y=XY,{(L_X)}_{\ast }V=XV,\\ R_{X}Y=Y{X}^{-1},{(R_{X^{-1}})}_{\ast }V=VX,\end{array}$$

(9)

where L and R respectively denote the left translation and the right translation, and ${(•)}_{\ast }$ represents the tangent mapping of •. The adjoint action $A{d}_A:\mathfrak{s}\to \mathfrak{s}$ is defined as

$$A{d}_{A}X=AX{A}^{-1}.$$

(10)

From (9) and (10), the following formula is easy to obtain:

$$A{d}_A=L_A{R}_A.$$

(11)

Therefore, for $X\in S$ and $U,V\in T_{X}S$, we obtain the left invariant metric on S as follows:

$${⟨U,V⟩}_X={⟨{(L_{X^{-1}})}_{\ast }U,{(L_{X^{-1}})}_{\ast }V⟩}_I=⟨X^{-1}U,X^{-1}V){⟩}_I:=\mathrm{tr}[{(X^{-1}U)}^T{X}^{-1}V].$$

(12)

1.3. Special Euclidean Group

Let $SO(n)$ denote the special orthogonal group acting on ${\mathbb{R}}^n$, then the special Euclidean group $SE(n)$ is represented as

$$SE(n)=\left\{\left(\begin{array}{cc}A& b\\ 0& 1\end{array}\right)|A\in SO(n),b\in {\mathbb{R}}^n\right\}.$$

(13)

It is well-known that the action of $SE(n)$ on ${\mathbb{R}}^n$ is equivalent to a rotation with a translation, namely, the semi-direct product of $SO(n)$ with ${\mathbb{R}}^n$ [30], as follows:

$$SE(n)=SO(n)⋉{\mathbb{R}}^n.$$

(14)

The Lie algebra $\mathfrak{se}(n)$ of $SE(n)$ can be expressed by

$$\mathfrak{se}(n)=\left\{\left(\begin{array}{cc}\Omega & v\\ 0& 0\end{array}\right)|{\Omega }^T=-\Omega ,v\in {\mathbb{R}}^n\right\}.$$

(15)

The tangent space $T_{X}SE(n)$ and the normal space $N_{X}SE(n)$ associated with $SE(n)$ can be characterized as follows:

$$T_{X}SE(n)=\left\{XU\right|U\in \mathfrak{se}(n)\},$$

(16)

$$N_{X}SE(n)=\left\{{(X^{-1})}^T\left(\begin{array}{c}\begin{array}{cc}\hfill s& \hfill 0\\ \hfill h& \hfill d\end{array}\end{array}\right)|X\in {\mathbb{R}}^{n\times n},s^T=s,h\in {\mathbb{R}}^{1\times n},d\in \mathbb{R}\right\}.$$

(17)

Notice that $SE(n)$ is connected, which means that for any given pair $X,Y$, we can find a geodesic

$$\gamma (t)=Xexp(tlog(X^{-1}Y))$$

(18)

such that $\gamma (0)=X$ and $\gamma (1)=Y$, by taking the initial velocity as $\dot{\gamma }(0)=log(X^{-1}Y)\in \mathfrak{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 $\Omega$ in $\mathfrak{se}(3)$ can be uniquely written as

$$\Omega =\left(\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right),$$

(19)

with $\omega =({\omega }_x,{\omega }_y,{\omega }_z)\in {\mathbb{R}}^3$. Let ${\parallel ·\parallel }_F$ denote the Frobenius norm, then ${\parallel \omega \parallel }_F$ expresses the amount of rotation corresponding to the identity vector along $\omega$. Physically, $\omega$ stands for the angular velocity of the rigid body motion and v represents its linear velocity [31].

2. Proposed Method for Change-Point Detection

2.1. Design for Change-Point Detection Method

Let $(X_1,\dots ,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

$$d(X,Y)=\parallel log(X^{-1}Y){\parallel }_F.$$

(20)

Thus, for each point $X_i$, the Riemannian mean ${\mathcal{M}}_f(X_i)$ of $2N+1$ points from $X_{i-N}$ to $X_{i+N}$ is the point X minimizing

$$f(X)=\frac{1}{2(2N+1)}{\displaystyle \sum _{j=i-N}^{i+N}}d{(X,X_j)}^2.$$

(21)

Then, our principle can be redescribed as that $X_i$ is found to possibly be a change point if $d(X_i,{\mathcal{M}}_f(X_i))$ is a local maximum of the Riemannian distances ${(d(X_i,{\mathcal{M}}_f(X_i)))}_i$. As Figure 1 shows, whenever a change-point appears, the series ${(d(X_i,{\mathcal{M}}_f(X_i)))}_i$ displays a local maximum point.

Figure 1. When a change-point occurs, the series ${(d(X_i,{\mathcal{M}}_f(X_i)))}_i$ displays a local maximum.

2.2. Method for Change-Point Detection

As previously discussed, this subsection proposes an algorithm to calculate the series ${(d(X_i,{\mathcal{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 $\varphi$ 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

$${\nabla }_{X}f(X)=\frac{1}{2N+1}X{\displaystyle \sum _{j=i-N}^{i+N}}log(X_j^{-1}X).$$

(22)

Proof. 

Let $s(t)=Xexp(tV)$ denote the geodesic emanating from X in the direction $V=\dot{s}(0)$, then the real-valued function is written as

$$f\left(s(t)\right)=\frac{1}{2(2N+1)}{\displaystyle \sum _{j=i-N}^{i+N}}\mathrm{tr}[log{(X_j^{-1}s(t))}^{T}log(X_j^{-1}s(t))].$$

(23)

Using Proposition 2.1 in [33] and some properties of the trace, we have the derivative of $f\left(s(t)\right)$ with respect to t as follows:

$$\frac{\mathrm{d}}{\mathrm{d}t}f\left(s(t)\right){|}_{t=0}=\frac{1}{2N+1}{\displaystyle \sum _{j=i-N}^{i+N}}\mathrm{tr}\left[log{(X_j^{-1}X)}^T{X}^{-1}V\right]={⟨\frac{1}{2N+1}X{\displaystyle \sum _{j=i-N}^{i+N}}log(X_j^{-1}X),V⟩}_X.$$

(24)

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]:

$$X:={exp}_X(-{\nabla }_{X}f(X)).$$

(25)

From (16), the Riemannian exponential map $expX$ on $SE(n)$ is defined by ${exp}_X(XA)=Xexp(A)$. By Theorem 1, the Riemannian gradient algorithm for calculating the Riemannian mean ${\mathcal{M}}_f(X_i)$ of $2N+1$ points from $X_{i-N}$ to $X_{i+N}$ is rewritten as

$$X:=Xexp(\frac{1}{2N+1}{\displaystyle \sum _{j=i-N}^{i+N}}log(X^{-1}{X}_j)).$$

(26)

According to (22), the Riemannian mean is the stable point of Algorithm (26) if and only if

$${\displaystyle \sum _{j=i-N}^{i+N}}log(X^{-1}{X}_j)=0.$$

(27)

As algorithm (26) converges rapidly to the Riemannian mean ${\mathcal{M}}_f(X_i)$, the first-order approximate value of ${\mathcal{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:

$$d(X,Y)\approx {\parallel logY-logX\parallel }_F.$$

(28)

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 ${\mathcal{M}}_f(X_i)$ is given by

$${\widehat{X}}_i=exp\left(\frac{1}{2N+1}{\displaystyle \sum _{j=i-N}^{i+N}}log(X_j)\right).$$

(29)

To sum up, the algorithm for calculating the series ${(d(X_i,{\mathcal{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,\dots ,X_n$ on $SE(3)$ and window size N. Output: The approximation of the series ${(d(X_i,{\mathcal{M}}_f(X_i)))}_i$. For $i=N+1,\dots ,n-N.$ Initialization: Set ${\widehat{X}}_i:=exp\left(\frac{1}{2N+1}{\sum }_{j=i-N}^{i+N}log(X_j)\right).$ Main loop: $d{X}_j={\widehat{X}}_i^{-1}{X}_j,j=i-N,\dots ,i,\dots ,i+N$. $d{\widehat{X}}_i=exp\left(\frac{1}{2N+1}{\sum }_{j=i-N}^{i+N}log(d{X}_j)\right)$. ${\widehat{X}}_i={\widehat{X}}_{i}d{\widehat{X}}_i$. If $\parallel {\sum }_{j=i-N}^{i+N}log(d{X}_j){\parallel }_F$ is sufficiently small, the output is $d(X_i,{\mathcal{M}}_f(X_i))\approx {\parallel log(X_i)-log({\widehat{X}}_i)\parallel }_F$. Otherwise continue looping. Endfor.

Geometrically, we left-multiply these points by ${\widehat{X}}_i^{-1}$, so that ${\widehat{X}}_i^{-1}{X}_j$ $(j=i-N,\dots ,i,\dots ,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.

3. 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.

3.1. Change-Point Detection for the Motion of a Cylinder

Consider a cylinder’s synthetic motion $(X_1,\dots ,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$.

Figure 2. (a) Blue cylinders denote identified change-points; (b) Detection results with different window sizes.

The proposed Algorithm 1 was applied to calculate the series ${(d(X_i,{\mathcal{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,{\mathcal{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.

3.2. 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.

Figure 3. (a) Scanning on the surface of a rectangular workpiece with rounded corners by a STAUBLI TX90XL manipulator; (b) Change points of the motion trajectory.

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.

Figure 4. Comparison of the original velocity without change-point detection to the correction velocity for each joint axis.

4. 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.