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

摘要

本文介紹了我們使用模擬設(shè)備的慣性測量單元（IMU）傳感器ADIS16470和PNI的地磁傳感器RM3100構(gòu)建的捷聯(lián)慣性導(dǎo)航系統(tǒng)（SINS）。實現(xiàn)了基于磁力，角速率和重力（MARG）的SINS的一些基本過程，包括電磁羅盤校準(zhǔn)，使用擴(kuò)展卡爾曼濾波器（EKF）的姿態(tài)和航向參考系統(tǒng)（AHRS）以及跟蹤跟蹤。還實現(xiàn)了使用最小平方誤差（MSE）方法松散耦合傳感器融合。顯示了每個過程步驟中的算法和實驗設(shè)置。本文最后討論了結(jié)果分析和提高精度的方法。

簡介

隨著服務(wù)機(jī)器人市場和技術(shù)的發(fā)展，導(dǎo)航成為一項熱門研究和應(yīng)用點。與車輛，船舶或飛機(jī)相比，服務(wù)機(jī)器人體積小，成本低，因此它們的導(dǎo)航系統(tǒng)應(yīng)該是捷聯(lián)和低成本的。傳統(tǒng)的穩(wěn)定平臺導(dǎo)航系統(tǒng)通常涉及單獨的加速器和光纖或基于激光的陀螺儀，所有部件都機(jī)械且剛性地安裝在與移動車輛隔離的穩(wěn)定平臺上。這導(dǎo)致尺寸大，可靠性差和成本高的缺點。相反，在捷聯(lián)導(dǎo)航系統(tǒng)中，慣性傳感器直接固定在車輛的車身上，這意味著傳感器與車輛一起旋轉(zhuǎn)。這種捆綁方法消除了穩(wěn)定平臺導(dǎo)航的缺點。但是，平臺導(dǎo)航的準(zhǔn)確性通常高于SINS。平臺導(dǎo)航通?？梢赃_(dá)到戰(zhàn)略等級（0.0001°/小時陀螺儀偏差，1μ g 加速器偏壓）或軍用級（0.005°/ hr陀螺儀偏差，30μ g 加速器偏差），而大多數(shù)SINS只能達(dá)到導(dǎo)航等級（0.01°/ hr陀螺儀偏差，50μ g 加速器偏壓）或戰(zhàn)術(shù)等級（10°/ hr陀螺儀偏差，1 m g < / em>加速器偏差）。對于大多數(shù)服務(wù)機(jī)器人或AGV導(dǎo)航應(yīng)用來說，這已足夠。

有多種導(dǎo)航方法，包括機(jī)器視覺，GPS，UWB，帶SLAM的激光雷達(dá)等。慣性導(dǎo)航始終是導(dǎo)航的重要組成部分，使用IMU的地方。然而，由于這種傳感器的限制 - 例如偏置誤差，橫軸誤差，噪聲，特別是偏置不穩(wěn)定性 - 慣性導(dǎo)航通常需要伙伴傳感器定期給它一個參考或校準(zhǔn)，這稱為傳感器融合這里。有許多傳感器可以與IMU融合，例如攝像機(jī)和里程表，但在這些傳感器中，地磁傳感器是與IMU一起獲得態(tài)度的低成本方式。

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

ADIS16470 IMU簡介

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

地磁傳感器介紹

地磁傳感器是一種用于測量羅盤體內(nèi)地磁場的傳感器坐標(biāo)（即框架），它為標(biāo)題提供絕對參考。其x，y和z分量值從本地地磁場投射。這種傳感器有兩個主要缺點 - 一個是它的精度和分辨率不高 - 例如，霍尼韋爾常用的羅盤傳感器HMC5883L只有12位分辨率。另一個缺點是傳感器容易受到周圍環(huán)境的干擾，因為地磁場非常弱，從毫高斯到8高斯。

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

< p>在本文中，我們使用高性能羅盤傳感器，PNI傳感器公司的RM3100，提供24位分辨率。 PNI使用主動激勵方法來提高抗噪聲能力。

羅盤傳感器的校準(zhǔn)

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

通常用于校準(zhǔn)磁傳感器的方法是將傳感器旋轉(zhuǎn)為xy平面中的圓，然后提取數(shù)據(jù)。一個地方的地磁場強(qiáng)度是一個常數(shù)值，因此繪制的數(shù)據(jù)應(yīng)該是一個圓;然而，事實上，我們將看到一個橢圓形，這意味著我們需要將橢圓移動并重新縮放到以零為中心的圓。

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

