详解惯性导航论文 RINS-W: Robust Inertial Navigation System on Wheels

本文介绍一篇惯性导航定位论文 RINS-W，论文发表于 IROS2019。在本论文中作者提出了仅使用一个IMU进行长时间惯性导航的方法。方法主要包括两个部分：

• 检测器使用循环神经网络来检测IMU的运动状况，如零速或零横向滑移；
• 使用Invariant Extended Kalman Filter结合检测器的输出（作为伪测量）来进行定位。

在公开数据集上的测试结果显示，在行驶超过21km之后，最终定位误差为20m（如下图所示）。

论文链接：https://arxiv.org/pdf/1903.02210.pdf

github 链接：https://github.com/mbrossar/RINS-W

---

1. Inertial Navigation System & Sensor Model

首先回顾下惯性导航方程，IMU方向用 ${\mathbf{R}}_n\in SO(3)$ 表示，表示从载体坐标到世界坐标的旋转变换；世界坐标系中速度为 ${\mathbf{v}}_n^{\mathrm{w}}\in {\mathbb{R}}^3$，世界坐标系中位置为 ${\mathbf{p}}_n^{\mathrm{w}}\in {\mathbb{R}}^3$，则运动方程可以写为：
$$\begin{array}{rl}{\mathbf{R}}_{n+1}& ={\mathbf{R}}_n{\exp }_{SO(3)}\left({\mathbf{\omega }}_{n}dt\right)\\ {\mathbf{v}}_{n+1}^{\mathrm{w}}& ={\mathbf{v}}_n^{\mathrm{W}}+\left({\mathbf{R}}_n{\mathbf{a}}_n+\mathbf{g}\right)dt\\ {\mathbf{p}}_{n+1}^{\mathrm{w}}& ={\mathbf{p}}_n^{\mathrm{w}}+{\mathbf{v}}_n^{\mathrm{w}}dt\end{array}$$

其中 $dt$ 时采样时间，$({\mathbf{R}}_0,{\mathbf{v}}_0^{\mathrm{w}},{\mathbf{p}}_0^{\mathrm{w}})$ 是初始状态。在本论文中，没有考虑地球自转和科氏力的影响。

下面回顾下IMU模型，IMU加速度和角速度模型可以写为：
$$\begin{array}{rl}& {\mathbf{\omega }}_n^{\mathrm{I}\mathrm{M}\mathrm{U}}={\mathbf{\omega }}_n+{\mathbf{b}}_n^{\mathbf{\omega }}+{\mathbf{w}}_n^{\mathbf{\omega }}\\ & {\mathbf{a}}_n^{\mathrm{I}\mathrm{M}\mathrm{U}}={\mathbf{a}}_n+{\mathbf{b}}_n^{\mathbf{a}}+{\mathbf{w}}_n^{\mathbf{a}}\end{array}$$

其中，${\mathbf{b}}_n^{\mathbf{\omega }},{\mathbf{b}}_n^{\mathbf{a}}$为角速度偏差和加速度偏差，${\mathbf{w}}_n^{\mathbf{\omega }}，{\mathbf{w}}_n^{\mathbf{a}}$ 为高斯噪声。角速度偏差和加速度偏差方程可以写为：
$$\begin{array}{r}{\mathbf{b}}_{n+1}^{\mathbf{\omega }}={\mathbf{b}}_n^{\mathbf{\omega }}+{\mathbf{w}}_n^{{\mathbf{b}}_{\mathbf{\omega }}}\\ {\mathbf{b}}_{n+1}^{\mathbf{a}}={\mathbf{b}}_n^{\mathbf{a}}+{\mathbf{w}}_n^{{\mathbf{b}}_{\mathbf{a}}}\end{array}$$

其中，${\mathbf{w}}_n^{{\mathbf{b}}_{\mathbf{\omega }}},{\mathbf{w}}_n^{{\mathbf{b}}_{\mathbf{a}}}$ 为随机游走噪声。

在惯性导航中，偏差的准确估计是至关重要的，即使很小的误差，也会导致很大的位置估计偏差。

---

