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)=R(t_0)exp({\omega }^{\wedge }(t-t_0))$$
此时会发现$\omega$刚好对应为角速度，而这个解描述的就是角速度不变的情况下的运动方程，如果让$t$无限接近$t_0$，也就是利用极限的思想，$\omega$就是瞬时角速度。

离散时间下的运动学模型

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

线性近似形式

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

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

四元数视角下的运动学

描述旋转的四元数具有单位性约束，也就是说必须是单位四元数，所以满足$q{q}^{\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)=q(t_0)exp(\varpi △t)$。

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

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

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

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

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

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

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

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

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

要注意的是，如果使用这种近似形式，更新后的四元数不再是单位四元数，所以长时间更新后要对四元数重新归一化。

参考资料：《自动驾驶与机器人中的SLAM技术》 高翔著