【解決方案】基於IMU和地磁感測器的捷聯慣性導航系統

摘要

本文旨在介紹我們使用ADI公司的慣性測量單元(IMU)感測器ADIS16470和PNI的地磁感測器RM3100建構的捷聯慣性導航系統(SINS)。實現了基於磁力、角速率和重力(MARG)的SINS的一些基本過程，包括電磁羅盤（地磁感測器）校準、使用擴展卡爾曼濾波器(EKF)的姿態和航向參考系統(AHRS)和航跡跟踪。也實現了使用最小平方誤差(MSE)方法的鬆散耦合感測器融合技術。文章展示了每個過程步驟使用的演算法和實驗設定。本文最後討論了結果分析和用於提高導航準確性的方法。

簡介

隨著服務機器人市場和技術的發展，導航已成為研究和應用的熱點。與車輛、船舶或飛機相比，服務機器人體積小，成本低，因此它們的導航系統應該具有捷聯和低成本的特性。傳統的穩定平台導航系統通常要採用獨立的加速度計和光纖或雷射陀螺儀，所有感測器都機械且剛性地安裝在與正在移動的車輛隔離的穩定平台上。這導致了尺寸大、可靠性差、成本高的缺點。相反，在捷聯慣導系統中，慣性感測器直接固定在車輛本體上，這意味著感測器會與車輛一起旋轉。這種捷聯方法消除了穩定平台慣導的缺點。然而，平台慣導的準確性通常高於SINS。平台慣導往往可以達到戰略級（0.0001°/時的陀螺儀偏置，1μg的加速器偏壓）或軍用級（0.005°/時的陀螺儀偏置，30μg的加速器偏置），而多數SINS只能到達導航等級（0.01°/時的陀螺儀偏置，50μg的加速器偏壓）或戰術級（10°/時的陀螺儀偏置，1mg的加速器偏置）。對於大多數服務機器人或AGV導航應用，這一精度就足夠了。

導航方法很多，包括機器視覺、GPS、UWB、SLAM型雷射雷達等。基於IMU的慣性導航始終是導航的重要組成部分。然而，由於這種感測器的限制——例如偏置誤差、軸間誤差、噪聲，特別是零偏不穩定性——慣性導航通常需要採用一個夥伴感測器，定期為它提供參考或校準，本文將這種情況稱為感測器融合。許多感測器都可以與IMU融合，例如攝影機和里程表，但在這些感測器中，地磁感測器是一種低成本的方案，可與IMU配合獲得姿態資訊。

在本文中，我們使用ADI的IMU ADIS16470和地磁感測器來開發平台和演算法，實現捷聯慣性導航系統。但是，地磁感測器只能提供姿態資訊。對於航位推算或距離測量，我們只能使用IMU中的加速度感測器。

ADIS16470 IMU簡介

ADI公司的ADIS16470是一款微型MEMS IMU，整合了3軸陀螺儀和3軸加速度計。其陀螺儀零偏穩定性為8°/時，加速計零偏穩定性為13μ g其關鍵參數都經過原廠校正。此外，ADIS16470的低價格在同級產品中具有吸引力，並得到了許多客戶的廣泛使用。在本文中，我們使用微控制器與ADIS16470透過SPI介面進行通訊。

地磁感測器介紹

地磁感測器是用於測量羅盤體座標（即座標系）中的地磁場的感測器，可為航向提供絕對參考。其x、y和z分量值由本地地磁場投影而來。這種感測器有兩個主要缺點——一是精度和解析度不高——例如，常用的霍尼韋爾羅盤感測器HMC5883L的解析度僅為12位元。另一個缺點是感測器容易受到周圍環境的干擾，因為地磁場非常弱，強度範圍為毫高斯到8高斯。

儘管有這些缺點，仍然可以在許多情況下使用，例如戶外、低EMI環境等。將地磁感測器與IMU進行鬆散耦合，就可以在大多數環境中使用這類感測器。

在本文中，我們使用PNI感測器公司的高性能電子羅盤感測器RM3100，它提供了24位元解析度。PNI使用主動激勵法來提高抗噪音能力。

羅盤感測器的校準

在使用羅盤感測器之前，需要對其進行校準以消除兩個主要誤差。一個是失調誤差，原本是由感測器和電路的失調誤差所造成的。另一個是標度誤差。這兩種誤差都容易受到周圍磁環境的干擾。例如，如果有一個x軸向的外部磁場施加到感測器上，就會給出外部x軸失調誤差。同時，x軸標度也將與y軸和z軸不同。

