VIO(notes) —— （1）IMU模型

一、概念介绍

1. VIO：Viausl-Inertial-Odometry

以视觉与IMU融合实现里程计

2. IMU: Inertial-Measurement-Unit，惯性测量单元

典型6轴IMU以较高频率（>100Hz）返回被测物体的角速度与加速度 受自身温度、零偏、振动等因素干扰，积分得到的平移和旋转容易漂移； 温度和振动的干扰可以标定。

3. VO: Visual Odometry

以图像形式记录数据，频率较低(15Hz-60Hz)居多 通过图像特征点或像素推断相机运动

二、IMU

六自由度 IMU 本身由一个陀螺仪和一个加速度计组成,分别测量自身的角速度和加速度。
IMU 与视觉定位方案优势与劣势对比:

方案	IMU	视觉
优势	快速响应不受成像质量影响角速度普遍比较准确可估计绝对尺度	不产生漂移直接测量旋转与平移
劣势	存在零偏低精度 IMU 积分位姿发散高精度价格昂贵	受图像遮挡、运动物体干扰单目视觉无法测量尺度单目纯旋转运动无法估计快速运动时易丢失

整体上，视觉和IMU定位方案存在一定互补性质：
可利用视觉定位信息来估计IMU的零偏，减少IMU由零偏导致的发散和累积误差；反之，IMU可以为视觉提供快速运动时的定位。
IMU适合计算短时间、快速的运动
视觉适合计算长时间、慢速的运动

内积（ $\cdot$ 乘）

$$\mathbf{a}\cdot \mathbf{b}={\mathbf{a}}^{\mathrm{T}}\mathbf{b}=\sum _{i=1}^3{a}_i{b}_i=|\mathbf{a}||\mathbf{b}|\cos ⟨\mathbf{a},\mathbf{b}⟩.$$
其中 $⟨\mathbf{a},\mathbf{b}⟩$ 指向量 $\mathbf{a},\mathbf{b}$ 的夹角。结果为标量，其大小符合平行四边形法则，内积也可以描述向量间的投影关系。

外积（ $\times$ 乘)

而外积则是这个样子:

$$\mathbf{a}\times \mathbf{b}=\Vert \begin{array}{ccc}{\mathbf{e}}_1& {\mathbf{e}}_2& {\mathbf{e}}_3\\ a_1& a_2& a_3\\ b_1& b_2& b_3\end{array}\Vert =\left[\begin{array}{l}{a}_2{b}_3-a_3{b}_2\\ a_3{b}_1-a_1{b}_3\\ a_1{b}_2-a_2{b}_1\end{array}\right]=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]\mathbf{b}\stackrel{\text{ def }}{=}{\mathbf{a}}^{\wedge }\mathbf{b}.$$
外积的结果是一个向量, 它的方向垂直于这两个向量, 大小为 $|\mathbf{a}||\mathbf{b}|\sin ⟨\mathbf{a},\mathbf{b}⟩$, 是两个向量张成的四边形的有向面积。对于外积运算, 我们引入 $\wedge$ 符号, 把 $\mathbf{a}$ 写成一个矩阵。事实上是一个反对称矩阵 (Skew-symmetric Matrix ) , 你可以将 ${}^{\wedge }$ 记成一个反对称符号。这样就把外积$\mathbf{a}\times \mathbf{b}$ 写成了矩阵与向量的乘法 ${\mathbf{a}}^{\wedge }\mathbf{b}$, 把它变成了线性运算。这个符号将在后文经常用到, 请记住它, 并且此符号是一个一 一映射, 意味着任意向量都对应着唯一的一个反对称矩阵, 反之亦然:
$${\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}0& -a_3& a_2\\ a_3& 0& -a_1\\ -a_2& a_1& 0\end{array}\right]$$
${\mathbf{a}}^{\wedge }$为向量 a 的反对称矩阵 A ；（任意向量都对应着唯一的一个反对称矩阵）

${\mathbf{a}}^{\wedge }=A$ ; $A^{\vee }=a$ ; $A^T=-A$

同时, 需要提醒读者的是, 向量和加减法、内外积, 即使在不谈论它们的坐标时也可以计算。 例如, 虽然内积在有坐标时, 可以用两个向量的分量乘积之和表达, 但是即使不知道它们的坐标, 也可以通过长度和夹角来计算二者的内积。所以两个向量的内积结果和坐标系的选取是无关的。

