深度学习与惯性导航 惯性导航系统论文

本文介绍一篇惯性导航定位论文 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{aligned} \mathbf{R}_{n+1} &=\mathbf{R}_{n} \exp _{S O(3)}\left(\boldsymbol{\omega}_{n} d t\right) \\ \mathbf{v}_{n+1}^{\mathrm{w}} &=\mathbf{v}_{n}^{\mathrm{W}}+\left(\mathbf{R}_{n} \mathbf{a}_{n}+\mathbf{g}\right) d t \\ \mathbf{p}_{n+1}^{\mathrm{w}} &=\mathbf{p}_{n}^{\mathrm{w}}+\mathbf{v}_{n}^{\mathrm{w}} d t \end{aligned}$

其中 $dt$ 时采样时间，$(\mathbf{R}_0,\mathbf{v}_{0}^{\mathrm{w}},\mathbf{p}_{0}^{\mathrm{w}})$

下面回顾下IMU模型，IMU加速度和角速度模型可以写为：
$\begin{aligned} &\boldsymbol{\omega}_{n}^{\mathrm{IMU}}=\boldsymbol{\omega}_{n}+\mathbf{b}_{n}^{\boldsymbol{\omega}}+\mathbf{w}_{n}^{\boldsymbol{\omega}} \\ &\mathbf{a}_{n}^{\mathrm{IMU}}=\mathbf{a}_{n}+\mathbf{b}_{n}^{\mathbf{a}}+\mathbf{w}_{n}^{\mathbf{a}} \end{aligned}$

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

其中，$\mathbf{w}_{n}^{\mathbf{b}_{\boldsymbol{\omega}}},\mathbf{w}_{n}^{\mathbf{b}_{\mathbf{a}}}$

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

---

2. Specific Motion Profiles For Wheeled Systemd

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

首先是四种特定的运动情况，将它们编码为向量形式为：
$\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}$

其中：

• Zero velocity，当 $z_n^{\mathrm{VEL}}=1$ 时，则有$\left\{\begin{array}{c}\mathbf{v}_{n}=\mathbf{0} \\ \mathbf{R}_{n} \mathbf{a}_{n}+\mathbf{g}=\mathbf{0}\end{array}\right.$，当检测到零速时，常会使用 ZUPT 算法进行更新。
• Zero angular velocity，当 $z_n^{\mathrm{ANG}}=1$ 时，则有 $\boldsymbol{\omega}_{n}=\mathbf{0}$。
• Zero lateral velocity，当 $z_n^{\mathrm{LAT}}=1$ 时，则有 $v_{n}^{\mathrm{LAT}} \simeq 0$，载体坐标速度和世界坐标速度转换方程为：$\mathbf{v}_{n}^{\mathrm{B}}=\mathbf{R}_{n}^{T} \mathbf{v}_{n}^{\mathrm{W}}=\left[\begin{array}{c}v_{n}^{\mathrm{FOR}} \\ v_{n}^{\mathrm{LAT}} \\ v_{n}^{\mathrm{UP}}\end{array}\right]$。
• Zero vertical velocity，当 $z_n^{\mathrm{UP}}=1$ 时，则有 $v_{n}^{\mathrm{UP}} \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{aligned} \hat{\mathbf{u}}_{n+1}, \mathbf{h}_{n+1} &=\operatorname{LSTM}\left(\left\{\boldsymbol{\omega}_{i}^{\mathrm{IMU}}, \mathbf{a}_{i}^{\mathrm{IMU}}\right\}_{i=0}^{n}\right) \\ &=\operatorname{LSTM}\left(\boldsymbol{\omega}_{n}^{\mathrm{IMU}}, \mathbf{a}_{n}^{\mathrm{IMU}}, \mathbf{h}_{n}\right) \end{aligned}$

其中，$\hat{\mathbf{u}}_{n+1} \in \mathbb{R^4}$ 包含每一个运动情形的概率值，$\mathbf{h}_n$ 是神经网络隐藏状态，概率值最后经过阈值运算来得到二元向量 $\hat{\mathbf{z}}_n=\mathrm{Threshold} \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}^{\boldsymbol{\omega}}, \mathbf{b}_{n}^{\mathbf{a}}\right)$，线性状态误差为：$\mathbf{e}_{n}=\left[\begin{array}{c}\boldsymbol{\xi}_{n} \\ \mathbf{e}_{n}^{\mathbf{b}}\end{array}\right] \sim \mathcal{N}\left(\mathbf{0}, \mathbf{P}_{n}\right)$，状态更新方程为：

$\begin{aligned} &\boldsymbol{\chi}_{n}=\exp _{S E_{2}(3)}\left(\boldsymbol{\xi}_{n}\right) \hat{\boldsymbol{\chi}}_{n} \\ &\mathbf{b}_{n}=\hat{\mathbf{b}}_{n}+\mathbf{e}_{n}^{\mathbf{b}}, \end{aligned}$

其中，$\boldsymbol{\chi}_{n} \in SE_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}^{\boldsymbol{\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}^{\boldsymbol{\omega}}, \mathbf{w}_{n}^{\mathbf{a}}, \mathbf{w}_{n}^{\mathbf{b} \omega}, \mathbf{w}_{n}^{\mathbf{b}^{\mathrm{a}}}\right) \sim \mathcal{N}\left(\mathbf{0}, \mathbf{Q}_{n}\right)$。