2. Specific Motion Profiles For Wheeled Systemd

在这一节，作者介绍了几种常见的运动特性，它们往往会提供有用的互补信息。

首先是四种特定的运动情况，将它们编码为向量形式为：
z n = ( z n V E L , z n A N G , z n L A T , z n U P ) ∈ { 0 , 1 } 4 \mathbf{z}_{n}=\left(z_{n}^{\mathrm{VEL}}, z_{n}^{\mathrm{ANG}}, z_{n}^{\mathrm{LAT}}, \quad z_{n}^{\mathrm{UP}}\right) \in\{0,1\}^{4} zn​=(znVEL​,znANG​,znLAT​,znUP​)∈{0,1}4

其中：

• Zero velocity，当 $z_n^{\mathrm{V}\mathrm{E}\mathrm{L}}=1$ 时，则有$\{\begin{array}{c}{\mathbf{v}}_n=0\\ {\mathbf{R}}_n{\mathbf{a}}_n+\mathbf{g}=0\end{array}$，当检测到零速时，常会使用 ZUPT 算法进行更新。
• Zero angular velocity，当 $z_n^{\mathrm{A}\mathrm{N}\mathrm{G}}=1$ 时，则有 ${\mathbf{\omega }}_n=0$。
• Zero lateral velocity，当 $z_n^{\mathrm{L}\mathrm{A}\mathrm{T}}=1$ 时，则有 $v_n^{\mathrm{L}\mathrm{A}\mathrm{T}}\simeq 0$，载体坐标速度和世界坐标速度转换方程为：${\mathbf{v}}_n^{\mathrm{B}}={\mathbf{R}}_n^T{\mathbf{v}}_n^{\mathrm{W}}=\left[\begin{array}{c}{v}_n^{\mathrm{F}\mathrm{O}\mathrm{R}}\\ v_n^{\mathrm{L}\mathrm{A}\mathrm{T}}\\ v_n^{\mathrm{U}\mathrm{P}}\end{array}\right]$。
• Zero vertical velocity，当 $z_n^{\mathrm{U}\mathrm{P}}=1$ 时，则有 $v_n^{\mathrm{U}\mathrm{P}}\simeq 0$。

其中，后两种运动情况经常用在轮速机器人或汽车运动中。以上四种情形中，零速约束（零速度和零角速度）用来修正IMU偏差和姿态俯仰角；零横向速度和垂直速度用来长期估计汽车位置（后面的实验会进行说明）。

---

3. Proposed RINS-W Algorithm

本文提出的方法如下图所示，由两部分组成：

• Detector 由循环神经网络组成，根据IMU测量值来输出二元向量 ${\mathbf{z}}_n$；
• IEKF 是一种新的卡尔曼滤波器，输入为IMU测量值和检测器输出（作为伪测量），对状态量进行估计；
  

---

3.1 Specific Motion Profile Detector

Detector 会决定在每一个时刻 $n$ 二元向量 ${\mathbf{z}}_n$中有几个元素是有效的，即有几种运动形式会发生，结构如下图所示。检测器的核心模块为LSTM，输入为IMU测量值，计算方程为：
$$\begin{array}{rl}{\hat{\mathbf{u}}}_{n+1},{\mathbf{h}}_{n+1}& =\mathrm{LSTM}\left({\left\{{\mathbf{\omega }}_i^{\mathrm{I}\mathrm{M}\mathrm{U}},{\mathbf{a}}_i^{\mathrm{I}\mathrm{M}\mathrm{U}}\right\}}_{i=0}^n\right)\\ & =\mathrm{LSTM}\left({\mathbf{\omega }}_n^{\mathrm{I}\mathrm{M}\mathrm{U}},{\mathbf{a}}_n^{\mathrm{I}\mathrm{M}\mathrm{U}},{\mathbf{h}}_n\right)\end{array}$$

其中，${\hat{\mathbf{u}}}_{n+1}\in {\mathbb{R}}^4$ 包含每一个运动情形的概率值，${\mathbf{h}}_n$ 是神经网络隐藏状态，概率值最后经过阈值运算来得到二元向量 ${\hat{\mathbf{z}}}_n=\mathrm{T}\mathrm{h}\mathrm{r}\mathrm{e}\mathrm{s}\mathrm{h}\mathrm{o}\mathrm{l}\mathrm{d}\left({\hat{\mathbf{u}}}_{n+1}\right)$

---

3.2 The Invariant Extended Kalman Filter（重点）

在本文中，作者选择使用IEKF而不是传统的EKF来作为进行状态估计，如下图所示，二元变量 ${\hat{\mathbf{z}}}_n$ 一方面会用于状态传播，另一方面会用于状态更新。

（1）首先定义IMU状态，IMU状态量为：${\mathbf{x}}_n=\left({\mathbf{R}}_n,{\mathbf{v}}_n^{\mathrm{w}},{\mathbf{p}}_n^{\mathrm{w}},{\mathbf{b}}_n^{\mathbf{\omega }},{\mathbf{b}}_n^{\mathbf{a}}\right)$，线性状态误差为：${\mathbf{e}}_n=\left[\begin{array}{c}{\mathbf{\xi }}_n\\ {\mathbf{e}}_n^{\mathbf{b}}\end{array}\right]\sim \mathcal{N}\left(0,{\mathbf{P}}_n\right)$，状态更新方程为：
$$\begin{array}{rl}& {\mathbf{\chi }}_n={\exp }_{S{E}_2(3)}\left({\mathbf{\xi }}_n\right){\hat{\mathbf{\chi }}}_n\\ & {\mathbf{b}}_n={\hat{\mathbf{b}}}_n+{\mathbf{e}}_n^{\mathbf{b}},\end{array}$$

其中，${\mathbf{\chi }}_n\in S{E}_2(3)$，表示为汽车状态 $({\mathbf{R}}_n,{\mathbf{v}}_n^{\mathrm{w}},{\mathbf{p}}_n^{\mathrm{w}})$ 在李群上的形式，误差状态协方差矩阵为 ${\mathbf{P}}_n\in {\mathbb{R}}^{15\times 15}$。偏差 ${\mathbf{b}}_n=({\mathbf{b}}_n^{\mathbf{\omega }},{\mathbf{b}}_n^{\mathbf{a}})\in {\mathbb{R}}^6$，偏差估计量为 ${\hat{\mathbf{b}}}_n=\left({\hat{\mathbf{b}}}_n^{\omega },{\hat{\mathbf{b}}}_n^{\mathbf{a}}\right)\in {\mathbb{R}}^6$。

（2）预测部分。当没有上述四种之一的运动情况被检测到时，使用第1节介绍的运动方程来计算新的状态量和协方差，协方差计算方程为：
$${\mathbf{P}}_{n+1}={\mathbf{F}}_n{\mathbf{P}}_n{\mathbf{F}}_n^T+{\mathbf{G}}_n{\mathbf{Q}}_n{\mathbf{G}}_n^T$$

雅可比矩阵 ${\mathbf{F}}_n,{\mathbf{G}}_n$ 将会在第5节进行介绍。${\mathbf{Q}}_n$ 表示为噪声协方差矩阵，噪声为 ${\mathbf{w}}_n=\left({\mathbf{w}}_n^{\mathbf{\omega }},{\mathbf{w}}_n^{\mathbf{a}},{\mathbf{w}}_n^{\mathbf{b}\omega },{\mathbf{w}}_n^{{\mathbf{b}}^{\mathrm{a}}}\right)\sim \mathcal{N}\left(0,{\mathbf{Q}}_n\right)$。

如果由特定的运动情况被检测到，将会按照下面的方程进行状态量修改，零速时（速度归0，位置不变，这里的0表示速度为0，原论文中作者未明确表示）：
$${\hat{z}}_{n+1}^{\mathrm{v}\mathrm{E}\mathrm{L}}=1\Rightarrow \{\begin{array}{l}{\mathbf{v}}_{n+1}^{\mathrm{w}}={\mathbf{v}}_n^{\mathrm{w}}=0\\ {\mathbf{p}}_{n+1}^{\mathrm{w}}={\mathbf{p}}_n^{\mathrm{w}}\end{array}$$

零角速度时（姿态不变）：
$${\hat{z}}_{n+1}^{\mathrm{A}\mathrm{N}\mathrm{G}}=1\Rightarrow {\mathbf{R}}_{n+1}={\mathbf{R}}_n$$

同时，状态估计量和协方差矩阵也要响应进行修改。

（3）更新。每一个运动情形将会产生下列的伪测量：
$$\begin{array}{rl}& {\mathbf{y}}_{n+1}^{\mathrm{V}\mathrm{E}\mathrm{L}}=\left[\begin{array}{c}{\mathbf{R}}_{n+1}^T{\mathbf{v}}_{n+1}^{\mathrm{W}}\\ {\mathbf{b}}_{n+1}^{\mathbf{a}}-{\mathbf{R}}_{n+1}^T\mathbf{g}\end{array}\right]=\left[\begin{array}{c}0\\ {\mathbf{a}}_n^{\mathrm{I}\mathrm{M}\mathrm{U}}\end{array}\right]\\ & {\mathbf{y}}_{n+1}^{\mathrm{A}\mathrm{N}\mathrm{G}}={\mathbf{b}}_{n+1}^{\mathbf{\omega }}={\mathbf{\omega }}_n^{\mathrm{I}\mathrm{M}\mathrm{U}}\\ & {\mathbf{y}}_{n+1}^{\mathrm{L}\mathrm{A}\mathrm{T}}=v_{n+1}^{\mathrm{L}\mathrm{A}\mathrm{T}}=0\\ & {\mathbf{y}}_{n+1}^{\mathrm{U}\mathrm{P}}=v_{n+1}^{\mathrm{U}\mathrm{P}}=0\end{array}$$

更新方程为：
$$\begin{array}{rl}\mathbf{K}& ={\mathbf{P}}_{n+1}{\mathbf{H}}_{n+1}^T/\left({\mathbf{H}}_{n+1}{\mathbf{P}}_{n+1}{\mathbf{H}}_{n+1}^T+{\mathbf{N}}_{n+1}\right)\\ {\mathbf{e}}^{+}& =\mathbf{K}\left({\mathbf{y}}_{n+1}-{\hat{\mathbf{y}}}_{n+1}\right)=\left[\begin{array}{c}{\mathbf{\xi }}^{+}\\ {\mathbf{e}}^{\mathbf{b}+}\end{array}\right]\\ {\hat{\mathbf{\chi }}}_{n+1}^{+}& =\exp \left({\mathbf{\xi }}^{+}\right){\hat{\mathbf{\chi }}}_{n+1},{\mathbf{b}}_{n+1}^{+}={\mathbf{b}}_{n+1}+{\mathbf{e}}^{\mathbf{b}+}\\ {\mathbf{P}}_{n+1}^{+}& =\left(\mathbf{I}-{\mathbf{K}\mathbf{H}}_{n+1}\right){\mathbf{P}}_{n+1}\end{array}$$

其中，$\mathbf{K}$ 为卡尔曼增益矩阵，${\mathbf{H}}_{n+1}$ 是测量雅可比矩阵（具体形式见第5节）。

（4）初始化。为了正确估计偏差和方向，在开始阶段，将会强制静止1秒钟用于估计偏差和俯仰角。

---

4. Results On Car Dataset

首先是数据集的介绍，使用的数据集为comples urban LiDAR Dataset，IMU如下图所示。

---

4.1 Implementation Details

下面是实现细节，detector由4个LSTMs组成，每一个LSTM由2个隐藏层（每层250个隐藏单元）组成，然后是2层多层感知机，最后是sigmoid函数。阈值设定为：0.95 $（z_n^{\mathrm{V}\mathrm{E}\mathrm{L}},z_n^{\mathrm{A}\mathrm{N}\mathrm{G}}）$，0.5 $（z_n^{\mathrm{L}\mathrm{A}\mathrm{T}},z_n^{\mathrm{U}\mathrm{P}}）$；滤波器工作频率为100Hz，噪声协方差矩阵为：
$$\begin{array}{rl}& {\mathbf{Q}}_n=\mathrm{diag}\left({\sigma }_{\omega }^2\mathbf{I},{\sigma }_{\mathbf{a}}^2\mathbf{I},{\sigma }_{{\mathbf{b}}_{\mathbf{\omega }}}^2\mathbf{I},{\sigma }_{{\mathbf{b}}_{\mathbf{a}}}^2\mathbf{I}\right)\\ & {\mathbf{N}}_n=\mathrm{diag}\left({\sigma }_{\mathrm{V}\mathrm{E}\mathrm{L},\mathbf{v}}^2\mathbf{I},{\sigma }_{\mathrm{V}\mathrm{E}\mathrm{L},\mathbf{a}}^2\mathbf{I},{\sigma }_{\mathrm{A}\mathrm{N}\mathrm{G}}^2\mathbf{I},{\sigma }_{\mathrm{L}\mathrm{A}\mathrm{T}}^2,{\sigma }_{\mathrm{U}\mathrm{P}}^2\right)\end{array}$$

其中，协方差矩阵 $Q$ 中 ${\sigma }_{\omega }=0.01\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s},{\sigma }_{\mathbf{a}}=0.2\mathrm{m}/{\mathrm{s}}^2,{\sigma }_{{\mathbf{b}}_{\mathbf{\omega }}}=0.001\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s},{\sigma }_{{\mathbf{b}}_{\mathbf{a}}}=0.02\mathrm{m}/{\mathrm{s}}^2$；噪声矩阵 ${\mathbf{N}}_n$ 中 ${\sigma }_{\mathrm{V}\mathrm{E}\mathrm{L},\mathbf{v}}=1\mathrm{m}/\mathrm{s},{\sigma }_{\mathrm{V}\mathrm{E}\mathrm{L},\mathbf{a}}=0.4\mathrm{m}/{\mathrm{s}}^2,{\sigma }_{\mathrm{A}\mathrm{N}\mathrm{G}}=0.04\mathrm{r}\mathrm{a}\mathrm{d}/\mathrm{s},{\sigma }_{\mathrm{L}\mathrm{A}\mathrm{T}}=3\mathrm{m}/\mathrm{s},{\sigma }_{\mathrm{U}\mathrm{P}}=3\mathrm{m}/\mathrm{s}$。

