基于IMU和地磁傳感器的捷聯(lián)慣性導航系統(tǒng)

作者：Joel Li 和 Van Yang

本文介紹我們使用ADI公司的慣性測量單元（IMU）傳感器ADIS16470和PNI的地磁傳感器RM3100構建的捷聯(lián)慣性導航系統(tǒng)（SINS）。實現了基于磁性、角速率和重力 （MARG） 的 SINS 的一些基本過程，包括電磁羅盤校準、使用擴展卡爾曼濾波器 （EKF） 的姿態(tài)和航向參考系統(tǒng) （AHRS） 以及跡線跟蹤。還實現了使用最小平方誤差（MSE）方法的傳感器融合松散耦合。顯示了每個工藝步驟中的算法和實驗設置。本文末尾討論了結果分析和用于提高準確性的方法。

介紹

隨著服務機器人的市場和技術的增長，導航已成為研究應用的熱點。與車輛、輪船或飛機相比，服務機器人體積小，成本低，因此它們的導航系統(tǒng)應該是捷聯(lián)和低成本的。傳統(tǒng)的穩(wěn)定平臺導航系統(tǒng)通常涉及單獨的加速器和光纖或基于激光的陀螺儀，所有組件都機械和剛性地安裝在與移動車輛隔離的穩(wěn)定平臺上。這導致了體積大、可靠性差、成本高的缺點。相比之下，在捷聯(lián)導航系統(tǒng)中，慣性傳感器直接固定在車身上，這意味著傳感器與車輛一起旋轉。這種捆綁式方法消除了穩(wěn)定平臺導航的缺點。但是，平臺導航的準確性通常高于SINS。平臺導航通?？梢赃_到戰(zhàn)略級（0.0001°/hr陀螺儀偏置，1μg加速器偏置）或軍用級（0.005°/hr陀螺儀偏置，30 μg加速器偏置），而大多數SINS只能達到導航級（0.01°/hr陀螺儀偏置，50μg加速器偏置）或戰(zhàn)術級（10°/hr陀螺儀偏置，1 mg加速器偏置）。對于大多數服務機器人或AGV導航應用，這就足夠了。

有多種導航方法，包括機器視覺、GPS、UWB、帶 SLAM 的激光雷達等。雖然慣性導航始終是導航中的重要組成部分，但使用 IMU。然而，由于這種傳感器的局限性，例如偏置誤差、跨軸誤差、噪聲，尤其是偏置不穩(wěn)定性，慣性導航通常需要一個伙伴傳感器來定期為其提供參考或校準，這在這里稱為傳感器融合。有許多傳感器可以與IMU融合，例如相機和里程表，但是在這些傳感器中，地磁傳感器是與IMU一起獲取姿態(tài)的低成本方法。

在本文中，我們使用ADI公司的IMU、ADIS16470和地磁傳感器來開發(fā)平臺和算法，以實現捷聯(lián)慣性導航系統(tǒng)。但是，傳感器只能提供姿態(tài)信息。對于航位推算或距離測量，我們只能使用 IMU 中的加速度傳感器。

ADIS16470 IMU簡介

ADI公司的ADIS16470是一款微型MEMS IMU，集成了3軸陀螺儀和3軸加速度計。它為陀螺儀提供 8°/hr 偏置穩(wěn)定性，為加速度計提供 13 μg 偏置穩(wěn)定性，同時其關鍵參數經過工廠校準。此外，ADIS16470的低成本價格在同電平器件中具有吸引力，并被許多客戶廣泛使用。在本文中，我們使用微控制器通過SPI接口與ADIS16470通信。

地磁傳感器簡介

地磁傳感器是用于測量指南針體坐標（即框架）中的地磁場的傳感器，它為航向提供絕對參考。其 x、y 和 z 分量值是從當地地磁場投影而來的。這種傳感器有兩個主要缺點，一個是它的精度和分辨率不能很高，例如，霍尼韋爾常用的羅盤傳感器HMC5883L只有12位分辨率。另一個缺點是傳感器很容易受到周圍環(huán)境的干擾，因為地磁場非常弱，從毫高斯到8高斯。

盡管存在這些缺點，但它仍然可以在許多情況下使用，例如露天場地、低 EMI 環(huán)境等。通過將地磁傳感器松散耦合到IMU，我們可以在大多數環(huán)境中使用它。

在本文中，我們使用PNI傳感器公司的高性能指南針傳感器RM3100，提供24位分辨率。PNI使用有源激勵方法來提高抗噪聲能力。