通常用於校準磁性感測器的方法是在xy平面上轉動感測器繞圈，然後抽取數據。一個地點的地磁場強度是一個常數值，因此繪製的數據應該是一個圓；然而，事實上，我們將看到一個橢圓形，這意味著我們需要移動橢圓並重新縮放到以零為中心的圓。

上述2D校準方法有一些缺點，並且需要用加速器來測量其傾斜度。我們使用3D球面擬合方法來校準羅盤感測器。首先，我們需要將感測器旋轉到xyz空間中的每個方向，並在3D座標中繪製其值。然後我們需要使用最小平方誤差(MSE)方法將資料擬合為橢球面。

橢球方程式可以表示為

其中，X、Y和Z是羅盤輸出在三個方向上的地磁分量。將這些值擬合為橢球面意味著，我們需要得到一組最優係數解。我們將係數定義為：

在擬合時，我們定義向量：

所以我們需要計算最優σ，並使用公式2找出最小值：

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

圖1. 原始羅盤資料分佈（左）與使用橢球擬合後的羅盤資料（右）。

為了校準感測器，我們需要拉伸或壓縮擬合的橢球面並將其移至以零為中心的球面上。我們使用矩陣奇異值分解(SVD)方法來進行這種校準。校準後的球體如圖2所示。1,2

圖2. 以SVD方法進行球體校準後的羅盤資料。

校準後，我們可以看到，測得的磁場強度（球半徑）幾乎恆定不變，如圖3所示。

圖3. 校準前和校準後的磁場比較。

使用ADIS16470和羅盤的姿態和航向參考系統

AHRS由三個軸上的感測器組成，提供姿態訊息，包括橫滾角、俯仰角和偏航角。AHRS是一個來自飛機導航的概念。我們用它來描述方向，即姿態。

在介紹我們的方法之前，有必要先解釋為什麼確定姿態需要進行融合。事實上，我們的系統現在有三種感測器：陀螺儀、加速器和羅盤（地磁感測器）。

陀螺儀提供圍繞各軸的旋轉角速度。透過角速率積分計算，我們可以得到旋轉角度。如果我們知道初始航向，透過角度就始終能夠得到航向姿態。積分將累積陀螺儀的不穩定零偏，這將導致角度誤差。此外，來自陀螺儀的高斯分佈雜訊將積分成一個布朗運動過程，並導致隨機遊走誤差。因此，我們很難長時間使用陀螺儀，陀螺儀需要定期校準。

加速度計提供每個軸方向的移動加速度。在靜態狀態下，我們可以得到每個軸與重力加速度之間的角度。由於重力加速度在方向和值上恆定不變，我們可以得到相對於重力方向的航向姿態。然而，此方法使用重力加速度作為參考，因此無法解出圍繞重力加速度的角度。

羅盤提供從地磁場投影的每個軸的值。我們可以從每個軸與恆為常數向量的地磁場方向之間的關係推導出角度值。如前一節所述，由於對外部磁場的抗擾性較差，羅盤需要一個低幹擾的環境。

從這個解釋中，我們可以看到，很難靠一個感測器來找到姿態，我們需要組合使用兩個或三個感測器並把資訊融合起來。本文用加速度計、陀螺儀和地磁羅盤找出姿態。這種融合也被稱為磁、角速率和重力(MARG)系統。

擴展卡爾曼濾波器的設計與感測器融合

有幾種方法可以將IMU和羅盤資料融合起來，例如互補濾波器、統計學ARMA濾波器，卡爾曼濾波器等。我們在本文中使用的是擴展卡爾曼濾波器。

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

座標定義

T航向或方向是兩個座標（即座標系）之間的關係。一個座標總在變化，另一個座標保持不變。對於座標定義方法，我們使用導航座標和體座標。與東北地(NED)座標系或地理方法相反，我們將測量的初始體座標值定義為導航座標系，此後該座標為恆定座標。從體座標到導航座標的映射（投影）矩陣定義為

姿態定義

與歐拉角或方向餘弦矩陣(DCM)不同，我們在這裡使用四元數，定義為

常用於導航以避免奇異性。

用卡爾曼濾波器更新姿態