4.2 Evaluation Metrics

这里使用了三个评价指标：

• Mean Absolute Trajectory Error (m-ATE)，平均绝对轨迹误差（估计位置和真值位置之间的误差平均值）；
• Mean Absolute Aligned Trajectory Error (aligned m-ATE)，首先对齐估计轨迹和真值轨迹，然后再计算m-ATE，主要是评估轨迹的一致性；
• Final distance error，估计轨迹和真值轨迹最终的距离误差。

4.3 Trajectory Results

下面是实验结果，作者采用了4种方法：

• IMU直接积分方法；
• 差分轮速编码器得到线性速度和角速度再积分；
• RINS-W，本文提出的方法；
• 里程计+光纤陀螺仪，里程计提供线性速度，角速度由FoG得到。

同时作者还比较了是否使用横向和垂直速度假设时的定位误差，结果如下，使用横向和垂直速度假设时效果更好。

---

5. Appendix

李群 $S{E}_2(3)$ 是李群 $SE(3)$ 的扩展，其形式可以写成 $5\times 5$ 的矩阵，即：
$${\mathbf{\chi }}_n=\left[\begin{array}{ccc}{\mathbf{R}}_n& {\mathbf{v}}_n^{\mathrm{w}}& {\mathbf{p}}_n^{\mathrm{w}}\\ 0& \mathbf{I}& \end{array}\right]\in S{E}_2(3)$$