如果由特定的运动情况被检测到，将会按照下面的方程进行状态量修改，零速时（速度归0，位置不变，这里的0表示速度为0，原论文中作者未明确表示）：
$\hat{z}_{n+1}^{\mathrm{vEL}}=1 \Rightarrow\left\{\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}\right.$

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

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

（3）更新。每一个运动情形将会产生下列的伪测量：
$\begin{aligned} &\mathbf{y}_{n+1}^{\mathrm{VEL}}=\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} \mathbf{0} \\ \mathbf{a}_{n}^{\mathrm{IMU}} \end{array}\right] \\ &\mathbf{y}_{n+1}^{\mathrm{ANG}}=\mathbf{b}_{n+1}^{\boldsymbol{\omega}}=\boldsymbol{\omega}_{n}^{\mathrm{IMU}} \\ &\mathbf{y}_{n+1}^{\mathrm{LAT}}=v_{n+1}^{\mathrm{LAT}}=0 \\ &\mathbf{y}_{n+1}^{\mathrm{UP}}=v_{n+1}^{\mathrm{UP}}=0 \end{aligned}$

更新方程为：
$\begin{aligned} \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} \boldsymbol{\xi}^{+} \\ \mathbf{e}^{\mathbf{b}+} \end{array}\right] \\ \hat{\boldsymbol{\chi}}_{n+1}^{+} &=\exp \left(\boldsymbol{\xi}^{+}\right) \hat{\boldsymbol{\chi}}_{n+1}, \mathbf{b}_{n+1}^{+}=\mathbf{b}_{n+1}+\mathbf{e}^{\mathbf{b}+} \\ \mathbf{P}_{n+1}^{+} &=\left(\mathbf{I}-\mathbf{K H}_{n+1}\right) \mathbf{P}_{n+1} \end{aligned}$

其中，$\mathbf{K}$ 为卡尔曼增益矩阵，$\mathbf{H}_{n+1}$

（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{VEL}},z_n^{\mathrm{ANG}}）$，0.5 $（z_n^{\mathrm{LAT}},z_n^{\mathrm{UP}}）$；滤波器工作频率为100Hz，噪声协方差矩阵为：
$\begin{aligned} &\mathbf{Q}_{n}=\operatorname{diag}\left(\sigma_{\omega}^{2} \mathbf{I}, \sigma_{\mathbf{a}}^{2} \mathbf{I}, \sigma_{\mathbf{b}_{\boldsymbol{\omega}}}^{2} \mathbf{I}, \sigma_{\mathbf{b}_{\mathbf{a}}}^{2} \mathbf{I}\right) \\ &\mathbf{N}_{n}=\operatorname{diag}\left(\sigma_{\mathrm{VEL}, \mathbf{v}}^{2} \mathbf{I}, \sigma_{\mathrm{VEL}, \mathbf{a}}^{2} \mathbf{I}, \sigma_{\mathrm{ANG}}^{2} \mathbf{I}, \sigma_{\mathrm{LAT}}^{2}, \sigma_{\mathrm{UP}}^{2}\right) \end{aligned}$

其中，协方差矩阵 $Q$ 中 $\sigma_{\omega}=0.01 \mathrm{rad/s},\sigma_{\mathbf{a}}=0.2 \mathrm{m/s^2},\sigma_{\mathbf{b}_{\boldsymbol{\omega}}}=0.001 \mathrm{rad/s},\sigma_{\mathbf{b}_{\mathbf{a}}}=0.02\mathrm{m/s^2}$；噪声矩阵 $\mathbf{N}_{n}$ 中 $\sigma_{\mathrm{VEL}, \mathbf{v}}=1\mathrm{m/s}, \sigma_{\mathrm{VEL}, \mathbf{a}}=0.4\mathrm{m/s^2}, \sigma_{\mathrm{ANG}}=0.04\mathrm{rad/s}, \sigma_{\mathrm{LAT}}=3\mathrm{m/s}, \sigma_{\mathrm{UP}}=3\mathrm{m/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

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

误差 $\boldsymbol{\xi}_{n}=\left(\boldsymbol{\xi}_{n}^{\mathbf{R}}, \boldsymbol{\xi}_{n}^{\mathbf{v}}, \boldsymbol{\xi}_{n}^{\mathrm{p}}\right) \in \mathrm{R}^9$ 的李代数形式为：
$\boldsymbol\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 e}_{2}(3)$

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

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

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

当 $\hat{z}_n^{\mathrm{VEL}}=1$ 时，矩阵 $\mathbf{F}_n,\mathbf{G}_n$ 的第4行-第9行为0；当 $\hat{z}_n^{\mathrm{ANG}}=1$ 时，矩阵 $\mathbf{F}_n,\mathbf{G}_n$

测量雅可比矩阵为：
$\begin{aligned} \mathbf{H}_{n}^{\mathrm{vEL}} &=\left[\begin{array}{ccccc} \mathbf{0} & & \mathbf{R}_{n}^{T} & \mathbf{0} & \mathbf{0} & \mathbf{0} \\ \mathbf{R}_{n}^{T}(\mathbf{g})_{\times} & \mathbf{0} & \mathbf{0} & \mathbf{0} & -\mathbf{I} \end{array}\right] \\ \mathbf{H}_{n}^{\mathrm{ANG}} &=\left[\begin{array}{ccccc} \mathbf{0} & \mathbf{0} & \mathbf{0} & -\mathbf{I} & \mathbf{0} \end{array}\right] \end{aligned}$

这里，矩阵 $\mathbf{H}_n^{\mathrm{VEL}}$