羅盤傳感器的校準

在使用指南針傳感器之前，需要對其進行校準以消除兩個主要誤差。一種是失調誤差，最初是由傳感器和電路的失調誤差引起的。另一個是比例錯誤。這兩種誤差都很容易受到周圍磁環(huán)境的干擾。例如，如果傳感器上施加了x軸方向的外部磁場，則會發(fā)出外部x軸偏移誤差。同時，x 軸刻度也將不同于 y 軸和 z 軸。

通常用于校準磁傳感器的方法是將傳感器旋轉為 x-y 平面上的圓，然后繪制數據。一個地方的地磁場強度是一個常數值，所以繪制的數據應該是一個圓;但是，實際上，我們將看到一個橢圓形，這意味著我們需要將橢圓移動并重新縮放為以零為中心的圓。

上面提到的2D校準方法有一些缺點，需要一個加速器來測量其傾斜度。我們使用3D球面擬合方法來校準羅盤傳感器。首先，我們需要將傳感器旋轉到 x-y-z 空間中的每個方向，并在 3D 坐標中繪制其值。然后，我們需要使用最小平方誤差（MSE）方法將數據擬合為橢球體。

橢球方程可以表示為

其中 X、Y 和 Z 是指南針在其三個方向上輸出的地磁分量。將這些值擬合到橢球體意味著我們需要獲得系數的最佳值集。我們將系數定義為：

擬合時，我們將向量定義為：

因此，我們需要計算一個最佳σ，并使用公式 2 找到最小值：

這樣我們就可以得到如圖 1 所示的擬合結果。

圖1.原始羅盤數據分布（左）和使用橢球擬合后的羅盤數據（右）。

為了校準傳感器，我們需要將擬合的橢球體拉伸并移動到以零為中心的球體上。我們使用矩陣奇異值分解（SVD）方法來進行此校準。校準后球如圖2所示。1,2

圖2.使用SVD方法進行球體校準后的指南針數據。

校準后，我們可以看到測量的磁場強度（球體半徑）將保持在幾乎恒定的值，如圖3所示。

圖 3.校準前和校準后校準之間的磁場比較。

使用ADIS16470和指南針的姿態(tài)和航向參考系統(tǒng)

AHRS 由三個軸上的傳感器組成，這些傳感器提供姿態(tài)信息，包括橫滾、俯仰和偏航。AHRS是飛機導航的一個概念。我們用它來描述方向;也就是態(tài)度。

在介紹我們的方法之前，有必要先解釋一下融合的原因，先找到態(tài)度。事實上，現在我們的系統(tǒng)中有三種傳感器：陀螺儀、加速器和指南針。

陀螺儀給出圍繞每個軸的角旋轉速率。通過積分角速率，我們可以找到旋轉角度。如果我們知道初始標題，我們將始終得到標題態(tài)度。由于集成，陀螺儀的不穩(wěn)定偏置會累積，從而導致角度誤差。此外，來自陀螺儀的高斯分布噪聲將被積分到布朗運動過程中，并導致隨機行走誤差。因此，我們很難長時間使用陀螺儀，陀螺儀需要定期校準。

加速器提供每個軸方向的運動加速度。在靜態(tài)狀態(tài)下，我們可以得到每個軸之間的角度和重力加速度。由于重力加速度在方向和值上是恒定的，因此我們可以得到相對于重力方向的航向姿態(tài)。但是，該方法以重力加速度為參考，因此無法求解圍繞重力加速度旋轉的角度。

指南針提供從地磁場投射的每個軸上的值。我們可以從每個軸與地磁場方向之間的關系中推導出角度值，這也是一個常量矢量。如上一節(jié)所述，由于對外部磁場的抗擾度差，指南針需要低干擾環(huán)境。

從這個解釋中，我們可以看到，僅僅依靠一個傳感器來找到姿態(tài)是很困難的，我們需要將兩個或三個傳感器組合使用并融合信息。本文涉及加速器、陀螺儀和地磁羅盤來尋找姿態(tài)。這種融合被稱為磁、角速率和重力（MARG）系統(tǒng)。

擴展卡爾曼濾波器設計和傳感器融合

有多種方法可以將 IMU 和指南針數據融合在一起，例如互補濾波器、統(tǒng)計 ARMA 濾波器、卡爾曼濾波器等。在本文中，我們使用擴展的卡爾曼濾波器。

首先，我們需要介紹本文中使用的一些定義。

坐標定義