我們在本文中使用的運動學方程（即狀態轉移方程）是非線性微分方程，因此需要使用一個EKF，用於對此微分方程進行一階近似。對於EKF設計，我們定義

一個1×7向量作為狀態變量，其中

為角速率；

為姿態四元數。

一個1×7向量作為觀測變量，與狀態變數具有相同的分量。

一個7×7矩陣作為狀態轉移矩陣，其中，A的第一部分是角速率的數字化微分方程，第二部分是數字化四元數更新方程，後者從運動學方程推導而來。

一個7×7矩陣為觀察矩陣。

為誤差協方差矩陣，這是一個7×7矩陣，其中

估計向量x̂真實值x x之間的誤差我們在測試中將初始誤差設為相對較小的值。該值會自動收斂到一個小值。

被設為狀態轉移雜訊和觀測雜訊的協方差矩陣。我們得到它們的初始值，

 

在保持IMU和羅盤處於靜止狀態的同時，透過測量陀螺儀和加速器的交流均方根值的平方得到。我們設

根據以上定義，卡爾曼濾波器將透過以下五個步驟完成：

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

步驟2：計算誤差協方差矩陣，P :

步驟3：輸出估算狀態x̂ :

步驟4：更新狀態x̂ – :

步驟5：更新誤差協方差矩陣P – :

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

圖4. 用於更新姿態的卡爾曼濾波器流程圖。

基於MSE的感測器融合

在上一節中，觀測變數是

其中沒有來自羅盤的訊息。由於ω是角速率，我們只能使用四元數來導入羅盤資料q . 我們使用MSE方法來獲得q , 即觀測變數中的組分。

我們將各變數定義如下：

m b和a b : 體座標系裡的羅盤磁值和加速度值。

m n和a n : 導航座標系裡的羅盤磁值和加速度值。

m n0和a n0 : 導航座標系裡的初始靜態羅盤磁值和加速度值。

為從體座標係到導航座標系的姿態轉換矩陣，以四元數表示，可以寫成

其給出了導航座標系中初始值與即時從體座標系映射到導航座標系的值之間的誤差ε。

根據先前的定義，MSE方法可用來求取最優值。

透過求方程式8的最小值：

對f(q)求導並使其等於零，

我們將得到方差意義上的最優q。我們使用高斯-牛頓方法，用一階梯度收斂來解上述非線性方程式。

透過組合角速率，我們將得到觀測變量

其中融合了卡爾曼濾波器中的羅盤資料和IMU資料。

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

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

鬆散耦合

如前所述，我們經常遇到無法使用羅盤感測器的情況。如果磁性資料受到干擾，則求解的姿態精度將比僅使用IMU時更差。因此，我們使用鬆散耦合來判斷磁感測器是否可用。當磁性感測器不可用時，我們只用IMU來求解姿態；當磁性感測器可用時，我們將使用融合演算法找到姿態，如圖6所示。

圖6. 姿態計算流程圖。

在獲得新資料之後或在求解新的姿態時（在某些系統中，採樣週期與姿態解算週期不同，但我們在此處進行的是單採樣週期解算），我們計算加速度的大小，如果結果不等於1 g , 我們就不會使用加速器的輸出來進行姿態計算。然後我們計算羅盤輸出的大小並與初始值進行比較。如果它們彼此不相等，我們就不會在此週期中使用地磁感測器的資料。當滿足兩個條件時，我們會使用卡爾曼濾波器並執行MSE融合。

使用ADIS16470進行航位推算(DR)

在導覽中，航位推算是計算目前位置的過程，先使用先前確定的位置，然後在解算週期中基於已知或估計的速度或加速度更新該位置。這裡將使用ADIS16470裡的加速度計。基於上一節解出的姿態，我們可以得到捷聯繫統的移動方向，然後需要計算該方向上的距離，最後確定位置。

捷聯航位推算需要使用基於加速度測量的比力方程式來追蹤INS的位置。比力方程式可以簡單描述為等式10、等式11和等式12：

其中a e是地球座標系的加速度，a b是體座標系裡的加速度，v e是地球座標系裡的速度，s e是地球座標系裡的距離，g e是地球座標系裡的重力加速為[0 0 1]，單位為g。需要強調的是，地球座標系與導航座標係不同－地球座標係是基於NED的。該δ t t是解算週期。

用第一個等式可以得到從IMU體座標係到地球座標系的加速度映射，如格式

第二個等式將加速度積分或累積為速度；然而，由於測量的加速度包含了重力分量，所以需要減去重力。

