Подробный документ по инерциальной навигации RINS-W: Надежная инерциальная навигационная система на колесах

В этом документе представлен документ по инерциальному навигационному позиционированию.RINS-W, газета была опубликована вIROS2019. В данной работе автор представляет仅使用一个IMUСпособ длительной инерциальной навигации. Метод в основном состоит из двух частей:

Детектор использует рекуррентную нейронную сеть для обнаружения движения IMU, такого как нулевая скорость или нулевое боковое скольжение;
использоватьInvariant Extended Kalman FilterВ сочетании с выходом детектора (в качестве псевдоизмерения) для локализации.

Результаты тестирования общедоступного набора данных показывают, что после проезда более 21 км окончательная ошибка позиционирования составляет 20 м (как показано на рисунке ниже).

Ссылка на бумагу:АР Вест V.org/PDF/1903.02…

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 Инвариантный расширенный фильтр Калмана

В этой статье автор решил использовать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}$ — матрица Якоби измерений (конкретную форму см. в разделе 5).

(4) Инициализация. Чтобы правильно оценить смещение и ориентацию, вначале используется вынужденная неподвижность в течение 1 секунды для оценки смещения и угла тангажа.

4. Results On Car Dataset

Во-первых, это введение набора данных, используемый набор данныхcomples urban LiDAR Dataset, IMU показан на рисунке ниже.

4.1 Implementation Details

Ниже приведены подробности реализации,detectorна 4LSTMsсостав, каждыйLSTMСостоит из 2 скрытых слоев (по 250 скрытых единиц в каждом), затем 2 слоев многослойных персептронов и, наконец,sigmoidфункция. Порог установлен на: 0,95 $(z_n ^ {\ mathrm {ВЭЛ}}, z_n ^ {\ mathrm {АНГ}})$ , 0,5 $(z_n ^ {\ mathrm {LAT}}, z_n ^ {\ mathrm {UP}})$ ; Рабочая частота фильтра составляет 100 Гц, а ковариационная матрица шума:

\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

Здесь используются три метрики оценки:

Средняя абсолютная ошибка траектории (m-ATE), средняя абсолютная ошибка траектории (среднее значение ошибки между расчетным положением и наземным истинным положением);
Средняя абсолютная ошибка выровненной траектории (выровненная m-ATE), сначала выровняйте расчетную траекторию и траекторию истинного значения, а затем вычислите m-ATE, в основном для оценки согласованности траектории;
Окончательная ошибка расстояния, окончательная ошибка расстояния между расчетной траекторией и истинной траекторией.

4.3 Trajectory Results

Ниже приведены экспериментальные результаты, авторы использовали 4 метода:

Метод прямой интеграции IMU;
Кодер дифференциальной скорости колеса получает реинтеграцию линейной скорости и угловой скорости;
RINS-W – метод, предложенный в данной статье;
Одометр + волоконно-оптический гироскоп, одометр обеспечивает линейную скорость, а угловая скорость получается по FoG.

В то же время автор также сравнил ошибку позиционирования при использовании предположений о поперечной и вертикальной скорости.Результаты следующие:使用横向和垂直速度假设时效果更好.