標題或方向是兩個坐標（即框架）之間的關系。一個坐標總是在變化，另一個坐標保持不變。對于坐標定義方法，我們使用導航坐標和正文坐標。與東北向下 （NED） 或地理方法相反，我們將測量的初始主體坐標值定義為導航坐標，之后是一個常量坐標。從正文坐標到導航坐標的映射（投影）矩陣定義為

態(tài)度定義

與歐拉角或方向余弦矩陣（DCM）矩陣不同，我們在這里使用四元數，定義為

這通常用于導航以避免爭論。3

使用卡爾曼濾波更新姿態(tài)

我們在本文中使用的運動學方程，即狀態(tài)轉移方程是一種偏差格式，它不是線性的，因此我們需要使用具有一階線性近似的 EKF 來表示偏差方程。對于 EKF 設計，我們定義

一個 1 × 7 向量作為狀態(tài)變量，其中

是角速率，并且

是態(tài)度四元數。

1 × 7 向量是觀測變量，它與狀態(tài)變量具有相同的分量。

一個7×7矩陣，是狀態(tài)轉移矩陣，其中A的第一部分是角速率的數字化微分方程，第二部分是數字化的四元數更新方程，由運動學方程推導出來。

7×7矩陣是觀測矩陣。

是誤差協(xié)方差矩陣，一個 7 × 7 矩陣，其中

是從實數 x 估計的誤差向量 x?。我們在測試中將初始誤差設置為相對較小的值。它將自動收斂到一個小值。

設置為狀態(tài)轉換噪聲和觀測噪聲的噪聲協(xié)方差。我們得到它們的初始值，

和

通過測量陀螺儀和加速器的交流均方根值的平方。在保持 IMU 和指南針處于靜態(tài)狀態(tài)的同時，我們設置

有了這個定義，我們的卡爾曼濾波器將通過以下五個步驟完成：

步驟1：使用公式3計算卡爾曼增益K：

第 2 步：計算誤差協(xié)方差矩陣 P：

步驟3：輸出估計狀態(tài)，x?：

第 4 步：預測狀態(tài)，x?–:

步驟5：投影誤差協(xié)方差矩陣，P–:

該過程可以簡單地描述為圖4中的框圖。

圖4.用于更新態(tài)度的卡爾曼濾波流程圖。

基于MSE的傳感器融合

在上一節(jié)中，觀測變量為

其中沒有來自指南針的信息。由于ω是角速率，我們只能使用四元數來導入羅盤數據q。我們使用MSE方法得到q，即觀測變量中的分量。

我們定義變量如下：

mb和一個b：體框內的羅盤磁力值和加速度值。

mn和一個n：導航框中的羅盤磁值和加速度值。

mn0和一個n0：導航坐標系中的初始靜態(tài)羅盤磁值和加速度值。

是從車身框架到導航框架的姿態(tài)變換矩陣，用四元數表示，可以寫成

這給了我們導航框架中的初始值與從正文框架實時映射到導航框架的值之間的誤差，ε。

基于前面的定義，MSE方法可用于獲得最佳

通過最小化公式8：

通過取 f（q） 的推導并使其等于零，

我們將得到方差含義的最佳 Q。我們使用高斯-牛頓方法求解上述具有一階梯度收斂的非線性方程。

通過結合角速率，我們將得到觀測變量，

在卡爾曼濾波器中融合了指南針數據和 IMU 數據。

該過程可以通過圖5中的框圖簡單描述。

圖5.使用 MSE 方法的傳感器融合框圖。

松耦合

如前所述，我們經常遇到無法使用指南針傳感器的情況。如果磁數據受到干擾，求解的姿態(tài)精度將比僅使用IMU時差。因此，我們使用松耦合來判斷磁性傳感器是否可用。當磁性傳感器不可用時，我們將僅使用 IMU 來查找姿態(tài)，當磁性傳感器可用時，我們將使用融合算法來查找姿態(tài)，如圖 6 所示。

圖6.姿態(tài)計算流程圖。

在獲得新數據后，或者在新的姿態(tài)求解周期中（在某些系統(tǒng)中，采樣周期與姿態(tài)解析周期不同，但我們在這里進行單樣本周期解析），我們計算加速度的大小。如果它不等于 1 g，我們將不使用加速器的輸出進行姿態(tài)計算。然后我們計算指南針輸出的大小，并將其與初始值進行比較。如果它們彼此不相等，我們將不會在此循環(huán)中使用地磁傳感器的數據。當兩個條件都滿足時，我們將使用卡爾曼濾波器并進行MSE融合。