三、四元数的微分函数

三维世界中的刚体运动描述方式主要包括：旋转向量；旋转矩阵；欧拉角；四元数等
IMU中最常用的是：旋转矩阵；四元数。

1.四元数

  四元数 q 有一个实部和三个虚部。四元数与轴角的表示很接近，也是使用一个3维向量表示转轴和一个角度分量表示绕此转轴的旋转角度我们把实部写在前：$q=[q_0,q_1,q_2,q_3{]}^{\top }$ 或 $q=[w,x,y,z{]}^{\top }$

其中 $q_0$ 为实部，$[q_1,q_2,q_3{]}^{\top }$ 为虚部。因为实部为标量，虚部为矢量，所以也可记为：
$$\mathbf{q}=[s,v{]}^{\top }$$
其中 s 为标量，v 为虚部的矢量。

四元数乘法

$$\begin{array}{c}{\mathbf{q}}_a\otimes {\mathbf{q}}_b=w_a{w}_b-x_a{x}_b-y_a{y}_b-z_a{z}_b\\ +\left(w_a{x}_b+x_a{w}_b+y_a{z}_b-z_a{y}_b\right)i\\ +\left(w_a{y}_b-x_a{z}_b+y_a{w}_b+z_a{x}_b\right)j\\ +\left(w_a{z}_b+x_a{y}_b-y_a{x}_b+z_a{w}_b\right)k\\ {\mathbf{q}}_a\otimes {\mathbf{q}}_b={\left[s_a{s}_b-{\mathbf{v}}_a^{\top }{\mathbf{v}}_b,s_a{\mathbf{v}}_b+s_b{\mathbf{v}}_a+{\mathbf{v}}_a\times {\mathbf{v}}_b\right]}^{\top }\end{array}$$
此外，四元数可类似复数，定义加减、模长、逆、共轭等运算，不一一展开。

2.四元数的微分函数(方法一)：

假设某个旋转运动的旋转轴为单位向量 $\mathrm{u}$ ，绕该轴旋转的角度为 $\theta$ ，那么它对应的单位四元数为:
$$q=\left[\begin{array}{c}\cos \frac{\theta }{2}\\ \mathrm{u}\sin \frac{\theta }{2}\end{array}\right]$$
当旋转一段微小的时间，即旋转角度趋于零时，容易有：
$$\Delta \mathbf{q}=\left[\begin{array}{c}\cos \frac{\delta \theta }{2}\\ \mathbf{u}\sin \frac{\delta \theta }{2}\end{array}\right]\approx \left[\begin{array}{c}1\\ \mathbf{u}\frac{\delta \theta }{2}\end{array}\right]=\left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\delta }\mathbf{\theta }\end{array}\right]$$
其中 $\delta \theta$ 的方向表示旋转轴，模长表示旋转角度。

角速度：当旋转一段微小的时间，即旋转角度趋于零时，容易有：

$$\omega =\underset{\Delta \to 0}{\lim }\frac{\delta \theta }{\Delta t}$$
则四元数的导数为：

$$\begin{array}{rl}\dot{\mathbf{q}}& \triangleq \underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}(t+\Delta t)-\mathbf{q}(t)}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}-\mathbf{q}}{\Delta t}\\ & =\underset{\Delta t\to 0}{\lim }\frac{\mathbf{q}\otimes \left(\left[\begin{array}{c}1\\ \frac{1}{2}\delta \mathbf{\theta }\end{array}\right]-\left[\begin{array}{l}1\\ 0\end{array}\right]\right)}{\Delta t}\\ & =\mathbf{q}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\mathbf{\omega }\end{array}\right]\end{array}$$

3.四元数的微分函数(方法二)：

