视觉SLAM理论到实践系列文章

下面是《视觉SLAM十四讲》学习笔记的系列记录的总链接，本人发表这个系列的文章链接均收录于此

视觉SLAM理论到实践系列文章链接

---

下面是专栏地址：

视觉SLAM理论到实践专栏

---

---

前言

高翔博士的《视觉SLAM十四讲》学习笔记的系列记录

---

视觉SLAM理论到实践系列：补充——泊松公式 $\dot{\mathbf{R}}=\mathbf{R}{\omega }^{\wedge }$ 证明

如上图所示，在三维空间中，圆周运动所在的平面可以任意选取，我们可以将角速度拓展成一个矢量 $\omega$，其方向垂直于该平面并由右手定则确定。令坐标系的原点在圆周运动的轴上，用位矢 $\mathbf{r}$ 表示点 $P$ 的位置，则圆周运动的半径为 $r_{\perp }=r\sin \theta$，其中 $\theta$ 是 $\mathbf{r}$ 与 $\omega$ 的夹角。所以圆周运动速度的大小为 $v=\omega r\sin \theta$。根据矢量叉乘的几何定义，有

$$\mathbf{v}=\omega \times \mathbf{r}$$

线速度与角速度的关系还有如下推导

粒子在坐标系中 $z=h$ 中的平面做圆周运动，圆周运动的半径为 $a$，粒子的坐标为: $\mathbf{r}=(a\cos \theta ,a\sin \theta ,h{)}^T$

坐标对时间 $t$ 求导得：
$$\begin{array}{rl}\dot{\mathbf{r}}& =(-a\dot{\theta }\sin \theta ,a\dot{\theta }\cos \theta ,0{)}^{\top }\\ & =\left[\begin{array}{ccc}0& -\dot{\theta }& 0\\ \dot{\theta }& 0& 0\\ 0& 0& 0\end{array}\right]\left[\begin{array}{c}a\cos \theta \\ a\sin \theta \\ h\end{array}\right]\\ & =\mathbf{\omega }\times \mathbf{r}\end{array}$$
其中 $\omega =\dot{\theta }\mathbf{z}$，表示角速度的向量，方向与$z$ 轴正方向一致，$|\dot{\theta }|$ 是角速度大小，$|\dot{\mathbf{r}}|$ 为线速度大小

对上式取模，得：
$$|\dot{\mathbf{r}}|=|\mathbf{\omega }||\mathbf{r}|\sin \varphi =a|\dot{\theta }|$$
也就是中学学过的：线速度大小=角速度大小$\times$半径

这里有两个坐标系，惯性系（inertial frame），也叫世界坐标系，还有一个是物体坐标系（body frame）

为了计算简便，假设两个坐标系的原点为同一个，即只考虑旋转，不考虑坐标系之间的平移

$\dot{\mathbf{R}}=[\omega {]}_{\times }\mathbf{R}$ 证明

首先看这样一个公式：
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=\omega \times \mathbf{R}$$

第一种证明视角