橢球方程可以表示為

其中X，Y和Z是羅盤輸出三個方向的地磁分量。將這些值擬合為橢圓體意味著我們需要獲得系數(shù)的最佳值集。我們將系數(shù)定義為：

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

所以我們需要計算最佳σ，并使用公式2來找到最小值：

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

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

校準(zhǔn)后，我們可以看出測得的磁場強(qiáng)度（球半徑）將保持幾乎恒定的值，如圖3所示。

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

AHRS由三個軸上的傳感器組成，提供姿態(tài)信息，包括滾轉(zhuǎn)，俯仰和偏航。 AHRS是飛機(jī)導(dǎo)航的概念。我們用它來描述方向;也就是說，態(tài)度。

在介紹我們的方法之前，有必要首先解釋融合的原因。實際上，我們現(xiàn)在的系統(tǒng)中有三種傳感器：陀螺儀，加速器和羅盤。

陀螺儀給出了圍繞每個軸的旋轉(zhuǎn)角速度。通過積分角速率，我們可以找到旋轉(zhuǎn)角度。如果我們知道最初的標(biāo)題，我們將始終獲得標(biāo)題態(tài)度。由于積分，陀螺儀的不穩(wěn)定偏差將被累積，這將導(dǎo)致角度誤差。此外，來自陀螺儀的高斯分布噪聲將被整合到布朗運(yùn)動過程中并導(dǎo)致隨機(jī)行走誤差。因此，我們很難長時間使用陀螺儀，并且陀螺儀需要定期校準(zhǔn)。

加速器提供每個軸方向的加速度。在靜態(tài)狀態(tài)下，我們可以得到每個軸與重力加速度之間的角度。由于重力加速度在方向和值上是恒定的，我們可以獲得相對于重力方向的航向姿態(tài)。然而，這種方法使用重力加速度作為參考，因此無法解決圍繞重力加速度旋轉(zhuǎn)的角度。

羅盤提供從地磁場投射的每個軸的值。我們可以從每個軸與地磁場方向之間的關(guān)系推導(dǎo)出角度值，地磁場方向也是一個常數(shù)向量。如前一節(jié)所述，由于對外部磁場的抗擾性較差，羅盤需要一個低干擾環(huán)境。

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

擴(kuò)展卡爾曼濾波器設(shè)計和傳感器融合

將IMU和羅盤數(shù)據(jù)融合在一起有多種方法，例如互補(bǔ)濾波器，統(tǒng)計ARMA濾波器，卡爾曼濾波器等。我們在本文中使用擴(kuò)展卡爾曼濾波器。

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

坐標(biāo)定義

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

姿態(tài)定義

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

，它通常用于導(dǎo)航以避免參數(shù)。 3

使用卡爾曼濾波器更新姿態(tài)< / h3>

運(yùn)動學(xué)方程，即狀態(tài)轉(zhuǎn)移方程，我們在本文中使用的是偏差格式，它不是線性的，因此我們需要使用EKF和偏差方程的一階線性逼近。對于EKF設(shè)計，我們定義

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

是角速率，

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

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

一個7×7矩陣，是狀態(tài)轉(zhuǎn)移矩陣，其中 A 中的第一部分是角速率的數(shù)字化微分方程，第二部分是數(shù)字化四元數(shù)更新方程，它是從運(yùn)動學(xué)方程導(dǎo)出的。

7×7矩陣，是觀察矩陣。

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

是估計的誤差向量， x ，來自真實， X 。我們在測試中將初始誤差設(shè)置為相對較小的值。它將自動收斂到一個小值。

被設(shè)置為狀態(tài)轉(zhuǎn)換噪聲和觀察噪聲的噪聲協(xié)方差。我們通過測量陀螺儀和加速器的交流有效值的平方來得到它們的初始值，

和

。在保持IMU和羅盤處于靜止?fàn)顟B(tài)的同時，我們設(shè)置

根據(jù)這個定義，我們的卡爾曼濾波器將通過以下五個步驟完成：

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

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

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

步驟4：預(yù)測狀態(tài)，< em> x - ：

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

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

基于MSE的傳感器融合

在上一節(jié)中，觀察變量是

