二维和三维空间李群李代数转换关系

本文链接：https://blog.csdn.net/zzb714121/article/details/141403564

1 二维空间刚体变换 (3个自由度)

二维空间的李群李代数参考Lie Groups for 2D and 3D Transformations。

1.1 李群 $\mathrm{SE}(2)$

二维刚体变换公式为 $\in \mathrm{SE}(2)$ , 如下所示:
$\begin{aligned} & \mathbf{R}=\left[\begin{array}{cc} \cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right] \in \operatorname{SO}(2), \mathbf{t}=\left[\begin{array}{l} t_x \\ t_y \end{array}\right] \in \mathbb{R}^2 \\ & T=\left(\begin{array}{ll} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{array}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & t_x \\ \sin \theta & \cos \theta & t_y \\ 0 & 0 & 1 \end{array}\right] \in \operatorname{SE}(2) \end{aligned}$

1.2 李代数 ${{s e}(2)}$

与 $\mathrm{SE}(2)$ 对应的李代数 $\boldsymbol{\phi}^{\wedge} \in \mathrm{se}(2)$ , 如下
$\begin{aligned} & G_1=\left(\begin{array}{lll} 0 & 0 & 1 \\ 0 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right), G_2=\left(\begin{array}{ccc} 0 & 0 & 0 \\ 0 & 0 & 1 \\ 0 & 0 & 0 \end{array}\right), G_3=\left(\begin{array}{ccc} 0 & -1 & 0 \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{array}\right) \\ & \boldsymbol{\phi}=\left(\begin{array}{lll} {u}_1 & {u}_2 & \theta \end{array}\right)^T \quad \in \mathbb{R}^3 \\ & \boldsymbol{\phi}^{\wedge}={u}_1 G_1+{u}_2 G_2+\theta G_3 \quad=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \text { se (2) } & \end{aligned}$ 上式中的 $\boldsymbol{\phi}$ 即为我们理解的旋转向量, 或者称为角轴。注意 $\boldsymbol{\phi}^{\wedge}$ 是反对称阵, 但是和 $\mathbf{so}(3)$ 中的反对称阵写法不同。在视觉 SLAM 十四讲中, 主要介绍的是三维空间。

1.3 指数映射(将李代数 $\operatorname{se}(2)$ 转换为李群 $\mathrm{SE}(2)$ )

已知李代数 $\operatorname{se}(2)$ 如下:
$\begin{gathered} \boldsymbol{\phi}=\left[\begin{array}{c} \mathrm{u}_1 \\ \mathrm{u}_1 \\ \theta \end{array}\right] \in \mathbb{R}^3 \\ \boldsymbol{\phi}^{\wedge}=\left[\begin{array}{ccc} 0 & -\theta & \mathrm{u}_1 \\ \theta & 0 & \mathrm{u}_2 \\ 0 & 0 & 0 \end{array}\right] \in \operatorname{se}(2) \end{gathered}$ 那么对应的李群 $\mathrm{SE}(2)$ 为:
$\mathrm{T}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\left(\begin{array}{cc} \exp \left(\theta^{\wedge}\right) & \mathbf{V u} \\ \mathbf{0} & 0 \end{array}\right) \ldots \ldots \ldots \ldots \text { (1) }$ 其中 $\exp \left(\theta^{\wedge}\right)=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta\end{array}\right], \boldsymbol{V}=\frac{1}{\theta} \cdot\left(\begin{array}{cc}\sin \theta & -(1-\cos \theta) \\ 1-\cos \theta & \sin \theta\end{array}\right)$ , $\mathbf{u}=\left[\begin{array}{l}\mathrm{u}_1 \\ \mathrm{u}_2\end{array}\right]$ 。那么
$\mathrm{T}=\exp \left(\boldsymbol{\phi}^{\wedge}\right)=\left[\begin{array}{ccc} \cos \theta & -\sin \theta & \frac{\sin \theta}{\theta} \mathrm{u}_1-\frac{1-\cos \theta}{\theta} \mathrm{u}_2 \\ \sin \theta & \cos \theta & \frac{1-\cos \theta}{\theta} \mathrm{u}_1+\frac{\sin \delta_\theta}{\delta_\theta} \mathrm{u}_2 \\ 0 & 0 & 1 \end{array}\right] \ldots \ldots \ldots \ldots \text { (2) }$ 从公式 (2) 可以知道, 李代数的 $\boldsymbol{\phi}$ 这个向量中的三个量 $\mathrm{u}_1$ 、 $\mathrm{u}_2 、 \theta$ , 刚好能对应到 $\mathbf{T}$ 中的 $\theta$ , 因此李代数中的 $\theta$ 确实真实反映了旋转,可是 $\mathrm{u}_1 、 \mathrm{u}_2$ 只是部分反映了平移, 需要乘以 $\boldsymbol{V}$ , 才是变换成 $\mathbf{T}$ 后真正的平移量。