要证上式就是考虑旋转矩阵按列分块
$$\mathbf{R}=\left[\begin{array}{ccc}\hat{\mathbf{i}}& \hat{\mathbf{j}}& \hat{\mathbf{k}}\end{array}\right]$$
即证明
$$\begin{array}{c}\frac{\mathrm{d}}{\mathrm{d}t}\hat{\mathbf{i}}=\omega \times \hat{\mathbf{i}}\\ \frac{\mathrm{d}}{\mathrm{d}t}\hat{\mathbf{j}}=\omega \times \hat{\mathbf{j}}\\ \frac{\mathrm{d}}{\mathrm{d}t}\hat{\mathbf{k}}=\omega \times \hat{\mathbf{k}}\end{array}$$
我们知道旋转矩阵的每一列都有这样的性质：
$$\Vert \hat{\mathbf{u}}\Vert =1$$
这是因为旋转矩阵是正交矩阵：
$${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$$
所以${\hat{\mathbf{u}}}^T\hat{\mathbf{u}}=1$，于是我们有：
$$\frac{\mathrm{d}}{\mathrm{d}t}{\hat{\mathbf{u}}}^T\hat{\mathbf{u}}=0$$
继而得到：
$$\hat{\mathbf{u}}\cdot \frac{\mathrm{d}}{\mathrm{d}t}\hat{\mathbf{u}}=0$$
这不就说明每一列的 $\hat{u}$ 和其微分垂直吗？其微分可以表示为叉乘的形式（保证垂直）：
$$\hat{\mathbf{u}}\cdot (\omega \times \hat{\mathbf{u}})=0$$
也即我们有：
$$\frac{\mathrm{d}}{\mathrm{d}t}\hat{\mathbf{u}}=\omega \times \hat{\mathbf{u}}$$
于是上式得到了证明。
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=\omega \times \mathbf{R}=[\omega {]}_{\times }\mathbf{R}$$
式中：
$$[\omega {]}_{\times }=\left[\begin{array}{ccc}0& -{\omega }_z& {\omega }_y\\ {\omega }_z& 0& -{\omega }_x\\ -{\omega }_y& {\omega }_x& 0\end{array}\right]$$
$[\omega {]}_{\times }$ 的定义下同。

第二种证明视角

设惯性系（世界坐标系）下的角标为 $W$ 或者 $w$ 或者 $I$，本体坐标系下的角标为 $B$ 或者 $b$，Body系的空间中有一个点坐标为 ${\mathbf{r}}_B=(x_1,y_1,z_1{)}^T$，有下面的关系
$${\mathbf{r}}_w={\mathbf{R}}_{wb}{\mathbf{r}}_b$$
下标 $({)}_{wb}$ 表示本体坐标系到世界坐标系的变换

两边对时间 $t$ 求导我们可以得到：
$${\dot{\mathbf{r}}}_w={\mathbf{v}}_w=\left(\frac{\mathrm{d}}{\mathrm{d}t}{\mathbf{R}}_{wb}\right){\mathbf{r}}_b={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}_b$$
上式中 ${\mathbf{v}}_w$ 是世界坐标系下的速度，等式右边没有 ${\mathbf{r}}_b$ 的导数是因为本体坐标系内观察坐标始终没有变化（这里假设本体坐标系在运动，而坐标不动），导数为0

（若点也在运动，则详见。。。的推导）

于是我们有
$$\begin{array}{rl}{\mathbf{v}}_w& ={\omega }_w\times {\mathbf{r}}_w\\ & =[{\omega }_w{]}_{\times }{\mathbf{R}}_{wb}{\mathbf{r}}_b\end{array}$$
对比可得
$$\begin{array}{r}\frac{\mathrm{d}}{\mathrm{d}t}{\mathbf{R}}_{wb}=[{\omega }_w{]}_{\times }{\mathbf{R}}_{wb}\end{array}$$
即所谓的
$$\dot{\mathbf{R}}=[\omega {]}_{\times }\mathbf{R}$$
注意，这里的 $\omega$ 是在世界坐标系下的表示，即 ${\omega }_w$ ，和下面要将的 ${\omega }_b$ 不同

$\dot{\mathbf{R}}=\mathbf{R}{\omega }^{\wedge }$ 证明

我们经常在论文里看到反过来写的旋转矩阵的导数，这又是为什么呢？
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=\mathbf{R}[\omega {]}_{\times }$$
准确地说，这里的 $[\omega {]}_{\times }$ 应该指的是物体角速度 $[{\omega }_b{]}_{\times }$
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=\mathbf{R}[{\omega }_b{]}_{\times }$$
我们需要搞清楚的是前面所推导和论证的公式准确地说应该描述的是惯性坐标系（世界坐标系）的空间角速度
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=[{\omega }_w{]}_{\times }\mathbf{R}$$
为了方便我们把旋转矩阵的微分写作：
$$\dot{\mathbf{R}}=\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}$$
于是我们便得到：
$$\begin{array}{rl}{[{\omega }_w]}_{\times }& =\dot{\mathbf{R}}{\mathbf{R}}^{-1}\\ & \\ {[{\omega }_b]}_{\times }& ={\mathbf{R}}^{-1}\dot{\mathbf{R}}\end{array}$$
空间角速度在前面（第二种证明视角）我们已经推导过了，下面我们推导一下物体角速度，根据坐标变换关系我们有：
$$\begin{gathered} {\mathbf{v}}_w={\mathbf{R}}_{wb}{\mathbf{v}}_b \\ {\omega }_w={\mathbf{R}}_{wb}{\omega }_b \end{gathered}$$
根据前面的
$${\mathbf{v}}_w={\dot{\mathbf{R}}}_{wb}{\mathbf{r}}_b$$
则有（为了简便则省去 $\mathbf{R}$ 的脚标）：
$${\mathbf{R}}^{-1}{\mathbf{v}}_w={\mathbf{R}}^{-1}\dot{\mathbf{R}}{\mathbf{r}}_b$$
也即：
$${\mathbf{v}}_b={\mathbf{R}}^{-1}\dot{\mathbf{R}}{\mathbf{r}}_b=[{\omega }_b{]}_{\times }{\mathbf{r}}_b$$
于是我们便得到：
$$[{\omega }_b{]}_{\times }={\mathbf{R}}^{-1}\dot{\mathbf{R}}$$
也即证明了：
$$\frac{\mathrm{d}}{\mathrm{d}t}\mathbf{R}=\mathbf{R}[{\omega }_b{]}_{\times }$$