使用ADIS16470的航位推算（DR）

在導航中，航位推算是通過使用先前確定的位置計算當前位置的過程，并根據已知或估計的速度或加速度在解析周期內推進該位置。此處將使用ADIS16470加速器。根據上一節(jié)中解析的姿態(tài)，我們得到捷聯(lián)系統(tǒng)的移動方向，然后我們需要計算方向上的距離，最終確定位置。

航位推算方法介紹

捷聯(lián)航位推算需要使用基于加速度測量的特定力方程跟蹤INS的位置。比力方程可以簡單地描述為公式10、公式11和公式12：

哪里一個e是地球框架中的加速度，一個b是身體框架中的加速度，ve是地球坐標系中的速度，se是地球坐標系中的距離，ge是地球坐標系中的重力加速度，以 g 為單位為 [0 0 1]。我們需要強調的是，地球框架與導航框架不同——地球框架是面向NED的。這個δt是解析周期。

第一個方程找到從 IMU 體框架到地球框架的加速度投影，如格式所示。

第二個方程將加速度積分或累積為速度;但是，由于測量的加速度涉及重力分量，因此需要減去重力。

與公式11類似，公式12將速度積分到距離中。

傳統(tǒng)方法存在幾個問題。

加速器輸出總是有偏置，偏置與重力相結合，因此很難在公式10中減去，因此更準確的表達式應該是：

除非使用一些專業(yè)設備，例如分界針。

傳統(tǒng)方法的數值積分法，通常采用零階持有者法（前一個值）進行積分。但是，對于連續(xù)運動，這將引入明顯的誤差。例如，讓我們比較以下方法：

方法一：

（零階持有者）

方法2：

（線性插值）

加速度為 0.5 m/s2在 5 秒內，位移將相差 4 m。仿真結果如圖7所示。

圖7.速度計算中零階積分方法與一階積分方法的比較.

基于前面的討論，我們修改了傳統(tǒng)比力方程的兩點：

我們不使用地球坐標作為導航框架。相反，正如我們在尋找先前的態(tài)度時所做的那樣，我們使用最初的態(tài)度

作為導航框架。通過這種方式，偏差和重力都可以很容易地消除，如公式14所示：



雖然偏置和重力分量包含在初始姿態(tài)中，但這樣我們就不需要將它們分開來得到每個分量，而是可以直接減去它們。

與零階持碼和一階插值相比，我們使用一階來獲得更準確的積分結果。

運動學模式和零速更新技術（ZUPT）

通過使用 IMU 的初始值作為導航框架，我們可以部分抵消加速器的初始偏置影響。但是，即使我們可以在使用設備之前使用分頻頭準確測量偏差，仍然很難消除，除非我們使用另一個精確的傳感器定期校準它。這主要是由兩部分引起的：一是偏置不穩(wěn)定性，這意味著我們之前測量的偏置不是現在的實際偏置。另一種是速度隨機游走，這是加速度不可或缺的一部分。前面提到的不良特性會使我們計算的距離漂移顯著。即使我們停止移動并保持靜止，加速度積分的速度仍然存在，距離仍然會增加。

為了解決這個問題，我們需要找到一種通過使用ZUPT技術重置速度的方法。ZUPT技術緊密依賴于應用，因此我們需要獲得系統(tǒng)和應用的運動學特性，然后為我們的算法給出一些規(guī)則。我們發(fā)現的運動學模式越多，我們的結果就越準確。

我們通過移動帶有SINS系統(tǒng)的轉椅來應用我們的實驗。由于我們的研究不僅限于特定應用，因此我們使用以下運動學假設：

對于航位推算，導航框架中沒有 z 軸移動。此限制僅用于航位推算，不用于姿態(tài)解析。顯然，我們正在2D空間中移動系統(tǒng)。這有助于消除 z 軸誤差。

所有轉彎都在停止后發(fā)生。如果在移動時發(fā)生轉彎，姿態(tài)解析將受到干擾，因為將涉及額外的加速度。

如果系統(tǒng)正在移動，加速度不能保持不變超過 500 毫秒。速度不能保持不變超過 2 秒。由于我們正在推轉椅，因此很難手動保持力準確不變超過500毫秒，人類也很難以勻速持續(xù)推動轉椅超過2秒。事實上，我們正在使用此規(guī)則來執(zhí)行ZUPT。