2 三维空间刚体变换(6个自由度)

三维空间中旋转矩阵的写法, 我发现大家习惯这样写这个旋转矩阵的公式, 如下: $\mathbf{R}=\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]_{3 \times 3} \in \operatorname{SO}(3)$ 因为之前上过牛小骥（武汉大学）的惯导课，感兴趣可以去b站看一下，同时对于一个载体的姿态比较敏感。遂将上面的旋转矩阵和上课时用的旋转矩阵进行了对比, 截取了 PPT 中的内容, 其中旋转矩阵公式如下。下面的旋转矩阵的含义是将 b 系中的点转换到 n 系下。但是旋转的顺序是从 n 系旋转到 b 系得到的旋转矩阵。 $\mathbf{C}_b^n=\left(\begin{array}{ccc} c \theta c \psi & -c \phi s \psi+s \phi s \theta c \psi & s \phi s \psi+c \phi s \theta c \psi \\ c \theta s \psi & c \phi c \psi+s \phi s \theta s \psi & -s \phi c \psi+c \phi s \theta s \psi \\ -s \theta & s \phi c \theta & c \phi c \theta \end{array}\right)$ 说明一下: 公式中的 $\psi$ 为航向角(绕 z 轴旋转), $\theta$ 为俯仰角(绕 y 轴旋转), $\phi$ 为横滚角( 绕 x 轴旋转)。旋转顺序分别是 ZYX。
与大家常用的公式进行了对比, 上面公式中的 $\alpha$ 对应的是 $\phi$ (横滚角), $\beta$ 对应的是 $\theta$ (俯仰角), $\gamma$ 对应的是 $\psi$ (航向角)。

2.1 李群 $\mathrm{SE}(3)$

三维刚体变换公式为 $\in \mathrm{SE}(3)$ , 如下所示: $\begin{gathered} \mathbf{R}=\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right] \in \operatorname{SO}(3), \\ \mathbf{t}=\left[\begin{array}{c} t_x \\ t_y \\ t_z \end{array}\right] \in \mathbb{R}^3 \\ \operatorname{T}=\left(\begin{array}{ll} \mathbf{R} & \mathbf{t} \\ \mathbf{0} & 1 \end{array}\right) \\ =\left[\begin{array}{clll} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha & t_x \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha & t_y \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha & t_z \\ 0 & 0 & 0 & 1 \end{array}\right] \in \operatorname{SE}(3) \end{gathered}$

2.2 李代数 ${{s e}(3)}$