或者写作
$$\begin{array}{rl}\dot{\mathbf{R}}& =\mathbf{R}[{\omega }_b{]}_{\times }\\ & \\ & =\mathbf{R}{{\omega }_b}^{\wedge }\end{array}$$

其他证明方法

高翔书中有介绍

这里证明了
$$\dot{\mathbf{R}}(t)=\mathbf{\varphi }(t_0{)}^{\wedge }\mathbf{R}(t)={\mathbf{\varphi }}_0^{\wedge }\mathbf{R}(t)$$
但书中并未说明 ${\varphi }_0$ 和 $\omega$ 的关系

这里也是一样的思路
$${\mathbf{R}}^T\mathbf{R}=\mathbf{I}$$
对左右两边对时间求导数：
$$\begin{gathered} {\mathbf{R}}^T\dot{\mathbf{R}}+{\dot{\mathbf{R}}}^T\mathbf{R}=0 \\ [\omega {]}_{\times }={\mathbf{R}}^T\dot{\mathbf{R}}\mathrm{and}[\omega {]}_{\times }+[\omega {]}_{\times }^T=0 \\ \dot{\mathbf{R}}=\mathbf{R}[\omega {]}_{\times } \end{gathered}$$

事实上，我也可以对下面的式子做同样的操作
$$\mathbf{R}{\mathbf{R}}^T=\mathbf{I}$$
对左右两边对时间求导数：
$$\begin{gathered} \dot{\mathbf{R}}{\mathbf{R}}^T+\mathbf{R}{\dot{\mathbf{R}}}^T=0 \\ [\omega {]}_{\times }=\dot{\mathbf{R}}{\mathbf{R}}^T\mathrm{and}[\omega {]}_{\times }+[\omega {]}_{\times }^T=0 \\ \dot{\mathbf{R}}=[\omega {]}_{\times }\mathbf{R} \end{gathered}$$
以上两个都证明可以从数学的定义说明旋转矩阵群 $SO(3)$ 的李代数是反对称矩阵。然而这样的证明并没有反映究竟是物体角速度还是空间角速度，实际应用中可能会让读者产生困惑，关于物体角速度和空间角速度和旋转矩阵的关系还是要用前面的推导说明。