误差 ${\mathbf{\xi }}_n=\left({\mathbf{\xi }}_n^{\mathbf{R}},{\mathbf{\xi }}_n^{\mathbf{v}},{\mathbf{\xi }}_n^{\mathrm{p}}\right)\in {\mathrm{R}}^9$ 的李代数形式为：
$${\mathbf{\xi }}_n^{\wedge }=\left[\begin{array}{ccc}{\left({\xi }_n^{\mathrm{R}}\right)}_{\times }& {\xi }_n^{\mathrm{v}}& {\xi }_n^{\mathrm{p}}\\ 0& & \end{array}\right]\in {\mathfrak{s}\mathfrak{e}}_2(3)$$

李代数 ${\xi }_n^{\wedge }$ 的指数映射形式为：
$${\exp }_{S{E}_2(3)}\left({\mathbf{\xi }}_n\right)=\mathbf{I}+{\mathbf{\xi }}_n^{\wedge }+a{\left({\mathbf{\xi }}_n^{\wedge }\right)}^2+b{\left({\mathbf{\xi }}_n^{\wedge }\right)}^3$$

其中，$a=\frac{1-\cos \left(\Vert {\mathbf{\xi }}_n^{\mathbf{R}}\Vert \right)}{{\Vert {\mathbf{\xi }}_n^{\mathrm{R}}\Vert }^2}$，$b=\frac{\Vert {\mathbf{\xi }}_n^{\mathbf{R}}\Vert -\sin \left(\Vert {\mathbf{\xi }}_n^{\mathbf{R}}\Vert \right)}{{\Vert {\mathbf{\xi }}_n^{\mathbf{R}}\Vert }^3}$。

