點擊上方“小白學視覺”，選擇加"星標"或“置頂”
重磅干貨，第一時間送達

前面文章中我們介紹過SLAM中的兩大類傳感器：激光雷達和視覺傳感器，并詳細分析了其優(yōu)缺點。而實際上，在基于移動機器人的SLAM中，還有一類很重要的傳感器就是慣性測量單元（Inertial Measurement Unit，簡稱IMU）。

單目、雙目、RGBD等視覺傳感器可以獲得非常豐富的環(huán)境信息，可以方便的用于三維重建、識別去過的地方（回環(huán)檢測）等。而IMU傳感器則可以獲得移動機器人本身的運動信息，比如重心方向、俯仰角、偏航角等。而最近幾年基于 camera + IMU的VIO（視覺慣性里程計）SLAM技術方案也是研究的熱點，具有代表性的如OKVIS、VI-ORB等。

最近剛好調研了慣性導航系統（Inertial Navigation System，簡稱INS）相關知識。在此分享一下，希望能對讀者有所幫助。

慣性導航系統是一種利用慣性敏感器件、基準方向及最初的位置信息來確定運載體在慣性空間中的位置、方向和速度的自主式導航系統，有時也簡稱為慣導。其工作環(huán)境不僅包括空中、地面，還可以在水下。

你知道自己的初始位置，知道自己的初始朝向（姿態(tài)），知道自己每一時刻如何改變了朝向，知道自己每一時刻相對朝向是怎樣走的，把這些加一起不停地推，走一步推一步，在不考慮各種誤差時，得出的結果就應該正好是你現在的朝向和位置。

但是要怎么知道自己的方向和位置是怎么改變的呢？不同的導航系統用不同的傳感器，有不同的方法，比如里程計用車輛上輪子轉的周數，多普勒計程儀像蝙蝠一樣往水底發(fā)射聲波……而慣性導航之所以叫【慣性】導航，就是因為使用的是【慣性器件】，也就是加速度計、陀螺儀、磁力計、氣壓計等。

因此，慣性導航系統至少需要包括含有加速度計、陀螺儀等的慣性測量單元和用于推理的計算單元兩大部分。

想象一個笛卡爾坐標系，形如下圖所示，具有x軸、y軸和z軸，傳感器能夠測量各軸方向的線性運動，以及圍繞各軸的旋轉運動。這就是所有慣性測量單元的根本出發(fā)點，所有慣性導航系統都是據此而構建。

加速度計測量加速度，利用的原理是 a=F/M，測量物體的“慣性力”。加速度計在慣性參照系中用于測量系統的線加速度，但只能測量相對于系統運動方向的加速度（由于加速度計與系統固定并隨系統轉動，不知道自身的方向）。? 這可以想象成一個被蒙上眼睛的乘客在汽車加速時向后擠壓座位，汽車剎車時身體前傾，汽車加速上坡時下壓座位，汽車越過山頂下坡時從座位上彈起，僅根據這些信息，乘客知道汽車相對自身怎樣加速，即向前、向后、向上、向下、向左 或向右，但不知道相對地面的方向。

可以通過對加速度進行解算，求得角速度，但由于精度不高，不具有很好的使用價值。但是加速度計可以輔助陀螺儀進行角度解算。

陀螺在慣性參照系中用于測量系統的角速率。通過以慣性參照系中系統初始方位作為初始條件，對角速率進行積分，就可以時刻得到系統的當前方向。這可以想象成被蒙上眼睛的乘客坐在汽車中，感覺汽車左轉、右轉、上坡、下坡，僅根據這些信息他知道了汽車朝哪里開，但不知道汽車是快，是慢或是否汽車滑向路邊。

我們現在智能手機上采用的陀螺儀是采用了MEMS微機電技術的MEMS陀螺儀，它需要參考其他傳感器的數據才能實現功能，但其體積小、功耗低、易于數字化和智能化，特別是成本低，非常適合手機、汽車牽引控制系統、醫(yī)療器材這些需要大規(guī)模生產的設備。

