SLAM 中关于姿态导数的计算

在学习SLAM的过程中，不可避免地会遇到需要对姿态求导的场景。姿态分别有：旋转向量、旋转矩阵、欧拉角、四元数四种表示方法。旋转矩阵适合对坐标进行坐标变换，即将某一个坐标系下的坐标转换至另一个坐标系；欧拉角适合直观的描述载体当前的姿态，便于人们感受姿态；四元数适合更新姿态，只要我们记着及时将四元数归一化；而旋转向量更适合在求导中使用，下面我将试图说清楚旋转向量在求导中的使用。讲清楚此部分内容可能需要较多角标符号，所以请大家仔细分辨这些符号，可能我自己都分辨不清楚。由于本人能力有限，难免会有出错，请大家多多指正。

符号约定

为避免符号约定的歧义，在此说明，在本文中：

对于旋转矩阵 $R_b^w$，其表示将坐标系b中的坐标旋转至坐标系 w 中的坐标变换矩阵，即若存在一个 b 系下的坐标 $p^b={\left[x^b{y}^b{z}^b\right]}^T$，那么有 $p^w=R_b^w{p}^b$。这里的 b 系，我们通常认为是载体系，w 系通常认为是世界系。

四元数 $q_w^b$ 在表示坐标变换时，与 $R_b^w$ 表达的含义相反，但与 $R_w^b$ 的含义相同，四元数具体的坐标转换方式见SLAM14 讲第二版3.4.3 节。我习惯称 $q_w^b/R_w^b$ 形式的姿态为 w 系在 b 系下的姿态。

旋转向量 ${\varphi }_{bw}^b$，令其为 ${\varphi }_{bw}^b=\theta a^b$，θ 为标量表示旋转向量转过的角度，$a^b$ 是 3 维单位向量，位于 b 系。那么 ${\varphi }_{bw}^b$ 的完整含义为：按右手定则，让 b 系绕 $a^b$ 轴转动 θ 角度，即可让 b 系三轴与 w 系三轴重合，这里说的重合，是指 b 系的 x、y、z 轴依次与 w 系的 x、y、z 轴重合。

各姿态表示之间的一些必要转换

首先对于旋转向量有：

$$-{\varphi }_{bw}^b={\varphi }_{wb}^w$$

如果 $R_{3\times 3}(\varphi )$ 表示用罗德里格斯公式将旋转向量转化为旋转矩阵，那么有下式：

$$R_{3\times 3}\left({\varphi }_{bw}^b\right)=\cos (\theta )I+\left(1-\cos (\theta )\right)a^b{\left(a^b\right)}^T+\sin \theta \left(a^b\times \right)$$

$R_{3\times 3}({\varphi }_{bw}^b)$ 表示的其实是$R_w^b$，$R_{3\times 3}(-{\varphi }_{bw}^b)$ 表示 $R_b^w$。当 θ 为小量时：

$$R_w^b\approx I-{\varphi }_{wb}^w\times =I+{\varphi }_{bw}^b\times$$

$$R_b^w\approx I+{\varphi }_{wb}^w\times =I-{\varphi }_{bw}^b\times$$

“×”符号在这里表示取左侧向量的反对称阵。
对于 $q_w^b$ 与 ${\varphi }_{bw}^b$，我们有如下转化关系：

$$q_w^b=\left[\begin{array}{c}\cos \left(\frac{|{\varphi }_{bw}^b|}{2}\right)\\ \frac{{\varphi }_{bw}^b}{|{\varphi }_{bw}^b|}\sin \left(\frac{|{\varphi }_{bw}^b|}{2}\right)\end{array}\right]=\left[\begin{array}{c}\cos \left(\frac{|-{\varphi }_{wb}^w|}{2}\right)\\ \frac{-{\varphi }_{wb}^w}{|-{\varphi }_{wb}^w|}\sin \left(\frac{|-{\varphi }_{wb}^w|}{2}\right)\end{array}\right]$$

当 θ 为小量时，有：