这里使用的是Right IEKF，雅可比矩阵为：
$$\begin{gathered} {\mathbf{F}}_n=\mathbf{I}+\left[\begin{array}{ccccc}0& 0& 0& -{\mathbf{R}}_n& 0\\ (\mathbf{g}{)}_{\times }& 0& 0& -{\left({\mathbf{v}}_n^{\mathrm{W}}\right)}_{\times }{\mathbf{R}}_n& -{\mathbf{R}}_n\\ 0& \mathbf{I}& 0& -{\left({\mathbf{p}}_n^{\mathrm{W}}\right)}_{\times }{\mathbf{R}}_n& 0\\ 0& 0& 0& 0& 0\\ 0& 0& 0& 0& 0\end{array}\right]dt, \\ {\mathbf{G}}_n=\left[\begin{array}{cccc}{\mathbf{R}}_n& 0& 0& 0\\ {\left({\mathbf{v}}_n^{\mathbf{w}}\right)}_{\times }{\mathbf{R}}_n& {\mathbf{R}}_n& 0& 0\\ {\left({\mathbf{p}}_n^{\mathbf{w}}\right)}_{\times }{\mathbf{R}}_n& 0& 0& 0\\ 0& 0& \mathbf{I}& 0\\ 0& 0& 0& \mathbf{I}\end{array}\right]dt \end{gathered}$$