磁力計/地磁場傳感器，它有個通俗的名字：電子羅盤。當加速度傳感器完全水平的時候，可以預料，重力傳感器無法分辨出在水平面旋轉的角度即繞Z軸的旋轉無法顯示出來，此時只有陀螺儀可以檢測。

?陀螺儀雖然動態(tài)十分快速，但由于其工作原理是積分，所以在靜態(tài)會有累計誤差，表現為角度會一直增加或者一直減少。于是我們會需要一個在水平位置能確認朝向的傳感器，這就是如今IMU必備的第三個傳感器，地磁場傳感器，通過這3個傳感器的相互校正，我們終于在大的理論上可以得到比較準確的姿態(tài)參數了。

氣壓傳感器用于檢測大氣壓強的儀器,實際應用當中氣壓傳感器可作高度計。在慣導系統中有時通過增加氣壓計增強Z軸動態(tài)與精度。

導航系統傳感器功能示意圖

每個傳感器都有自己的優(yōu)點和缺點，相互配合，取長補短才能達到比較好的導航效果。

計算單元主要由三部分組成：姿態(tài)解算單元，積分單元和誤差補償單元。

• 姿態(tài)解算單元的主要任務是負責將測量得到的慣性數據由載體自身的坐標系轉換到地球坐標系。

• 加速度積分單元負責在系統所提供初始位置及速度的基礎上，對運動傳感器的信息進行整合計算，不斷更新當前位置及速度。

• 誤差補償單元負責對積分單元的輸出進行適當的修正，提高定位和姿態(tài)精度。

計算單元流程示意圖

姿態(tài)就是指載體的俯仰/橫滾/航向情況。姿態(tài)是用來描述一個剛體的固連坐標系和參考坐標系之間的角位置關系。姿態(tài)有多種數學表示方式，常見的是四元數、歐拉角、矩陣和軸角。他們各自有其自身的優(yōu)點，在不同的領域使用不同的表示方式。

在實際應用慣性導航的時候，慣性測量器件是直接安裝在運動載體上的，因此慣性器件測得的角速度和加速度的數值都是在載體運動坐標系下的量。而研究者在實際的操控過程中是在一個靜止的坐標空間中來衡量這些被測數值的，所以這里涉及到不同坐標系之間的相互轉換問題。

例如，地球坐標系是固定的R，載體上有自身的坐標系r，r在R中運動，那么如何知道r和R的角位置關系呢，也就是怎么知道載體相對于地球這個固定坐標系R轉動了多少航向，側翻了多少機身或者調整了多少俯仰。這就是姿態(tài)解算。

姿態(tài)解算是一個非常復雜的過程，并不像我們認為的那么簡單。

姿態(tài)解算（altitude algorithm）,也叫做姿態(tài)分析、姿態(tài)估計、姿態(tài)融合。姿態(tài)解算是指根據IMU數據求解出姿態(tài)。載體根據陀螺儀的三軸角速度對時間積分得到的俯仰/橫滾/航向角，這是快速解算。快速解算得到的姿態(tài)是存在誤差的，而且誤差會累加，如果再結合三軸地磁和三軸加速度數據進行校正，得到準確的姿態(tài)，這就是深度解算。

快速解算得到的數據一般是不能夠直接使用的，因為誤差太大。從慣性導航出現、發(fā)展到現在，人們總結出了多種姿態(tài)矩陣的解算方法，目前主要應用的有歐拉角、方向余弦、旋轉矢量和四元數等算法。

姿態(tài)矩陣算法對比

姿態(tài)解算是慣性導航系統的第一步，加速度積分以及位置計算是第二步，且必須在第一步之后完成。

主要分為

1）、根據姿態(tài)解算得到的姿態(tài)矩陣，將載體坐標系下的加速度轉換為地球坐標系下的三軸加速度

2）、在給定初始位置的基礎上，對1）步輸出的加速度關于時間進行積分，得到速度

3）、對速度關于時間再次積分，得到位置

?

而關于如何積分，屬于數值計算部分的內容，在此就不展開了。