$$q_w^b\approx \left[\begin{array}{c}1\\ \frac{{\varphi }_{bw}^b}{2}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{-{\varphi }_{wb}^w}{2}\end{array}\right]$$

我习惯称 ${\varphi }_{bw}^b$ 对应的旋转向量和四元数为：$R_w^b$ 和 $q_w^b$，那么 ${\varphi }_{wb}^w$ 就与 $R_b^w$ 和 $q_b^w$ 对应。

姿态如何累积

各种姿态的表示方法都不满足普通的加法，我们需要使用以下方法将姿态累积起来：

$$R_b^w=R_{w1}^w{R}_b^{w1}$$

$$q_b^w=q_{w1}^w\otimes q_b^{w1}$$

如上，分别以旋转矩阵和四元数的形式将 b 系在 $w1$ 系的姿态与 $w1$ 系在 w 系的姿态叠加起来。按照之前的习惯，$R_{w1}^w$ 对应的旋转向量为：${\varphi }_{ww1}^w$。如果 ${\varphi }_{ww1}^w$ 的模值是小量，那么此时就有：

$$R_{w1}^w\approx I+{\varphi }_{ww1}^w\times$$

姿态的导数

我们设定一个关于姿态的函数：

$$g\left({\varphi }_{w1b}^{w1}\right)=R_b^{w1}{p}^b-p^w$$

该函数表示将坐标 $p^b$ 旋转至 $w1$ 系，然后与坐标 $p^w$ 相减，$p^w$ 表示点 p 在世界系下的真实坐标。我们选择将 ${\varphi }_{w1b}^{w1}$ 作为函数的自变量，这是因为我们后面要对旋转向量求导。那么世界系 w 为什么要添加符号1 呢？这是因为在估计问题中，我们拿到的状态并不准确，$R_b^w$ 表示真值，但是我们只能获取到有误差的姿态，这个误差如何体现出来呢？$R_b^w$ 表示将坐标系 b 中的坐标旋转至坐标系 w 中，但是如果有误差了，那么我们拿到的姿态可能就表示将坐标系b 中的坐标旋转至坐标系 $w1$ 中，于是就有了 $R_b^{w1}$，所以符号1 表达了我们对当前所获得世界系的不信任，也就是误差。关于这个1 为何加在 w 上，不是b 上，我们后面解释。

下面解释为什么要对 $g({\varphi }_{w1b}^{w1})$ 求导，我们注意到 $g({\varphi }_{w1b}^{w1})$ 实质为两个坐标的相减，但因为 $R_b^{w1}$ 不是 $R_b^w$，所以 $R_b^{w1}{p}^b$ 与 $p^w$ 相减后得到的很可能是非零向量。如果有一种方法能让我们调整 $R_b^{w1}$ 中的 $w1$ 系，使其不断靠近 w 系，那我们就逐渐估计出了 $R_b^w$。一种可行的、估计 $R_b^w$ 的方法就是高斯牛顿法(具体介绍见SLAM 十四讲6.2.2 节)。在我们这个问题中，高斯牛顿法是在做这样一件事：在 ${\varphi }_{w1b}^{w1}$ 的基础上叠加一个微弱转动，叠加微弱转动后的旋转向量所对应的旋转矩阵可以让 $g(\cdot )$ 函数更接近0向量。因为每次添加微弱转动都可以让 $g(\cdot )$ 函数更靠近0向量，所以不断重复上述过程，那么就会估计出较为理想的旋转向量，当然，对应的旋转矩阵也会得到。

下面介绍如何在 ${\varphi }_{w1b}^{w1}$ 上叠加微弱转动以及 $g({\varphi }_{w1b}^{w1})$ 的导数如何计算。

对于函数 $f(x)$，其在 x 处的导数为：

$$f^{\prime }(x)=\underset{\delta x\to 0}{\lim }\frac{f(x\oplus \delta x)-f(x)}{\delta x}$$