與等式11類似，等式12將速度積分成距離。

傳統方法存在幾個問題。

• 加速度計輸出總是有偏置，與重力結合後，難以從公式10中減去，因此更準確的表達式應為：

  

  除非是用一些專業設備來測量該偏置，例如分度頭。

• 基於數值積分的實作方式，通常使用零階保持器方法（前一個值）進行積分。但是，對於連續移動，這將帶來重大的誤差。例如，我們來比較以下方法：

方法1：

（零階保持器）

方法2：

（線性內插）

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

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

基於前面的討論，基於應用，我們修改了傳統比例方程式的兩個地方：

• X 我們不使用地球座標作為導航座標系。相反，正如我們在計算先前姿態時所做的那樣，我們用初始姿態

  

  作為導航座標系。透過這種方式，偏壓和重力都可以輕鬆取消，如公式14所示：雖然
  
  
  
  姿態中同時包含了偏壓和重力分量，但這樣我們就不需要將它們分開成單獨的分量，而是直接一起減去它們。

• 基於零階保持器與一階插值之間的比較，我們使用一階方法來獲得更準確的積分結果。

運動學模式與零速更新技術(ZUPT)

透過使用IMU的初始值作為導航座標系，我們可以消除一部分加速度的偏壓影響。然而，即使我們在使用設備之前能用分度頭準確測量偏置，仍然很難取消，除非使用另一個精確的感測器來定期校準它。這主要是由兩個原因引起的：一是偏置不穩定，這意味著我們先前測量的偏移不是現在的實際偏移。二是速度隨機遊走，由加速度雜訊積分而來。前面提到的不良特性會使我們計算的距離顯著漂移。即使我們停止移動並保持靜止，從加速度積分而來的速度仍然存在，距離仍會增加。

要解決這個問題，我們需要找到一種透過使用ZUPT技術重置速度的方法。ZUPT技術與特定的應用密切相關，因此我們需要獲得系統和應用的運動學特徵，然後給出一些演算法規則。我們發現的運動學模式越多，結果就越準確。

我們透過移動帶有SINS系統的轉椅來進行實驗。由於我們的研究不限於特定應用，我們使用以下運動學假設：

• 對於航位推算，導航座標系中沒有z軸移動。此限制僅適用於航位推算，不適用於姿態求解。顯然，我們是在二維空間中移動系統。這有助於消除z軸誤差。

• 所有轉彎都發生在停止後。如果在移動時發生轉彎，則會因為引入額外加速而乾擾姿態求解。

• 如果系統正在移動，加速度不能保持不變超過500毫秒。速度不能維持不變超過2秒。由於我們正在推動或拉動轉椅，因此很難手動使力精確地保持不變超過500毫秒，並且個人很難以勻速持續推動轉椅2秒以上。事實上，我們正​​是運用這項規則來實施ZUPT。

• 加速度不能大於±1 m/s2。這個規則用於一些噪音過濾，後者基於我們施加於椅子上、不會很大的拉力或推力。

如圖8所示，當系統在X方向上移動時（投影到導航座標係後），Y方向也會產生加速度；積分後，Y方向速度不會為零，這意味著即使我們只是在X方向上移動，航位推算系統仍會為我們帶來Y分量。

圖8. 導航座標系中三個方向的加速度。

基於第三條運動學假設，我們可以使用ZUPT來消除此誤差。經ZUPT處理後的積分速度如圖9所示。

圖9. 導航座標系中三個方向的速度。

雖然我們使用了第三個假設，如前所示，誤差仍然無法完全取消。誤差消除取決於設定的零加速度和零速度的判斷閾值。但是，大多數誤差已修正。

雖然使用了ZUPT，但有時仍然無法達到零速。這由兩個因素導致：

• 我們無法用ZUPT完全消除偏移不穩定誤差和速度隨機遊走。
• 我們求出的姿態有一些誤差，結果會導致投影（從體座標係到導航座標系）後的加速度誤差。

以圖10為例。圖10中的左圖是ADIS16470的原始資料（體座標系），圖10的右圖是投影到導航座標系的加速度。可以看出，停止移動時，投影加速度不為零。由於它總是在變化，我們在此處稱之為基線漂移。

圖10. 體座標系（左）和導航座標系（右）的加速度。

為了消除基線漂移，我們需要即時連續獲得偏移偏壓並從投影加速度中減去該值。結果如圖11所示