，其中沒有來自指南針的信息。由于ω是角速率，我們只能使用四元數(shù)來導(dǎo)入羅盤數(shù)據(jù) q 。我們使用MSE方法得到 q ，即觀察變量中的組件。

我們將變量定義如下：

m b 和 a b ：車身框架中的羅盤磁性值和加速度值。

m n 和 a n ：導(dǎo)航框中的羅盤磁性值和加速度值。

m n0 和 a n0 ：導(dǎo)航框架中的初始靜態(tài)羅盤磁性值和加速度值。

是從身體框架到導(dǎo)航框架的姿態(tài)轉(zhuǎn)換矩陣，用四元數(shù)表示，可以寫成

，它給出了錯誤ε，導(dǎo)航框架中的初始值和實時從主體框架映射到導(dǎo)航框架的值。

根據(jù)之前的定義，MSE方法可用于獲得最佳

通過最小化公式8：

通過推導(dǎo) f（q）并使其等于零，

< img src>

我們將獲得方差意義上的最佳 q 。我們使用Gauss-Newton方法用一階梯度收斂來求解上面的非線性方程。

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

，它將羅盤數(shù)據(jù)和IMU數(shù)據(jù)融合在卡爾曼濾波器中。

過程可以是簡單地用圖5中的方框圖描述。

松散耦合

如前所述，我們經(jīng)常遇到無法使用羅盤傳感器的情況。如果磁數(shù)據(jù)受到干擾，則解決的姿態(tài)精度將比僅使用IMU時更差。因此，我們使用松耦合，判斷磁性傳感器是否可用。當(dāng)磁力傳感器不可用時，我們將僅使用IMU來找到姿態(tài)，當(dāng)磁力傳感器可用時，我們將使用融合算法找到姿態(tài)，如圖6所示。

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

航位推算（DR）使用ADIS16470

在導(dǎo)航中，航位推算是計算當(dāng)前電流的過程通過使用先前確定的位置來定位，并基于解析周期內(nèi)的已知或估計的速度或加速度推進(jìn)該位置。這里將使用ADIS16470加速器?；谏弦还?jié)中解決的態(tài)度，我們得到捷聯(lián)系統(tǒng)的移動方向，然后我們需要計算方向上的距離，最終確定位置。

航位推算方法簡介

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

其中 a e 是地球框架中的加速度， a b 是車身框架中的加速度， v e 是地球框架中的速度， s e 是地球框架中的距離， g e 是地球框架中的重力加速度，以 g 為單位為[0 0 1]。我們需要強(qiáng)調(diào)的是，地球框架與導(dǎo)航框架不同 - 地球框架是面向NED的。這個δ t 是解析周期。

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

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

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

傳統(tǒng)方法存在一些問題。

加速器輸出總是有偏差，與重力相結(jié)合，使其難以在公式10中減去，因此更準(zhǔn)確的表達(dá)應(yīng)該是：
除非使用一些專業(yè)設(shè)備，例如作為分割頭。

數(shù)值積分方法，傳統(tǒng)方法，通常使用零階持有者方法（前一個值）進(jìn)行積分。但是，對于連續(xù)移動，這將引入重大錯誤。例如，讓我們比較以下方法：

方法1：

（Zeroth-order holder）

方法2：

（線性插值）

在5秒內(nèi)加速度為0.5 m / s 2 時，位移將相差4 m 。仿真結(jié)果如圖7所示。

基于前面的討論，我們修改了傳統(tǒng)特定力方程的兩個點用于我們的應(yīng)用：

我們不要將地球坐標(biāo)用作導(dǎo)航框架。相反，正如我們在尋找先前態(tài)度時所做的那樣，我們使用初始態(tài)度

作為導(dǎo)航框架。通過這種方式，偏差和重力都可以輕松取消，如公式14所示：

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

比較零階保持器和一階插值，我們使用一階來獲得更準(zhǔn)確的積分結(jié)果。

運(yùn)動學(xué)模式和零速度更新技術(shù)（ZUPT）

通過使用IMU的初始值作為導(dǎo)航框架，我們可以部分取消加速器的初始偏差影響。然而，即使我們在使用設(shè)備之前使用分割頭可以準(zhǔn)確地測量偏差，仍然很難取消，除非我們使用另一個精確的傳感器來定期校準(zhǔn)它。這主要是由兩部分引起的：一部分是偏差不穩(wěn)定，這意味著我們之前測量的偏差現(xiàn)在不是實際偏差。另一種是速度隨機(jī)游走，它是加速度的組成部分。前面提到的不希望的特性將使我們計算的距離顯著漂移。即使我們停止移動并保持靜止，加速度積分的速度仍然存在，距離仍然會增加。