加速度不能大于±1米/秒2.這個規(guī)則用于一些噪音過濾，這是基于我們對椅子的拉力或推力，不會很大。

如圖 8 所示，當系統(tǒng)沿 X 方向移動時（投影到導航幀后），Y 方向也會產生加速度，積分后，Y 方向速度不會為零，這意味著即使我們只在 X 方向移動，航位推算系統(tǒng)仍然會給我們 Y 分量。

圖8.導航框架中三個方向的加速度。

基于第三個運動學假設，我們可以使用ZUPT來消除這個誤差。ZUPT后的積分速度如圖9所示。

圖9.導航框架中三個方向的速度。

盡管我們使用了第三個假設，如前所述，但仍然無法完全消除該錯誤。誤差消除取決于設置的零加速度和零速度的閾值。但是，大多數錯誤已得到糾正。

基線移位取消

盡管使用了ZUPT，但有時仍可能無法達到零加速度。這導致兩個因素：

我們不能使用ZUPT完全消除偏置不穩(wěn)定性誤差和速度隨機游走。

我們解決的姿態(tài)有一些誤差，這將導致投影（從車身框架到導航框架）加速度誤差。

以圖 10 為例。圖10中的左圖是ADIS16470的原始數據（主體框架），圖10中的右圖是導航幀中預測的加速度。可以看出，當它停止運動時，預計加速度不為零。由于它總是在變化，我們稱之為基線轉移。

圖 10.車身框架（左）和導航框架（右）中的加速度。

為了消除基線偏移，我們需要實時連續(xù)獲取偏移偏差，并將其從預計加速度中減去。結果如圖 11 所示。

圖 11.基線偏移消除之前（頂部）和之后（底部）的加速度。

上圖是基線偏移消除前的加速度，下圖中的綠色軌跡是我們計算的基線偏移，紅色曲線是基線偏移取消后的加速度。

航位推算過程可以使用圖 12 中的框圖簡要描述。我們輸入車身框架加速度一個b和態(tài)度轉換矩陣（來自 AHRS）

到災難恢復系統(tǒng)。完成此操作后，我們將在導航框架中獲得位置。

圖 12.航位推算的流程圖。

實驗結果及結論

實驗結果

如圖13所示，我們使用SPI端口將ADIS16470評估板和RM3100羅盤評估板連接到ADI的ADuCM4050板，從而構建系統(tǒng)。ADuCM4050調整數據格式并進行時間同步（因為IMU和指南針的數據速率不同）。然后使用UART將捕獲的數據傳輸到計算機。所有計算，包括校準、AHRS 和 DR，都在 MATLAB 中執(zhí)行。?

圖 13.實驗平臺設置。

該實驗是通過將電路板和計算機放在轉椅上并在我們的實驗室中將轉椅推一圈來實現的。

AHRS輸出：姿態(tài)以四元數格式和DCM格式顯示，如圖14所示。

圖 14.四元數格式（左）和DCM格式（右）的姿態(tài)。

DR 輸出：具有 X-Y-Z 位置和 3D 圖的航位推算結果如圖 15 所示。

圖 15.倉位計算結果。

結論

本文介紹了使用ADI公司的IMU ADIS16470和地磁傳感器RM3100構建導航系統(tǒng)的基本過程，介紹了我們使用的校準、AHRS和DR方法。在平臺和實驗環(huán)境等條件有限的情況下，我們很難進一步測試平臺和算法。

有很多方法可以用來改善結果，例如：

使用里程表或 UWB 距離測量將加速器與 IMU 融合，以獲得更好的 DR 距離。

使用更復雜的運動學模型，在AHRS和DR中涉及傳感器級別和系統(tǒng)級別的更多特性，例如系統(tǒng)的振動，加速度和減速模型，地面平整度等。這意味著為我們的計算提供更多的邊界條件，以獲得更準確的結果。

使用更精確的數值計算方法，如使用辛普森法則或三次樣條插值在DR中進行積分，或者使用牛頓法代替高斯-牛頓法求解非線性MSE方程等。

我們在實驗中發(fā)現的最后一點，也是最重要的一點是，INS與應用或運動學模式密切相關。例如，我們在兩個地方進行了實驗：一個沒有地毯的實驗室和一個鋪有地毯的辦公室。如果我們使用相同的參數集，DR 結果顯示出巨大的差異。因此，無論是哪種應用，如患者跟蹤、AGV導航、停車定位，還是針對同一應用中的不同工況，我們都需要全面了解其運動學模型。

審核編輯：郭婷