“⊕”表示广义的加法，因为姿态不满足普通加法，所以我们这里用它泛指一种叠加行为。$\delta x$ 表示在 x 基础上的一个轻微扰动，那么${\varphi }_{w1b}^{w1}$ 的轻微扰动是什么？我们知道 ${\varphi }_{w1b}^{w1}$ 的含义是：让 $w1$ 系绕某个轴转动某个角度，使得 $w1$ 系三轴与 b 系三轴重合。那么这个小扰动就可以是先让 $w2$ 系绕某个轴转动某个很小的角度，使得 $w2$ 系三轴与 $w1$ 系三轴重合，这个小扰动对应的符号就是 ${\varphi }_{w2w1}^{w2}$。而 ${\varphi }_{w2w1}^{w2}$ 对应的旋转矩阵为 $R_{w1}^{w2}$。于是

$$\frac{\partial g\left({\varphi }_{w1b}^{w1}\right)}{\partial {\varphi }_{w1b}^{w1}}=\underset{{\varphi }_{w2w1}^{w2}\to 0}{\lim }\frac{g\left({\varphi }_{w1b}^{w1}\oplus {\varphi }_{w2w1}^{w2}\right)-g\left({\varphi }_{w1b}^{w1}\right)}{{\varphi }_{w2w1}^{w2}}$$

那么 $g({\varphi }_{w1b}^{w1}\oplus {\varphi }_{w2w1}^{w2})$ 等于什么呢？${\varphi }_{w1b}^{w1}\oplus {\varphi }_{w2w1}^{w2}$ 从物理含义上来说是：让 $w2$ 系绕某个轴转动某个角度，使得 $w2$ 系三轴与 b 系三轴重合，即旋转向量 ${\varphi }_{w2b}^{w2}$，而 ${\varphi }_{w2b}^{w2}$ 对应的旋转矩阵是 $R_b^{w2}$，又因为有 $R_b^{w2}=R_{w1}^{w2}{R}_b^{w1}$，所以有：

$$g\left({\varphi }_{w1b}^{w1}\oplus {\varphi }_{w2w1}^{w2}\right)=g\left({\varphi }_{w2b}^{w2}\right)=R_b^{w2}{p}^b-p^w=R_{w1}^{w2}{R}_b^{w1}{p}^b-p^w=\left[I+{\varphi }_{w2w1}^{w2}\times \right]R_b^{w1}{p}^b-p^w$$

于是有：

$$\frac{\partial g\left({\varphi }_{w1b}^{w1}\right)}{\partial {\varphi }_{w1b}^{w1}}=\underset{{\varphi }_{w2w1}^{w2}\to 0}{\lim }\frac{\left[I+{\varphi }_{w2w1}^{w2}\times \right]R_b^{w1}{p}^b-R_b^{w1}{p}^b}{{\varphi }_{w2w1}^{w2}}$$

$$=\underset{{\varphi }_{w2w1}^{w2}\to 0}{\lim }\frac{\left[{\varphi }_{w2w1}^{w2}\times \right]R_b^{w1}{p}^b}{{\varphi }_{w2w1}^{w2}}=\underset{{\varphi }_{w2w1}^{w2}\to 0}{\lim }-\frac{\left[\left(R_b^{w1}{p}^b\right)\times \right]{\varphi }_{w2w1}^{w2}}{{\varphi }_{w2w1}^{w2}}=-\left(R_b^{w1}{p}^b\right)\times$$

请注意，上面在 ${\varphi }_{w1b}^{w1}$ 上添加扰动${\varphi }_{w2w1}^{w2}$是为了按照导数的定义计算 $\frac{\partial g({\varphi }_{w1b}^{w1})}{\partial {\varphi }_{w1b}^{w1}}$，扰动 ${\varphi }_{w2w1}^{w2}$ 并不是高斯牛顿法中待求的扰动，且${\varphi }_{w2w1}^{w2}$ 在此处是一个趋于无穷小的量，这些注意事项我们最好都留意一下。上述内容解释了扰动如何加在姿态上，下面我将提到高斯牛顿法中的“扰动”。

