AI-IMU Dead-Reckoning Martin
論文出處
論文：https://arxiv.org/abs/1904.06064
代碼：https://github.com/mbrossar/ai-imu-dr
整體介紹
a. 本文主要提出了一種方法，能夠僅僅使用IMU的航位推算得到較為精準的軌迹，如下圖所示：
b. 該方法隻适用于像汽車一樣隻能向前跑的系統，因為這樣的系統在進行kalman濾波器的時候可以加入動力模型的輔助資訊，在這裡，輔助資訊主要是指系統在側向 y l a t y^{lat} ylat和與地面垂直的方向 y u p y^{up} yup上是沒有移動的，文中稱之為僞測量；
c. 論文的重要改進點在于，(b)中輔助資訊的均值可以認為是0，但是協方差是否一直都是一個值呢？顯然，在系統進行轉彎的時候，水準方向的協方差要比直線方向的協方差是要大很多的，同理，在爬坡的時候，垂直地面方向的協方差也應該比直線的時候協方差是要大的；鑒于一般的濾波器都是人為設計協方差矩陣，同時設計好的協方差不能修改，是以文章希望使用CNN（并非RNN，作者說RNN不是很好）對觀測的協方差進行動态的更新，進一步的，作者使用CNN也同時預測出來了上述的僞測量的值(這裡沒有給出僞量測的值)。
系統實作
IMU模型定義
一般情況下，IMU的模型建立為真值+零偏+高斯白噪聲，如下：
(1) w n I M U = w n + b n w + w n w a n I M U = a n + b n a + w n a \begin{aligned} w_{n}^{IMU} &= w_{n}+b_{n}^{w}+w_{n}^{w} \\ a_{n}^{IMU} &=a_{n}+b_{n}^{a}+w_{n}^{a} \end{aligned} \tag{1} wnIMUanIMU=wn+bnw+wnw=an+bna+wna(1)
其中零偏一般是一個随機遊走的“準常量”（quasi-constant）（一般來說與溫度有關），是以對零偏進行模組化，其模型如下：
(2) b n + 1 w = b n w + w n b w b n + 1 a = b n a + w n b a \begin{aligned} b_{n+1}^{w} &= b_{n}^{w}+w_{n}^{b_w} \\ b_{n+1}^{a} &= b_{n}^{a}+w_{n}^{b_a} \end{aligned} \tag{2} bn+1wbn+1a=bnw+wnbw=bna+wnba(2)
其中 w n w , w n a , w n b w , w n b a w_{n}^{w}, w_{n}^{a}, w_{n}^{b_w}, w_{n}^{b_a} wnw,wna,wnbw,wnba為高斯分布的白噪聲。
IMU的運動模型
IMU的運動模型可以定義為如下形式：
(3) R n + 1 I M U = R n I M U exp ( ( ω n d t ) × ) v n + 1 I M U = v n I M U + ( R n I M U a n + g ) d t p n + 1 I M U = p n I M U + v n I M U d t \begin{aligned} \mathbf{R}_{n+1}^{\mathrm{IMU}} &=\mathbf{R}_{n}^{\mathrm{IMU}} \exp \left(\left(\boldsymbol{\omega}_{n} d t\right)_{ \times}\right) \\ \mathbf{v}_{n+1}^{\mathrm{IMU}} &=\mathbf{v}_{n}^{\mathrm{IMU}}+\left(\mathbf{R}_{n}^{\mathrm{IMU}} \mathbf{a}_{n}+\mathbf{g}\right) d t \\ \mathbf{p}_{n+1}^{\mathrm{IMU}} &=\mathbf{p}_{n}^{\mathrm{IMU}}+\mathbf{v}_{n}^{\mathrm{IMU}}dt \end{aligned} \tag{3} Rn+1IMUvn+1IMUpn+1IMU=RnIMUexp((ωndt)×)=vnIMU+(RnIMUan+g)dt=pnIMU+vnIMUdt(3)
其中的所有變量都是真值形式，沒有考慮到偏差等等的因素，如果把帶有偏差的值運用到上面動态模型中，則誤差會十分大，甚至比直接使用輪子的積分來的更差。
KalmanFilter的模組化
1. 狀态變量
狀态變量選擇為：
x n : = ( R n I M U , v n I M U , p n I M U , b n ω , b n a , R n c , p n c ) \mathbf{x}_{n} :=\left(\mathbf{R}_{n}^{\mathrm{IMU}}, \mathbf{v}_{n}^{\mathrm{IMU}}, \mathbf{p}_{n}^{\mathrm{IMU}}, \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}}, \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}}\right) xn:=(RnIMU,vnIMU,pnIMU,bnω,bna,Rnc,pnc)
其中系統的輸入為IMU的測量值， R n I M U ， v n I M U ， p n I M U \mathbf{R}_{n}^{\mathrm{IMU}}，\mathbf{v}_{n}^{\mathrm{IMU}}，\mathbf{p}_{n}^{\mathrm{IMU}} RnIMU，vnIMU，pnIMU是載體在第n步（或者了解為t時刻）的姿态，速度和位置，其中的參考系都是世界坐标系，坐标系的資訊如下圖:
可以看到每個變量都是從載體系到世界坐标系的轉換，不要被變量上面的IMU所迷惑了； b n ω , b n a \mathbf{b}_{n}^{\omega}, \mathbf{b}_{n}^{\mathrm{a}} bnω,bna是IMU的零偏， R n c , p n c \mathbf{R}_{n}^{\mathrm{c}}, \mathbf{p}_{n}^{\mathrm{c}} Rnc,pnc是IMU到載體系的外參矩陣，這裡也做作為一個更新量進行了估計。
2. 系統方程
同正常的kalman濾波步驟一樣，我們首先建立預測方程（文章中把預測階段稱為傳播propagate）和更新方程：
(5) x n + 1 = f ( x n , u n ) + w n y n = h ( x n ) + n n \begin{aligned} \mathbf{x}_{n+1} &=f\left(\mathbf{x}_{n}, \mathbf{u}_{n}\right)+\mathbf{w}_{n} \\ \mathbf{y}_{n} &=h\left(\mathbf{x}_{n}\right)+\mathbf{n}_{n} \tag{5} \end{aligned} xn+1yn=f(xn,un)+wn=h(xn)+nn(5)
其中正如上面所說， y n y_n yn是輔助變量，理想情況下，我們的設定值為[0, 0]，即 h ( x n ) ≈ 0 h\left(\mathbf{x}_{n}\right)\approx0 h(xn)≈0
3. 僞測量的建立
這部分更類似于觀測方程的建立，主要是把狀态估計中的速度（載體在世界坐标系下的速度，即 W V n ^WV_n WVn）轉移到載體系下
v n c = [ v n f o r v n l a t v n u p ] = ( R n c ) T ( R n I M U ) T v n I M U + ( ω n ) × p n c \mathbf{v}_{n}^{\mathrm{c}}=\left[ \begin{array}{c}{v_{n}^{\mathrm{for}}} \\ {v_{n}^{\mathrm{lat}}} \\ {v_{n}^{\mathrm{up}}}\end{array}\right]=(\mathbf{R}_{n}^{\mathrm{c}})^T (\mathbf{R}_{n}^{\mathrm{IMU}})^T \mathbf{v}_{n}^{\mathrm{IMU}}+\left(\boldsymbol{\omega}_{n}\right) \times \mathbf{p}_{n}^{\mathrm{c}} vnc=⎣⎡vnforvnlatvnup⎦⎤=(Rnc)T(RnIMU)TvnIMU+(ωn)×pnc
對于汽車等隻能向前的物體，上述的 v n l a t , v n u p v_n^{lat}, v_n^{up} vnlat,vnup的值應該近似于0，是以僞量測為
y n = [ y n l a t y n u p ] = [ h l a t ( x n ) + n n l a t h u p ( x n ) + n n u p ] = [ v n l a t v n u p ] + n n y_n=\left[\begin{array}{c}{y_n^{lat}} \\ {y_n^{up}}\end{array}\right]=\left[\begin{array}{c}{h^{lat}(x_n)+n_n^{lat}} \\ {h^{up}(x_n)+n_n^{up}}\end{array} \right]=\left[\begin{array}{c}{v_n^{lat}} \\ {v_n^{up}}\end{array} \right] + n_n yn=[ynlatynup]=[hlat(xn)+nnlathup(xn)+nnup]=[vnlatvnup]+nn
其中 n n n_n nn為噪聲，這裡算作一個松散的限制，這樣的效果往往比強硬的把僞測量看做0要好的多。
系統的整體結構
整體結構如下圖：
整體而言，系統使用IEKF進行狀态的估計，而使用神經網絡根據IMU的測量值産生出僞量測及其協方差，整個算法中預測階段的系統噪聲的協方差 Q Q Q是固定的，因為都是高斯白噪聲。筆者還是比較看好這樣的slam系統的，因為個人認為神經網絡不是很适合slam這種幾何學的問題，而作為輔助的話還是很看好的。
網絡的設計
從系統結構圖上可以看到，系統使用的是IMU的量測值，其為基于時間的一組序列，作者選擇其中N組資料進行卷積，之後産生對應的測量協方差：
N n + 1 = C N N ( { ω i I M U , a i I M U } i = n − N n ) \mathbf{N}_{n+1}=\mathrm{CNN}\left(\left\{\omega_{i}^{\mathrm{IMU}}, \mathbf{a}_{i}^{\mathrm{IMU}}\right\}_{i=n-N}^{n}\right) Nn+1=CNN({ωiIMU,aiIMU}i=n−Nn)
作者說道設計中的三個動機如下：
- 盡量使用小的參數防止過拟合;
- 希望得到一個可解釋的規則，例如轉彎的時候，網絡的輸出要大一些;
- 沒有使用RNN是因為RNN的訓練更加的艱難;
網絡最終的輸出為一個2維向量 z n = [ z n l a t , z n u p ] T ∈ R 2 z_n=[z_n^{lat}, z_n^{up}]^T \in R^2 zn=[znlat,znup]T∈R2，随後作者引入一個超參數 β \beta β用于控制協方差的擴張上下限，最終的協方內插補點取做：
N n + 1 = diag ( σ lat 2 1 0 β tanh ( z n lat ) , σ up 2 1 0 β tanh ( z n up ) ) \mathbf{N}_{n+1}=\operatorname{diag}\left(\sigma_{\text { lat }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { lat }}\right)}, \sigma_{\text { up }}^{2} 10^{\beta \tanh \left(z_{n}^{\text { up }}\right)}\right) Nn+1=diag(σ lat 210βtanh(zn lat ),σ up 210βtanh(zn up ))
是以協方差的動态變化範圍為 1 0 − β ～ 1 0 β 10^{-\beta}～10^{\beta} 10−β～10β。
網絡采用1維卷積的方式，共2層，第一層卷積核的大小為5，輸出的深度為32，膨脹系數為1，第二層卷積核大小為5，輸出深度為32，膨脹系數為3，最後還有一個全連接配接層将特征輸出綜合輸出為2，取得滑動視窗的大小為N=15，最終一共産生的參數量為6210（這個參數量筆者計算的稍微不太對，按照stride=2來計算的話最終的結果是6304）。
在實作的一些初始參數這裡就不過多解釋了，需要注意的是，用于不同的機器人的時候，需要對這些參數稍加修改，具體還是參照論文中的部分。
網絡的訓練
網絡的訓練步驟就是：
- 選取資料；
- 計算濾波器的輸出與真實軌迹之間的誤差e，之後按照公式更新kalman濾波器的參數，包括協方差矩陣等等，對于量測方程中的量測協方差 R R R，一般濾波器中該參數為人為設定，這裡應該看做變量，之後根據鍊式法則将誤差e傳遞回來，送給網絡進行權重的調整；
- 更新網絡的參數；
具體的一些超參數這裡就不贅述了。
最終的結果
論文最終對比了IMLS（以雷射雷達資料為基礎），雙目ORB-SLAM2和純IMU進行積分的方法，結果如下：
結論還是很強勢的，畢竟隻是使用IMU的資料進行航位推算，最終能達到比ORB-SLAM2的效果還要還一點也确實很不錯了。
除此之外，還記得作者設計網絡時候的三個動機嗎？作者也把其中第二個動機進行了驗證，結果如下圖：
恩～果然在轉彎的時候，方差出現了變化。
總結
筆者最後把github上的代碼下載下傳下來運作了一下，幾個測試資料集的效果還是不錯的，是一個足以期待的方向。