SLAM中的三维运动学【SLAM】

李群视角下的运动学

连续时间下的运动学模型

物体在连续时间下运动的位姿由 $R (t)$ 和 $t (t)$ 表示，根据 $R$ 的正交性质有：
$R(t)^{T}R(t) = I$
公式两边对时间 $t$ 求导有：
$\dot{R(t)}^{T}R(t) + R(t)^{T}\dot{R(t)}= 0$
$R(t)^{T}\dot{R(t)} = -(R(t)^{T}\dot{R(t)})^{T}$
可以发现 $R(t)^{T}\dot{R(t)}$ 是反对称矩阵，不妨取 $\omega(t)^{\wedge} = R(t)^{T}\dot{R(t)}$ ，后面推导会发现 $\omega(t)$ 的意义其实就是瞬时角速度，此时有：
$\dot{R(t)} = R(t)\omega(t)^{\wedge}$
这里本人认为明确标出 $(t)$ 可以更容易理解变量之间的关系，否则很容易产生误解或疑惑。

上面的方程称为泊松方程（Poinsson Formula），这是一个微分方程，但是不要对它直接求解，我们假设 $\omega$ 是固定的值，并给定初值 $t_{0}$ 时刻的旋转矩阵为 $R(t_{0})$ ，此时微分方程有解：
$R(t_{0}) exp(\omega^{\wedge}(t - t_{0}))$
此时会发现 $\omega$ 刚好对应为角速度，而这个解描述的就是角速度不变的情况下的运动方程，如果让 $t$ 无限接近 $t_{0}$ ，也就是利用极限的思想， $\omega$ 就是瞬时角速度。

离散时间下的运动学模型

离散时间下认为瞬时角速度在短时间内不变，也就是 $t_{0}$ 到 $t$ 时刻内不变，于是有：
$R(t_{0}) exp(\omega^{\wedge}(t - t_{0})) = R(t_{0}) exp(\omega^{\wedge}\triangle t)$

线性近似形式

对 $R (t)$ 在 $t_{0}$ 时刻做一阶泰勒展开：
$R(t_{0} + \triangle t) \approx R(t_{0}) + \dot{R(t_{0})}\triangle t = R(t_{0}) +R(t_{0})\omega^{\wedge} \triangle t = R(t_{0})(I + \omega^{\wedge}\triangle t)$

离散形式和线性近似形式是更常用的处理形式。

四元数视角下的运动学

描述旋转的四元数具有单位性约束，也就是说必须是单位四元数，所以满足 $qq^{\ast} = q^{\ast}q =1$ 。

$q^{\ast}q =1$ 两侧对时间求导有：
$\dot{q^{\ast}}q + q^{\ast}\dot{q} =0$
$q^{\ast}\dot{q} = -\dot{q^{\ast}}q = -(q^{\ast}\dot{q})^{\ast}$
可以发现 $q^{\ast}\dot{q}$ 是一个纯虚四元数，不妨令其为 $\varpi$ ，后面会发现 $\varpi$ 和李群下的 $\omega$ 有关系，准确的说，虚部为 $\frac{1}{2} \omega$ ，也体现了瞬时角速度的意义。

于是有：
$q^{\ast}\dot{q} = \varpi$
$\dot{q} = q\varpi$

同样假设 $\varpi$ 为固定值，上面的微分方程的解为 $q(t_{0})exp(\varpi \triangle t)$ 。

这里要说明的是这里的指数映射为纯虚四元数的指数映射，结果为单位四元数，具体的定义为：
$exp(\varpi) = exp(\theta \mathbf{u}) = cos\theta + \mathbf{u}sin \theta$

不妨把纯虚四元数 $\varpi$ 看成四元数形式的李代数。

四元数视角下的运动学与李群视角下运动学的关系

下面要介绍的是上面两中运动学表达之间的关系，我们的关注点在于如何把两种表达的物理意义统一在一个描述下。

假设有一个旋转矩阵 $R$ 和对应的旋转向量 $\phi = \theta\mathbf{n}$ ，对应的四元数为 $q=exp(\varpi)$ 。

根据四元数与旋转向量之间的转换关系可知 $[cos\frac{\theta}{2}, \mathbf{n}^{T}sin\frac{\theta}{2}]^{T}$ ，所以有 $\varpi = [0, \frac{1}{2}\phi^{T}]^{T}$ 。

所以四元数视角下的“角速度” $\varpi$ 其实是瞬时角速度的一半。

为了统一物理意义，我们让 $\omega$ 仍然代表瞬时角速度，此时四元数视角下的运动学方程为：
$\dot{q} = \frac{1}{2}q[0, \omega^{T}]^{T}$

假设某一时刻 $t_{0}$ 的瞬时角速度为 $\omega = \phi = \theta \mathbf{n}$ ，由上一部分的四元数微分方程的解 $q(t_{0})exp(\varpi \triangle t)$ 可以看出离散形式下的四元数更新方式：
$q(t_{0})exp(\varpi \triangle t) = q(t_{0})exp([0, \frac{1}{2}\phi^{T}]^{T} \triangle t)$
因为 $\frac{1}{2}\phi^{T}]^{T}) = [cos\frac{\theta}{2}, \mathbf{n}^{T}sin\frac{\theta}{2}]^{T}$ ，当 $\omega$ 很小时，就可以近似，于是四元数的更新近似为：
$q(t_{0})[1, \frac{1}{2}\omega^{T}]^{T}$