1、单位四元数可表达任意三维旋转，且无奇异性。
四元数和角轴的转换关系: 设角轴为 $\omega$ 和 $\theta$ ，那么它对应的四元数为:
$$\mathbf{q}={\left[\cos \frac{\theta }{2},\omega \sin \frac{\theta }{2}\right]}^{\mathbf{T}}$$
2、四元数时间导数
设初始旋转为 $q=[\mathrm{s},\mathrm{v}]$ ，然后，发生了角轴为 $\omega$ 和 $\theta$ 的旋转 (右乘，对应四元数记作 $\Delta q$ ），那么 $q$ 相对该旋转的导数为:
$$\begin{array}{rl}\underset{\theta \to 0}{\lim }\frac{\mathbf{q}\otimes \Delta \mathbf{q}-\mathbf{q}}{\theta }& =\underset{\theta \to 0}{\lim }\frac{{\left[s\cos \frac{\theta }{2}-{\mathbf{v}}^T\mathbf{\omega }\sin \frac{\theta }{2},s\mathbf{\omega }\sin \frac{\theta }{2}+\cos \frac{\theta }{2}\mathbf{v}+\mathbf{v}\times \mathbf{\omega }\sin \frac{\theta }{2}\right]}^{\mathrm{T}}-\mathbf{q}}{\theta }\\ & =\underset{\theta \to 0}{\lim }\frac{{\left[s\left(\cos \frac{\theta }{2}-1\right)-{\mathbf{v}}^T\mathbf{\omega }\sin \frac{\theta }{2},s\mathbf{\omega }\sin \frac{\theta }{2}+\left(\cos \frac{\theta }{2}-1\right)\mathbf{v}+\mathbf{v}\times \mathbf{\omega }\sin \frac{\theta }{2}\right]}^{\mathrm{T}}}{\theta }\\ & ={\left[-\frac{1}{2}{\mathbf{v}}^T\mathbf{\omega },\frac{1}{2}s\mathbf{\omega }+\frac{1}{2}\mathbf{V}\times \mathbf{\omega }\right]}^{\mathrm{T}}\\ & =\mathbf{q}\otimes {\left[0,\frac{1}{2}\mathbf{\omega }\right]}^{\mathrm{T}}\end{array}$$
即四元数表示旋转时对时间的导数为:
$$\dot{\mathbf{q}}=\mathbf{q}\otimes {\left[0,\frac{1}{2}\mathbf{\omega }\right]}^{\mathrm{T}}$$

4.为什么（Why？）

为啥非要对四元数求微分泥？
  由于实际工程中我们都是通过固连在机体上的陀螺仪等传感器来获知机体角速度$w^b$
它与地理坐标系下的角速度表示有如下关系
$$w=q(t)w^{b}q(t{)}^{-1}$$
带入上式即可得到姿态解具过程中常用的四元数的微分形式
$$\dot{q}(t)=\frac{1}{2}q(t)w^b$$
可以看出，通过一次四元数乘法运算便可得到四元数的微分。
上式可以写成如下的矩阵形式:
$$\left[\begin{array}{l}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}{q}_0& -q_1& -q_2& -q_3\\ q_1& q_0& -q_3& q_2\\ q_2& q_3& q_0& -q_1\\ q_3& -q_2& q_1& q_0\end{array}\right]\left[\begin{array}{c}0\\ {\omega }_{\xi }^b\\ {\omega }_{\eta }^b\\ {\omega }_z^b\end{array}\right]$$
或者：
$$\left[\begin{array}{c}{\dot{q}}_0\\ {\dot{q}}_1\\ {\dot{q}}_2\\ {\dot{q}}_3\end{array}\right]=\frac{1}{2}\left[\begin{array}{cccc}0& -{\omega }_x^b& -{\omega }_y^b& -{\omega }_z^b\\ {\omega }_z^b& 0& {\omega }_z^b& -{\omega }_y^b\\ {\omega }_y^b& -{\omega }_z^b& 0& {\omega }_z^b\\ {\omega }_z^b& {\omega }_y^b& -{\omega }_z^b& 0\end{array}\right]\left[\begin{array}{l}{q}_0\\ q_1\\ q_2\\ q_3\end{array}\right]$$

四、旋转矩阵 $\mathrm{R}$ 的微分函数

