SE(3)上的指数映射和对数映射及其实现代码

目的

　　本文简单介绍了 $\textrm{SE(3)}$ （特殊欧氏群）上的指数映射和对数映射，并给出实现代码。指数映射经常用于机器人的正运动学建模，对数映射则可用于机器人标定中的误差建模。

1　一般矩阵

　　作为基础，先给出一般矩阵的指数映射和对数映射定义，如下 $^{[1]}$ 。后面的公式都是从这两个公式推导出来的。别担心，你不需要背下这两个复杂的公式，我们后面基本不会用到它们。

$\textrm{指数映射}：　　e ^ { X } = \sum _ { k = 0 } ^ { \infty } \frac { X ^ { k } } { k ! }$

$\textrm{对数映射}：　　 \log X = \sum _ { k = 1 } ^ { \infty } ( - 1 ) ^ { k + 1 } \frac { ( X - I ) ^ { k } } { k }$

2　 $\textrm{SE(3)}$ 矩阵

2.1　指数映射

　　相较于一般的矩阵群，描述三维空间刚体运动的 $\textrm{SE(3)}$ 只是个特例。对于李代数（旋量） $\xi=(v, \omega)$ ，假设 $\|\omega\|=1$ ， $\textrm{SE(3)}$ 上的指数映射可以通过下式计算：
$e^{\widehat{\xi} \theta}=\left[ \begin{array}{cc}{e^{\widehat{\omega} \theta}} & {\left(I-e^{\widehat{\omega} \theta}\right)(\omega \times v)+\omega \omega^{T} v \theta} \\ {0} & {1}\end{array}\right]$
　　其中， $I$ 为 $3\times3$ 单位矩阵，左上角的 $e^{\widehat{\omega} \theta}$ 的计算方法如下：
$e^{\widehat{\omega} \theta}=I+\widehat{\omega} \sin \theta+\widehat{\omega}^{2}(1-\cos \theta)$

2.2　对数映射

　　对数映射的计算方法为 $^{[3]}$ ：
$\widehat{\xi}=\log \left[ \begin{array}{cc}{R} & {p} \\ {0} & {1}\end{array}\right]=\left[ \begin{array}{cc}{\widehat{\omega}} & {A^{-1} p} \\ {0} & {0}\end{array}\right]$
　　其中， $\widehat{\omega}$ 和 $A^{-1}$ 的计算方法如下：
$\widehat{\omega}=\frac{1}{2 \sin \theta}\left(R-R^{T}\right)$

$A^{-1}=\frac{1}{\theta} I-\frac{1}{2} \widehat{\omega}+\left(\frac{1}{\theta}-\frac{1}{2} \cot \frac{\theta}{2}\right) \widehat{\omega}^{2}$
　　其中， $\theta$ 的计算方法如下：
$\theta=\arccos\left(\frac{\operatorname{trace}(R)-1}{2}\right)$

3　实现代码

　　下面在Mathematica软件中编程实现上述指数映射和对数映射。首先是指数映射，代码如下，这段代码来自于 ${[2]}$ 。为了节省空间，依赖函数没有列出，读者如果感兴趣可自行查阅。

TwistExp[xi_, \[Theta]_]:= Module[{v = xi[[1;;3]], w = xi[[4;;6]], R, p},
    If[Norm[w-{0,0,0}]==0,
        R = IdentityMatrix[3];  (* 旋转矩阵 *)
        p = v * \[Theta],  (* 平移向量 *)
      (* else *)
        R = SkewExp[w, \[Theta]];
        p = (IdentityMatrix[3] - R) . (AxisToSkew[w] . v) + w (w.v) \[Theta];
      ];
    RPToH[R, p]
  ];

　　如果你担心上述代码的正确性，我们可以验证一下。Mathematica中自带的函数MatrixExp适用于一般矩阵的指数映射。既然 $\textrm{SE(3)}$ 是个特例，MatrixExp当然也适用于它。因此我们可以借助MatrixExp来验证，验证所使用的代码如下：

\[Omega] = Normalize@RandomReal[{-1, 1}, 3];
l = RandomReal[{-1, 1}, 3];
\[Xi] = Join[Cross[-\[Omega], l], \[Omega]];
g1 = TwistExp[\[Xi], 1.0];
g2 = MatrixExp[hat[\[Xi]]];
g1 == g2