要解決這個問題，我們需要找到一種方法來使用ZUPT重置速度技術(shù)。 ZUPT技術(shù)緊密依賴于應(yīng)用，因此我們需要獲得系統(tǒng)和應(yīng)用的運(yùn)動學(xué)特征，然后給出我們算法的一些規(guī)則。我們找到的運(yùn)動學(xué)模式越多，我們的結(jié)果就越準(zhǔn)確。

我們通過移動帶有SINS系統(tǒng)的轉(zhuǎn)椅來應(yīng)用我們的實驗。由于我們的研究不僅限于特定應(yīng)用，我們使用以下運(yùn)動學(xué)假設(shè)：

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

停止后發(fā)生所有轉(zhuǎn)彎。如果在移動時發(fā)生轉(zhuǎn)彎，則會影響姿態(tài)分辨，因為將涉及額外的加速度。

如果系統(tǒng)正在移動，加速度不能保持不變超過500毫秒。速度不能保持不變超過2秒。由于我們正在推動或拉動轉(zhuǎn)椅，因此難以手動地將力精確地保持不變超過500毫秒，并且人們難以以均勻的速度持續(xù)推動轉(zhuǎn)椅超過2秒。事實上，我們正在使用此規(guī)則來執(zhí)行ZUPT。

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

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

基于第三個運(yùn)動學(xué)假設(shè)，我們可以使用ZUPT來取消此錯誤。 ZUPT之后的積分速度如圖9所示。

雖然我們使用了第三個假設(shè)，如前所示，但錯誤仍然無法完全取消。誤差消除取決于設(shè)定的零加速度和零速度的閾值。但是，大多數(shù)錯誤已得到糾正。

基線移位取消

雖然使用了ZUPT，但零加速有時仍可能無法到達(dá)。這導(dǎo)致兩個因素：

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

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

以圖10為例。圖10中的左圖是ADIS16470的原始數(shù)據(jù)（主體框架），圖10中的右圖是導(dǎo)航框架中投影的加速度?？梢钥闯?，當(dāng)它停止移動時，預(yù)計的加速度不為零。由于它總是在變化，我們稱之為基線變化。

為了取消基線偏移，我們需要實時連續(xù)獲得偏移偏差并從投影加速度中減去偏差。結(jié)果如圖11所示。

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

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

到DR系統(tǒng)。完成后，我們將獲得導(dǎo)航框架中的位置。

實驗結(jié)果和結(jié)論

實驗結(jié)果

我們構(gòu)建系統(tǒng)，如圖13所示，使用SPI端口將ADIS16470評估板和RM3100羅盤評估板連接到ADI公司的ADuCM4050電路板。 ADuCM4050調(diào)整數(shù)據(jù)格式并進(jìn)行時間同步（因為IMU和指南針的數(shù)據(jù)速率不同）。然后使用UART將捕獲的數(shù)據(jù)傳輸?shù)接嬎銠C(jī)。所有計算，包括校準(zhǔn)，AHRS和DR，都在MATLAB ?中進(jìn)行。

通過將電路板和計算機(jī)放在轉(zhuǎn)椅上實現(xiàn)實驗在我們的實驗室里把轉(zhuǎn)椅推到一個圓圈里。

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

DR輸出：航位推測結(jié)果XYZ位置和3D繪圖如圖15所示。

結(jié)論

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

有很多方法可用于改善結(jié)果，例如：

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

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

使用更精確的數(shù)值計算方法，例如使用Simpson規(guī)則或三次樣條插值在DR中進(jìn)行積分，或者使用牛頓法代替Gauss-Newton方法來求解非線性MSE方程等。

最后，但最重要的是，我們f在實驗中，INS與應(yīng)用或運(yùn)動學(xué)模式密切相關(guān)。例如，我們在兩個地方進(jìn)行了實驗：一個沒有鋪地毯的實驗室和一個鋪有地毯的辦公室。如果我們使用相同的參數(shù)集，DR結(jié)果會顯示出巨大的差異。因此，無論哪種應(yīng)用，例如患者跟蹤，AGV導(dǎo)航或停車定位，或同一應(yīng)用中的不同條件，我們都需要全面了解其運(yùn)動學(xué)模型。