相机运动过程时，旋转矩阵可以用 $R(t)，R$ 关于时间 $t$ 的函数表示
对于旋转矩阵
$$R{R}^T=I$$
于是有
$$R(t)R(t{)}^T=I$$
等式两边对时间求, 得
$$\begin{array}{c}\dot{R}(t)R(t{)}^T+R(t)\dot{R}(t{)}^T=0\\ \Rightarrow \dot{R}(t)R(t{)}^T=-R(t)\dot{R}(t{)}^T\\ \Rightarrow \dot{R}(t)R(t{)}^T=-{\left(\dot{R}(t)R(t{)}^T\right)}^T\end{array}$$
说明 $R(t)R(t{)}^T$ 为反对称矩阵，存在一个三维向量 $\varphi (t)$ 与之对应，有
$$\dot{R}(t)R(t{)}^T=\varphi (t{)}^{\wedge }$$
等式两边右乘 $R(t)$ ，得:
$$\dot{R}(t)=\varphi (t{)}^{\wedge }R(t)$$
总结:

1. 说明对旋转矩阵求导，等价于在旋转矩阵左乘一个反对称矩阵；

$$问题由对\dot{R}(t)\Rightarrow 求\varphi (t)$$

如何求得这个反对称矩阵呢?

对于 $t_0$ 时刻， $R(0)=I$, 把 $R(t)$ 在 $t=0$ 附近进行一阶泰勒展开:
$$\begin{array}{c}R(t)\approx R\left(t_0\right)+\dot{R}\left(t_0\right)\left(t-t_0\right)=R\left(t_0\right)+\varphi {\left(t_0\right)}^{\wedge }R\left(t_0\right)\left(t-t_0\right)\\ =I+\varphi {\left(t_0\right)}^{\wedge }t\end{array}$$
此为一个关于 $R$ 的微分方程，且 $R(0)=I$ ，便可解得:
$$R(t)=\exp \left({\varphi }_0^{\wedge }t\right)$$
3. 说明在 $t=0$ 的附近，旋转矩阵可以由 $\exp \left({\varphi }_{0}t\right)$ 解出。
给定某个时刻的 $R$, 我们可以求出一个 $\varphi$，描述了 $R$ 在局部的导数关系；
$\varphi$ 是 $SO(3)$ 对应的李代数 $so(3)$上的指数映射；
$$R=\exp \left({\varphi }^{\wedge }\right)$$
4. 实际使用过程中也可以写作下列形式：
使用旋转矩阵 $\mathrm{R}$ 时，角速度为 $\omega$ ，那么 $\mathrm{R}$ 相对于时间的导数可写作:
$$\dot{\mathrm{R}}=\mathrm{R}{\omega }^{\wedge }$$
其中
$${\mathbf{\omega }}^{\wedge }=\left[\begin{array}{ccc}0& -{\omega }_3& {\omega }_2\\ {\omega }_3& 0& -{\omega }_1\\ -{\omega }_2& {\omega }_1& 0\end{array}\right]$$
实际当中左右乘等价，仅为习惯上的差别。
注：(i) 不同的 R 函数，具体的导数形式也不同。
(ii) 在程序中，不必区分 R 是以矩阵存储或是以四元数存储，只需按照该式更新即可。

五、李群与李代数

  前面介绍了三维世界中刚体运动的描述方式，包括旋转矩阵、旋转向量、欧拉角、四元数等若干种方式。重点介绍了旋转的表示，但是在SLAM中，除了表示，还要对它们进行估计和优化。因为在SLAM中位姿是未知的，而我们需要解决形如“什么样的相机位姿最符合当前观测数据”这样的问题。一种典型的方式是把它构建成一个优化问题，求解最优的R,t,使得误差最小化。如前所言，旋转矩阵自身是带有约束的(正交且行列式为1 )。它们作为优化变量时，会引人入额外的约束，使优化变得困难。通过李群一李 代数间的转换关系，我们希望把位姿估计变成无约束的优化问题，简化求解方式。

1. 李群

三维旋转矩阵构成了特殊正交群 $\mathrm{S}\mathrm{O}(3)$, 而变换矩阵构成了特殊欧氏群 $\mathrm{S}\mathrm{E}(3)$ 。
$$\begin{array}{rl}& \mathrm{S}\mathrm{O}(3)=\left\{\mathbf{R}\in {\mathbb{R}}^{3\times 3}\mid \mathbf{R}{\mathbf{R}}^{\mathrm{T}}=\mathbf{I},\det (\mathbf{R})=1\right\}\\ & \mathrm{S}\mathrm{E}(3)=\left\{\mathbf{T}=\left[\begin{array}{ll}\mathbf{R}& \mathbf{t}\\ 0^{\mathrm{T}}& 1\end{array}\right]\in {\mathbb{R}}^{4\times 4}\mid \mathbf{R}\in \mathrm{S}\mathrm{O}(3),\mathbf{t}\in {\mathbb{R}}^3\right\}\end{array}$$