当 ${\hat{z}}_n^{\mathrm{V}\mathrm{E}\mathrm{L}}=1$ 时，矩阵 ${\mathbf{F}}_n,{\mathbf{G}}_n$ 的第4行-第9行为0；当 ${\hat{z}}_n^{\mathrm{A}\mathrm{N}\mathrm{G}}=1$ 时，矩阵 ${\mathbf{F}}_n,{\mathbf{G}}_n$ 的前3行为0。

测量雅可比矩阵为：
$$\begin{array}{rl}{\mathbf{H}}_n^{\mathrm{v}\mathrm{E}\mathrm{L}}& =\left[\begin{array}{cccccc}0& & {\mathbf{R}}_n^T& 0& 0& 0\\ {\mathbf{R}}_n^T(\mathbf{g}{)}_{\times }& 0& 0& 0& -\mathbf{I}& \end{array}\right]\\ {\mathbf{H}}_n^{\mathrm{A}\mathrm{N}\mathrm{G}}& =\left[\begin{array}{ccccc}0& 0& 0& -\mathbf{I}& 0\end{array}\right]\end{array}$$

这里，矩阵 ${\mathbf{H}}_n^{\mathrm{V}\mathrm{E}\mathrm{L}}$ 的形式值得商榷，在上面第一种作者可能忘了讨论对旋转矩阵的求导。