　　计算结果如下，可以看到TwistExp和MatrixExp计算的结果相等，这说明TwistExp的代码是正确的。

True

　　对数映射的实现代码如下，这段代码来自于 ${[3]}$ ，但是原版本与书中公式不能对应，这里改正了。

MatrixLog6[g_]:= Module[{R,p,acosinput,theta,omgmat},
	R = g[[1;;3,1;;3]]; 
	p = g[[1;;3,4]]; 
	If[NearZero[Norm[R-IdentityMatrix[3]]],
	     ArrayFlatten[{{ConstantArray[0,{3,3}],{p}\[Transpose]},{0,0}}],
	   (* else *)
	     acosinput=(Tr[R]-1)/2;
	     Which[acosinput>1,acosinput=1,acosinput<-1,acosinput=-1];
	     theta=ArcCos[acosinput];
	     omgmat=MatrixLog3[R];
	     ArrayFlatten[{{omgmat,{(IdentityMatrix[3]/theta-omgmat/2+(1/theta-Cot[theta/2]/2)*omgmat^2).p}\[Transpose]},{0,0}}]
	]
]

　　对数映射与指数映射互为逆运算，既然我们已经验证了指数映射代码的正确性，下面就可以借助它来验证对数映射代码的正确性，验证代码如下。

\[Omega] = Normalize@RandomReal[{-1, 1}, 3];
l = RandomReal[{-1, 1}, 3];
\[Xi] = Join[Cross[-\[Omega], l], \[Omega]];
g = TwistExp[\[Xi], 1.0];
Chop[\[Xi] - dehat@MatrixLog6[g]]

　　计算结果如下，可见对数映射函数还原得到了旋量 $\xi$ ，所以MatrixLog6也是正确的。

{0, 0, 0, 0, 0, 0}

　　公式总是没有图形直观，尤其是复杂的公式。你可以将旋量和齐次变换矩阵显示出来，如下图所示。图中浅蓝色的箭头表示向量 $\omega$ （就是旋量的旋转轴），灰色的箭头表示经过对数映射得到旋量旋转轴。正方体的位姿反映了相应的齐次变换矩阵 $e^{\widehat{\xi} \theta}$ 。你可以显示更多的内容，以检验自己理解的是否正确。

　　显示所用的代码如下，其中的函数定义可以参考 ${[4]}$ 。

Manipulate[
 \[Omega] = Normalize@{x, y, z};
 l = {0, 0, 0};
 \[Xi]1 = Join[Cross[-\[Omega], l], \[Omega]];
 g1 = TwistExp[\[Xi]1, \[Theta]];
 g2 = MatrixExp[hat[\[Xi]1*\[Theta]]];
 \[Xi]2 = dehat@MatrixLog6[g1];
 Graphics3D[{Cyan, Arrow[Tube[{l, \[Omega]}]],Gray, Arrow[Tube[{l, \[Xi]2[[4 ;; 6]]}]], frame3D, move3D[frame3D, g1], FaceForm[{Opacity[0.2], Yellow}], move3D[Cuboid[], g2]}, PlotRange -> {{-1, 1}, {-1, 1}, {-1, 1}}*1.8, ImageSize -> 400], Grid[{{Control[{x, -1, 1}],    Control[{y, -1, 1}]}, {Control[{z, -1, 1}], Control[{\[Theta], -Pi, Pi}]}}], Initialization :> {{x, y, z, \[Theta]} = {0, 0, 0, 0}}]

4　参考文献

[1]　Lie Groups, Lie Algebras, and Representations: An Elementary Introduction, Hall Brian C, 2003 , Chapter 2.
[2]　A Mathematical Introduction to Robotic Manipulation, Richard M. Murray, 1994, 随书附带程序包
[3]　Modern Robotics: Mechanics, Planning, and Control, Kevin M. Lynch, 2016, p106, 随书附带程序包
[4]　基于Mathematica的机器人仿真环境（机械臂篇）

SE(3)上的指数映射和对数映射及其实现代码

猜你喜欢