1. 它们对加法是不封闭的。换句话说, 对于任意两个旋转矩阵 ${\mathbf{R}}_1,{\mathbf{R}}_2$, 按照矩阵加法的定义, 和不再是一个旋转矩阵:
   $${\mathbf{R}}_1+{\mathbf{R}}_2\notin \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1+{\mathbf{T}}_2\notin \mathrm{S}\mathrm{E}(3).$$
   通常矩阵加法对这两个集合不封闭。
2. 它们只有一种较好的运算: 乘法。 $\mathrm{S}\mathrm{O}(3)$ 和 $\mathrm{S}\mathrm{E}(3)$ 关于乘法是封闭的:
   $${\mathbf{R}}_1{\mathbf{R}}_2\in \mathrm{S}\mathrm{O}(3),{\mathbf{T}}_1{\mathbf{T}}_2\in \mathrm{S}\mathrm{E}(3).$$
   也可以对任何一个旋转或变换矩阵（在乘法的意义上）求逆。我们知道, 乘法对应着旋转或变换的复合, 两个旋转矩阵相乘表示做了两次旋转。对于这种只有一个 (良好的) 运算的 集合, 我们称之为群。

2. 李代数

每个李群都有与之对应的李代数。李代数描述了李群的局部性质,准确地说，是单位元附近的正切空间。之前提到的$\varphi (t)$，事实上是一种李代数。
$\varphi$ 是 $SO(3)$ 对应的李代数 $so(3)$上的指数映射；
$$R=\exp \left({\varphi }^{\wedge }\right)$$
现在来考虑第一个问题：如何计算 $\exp \left({\varphi }^{\wedge }\right)?$
任意矩阵的指数映射可以写成一个泰勒展开, 但是只有在收敛的情况下才会有结果, 其结果仍是一个矩阵:
$$\exp (\mathbf{A})=\sum _{n=0}^{\infty }\frac{1}{n!}{\mathbf{A}}^n$$
同样地, 对 $\mathfrak{s}\mathfrak{o}(3)$ 中的任意元素 $\varphi$, 我们也可按此方式定义它的指数映射:
$$\exp \left({\varphi }^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left({\varphi }^{\wedge }\right)}^n$$
但这个定义没法直接计算, 因为我们不想计算矩阵的无穷次幂。下面我们推导一种计算指数映 射的简便方法。由于 $\varphi$ 是三维向量, 我们可以定义它的模长和方向, 分别记作 $\theta$ 和 $a$, 于是有 $\mathbf{\varphi }=\theta \mathbf{a}$ 。这里 $\mathbf{a}$ 是一个长度为 1 的方向向量, 即 $\Vert \mathbf{a}\Vert =1$ 。首先, 对于 ${\mathbf{a}}^{\wedge }$, 有以下两条性质：
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }=\left[\begin{array}{ccc}-a_2^2-a_3^2& a_1{a}_2& a_1{a}_3\\ a_1{a}_2& -a_1^2-a_3^2& a_2{a}_3\\ a_1{a}_3& a_2{a}_3& -a_1^2-a_2^2\end{array}\right]=\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}$$
以及
$${\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }{\mathbf{a}}^{\wedge }={\mathbf{a}}^{\wedge }\left(\mathbf{a}{\mathbf{a}}^{\mathrm{T}}-\mathbf{I}\right)=-{\mathbf{a}}^{\wedge }.$$
这两个式子提供了处理 ${\mathbf{a}}^{\wedge }$ 高阶项的方法。我们可以把指数映射写成
$$\exp \left({\varphi }^{\wedge }\right)=\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\sum _{n=0}^{\infty }\frac{1}{n!}{\left(\theta {\mathbf{a}}^{\wedge }\right)}^n$$
化简最后, 得到罗德里格斯公式:
$$\exp \left(\theta {\mathbf{a}}^{\wedge }\right)=\cos \theta \mathbf{I}+(1-\cos \theta )\mathbf{a}{\mathbf{a}}^{\mathrm{T}}+\sin \theta {\mathbf{a}}^{\wedge }$$
这表明, $\mathfrak{s}\mathfrak{o}(3)$ 实际上就是 由所谓的旋转向量组成的空间, 而指数映射即罗德里格斯公式。通过它们, 我们把 $\mathfrak{s}\mathfrak{o}(3)$ 中任意 一个向量对应到了一个位于 $\mathrm{S}\mathrm{O}(3)$ 中的旋转矩阵。反之, 如果定义对数映射, 也能把 $\mathrm{S}\mathrm{O}(3)$ 中的 元素对应到 $\mathfrak{s}\mathfrak{o}(3)$ 中:
$$\varphi =\ln (\mathbf{R}{)}^{\vee }={\left(\sum _{n=0}^{\infty }\frac{(-1{)}^n}{n+1}(\mathbf{R}-\mathbf{I}{)}^{n+1}\right)}^{\vee }.$$
和指数映射一样, 我们没必要直接用泰勒展开计算对数映射。
是否对于任 意的 $\mathbf{R}$ 都能找到一个唯一的 $\mathbf{\varphi }$ ?
  指数映射只是一个满射, 并不是单射。这意味着每个 $\mathrm{S}\mathrm{O}(3)$ 中的元素, 都可以找到一个 $\mathfrak{s}\mathfrak{o}(3)$ 元素与之对应; 但是可能存在多个 $\mathfrak{s}\mathfrak{o}(3)$ 中的元素, 对应 到同一个 $\mathrm{S}\mathrm{O}(3)$ 。至少对于旋转角 $\theta$, 我们知道多转 $360^{\circ }$ 和没有转是一样的一一它具有周期性。 但是, 如果我们把旋转角度固定在 $\pm \pi$ 之间, 那么李群和李代数元素是一一对应的。

