基於IMU和地磁感測器的捷聯慣性導航系統

作者:ADI應用工程師Joel Li 及 現場應用工程師Van Yang

摘要

本文旨在介紹使用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球面擬合方法來校準羅盤感測器。首先,我們需要將感測器旋轉到x-y-z空間中的每個方向,並在3D座標中繪製其值。之後我們需要使用最小平方誤差(MSE)方法將資料擬合為橢圓體。

橢球方程可以表示為

Equation 1

其中,X、Y和Z是羅盤輸出在三個方向上的地磁分量。將這些值擬合為橢圓體意味著,我們需要獲得係數的最優值集合。我們將係數定義為:

Text Equation 1

在擬合時,我們將向量定義為:

Text Equation 2

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

Equation 2

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

圖1.原始羅盤資料分佈(左)和使用橢球擬合後的羅盤資料(右)。

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

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

校準後,我們可以看到,測得的磁場強度(球半徑)幾乎恒定不變,如圖3所示。

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

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

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

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

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

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

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

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

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

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

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

座標定義

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

Text Equation 3

姿態定義

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

Text Equation 4

常用於導航以避免爭議。3

運用卡爾曼濾波器更新姿態

我們在本文中使用的運動學方程(即狀態轉移方程)是偏差方程而非線性方程,因此需要使用一個EKF,用於求該偏差方程的一階線性近似值。對於EKF設計,我們提出以下定義

Text Equation 5

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

Text Equation 6

為角速率;

Text Equation 7

為姿態四元數。

Text Equation 8

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

Text Equation 9

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

Text Equation 10

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

Text Equation 11

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

Text Equation 11.1

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

Text Equation 11.4

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

Text Equation 11.5

Text Equation 11.6

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

Text Equation 11.7

根據該定義,卡爾曼濾波器將透過以下五個步驟完成:

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

Equation 3

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

Equation 4

步驟3:輸出估算狀態:

Equation 5

步驟4:投影狀態:

Equation 6

步驟5:投影誤差協方差矩陣P:

Equation 7

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

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

基於MSE的感測器融合

在上一節中,觀察變數是

Text Equation 12

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

我們將各變數定義如下:

mbab: 體坐標系裡的羅盤磁值和加速度值。

mnan: 導航坐標系裡的羅盤磁值和加速度值。

mn0an0: 導航坐標系裡的初始靜態羅盤磁值和加速度值。

Text Equation 13

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

Text Equation 14

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

根據之前的定義,MSE方法可用於獲得最優

Text Equation 15

通過求方程8的最小值:

Equation 8

通過推導f(q)並使其等於零,

Equation 9

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

通過組合角速率,我們將得到觀察變數,

Text Equation 16

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

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

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

松耦合

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

圖6.姿態計算流程圖。

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

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

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

航位推算方法介紹

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

Equation 10
Equation 11
Equation 12

其中,ae是地球坐標系裡的加速度,ab是體坐標系裡的加速度,ve是地球坐標系裡的速度,se是地球坐標系裡的距離,ge是地球坐標系裡的重力加速度,為[0 0 1],單位為g。需要強調的是,地球坐標系與導航坐標系不同——地球坐標系是面向NED的。該δt是解析週期。

運用第一個等式可以得到從IMU體坐標系到地球坐標系的加速度投影,如格式所示。

Text Equation 17

第二個等式將加速度積分或累加為速度;然而,由於測量的加速度涉及重力分量,所以需要減去重力。

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

傳統方法存在幾個問題。

  • 加速器輸出總是有偏置,與重力相結合後,難以從公式10中減去,因此更準確的運算式應為:
    Equation 13
    除非使用一些專業設備,例如分度頭。
  • 作為傳統方法的數值積分方法通常使用零階保持器方法(前一個值)進行積分。但是,對於連續移動,這將帶來重大的誤差。例如,我們來比較以下方法:

方法1:

Text Equation 18

(零階保持器)

方法2:

Text Equation 19

(線性插值)

在5秒內加速度為0.5 m/s2時,位移最高將相差4m。模擬結果如圖7所示。

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

基於前面的討論,我們根據應用要求,修改了傳統比力方程的兩個點:

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

    Text Equation 20
    作為導航坐標系。通過這種方式,偏置和重力都可以輕鬆取消,如公式14所示:

    Equation 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所示。

圖11.基線偏移消除之前(頂)和之後(底)的加速度。

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

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

Text Equation 21

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

圖12.航位推算流程圖。

實驗結果與結論

實驗結果

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

圖13.實驗平台設置。

圖14.四元數格式(左)和DCM格式(右)的姿態。

  • AHRS輸出:姿態以四元數格式和DCM格式表示,如圖14所示。
圖14.四元數格式(左)和DCM格式(右)的姿態。
  • DR輸出:帶XYZ位置的航位推算結果和三維圖如圖15所示。
圖15.位置計算結果。

結論

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

有很多方法可用於改善結果,例如:

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

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

參考文獻

1. Long Li and Zhang He. "三軸磁力計的自動和自我調整校準方法",《中國儀器儀錶學報》,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. "因應基於MARG感測器的四元數方向估算應用的擴展卡爾曼濾波器", IEEE, 2001.

Prikhodko, Igor P. and Brock Bearss. "使用MEMS IMU邁向自動駕駛汽車:挑戰與機遇", IEEE, 2018.

RM3100,PNI感測器公司,2018。

Woodman, Oliver J. "慣性導航簡介",劍橋大學,2007年8月。


Authors

Joel Li

Joel Li

Joel Li (Zhao Li) joined ADI in 2017 as an applications engineer and currently works on some applications of IMU and lidar in China. Joel received his master's degree in photoelectric detection from the Chinese Academy of Sciences in 2013.

Van Yang

Van Yang

Van Yang is a Field Application Engineer at Analog Devices. Inc., in Shanghai, China. He joined ADI in 2015 to support regional medical and industrial customers in China. Prior to joining ADI, Van worked at TI as an FAE for 4 years. Van earned his master's degree in communication and information system from Huazhong University of Science and Technology in Wuhan, in 2011. In his spare time he is a super fan of basketball, and enjoys hiking.