Figure圖11. 基線漂移消除之前（上）和之後（下）的加速度。

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

可以使用圖12的框圖簡要描述航位推算過程。我們將體座標系加速度ab和姿態轉移矩陣（來自AHRS）輸入

到DR系統。完成後，我們將獲得導航座標系中的位置。

圖12. 航位推算流程圖

實驗結果與結論

實驗結果

使用SPI端口，我們將ADIS16470評估板和RM3100羅盤評估板連接到ADI公司的ADuCM4050電路板，建構出我們的系統，如圖13所示。ADuCM4050調整資料格式並進行時間同步（因為IMU和羅盤的資料速率不同）。然後使用UART將捕獲的數據傳輸到計算機。所有計算（包括校準、AHRS和DR在MATLAB）均在MATLAB ®中執行。

圖13. 實驗平台設置。

將評估板和計算機放在轉椅上，並在實驗室中推著轉椅繞圈。

• AHRS輸出：姿態以四元數格式和DCM格式表示，如圖14所示。

 

圖14. 四元數格式（左）與DCM格式（右）的姿態。

• DR輸出：有XYZ位置的航位推算結果與立體圖如圖15所示。

圖15. 位置計算結果。

結論

本文介紹了使用ADI公司的IMU ADIS16470和地磁感測器RM3100建構捷聯慣導系統的基本流程，介紹了我們使用的校準、AHRS和DR方法。在平台和實驗環境等條件有限的情況下，很難進一步測試平台和演算法。

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

• 使用里程表或UWB距離測量方法與IMU中的加速度計融合，以在DR中獲得更準確的距離值。
• 使用更複雜的運動學模型，從而在AHRS和DR中在感測器和系統層次引入更多特性，例如係統的振動、加速和減速模型、地面平整度等。這意味著為了提高導航結果的準確性，需要給出更多的邊界條件。
• X 使用更精確的數值計算方法，例如用辛普森規則或三次樣條插值在DR中進行積分，或使用牛頓方法而非高斯-牛頓方法求解非線性MSE方程式等。

最後但也是最重要的一點，我們在試驗中發現INS與應用或運動學模型緊密相關。例如，我們在兩個地方進行了實驗：未鋪地毯的實驗室和鋪有地毯的辦公室。如果我們使用相同的參數集，DR結果會顯示出巨大的差異。因此，無論哪種應用，例如患者追蹤、AGV導航或停車定位，或對於同一應用中的不同條件，我們都需要全面了解其運動學模型。

參考電路

1 Long Li和Zhang He，“Automatic and Adaptive Calibration Method of Tri-axial Magnetometer”（三軸磁力計的自動和自適應校準方法），《中國儀器儀表學報》，2013。

2 Timothy Sauer，Numerical Analysis（數值分析（第2版）），Pearson，2011。

3 David H. Titterton，Strapdown Inertial Navigation Technology（捷聯慣性導航技術（第2版）），電氣工程師學會，2004。

Gongmin, Yan，「Research on Vehicle Autonomous Positioning and Orientation System」（車輛自主定位定向系統研究），博士論文，中國西北工業大學，2006。

Marins, João Luís，「An Extended Kalman Filter for Quaternion-Based Orientation Estimation Using MARG Sensors」（面向基於MARG感測器的四元數方向估算應用的擴展卡爾曼濾波器），IEEE，2001。

Prikhodko、Igor P.和Brock Bearss，「Towards Self-Navigating Cars Using MEMS IMU：Challenges and Opportunities」（使用MEMS IMU邁向自動駕駛汽車：挑戰與機會），IEEE，2018。

RM3100 .PNI感測器公司，2018。

Woodman, Oliver J. “ An Introduction to Inertial Navigation .” （慣性導航簡介），劍橋大學，2007年8月。

作者

Joel Li

 

 

---

聯繫我們

ADI 所有產品請洽【 安馳科技 】
安馳科技｜ADI亞德諾半導體網站：https://anstekadi.com
安馳科技 LINE 官方帳號：https://lin.ee/5gcKNi7
安馳科技 Facebook 官方帳號：https://www.facebook.com/ANStek3528
安馳科技EDM訂閱 https://anstekadi.com/Home/EDM
申請樣品與技術支援：https://www.surveycake.com/s/dQ3Y2 
與我聯絡：Marketing.anstek@macnica.com