在慣性導航系統中存在不可消除的位置積分誤差和傳感器誤差,在導航過程中，小誤差會隨時間累積成大誤差，其誤差大體上與時間成正比，因此需要不斷進行修正。現代慣性導航系統使用各種信號（例如全球定位系統及磁羅盤等）對其進行修正，采取控制論原理對不同信號進行權級過濾，保證慣性導航系統的精度及可靠性。

慣性導航系統由很多部分組成，誤差補償算法是慣性導航系統中不可或缺的一部分。為了解決誤差累積問題可以考慮設計濾波環(huán)節(jié)，算法的輸入分別為姿態(tài)角值和位置，輸出為最優(yōu)估計姿態(tài)角和位置值。

常用的濾波算法包括：卡爾曼濾波算法、UKF濾波 、平方根無跡卡爾曼（SRUKF）濾波算法、GP-SRUKF濾波算法等。

組合導航的基本原理是利用信息融合技術，通過最優(yōu)估計、數字濾波等信號處理方法把各種導航系統如無線電、衛(wèi)星、天文、地形及景象匹配等導航系統的結合。以發(fā)揮各種導航技術優(yōu)勢，達到比任何單一導航方式更高的導航精度和可靠性。

常見有以慣性導航和 GPS 衛(wèi)星導航組合的（INS/GPS）導航系統。與慣導相比 GPS 具有成本低，導航精度高，且誤差不隨時間積累等優(yōu)點，GPS 導航系統輸出的導航信息作為系統狀態(tài)的觀測量，通過卡爾曼濾波對系統的狀態(tài)（位置、速度等）及誤差進行最優(yōu)估計，以實現對慣導系統的校準和誤差補償。而慣導系統自主、實時、連續(xù)等優(yōu)點可彌補 GPS 易受干擾、動態(tài)環(huán)境可靠性差的不足。

慣性系統最先應用于火箭制導，美國火箭先驅羅伯特?戈達爾試驗了早期的陀螺系統。二戰(zhàn)期間經德國人馮布勞恩改進后，應用于V-2火箭制導。戰(zhàn)后美國麻省理工學院等研究機構及人員對慣性制導進行深入研究，從而發(fā)展成應用于飛機、火箭、航天飛機、潛艇的現代慣性導航系統，然而成本及復雜性限制了其可以應用的場合。

國家的很多戰(zhàn)略、戰(zhàn)術武器，以及洲際飛行的民航飛機等，都必須依賴慣性導航系統或者慣導系統與其他類型的導航系統的組合。它的造價比較昂貴，一臺導航級的慣導系統，至少要幾十萬，而這種精度的導航系統已經足夠配備在波音747這樣的飛機上了。現在，隨著MEMS慣性器件技術的進步，商業(yè)級、消費品級的慣性導航才逐漸走進尋常百姓家。

僅采用手機里的加速度計和陀螺儀，通過多傳感器融合算法，在多數應用條件下（載體受到的加速度在大多數時間內主要是重力加速度的情況下），可以獲得較好的姿態(tài)信息。但如果僅依靠這兩個傳感器來做傳統的慣性導航，由于長期精度問題，無法獲得有價值的位置數據。如果在手機上實現類似應用的話，通常配合GPS來實現此類功能。某些沒有GPS信號的地方，比如大型場所的室內手機定位，目前通常通過慣導來做動態(tài)，通過室內基站（Cell/WiFi/BT）來修正長期精度，某些新型手機增加的氣壓計也主要是為了此類場合使用。

交流群

歡迎加入公眾號讀者群一起和同行交流，目前有SLAM、三維視覺、傳感器、自動駕駛、計算攝影、檢測、分割、識別、醫(yī)學影像、GAN、算法競賽等微信群（以后會逐漸細分），請掃描下面微信號加群，備注：”昵稱+學校/公司+研究方向“，例如：”張三?+?上海交大?+?視覺SLAM“。請按照格式備注，否則不予通過。添加成功后會根據研究方向邀請進入相關微信群。請勿在群內發(fā)送廣告，否則會請出群，謝謝理解~