3. 李代数求导与扰动

  由于李群对加法不封闭导致无法进行最优化求解，李代数由向量组成，具有良好的加法运算，因此，使用李代数解决求导问题的思路分为两种:

1. 用李代数表示姿态，然后根据李代数加法对李代数求导。
2. 对李群左乘或右乘微小扰动，然后对该扰动求导，称为左扰动和右扰动模型。
   第一种方式对应到李代数的求导模型，而第二种方式则对应到扰动模型。下面讨论这两种思路的异同。

1. 李代数求导

首先, 考虑 $\mathrm{S}\mathrm{O}(3)$ 上的情况。假设我们对一个空间点 $\mathbf{p}$ 进行了旋转, 得到了 $\mathbf{R}\mathbf{p}$ 。现在, 要 计算旋转之后点的坐标相对于旋转的导数, 我们非正式地记为
$$\frac{\partial (\mathbf{R}p)}{\partial \mathbf{R}}$$
由于 $\mathrm{S}\mathrm{O}(3)$ 没有加法, 所以该导数无法按照导数的定义进行计算。设 $\mathbf{R}$ 对应的李代数为 $\mathbf{\varphi }$, 我 们转而计算：
$$\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }$$
按照导数的定义, 有
$$\begin{array}{rl}\frac{\partial \left(\exp \left({\varphi }^{\wedge }\right)p\right)}{\partial \varphi }& =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left((\varphi +\delta \varphi {)}^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\exp \left({\left(J_l\delta \varphi \right)}^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\delta \varphi }\\ & =\underset{\delta \varphi \to 0}{\lim }\frac{\left(\mathbf{I}+{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\right)\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}-\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{{\left({\mathbf{J}}_l\delta \mathbf{\varphi }\right)}^{\wedge }\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}}{\delta \mathbf{\varphi }}\\ & =\underset{\delta \mathbf{\varphi }\to 0}{\lim }\frac{-{\left(\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{p}\right)}^{\wedge }{\mathbf{J}}_l\delta \mathbf{\varphi }}{\delta \mathbf{\varphi }}\\ & =-(\mathbf{R}\mathbf{p}{)}^{\wedge }{\mathbf{J}}_l\end{array}$$
由于这里仍然含有形式比较复杂的${\mathbf{J}}_l$,我们不太希望计算它。而下面要讲的扰动模型则提供了更简单的导数计算方式。

2. 李群左右扰动

左扰动求导:
对 $R$ 进行一次左扰动 $△R$ ，左乘在 $R$ 左侧, $△R$ 对应的李代数设为 $\phi ,Rp$ 对 $\phi$ 求导:
$$\begin{array}{rl}\frac{\partial (Rp)}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\Delta RRp-Rp}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\exp \left({\phi }^{\wedge }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\left(I+{\phi }^{\Lambda }\right)\exp \left({\varphi }^{\wedge }\right)p-\exp \left({\varphi }^{\wedge }\right)p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }\exp \left({\varphi }^{\wedge }\right)p}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{{\phi }^{\wedge }Rp}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-(Rp{)}^{\wedge }\phi }{\phi }\\ & =-(Rp{)}^{\wedge }\end{array}$$
此式比上面用李代数求导少了一个雅克比计算，所以扰动模型更实用。
旋转点右扰动求导:
$$\begin{array}{rl}\frac{\partial (\mathbf{R}\mathbf{p})}{\partial \phi }& =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\mathbf{e}\mathbf{x}\mathbf{p}\left({\phi }^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}\left(\mathbf{I}+{\phi }^{\wedge }\right)\mathbf{p}-\mathbf{R}\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{\mathbf{R}{\phi }^{\wedge }\mathbf{p}}{\phi }\\ & =\underset{\phi \to 0}{\lim }\frac{-{\mathbf{R}\mathbf{p}}^{\wedge }\phi }{\phi }\\ & =-{\mathbf{R}\mathbf{p}}^{\wedge }\end{array}$$