与之对应的李代数 $\boldsymbol{\xi}=\left[\begin{array}{l}\boldsymbol{u} \\ \boldsymbol{\phi}\end{array}\right]=\left[\begin{array}{l}u_1 \\ u_2 \\ u_3 \\ \phi_1 \\ \phi_2 \\ \phi_3\end{array}\right] \in \mathbb{R}^6$ $G_1=\left(\begin{array}{llll} 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), \quad G_2=\left(\begin{array}{llll} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_3=\left(\begin{array}{llll} 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 \\ 0 & 0 & 0 & 0 \end{array}\right)$ $G_4=\left(\begin{array}{cccc} 0 & 0 & 0 & 0 \\ 0 & 0 & -1 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_5=\left(\begin{array}{cccc} 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 \\ -1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right), G_6=\left(\begin{array}{cccc} 0 & -1 & 0 & 0 \\ 1 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 0 \end{array}\right)$ $\begin{aligned} \xi^{\wedge}=u_1 G_1 & +u_2 G_2+u_3 G_3+\phi_1 G_4+\phi_2 G_5+\phi_3 G_6 \\ & =\left[\begin{array}{cccc} 0 & -\phi_3 & \phi_2 & u_1 \\ \phi_3 & 0 & -\phi_1 & u_2 \\ -\phi_2 & \phi_1 & 0 & u_3 \\ 0 & 0 & 0 & 0 \end{array}\right] \end{aligned}$ 换言之， $\boldsymbol{\xi}^{\wedge}=\left[\begin{array}{ll}\boldsymbol{\phi}^{\wedge} & \boldsymbol{u} \\ 0^T & 0\end{array}\right]$ ，这个是视觉 SLAM 十四讲中喜欢用的写法，上面的展开是从Lie Groups for 2D and 3D Transformations中摘抄的。
上式中的 $\boldsymbol{\xi}$ 即为我们理解的旋转向量, 或者称为角轴。注意 $\boldsymbol{\xi}^{\wedge}$ 是反对称阵。在视觉 SLAM 十四讲中，主要介绍的是三维空间，李群 SE (3)和李代数 se(3)是——对应（旋转角度在 $\pm \pi$ 之间，李群和李代数元素是——对应的）的， $\mathrm{SE}(3)$ 为 $\times 4$ 的旋转矩阵, $\mathrm{se}(3)$ 为 $\times 1$ 的旋转向量。李群 $\mathrm{SO}(3)$ 和李代数 $\mathrm{so}(3)$ 是一一对应的, $\mathrm{SO}(3)$ 为 $\times 3$ 的旋转矩阵， $\mathrm{so}(3)$ 为 $\times 1$ 的旋转向量。

2.3 指数映射（将李代数 $\mathrm{se}(3)$ 转换为李群SE(3)）

已知李代数 $\operatorname{se}(3)$ 如下: $\begin{gathered} \xi=\left[\begin{array}{l} \boldsymbol{u} \\ \boldsymbol{\phi} \end{array}\right]=\left[\begin{array}{l} u_1 \\ u_2 \\ u_3 \\ \phi_1 \\ \phi_2 \\ \phi_3 \end{array}\right] \in \mathbb{R}^6 \\ \xi^{\wedge}=\left[\begin{array}{cccc} 0 & -\phi_3 & \phi_2 & u_1 \\ \phi_3 & 0 & -\phi_1 & u_2 \\ -\phi_2 & \phi_1 & 0 & u_3 \\ 0 & 0 & 0 & 0 \end{array}\right] \end{gathered}$ 那么对应的李群 SE(3)为: $\mathrm{T}=\left[\begin{array}{cc} \exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{J} u \\ 0^T & 1 \end{array}\right]=\left[\begin{array}{cc} \mathbf{R} & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right]$ 其中 $\exp \left(\boldsymbol{\phi}^{\wedge}\right)=$ $\left[\begin{array}{ccc} \cos \gamma \cos \beta & \cos \gamma \sin \beta \sin \alpha-\sin \gamma \cos \alpha & \cos \gamma \sin \beta \cos \alpha+\sin \gamma \sin \alpha \\ \sin \gamma \cos \beta & \sin \gamma \sin \beta \sin \alpha+\cos \gamma \cos \alpha & \sin \gamma \sin \beta \cos \alpha-\cos \gamma \sin \alpha \\ -\sin \beta & \cos \beta \sin \alpha & \cos \beta \sin \alpha \end{array}\right]$ 不过这里面的 $\alpha, \beta$ 和 $\gamma$ 与 $\phi_1, \phi_2, \phi_3$ 的具体对应关系我还没有搞清楚。
$\boldsymbol{J}$ 文档Lie Groups for 2D and 3D Transformations中的写法与视觉 SLAM 十四讲中¹不一样，随后我进行了推倒，也就是将文档中的 $\boldsymbol{J}$ 进行了推倒，看看它是否和视觉 SLAM 十四讲中一致。此时我们提前预备两个知识点:

已知三维向量 $\boldsymbol{\phi}_{3 \times 1}=\theta \boldsymbol{a}, \boldsymbol{a}$ 是一个长度为 $\mathbf{1}$ 的方向向量, $\theta$ 是 $\boldsymbol{\phi}$ 的模长。那么, $\boldsymbol{\phi}^{\wedge}=\theta \boldsymbol{a}^{\wedge}$
单位向量 $\boldsymbol{a}$ 有一个性质: $\boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}=\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}$ 这里面符号统一一下： $\boldsymbol{a}^{\wedge}$ 和 $\boldsymbol{a}_{\times}$ 是一个意思，都是求三维向量的反对称阵。另外, 文档Lie Groups for 2D and 3D Transformations给的求 $\boldsymbol{J}$ 的公式如下: $\begin{aligned} \theta & =\sqrt{\boldsymbol{\phi}^T \boldsymbol{\phi}} \\ A & =\frac{\sin \theta}{\theta} \\ B & =\frac{1-\cos \theta}{\theta^2} \\ C & =\frac{1-A}{\theta^2} \\ \mathbf{R} & =\mathbf{I}+A \boldsymbol{\phi}^{\wedge}+B \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \\ \boldsymbol{J} & =\mathbf{I}+B \boldsymbol{\phi}^{\wedge}+C \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge} \end{aligned}$ 遂对文档中的 $\boldsymbol{J}$ 进行了推倒, 看看它是否能推导到和十四讲中一致，过程如下: $\begin{aligned} \boldsymbol{J}=\boldsymbol{I}+B \boldsymbol{\phi}^{\wedge} & +C \boldsymbol{\phi}^{\wedge} \boldsymbol{\phi}^{\wedge}=\boldsymbol{I}+\frac{1-\cos \theta}{\theta^2} \theta \boldsymbol{a}^{\wedge}+\frac{1-\frac{\sin \theta}{\theta}}{\theta^2}\left(\theta^2 \boldsymbol{a}^{\wedge} \boldsymbol{a}^{\wedge}\right) \\ & =\boldsymbol{I}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}+\left(1-\frac{\sin \theta}{\theta}\right)\left(\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}\right) \\ = & \boldsymbol{I}+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge}+\boldsymbol{a} \boldsymbol{a}^T-\boldsymbol{I}-\frac{\sin \theta}{\theta} \boldsymbol{a} \boldsymbol{a}^T+\frac{\sin \theta}{\theta} \boldsymbol{I} \\ = & \frac{\sin \theta}{\theta} \boldsymbol{I}+\left(1-\frac{\sin \theta}{\theta}\right) \boldsymbol{a} \boldsymbol{a}^T+\frac{1-\cos \theta}{\theta} \boldsymbol{a}^{\wedge} \end{aligned}$ 至此, 我们求出来的 $\boldsymbol{J}$ 和视觉 SLAM 十四讲中的公式一致了, 也解决了我心中的疑惑。
从公式 $\mathrm{T}=\left[\begin{array}{cc} \exp \left(\boldsymbol{\phi}^{\wedge}\right) & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right]=\left[\begin{array}{cc} \mathbf{R} & \boldsymbol{J} \boldsymbol{u} \\ 0^T & 1 \end{array}\right]$ 可知, 李代数 $\operatorname{se}(3)$ 中由两个三维向量( $\boldsymbol{u}$ 和 $\boldsymbol{\phi}$ )构成， $\boldsymbol{\phi}$ 这个量刚好能对应到 $\mathbf{T}$ 中的 $\mathbf{R}$ , 因此李代数中的 $\boldsymbol{\phi}$ 确实真实反映了旋转, 可是 $\boldsymbol{u}$ 只是部分反映了平移, 需要乘以 $\boldsymbol{J}$ , 才是变换成 $\mathbf{T}$ 后真正的平移量。这一点与二维空间中的se(2)转换到 SE(2) 类似。