我们假设在使用高斯牛顿法时，在 ${\varphi }_{w1b}^{w1}$ 基础上添加的“扰动”(扰动这个词不太好，此处更准确的说法应该是调整量)为 ${\varphi }_{w3w1}^{w3}$，${\varphi }_{w3w1}^{w3}$ 是一个有限小的量，这是与 ${\varphi }_{w2w1}^{w2}$ 的区别，但是他们表达的都是从其他系向 $w1$ 系转动的旋转向量。则有：

$$g\left({\varphi }_{w1b}^{w1}\oplus {\varphi }_{w3w1}^{w3}\right)\approx g\left({\varphi }_{w1b}^{w1}\right)+\frac{\partial g\left({\varphi }_{w1b}^{w1}\right)}{\partial {\varphi }_{w1b}^{w1}}{\varphi }_{w3w1}^{w3}$$

参考14 讲，${\varphi }_{w3w1}^{w3}$ 便可求解出来。那么 ${\varphi }_{w3w1}^{w3}$ 如何更新在 ${\varphi }_{w1b}^{w1}$ 上呢？直接令 ${\varphi }_{w1b}^{w1}={\varphi }_{w1b}^{w1}\oplus {\varphi }_{w3w1}^{w3}$ 就可以，两个旋转向量可以直接合并（我实际中很少见两个旋转向量直接合并），实际中可能更多是用以下方法更新姿态， ${\varphi }_{w1b}^{w1}$ 对应的四元数为$q_b^{w1}$，${\varphi }_{w3w1}^{w3}$ 对应的四元数为 $q_{w1}^{w3}$，所以利用 $q_b^{w3}=q_{w1}^{w3}\otimes q_b^{w1}$ 更新姿态也是可以的。

备注

(1)为什么使用 ${\varphi }_{w2w1}^{w2}$、${\varphi }_{w3w1}^{w3}$？使用 ${\varphi }_{w1w2}^{w1}$、${\varphi }_{w1w3}^{w1}$ 可以吗？当然可以，只不过需要注意两件事：
①

$$R_{w1}^{w2}\approx I-{\varphi }_{w1w2}^{w1}\times$$

$$R_{w1}^{w3}\approx I-{\varphi }_{w1w3}^{w1}\times$$
②

${\varphi }_{w2w1}^{w2}$、${\varphi }_{w3w1}^{w3}$ 是同一组的，${\varphi }_{w1w2}^{w1}$、${\varphi }_{w1w3}^{w1}$ 是同一组的，不要混搭，例如：${\varphi }_{w2w1}^{w2}$，${\varphi }_{w1w3}^{w1}$ 是不建议的。

(2)1 为何加在 w 上，不是 b 上？因为我们是在对当前的世界系调整，使其靠近真正的世界系，如果我们设定拿到的旋转矩阵是 $R_{b1}^w$，就代表我们认为 b 系是不准的，需要不断调整 b 系，那么此时的旋转矩阵的调整量就应该表示为 $R_{b3}^{b1}$，计算导数和利用高斯牛顿法时，对 $R_{b1}^w$ 施加扰动(调整)的方式就变为：$R_{b2}^w=R_{b1}^w{R}_{b2}^{b1}$，$R_{b3}^w=R_{b1}^w{R}_{b3}^{b1}$，对应的旋转向量也将变为：${\varphi }_{b2b1}^{b2}$、${\varphi }_{b3b1}^{b3}$ 或 ${\varphi }_{b1b2}^{b1}$、${\varphi }_{b1b3}^{b1}$。

(3)为什么选择 $R_b^{w1}$，不选择 $R_{b1}^w$，这是我随意假设的。看源码时，往往是作者怎么假设我就怎么看。

(4)$g({\varphi }_{w1b}^{w1})$ 这个残差函数很明显是无法约束住姿态的，即无法有效优化 $R_b^{w1}$，但我们的目的是讲解姿态的导数如何计算。