3. 旋转连乘雅克比

$$\begin{array}{rl}\frac{\mathrm{d}\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\mathrm{d}{\mathbf{R}}_2}& =\underset{\varphi \to 0}{\lim }\frac{\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\exp \left({\varphi }^{\wedge }\right)\right)}^{\vee }-\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }+{\mathbf{J}}_r^{-1}\mathbf{\varphi }-\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\varphi }\\ & ={\mathbf{J}}_r^{-1}\left(\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }\right)\end{array}$$
其中用到了对 $\forall \mathbf{R}$ :
$$\ln {\left(\mathbf{R}\exp \left({\varphi }^{\wedge }\right)\right)}^{\vee }=\ln (\mathbf{R}{)}^{\vee }+{\mathbf{J}}_r^{-1}\varphi$$
和 ${\mathbf{J}}_r^{-1}$ 为 $\mathrm{S}\mathrm{O}(3)$ 上的右雅可比:
$${\mathbf{J}}_r^{-1}(\theta \mathbf{\omega })=\frac{\theta }{2}\cot \frac{\theta }{2}\mathbf{I}+\left(1-\frac{\theta }{2}\cot \frac{\theta }{2}\right)\mathbf{\omega }{\mathbf{\omega }}^{\mathrm{T}}+\frac{\theta }{2}{\mathbf{\omega }}^{\wedge }$$
旋转连乘的雅可比:
$$\begin{array}{rl}\frac{\mathrm{d}\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\mathrm{d}{\mathbf{R}}_1}& =\underset{\varphi \to 0}{\lim }\frac{\ln {\left({\mathbf{R}}_1\exp \left({\varphi }^{\wedge }\right){\mathbf{R}}_2\right)}^{\vee }-\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\varphi }\\ & =\underset{\varphi \to 0}{\lim }\frac{\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\exp \left({\left({\mathbf{R}}_2^T\varphi \right)}^{\wedge }\right)\right)}^{\vee }-\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }}{\varphi }\\ & ={\mathbf{J}}_r^{-1}\left(\ln {\left({\mathbf{R}}_1{\mathbf{R}}_2\right)}^{\vee }\right){\mathbf{R}}_2^T\end{array}$$
这里用到了 $\mathrm{S}\mathrm{O}(3)$ 的伴随性质：
$${\mathbf{R}}^T\exp \left({\mathbf{\varphi }}^{\wedge }\right)\mathbf{R}=\exp \left({\left({\mathbf{R}}^T\mathbf{\varphi }\right)}^{\wedge }\right)$$

4. 总结

六、IMU模型

1. 基本模型

IMU 误差模型
误差分类

• 加速度计和陀螺仪的误差可以分为：确定性误差，随机误差。
• 确定性误差可以事先标定确定，包括：bias, scale …
• 随机误差通常假设噪声服从高斯分布，包括：高斯白噪声，bias随机游走…

忽略scale，只考虑高斯白噪声n和bias随机游走b：
$$\begin{array}{rl}{\tilde{\mathbf{\omega }}}^b& ={\mathbf{\omega }}^b+{\mathbf{b}}^g+{\mathbf{n}}^g\\ {\tilde{\mathbf{a}}}^b& ={\mathbf{q}}_{bw}\left({\mathbf{a}}^w+{\mathbf{g}}^w\right)+{\mathbf{b}}^a+{\mathbf{n}}^a\end{array}$$
这是VIO中的IMU模型，上标有波浪线的表示测量值，右上的b表示body系，w表示世界系或惯性系，a表示acc，g表示gyro。PVQ表示为:
$$\begin{array}{rl}{\dot{\mathbf{p}}}_{w{b}_t}& ={\mathbf{v}}_t^w\\ {\dot{\mathbf{v}}}_t^w& ={\mathbf{a}}_t^w\\ {\dot{\mathbf{q}}}_{w{b}_t}& ={\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}{\mathbf{\omega }}^{b_t}\end{array}\right]\end{array}$$

2. 连续时间下 IMU 运动模型

根据上面的导数关系, 可以从第 $\mathrm{i}$ 时刻的 $\mathrm{P}\mathrm{V}\mathrm{Q}$, 通过对 IMU 的测量 值进行积分, 得到第 $\mathrm{j}$ 时刻的 PVQ:
$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_j}={\mathbf{p}}_{w{b}_i}+{\mathbf{v}}_i^w\Delta t+{\iint }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t^2\\ & {\mathbf{v}}_j^w={\mathbf{v}}_i^w+{\int }_{t\in [i,j]}\left({\mathbf{q}}_{w{b}_t}{\mathbf{a}}^{b_t}-{\mathbf{g}}^w\right)\delta t\\ & {\mathbf{q}}_{w{b}_j}={\int }_{t\in [i,j]}{\mathbf{q}}_{w{b}_t}\otimes \left[\begin{array}{c}0\\ \frac{1}{2}\end{array}{\mathbf{\omega }}^{b_t}\right]\delta t\end{array}$$

3. 运动模型的离散积分一一欧拉法

使用欧拉法, 即两个相邻时刻 $k$ 到 $k+1$ 的位姿是用第 $k$ 时刻的测量 值 $\mathrm{a},\omega$ 来计算。
$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_{k+1}}={\mathbf{p}}_{w{b}_k}+{\mathbf{v}}_k^w\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2\\ & {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t\\ & {\mathbf{q}}_{w{b}_{k+1}}={\mathbf{q}}_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\mathbf{\omega }\delta t\end{array}\right]\end{array}$$
其中，
$$\begin{array}{rl}& \mathbf{a}={\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\\ & \mathbf{\omega }={\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\end{array}$$

4. 运动模型的离散积分一一中值积分法

$$\begin{array}{rl}& {\mathbf{p}}_{w{b}_{k+1}}={\mathbf{p}}_{w{b}_k}+{\mathbf{v}}_k^w\Delta t+\frac{1}{2}\mathbf{a}\Delta t^2\\ & {\mathbf{v}}_{k+1}^w={\mathbf{v}}_k^w+\mathbf{a}\Delta t\\ & {\mathbf{q}}_{w{b}_{k+1}}={\mathbf{q}}_{w{b}_k}\otimes \left[\begin{array}{c}1\\ \frac{1}{2}\omega \delta t\end{array}\right]\end{array}$$
其中，
$$\begin{array}{rl}& \mathbf{a}=\frac{1}{2}\left[{\mathbf{q}}_{w{b}_k}\left({\mathbf{a}}^{b_k}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w+{\mathbf{q}}_{w{b}_{k+1}}\left({\mathbf{a}}^{b_{k+1}}-{\mathbf{b}}_k^a\right)-{\mathbf{g}}^w\right]\\ & \mathbf{\omega }=\frac{1}{2}\left[\left({\mathbf{\omega }}^{b_k}-{\mathbf{b}}_k^g\right)+\left({\mathbf{\omega }}^{b_{k+1}}-{\mathbf{b}}_k^g\right)\right]\end{array}$$
更加仔细的推到过程见另一篇优秀的博客———IMU模型