【導讀】本文旨在介紹我們使用ADI公司的慣性測量單元(IMU)傳感器 ADIS16470 和PNI的地磁傳感器RM3100構(gòu)建的捷聯(lián)慣性導航系統(tǒng)(SINS)。實現(xiàn)了基于磁力、角速率和重力(MARG)的SINS的一些基本過程，包括電磁羅盤（地磁傳感器）校準、使用擴展卡爾曼濾波器(EKF)的姿態(tài)和航向參考系統(tǒng)(AHRS)和航跡跟蹤。還實現(xiàn)了使用最小平方誤差(MSE)方法的松耦合傳感器融合技術(shù)。文章展示了每個過程步驟使用的算法和實驗設(shè)置。本文最后討論了結(jié)果分析和用于提高導航準確性的方法。
簡介
隨著服務(wù)機器人市場和技術(shù)的發(fā)展，導航已成為研究和應(yīng)用中的一個熱點。與車輛、船舶或飛機相比，服務(wù)機器人體積小，成本低，因此它們的導航系統(tǒng)應(yīng)該具有捷聯(lián)和低成本的特點。傳統(tǒng)的穩(wěn)定平臺導航系統(tǒng)通常要采用獨立的加速度計和光纖或激光陀螺儀，所有傳感器都機械且剛性地安裝在與正在移動的車輛隔離的穩(wěn)定平臺上。這導致了尺寸大、可靠性差、成本高的缺點。相反，在捷聯(lián)慣導系統(tǒng)中，慣性傳感器直接固定在車輛本體上，這意味著傳感器會與車輛一起旋轉(zhuǎn)。這種捷聯(lián)方法消除了穩(wěn)定平臺慣導的缺點。然而，平臺慣導的準確性通常高于SINS。平臺慣導往往可以達到戰(zhàn)略級（0.0001°/時的陀螺儀偏置，1μg的加速器偏置）或軍用級（0.005°/時的陀螺儀偏置，30μg的加速器偏置），而多數(shù)SINS只能到達導航級（0.01°/時的陀螺儀偏置，50μg的加速器偏置）或戰(zhàn)術(shù)級（10°/時的陀螺儀偏置，1mg的加速器偏置）。對于大多數(shù)服務(wù)機器人或AGV導航應(yīng)用，這一精度足夠了。
導航方法很多，包括機器視覺、GPS、UWB、SLAM型激光雷達等。基于IMU的慣性導航始終是導航的重要組成部分。然而，由于這種傳感器的限制——例如偏置誤差、軸間誤差、噪聲，特別是零偏不穩(wěn)定性——慣性導航通常需要采用一個伙伴傳感器，定期為它提供參考或校準，本文將這種情況稱為傳感器融合。許多傳感器都可以與IMU融合，例如攝像頭和里程表，但在這些傳感器中，地磁傳感器是一種低成本的方案，可與IMU配合獲得姿態(tài)信息。
在本文中，我們使用ADI的IMU ADIS16470和地磁傳感器來開發(fā)平臺和算法，實現(xiàn)捷聯(lián)慣性導航系統(tǒng)。但是，地磁傳感器只能提供姿態(tài)信息。對于航位推算或距離測量，我們只能使用IMU中的 加速度傳感器。
ADIS16470 IMU簡介
ADI公司的ADIS16470是一款微型MEMS IMU，集成了3軸陀螺儀和3軸加速度計。其陀螺儀零偏穩(wěn)定性為8°/時，加速計零偏穩(wěn)定性為13μg 其關(guān)鍵參數(shù)都經(jīng)過出廠校準。此外，ADIS16470的 低價格在同級產(chǎn)品中具有吸引力，得到了許多客戶的廣泛使用。在本文中，我們使用微控制器與ADIS16470通過SPI接口進行通信。
地磁傳感器介紹
地磁傳感器是用于測量羅盤體坐標（即坐標系）中的地磁場的傳感器，可為航向提供絕對參考。其x、y和z分量值由本地地磁場投影而來。這種傳感器有兩個主要缺點——一是精度和分辨 率不高——例如，常用的霍尼韋爾羅盤傳感器HMC5883L的分辨率僅為12位。另一個缺點是傳感器容易受到周圍環(huán)境的干擾，因為地磁場非常弱，強度范圍為毫高斯到8高斯。
盡管有這些缺點，仍然可以在許多情況下使用，例如戶外、低EMI環(huán)境等。將地磁傳感器與IMU進行松耦合，就可以在大多數(shù)環(huán)境中使用這類傳感器。
在本文中，我們使用PNI傳感器公司的高性能電子羅盤傳感器RM3100，它提供了24位分辨率。PNI使用主動激勵法來提高抗噪聲能力。
羅盤傳感器的校準
在使用羅盤傳感器之前，需要對其進行校準以消除兩個主要誤差。一個是失調(diào)誤差，這原本是由傳感器和電路的失調(diào)誤差引起的。另一個是標度誤差。這兩種誤差都容易受到周圍磁環(huán)境的干擾。例如，如果有一個x軸向的外部磁場施加到傳感器上，就會給出外部x軸失調(diào)誤差。同時，x軸標度也將與y軸和z軸不同。
通常用于校準磁傳感器的方法是在xy平面上轉(zhuǎn)動傳感器繞圈，然后抽取數(shù)據(jù)。一個地點的地磁場強度是一個常數(shù)值，因此繪制的數(shù)據(jù)應(yīng)該是一個圓；然而，事實上，我們將看到一個橢圓形，這意味著我們需要移動橢圓并重新縮放到以零為中心的圓。
上述2D校準方法有一些缺點，并且需要用加速器來測量其傾斜度。我們使用3D球面擬合方法來校準羅盤傳感器。首先，我們需要將傳感器旋轉(zhuǎn)到x-y-z空間中的每個方向，并在3D坐標中繪制其值。然后我們需要使用最小平方誤差(MSE)方法將數(shù)據(jù)擬合為橢球面。
橢球方程可以表示為
其中，X、Y和Z是羅盤輸出在三個方向上的地磁分量。將這些值擬合為橢球面意味著，我們需要得到一組最優(yōu)系數(shù)解。我們將系數(shù)定義為：
在擬合時，我們定義向量：
所以我們需要計算最優(yōu)σ，并使用公式2來找出最小值：
這樣我們就可以得到圖1所示的擬合結(jié)果。
圖1. 原始羅盤數(shù)據(jù)分布（左）和使用橢球擬合后的羅盤數(shù)據(jù)（右）。
為了校準傳感器，我們需要拉伸或壓縮擬合的橢球面并將其移至以零為中心的球面上。我們使用矩陣奇異值分解(SVD)方法來進行這種校準。校準后的球體如圖2所示。1,2
圖2. 用SVD方法進行球體校準后的羅盤數(shù)據(jù)。
校準后，我們可以看到，測得的磁場強度（球半徑）幾乎恒定不變，如圖3所示。
圖3. 校準前和校準后的磁場比較。
使用ADIS16470和羅盤的姿態(tài)和航向參考系統(tǒng)
AHRS由三個軸上的傳感器組成，提供姿態(tài)信息，包括橫滾角、俯仰角和偏航角。AHRS是一個來自飛機導航的概念。我們用它來描述方向，即姿態(tài)。
在介紹我們的方法之前，有必要首先解釋為什么確定姿態(tài)需要進行融合。事實上，我們的系統(tǒng)現(xiàn)在有三種傳感器：陀螺儀、加速器和羅盤（地磁傳感器）。
陀螺儀提供圍繞各軸的旋轉(zhuǎn)角速度。通過角速率積分計算，我們可以得到旋轉(zhuǎn)角度。如果我們知道初始航向，通過角度就始終能夠得到航向姿態(tài)。積分將累積陀螺儀的不穩(wěn)定零偏，這將導致角度誤差。此外，來自陀螺儀的高斯分布噪聲將積分成一個布朗運動過程，并導致隨機游走誤差。因此，我們很難長時 間使用陀螺儀，陀螺儀需要定期校準。
加速度計提供每個軸方向的移動加速度。在靜態(tài)狀態(tài)下，我們可以得到每個軸與重力加速度之間的角度。由于重力加速度在方向和值上恒定不變，我們可以獲得相對于重力方向的航向姿態(tài)。然而，該方法使用重力加速度作為參考，因此不能解出圍繞重力加速度旋轉(zhuǎn)的角度。
羅盤提供從地磁場投影的每個軸的值。我們可以從每個軸與恒為常數(shù)向量的地磁場方向之間的關(guān)系推導出角度值。如前一節(jié)所述，由于對外部磁場的抗擾性較差，羅盤需要一個低干擾的環(huán)境。
從這一解釋中，我們可以看到，很難靠一個傳感器來找到姿態(tài)，我們需要組合使用兩個或三個傳感器并把信息融合起來。本文用加速度計、陀螺儀和地磁羅盤查找姿態(tài)。這種融合也被稱為磁、角速率和重力(MARG)系統(tǒng)。
擴展卡爾曼濾波器的設(shè)計與傳感器融合
有多種方法可以將IMU和羅盤數(shù)據(jù)融合起來，例如互補濾波器、統(tǒng)計學ARMA濾波器，卡爾曼濾波器等。我們在本文中使用的是擴展卡爾曼濾波器。
首先，我們需要介紹本文中使用的一些定義。
坐標定義
T航向或方向是兩個坐標（即坐標系）之間的關(guān)系。一個坐標總在變化，另一個坐標保持不變。對于坐標定義方法，我們使用導航坐標和體坐標。與東北地(NED)坐標系或地理方法相反，我們將測量的初始體坐標值定義為導航坐標系，此后該坐標為恒定坐標。從體坐標到導航坐標的映射（投影）矩陣定義為
姿態(tài)定義
與歐拉角或方向余弦矩陣(DCM)不同，我們在這里使用四元數(shù)，定義為
常用于導航以避免奇異性。
用卡爾曼濾波器更新姿態(tài)
我們在本文中使用的運動學方程（即狀態(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ù)更新方程，后者從運動學方程推導而來。
一個7×7矩陣作為觀察矩陣。
為誤差協(xié)方差矩陣，這是一個7×7矩陣，其中
估計向量 x?真實值xx之間的誤差我們在測試中將初始誤差設(shè)為相對較小的值。該值會自動收斂到一個小值。
被設(shè)為狀態(tài)轉(zhuǎn)移噪聲和觀測噪聲的協(xié)方差矩陣。我們得到它們的初始值，
在保持IMU和羅盤處于靜止狀態(tài)的同時，通過測量陀螺儀和加速器的交流均方根值的平方得到。我們設(shè)
根據(jù)以上定義，卡爾曼濾波器將通過以下五個步驟完成：
步驟1：使用公式3計算卡爾曼增益K
步驟2：計算誤差協(xié)方差矩陣，P:
步驟3：輸出估算狀態(tài)x?:
步驟4：更新狀態(tài)x?–:
步驟5：更新誤差協(xié)方差矩陣P–:
該過程可以簡單地描述為圖4中的框圖。
圖4. 用于更新姿態(tài)的卡爾曼濾波器流程圖。
基于MSE的傳感器融合
在上一節(jié)中，觀測變量是
其中沒有來自羅盤的信息。由于ω是角速率，我們只能使用四 元數(shù)來導入羅盤數(shù)據(jù)q. 我們使用MSE方法獲得q, 即觀測變量 中的組分。
我們將各變量定義如下：
mb和ab: 體坐標系里的羅盤磁值和加速度值。
mn和an: 導航坐標系里的羅盤磁值和加速度值。
mn0和an0: 導航坐標系里的初始靜態(tài)羅盤磁值和加速度值。
為從體坐標系到導航坐標系的姿態(tài)轉(zhuǎn)換矩陣，用四元數(shù)表示，可以寫成
其給出了導航坐標系中初始值與實時從體坐標系映射到導航坐標系的值之間的誤差ε。
根據(jù)之前的定義，MSE方法可用于求取最優(yōu)值。
通過求方程8的最小值：
對f(q)求導并使其等于零，
我們將獲得方差意義上的最優(yōu)q。我們使用高斯-牛頓方法，用一階梯度收斂來求解以上非線性方程。
通過組合角速率，我們將得到觀測變量
其中融合了卡爾曼濾波器中的羅盤數(shù)據(jù)和IMU數(shù)據(jù)。
該過程可以簡單地描述為圖5中的框圖。
圖5. 使用MSE方法的傳感器融合框圖。
松耦合
如前所述，我們經(jīng)常遇到無法使用羅盤傳感器的情況。如果磁數(shù)據(jù)受到干擾，則求解的姿態(tài)精度將比僅使用IMU時更差。因此，我們使用松耦合來判斷磁傳感器是否可用。當磁傳感器不可用時，我們只用IMU來求解姿態(tài)；當磁傳感器可用時，我們將使用融合算法找到姿態(tài)，如圖6所示。
圖6. 姿態(tài)計算流程圖。
在獲得新數(shù)據(jù)之后或者在求解新的姿態(tài)時（在某些系統(tǒng)中，采樣周期與姿態(tài)解算周期不同，但我們在此處進行的是單采樣周期解算），我們計算加速度的大小，如果結(jié)果不等于1g, 我們 就不會使用加速器的輸出進行姿態(tài)計算。然后我們計算羅盤輸出的大小并將其與初始值進行比較。如果它們彼此不相等，我們就不會在此周期中使用地磁傳感器的數(shù)據(jù)。當滿足兩個條件時，我們會使用卡爾曼濾波器并執(zhí)行MSE融合。
使用ADIS16470進行航位推算(DR)
在導航中，航位推算是計算當前位置的過程，先使用先前確定的位置，然后在解算周期中基于已知或估計的速度或加速度更新該位置。這里將使用ADIS16470里的加速度計。基于上一節(jié)解出的姿態(tài)，我們可以得到捷聯(lián)系統(tǒng)的移動方向，然后需要計算該方向上的距離，最后確定位置。
捷聯(lián)航位推算需要使用基于加速度測量的比力方程來跟蹤INS的位置。比力方程可以簡單描述為等式10、等式11和等式12：
其中ae是地球坐標系里的加速度，ab 是體坐標系里的加速度，ve是地球坐標系里的速度，se是地球坐標系里的距離，ge 是 地球坐標系里的重力加速為[0 0 1]，單位為 g。需要強調(diào)的是，地球坐標系與導航坐標系不同——地球坐標系是基于NED的。該 δtt是解算周期。
用第一個等式可以得到從IMU體坐標系到地球坐標系的加速度映射，如格式
第二個等式將加速度積分或累加為速度；然而，由于測量的加速度包含了重力分量，所以需要減去重力。
與等式11類似，等式12將速度積分成距離。
傳統(tǒng)方法存在幾個問題。
● 加速度計輸出總是有偏置，與重力相結(jié)合后，難以從公式10中減去，因此更準確的表達式應(yīng)為：
除非是用一些專業(yè)設(shè)備來測量該偏置，例如分度頭。
● 基于數(shù)值積分的實現(xiàn)方式，通常使用零階保持器方法（前一個值）進行積分。但是，對于連續(xù)移動，這將帶來重大的誤差。例如，我們來比較以下方法：
方法1：
（零階保持器）
方法2：
（線性插值）
在5秒內(nèi)加速度為為0.5 m/s2時，位移最高將相差4m。仿真結(jié)果如圖7所示。
圖7. 速度計算中的零階保持與一階積分方法比較。
基于前面的討論，基于應(yīng)用，我們修改了傳統(tǒng)比例方程中的兩個地方：
X 我們不使用地球坐標作為導航坐標系。相反，正如我們在計算先前姿態(tài)時所做的那樣，我們用初始姿態(tài)
作為導航坐標系。通過這種方式，偏置和重力都可以輕松取消，如公式14所示：
雖然姿態(tài)中同時包含了偏置和重力分量，但這樣我們就不需要將它們分開成單獨的分量，而是直接一起減去它們。
基于零階保持器與一階插值之間的比較，我們使用一階方法來獲得更準確的積分結(jié)果。
運動學模式和零速更新技術(shù)(ZUPT)
通過使用IMU的初始值作為導航坐標系，我們可以消除一部分加速度的偏置影響。然而，即使我們在使用設(shè)備之前能用分度頭準確測量偏置，仍然很難取消，除非使用另一個精確的傳感器來定期校準它。這主要是由兩個原因引起的：一是偏置不穩(wěn)定，這意味著我們之前測量的偏置不是現(xiàn)在的實際偏置。二是速度隨機游走，由加速度噪聲積分而來。前面提到的不良特性會使我們計算的距離顯著漂移。即使我們停止移動并保持靜止，從加速度積分而來的速度仍然存在，距離仍會增加。
要解決這個問題，我們需要找到一種通過使用ZUPT技術(shù)重置速度的方法。ZUPT技術(shù)與具體的應(yīng)用密切相關(guān)，因此我們需要獲得系統(tǒng)和應(yīng)用的運動學特征，然后給出一些算法規(guī)則。我們發(fā)現(xiàn)的運動學模式越多，結(jié)果就越準確。
我們通過移動帶有SINS系統(tǒng)的轉(zhuǎn)椅來進行實驗。由于我們的研究不限于特定應(yīng)用，我們使用以下運動學假設(shè)：
● 對于航位推算，導航坐標系中沒有z軸移動。此限制僅適用于航位推算，不適用于姿態(tài)求解。顯然，我們是在二維空間中移動系統(tǒng)。這有助于消除z軸誤差。
● 所有轉(zhuǎn)彎都發(fā)生在停止后。如果在移動時發(fā)生轉(zhuǎn)彎，則會因為引入額外加速而干擾姿態(tài)求解。
● 如果系統(tǒng)正在移動，加速度不能保持不變超過500毫秒。速度不能保持不變超過2秒。由于我們在推動或拉動轉(zhuǎn)椅，因此很難手動使力精確地保持不變超過500毫秒，并且個人很難以勻速持續(xù)推動轉(zhuǎn)椅2秒以上。事實上，我們正是運用這一規(guī)則來實施ZUPT。
● 加速度不能大于±1 m/s2。該規(guī)則用于一些噪音過濾，后者基于我們施加于椅子上、不會很大的拉力或推力。
如圖8所示，當系統(tǒng)在X方向上移動時（投影到導航坐標系后），Y方向也會產(chǎn)生加速度；積分后，Y方向速度不會為零，這意味著即使我們只是在X方向上移動，航位推算系統(tǒng)仍然會給我們帶來Y分量。
圖8. 導航坐標系中三個方向的加速度。
基于第三條運動學假設(shè)，我們可以使用ZUPT來消除此誤差。經(jīng)ZUPT處理之后的積分速度如圖9所示。
圖9. 導航坐標系中三個方向的速度。
雖然我們使用了第三條假設(shè)，如前所示，誤差仍然無法完全取消。誤差消除取決于設(shè)定的零加速度和零速度的判斷閾值。但是，大多數(shù)誤差已得到修正。
雖然使用了ZUPT，但有時仍然無法達到零速。這由兩個因素導致：
● 我們無法用ZUPT完全消除偏置不穩(wěn)定誤差和速度隨機游走。
● 我們求出的姿態(tài)有一些誤差，結(jié)果將導致投影（從體坐標系到導航坐標系）后的加速度誤差。
以圖10為例。圖10中的左圖是ADIS16470的原始數(shù)據(jù)（體坐標系），圖10中的右圖是投影到導航坐標系的加速度。可以看出，停止移動時，投影加速度不為零。由于它總是在變化，我們此處稱之為基線漂移。
圖10. 體坐標系（左）和導航坐標系（右）的加速度。
為了消除基線漂移，我們需要實時連續(xù)獲得偏移偏置并從投影加速度中減去該值。結(jié)果如圖11所示
Figure圖11. 基線漂移消除之前（上）和之后（下）的加速度。
上圖是基線漂移消除前的加速度，下圖中的綠色軌跡是我們計算的基線偏移，紅色軌跡是基線偏移消除后的加速度。
可以使用圖12中的框圖簡要描述航位推算過程。我們將體坐標系加速度ab和姿態(tài)轉(zhuǎn)移矩陣（來自AHRS）輸入
到DR系統(tǒng)。完成后，我們將獲得導航坐標系中的位置。
圖12. 航位推算流程圖
實驗結(jié)果與結(jié)論
實驗結(jié)果
使用SPI端口，我們將ADIS16470評估板和RM3100羅盤評估板連接到ADI公司的ADuCM4050電路板，構(gòu)建出我們的系統(tǒng)，如圖13所示。ADuCM4050 調(diào)整數(shù)據(jù)格式并進行時間同步（因為IMU和羅盤的數(shù)據(jù)速率不同）。然后使用UART將捕獲的數(shù)據(jù)傳輸?shù)接嬎銠C。所有計算（包括校準、AHRS和DR在MATLAB）均在MATLAB®中執(zhí)行。
圖13. 實驗平臺設(shè)置。
將評估板和計算機放在轉(zhuǎn)椅上，并在實驗室中推著轉(zhuǎn)椅繞圈。
● AHRS輸出：姿態(tài)以四元數(shù)格式和DCM格式表示，如圖14所示。
圖14. 四元數(shù)格式（左）和DCM格式（右）的姿態(tài)。
● DR輸出：帶XYZ位置的航位推算結(jié)果和三維圖如圖15所示。
圖15. 位置計算結(jié)果。
結(jié)論
本文介紹了使用ADI公司的IMU ADIS16470和地磁傳感器RM3100構(gòu)建捷聯(lián)慣導系統(tǒng)的基本過程，介紹了我們使用的校準、AHRS和DR方法。在平臺和實驗環(huán)境等條件有限的情況下，很難進一步測試平臺和算法。
有很多方法可用于改善結(jié)果，例如：
● 使用里程表或UWB距離測量方法與IMU中的加速度計融合，以在DR中獲得更準確的距離值。
● 使用更復雜的運動學模型，從而在AHRS和DR中在傳感器和系統(tǒng)層次引入更多特性，例如系統(tǒng)的振動、加速和減速模型、地面平整度等。這意味著為了提高導航結(jié)果的準確性，需要給出更多的邊界條件。
● X 使用更精確的數(shù)值計算方法，比如用辛普森規(guī)則或三次樣條插值在DR中進行積分，或者使用牛頓方法而非高斯-牛頓方法求解非線性MSE方程等。
最后但也是最重要的一點，我們在試驗中發(fā)現(xiàn)INS與應(yīng)用或運動學模型緊密相關(guān)。例如，我們在兩個地方進行了實驗：未鋪地毯的實驗室和鋪有地毯的辦公室。如果我們使用相同的參數(shù)集，DR結(jié)果會顯示出巨大的差異。因此，無論哪種應(yīng)用，例如患者跟蹤、AGV導航或停車定位，或者對于同一應(yīng)用中的不同條 件，我們都需要全面了解其運動學模型。
參考電路
1 Long Li和Zhang He，“Automatic and Adaptive Calibration Method of Tri-axial Magnetometer”（三軸磁力計的自動和自適應(yīng)校準方法），《中國儀器儀表學報》，2013。
2 Timothy Sauer，Numerical Analysis（數(shù)值分析（第2版）），Pearson，2011。
3 David H. Titterton，Strapdown Inertial Navigation Technology（捷聯(lián)慣性導航技術(shù)（第2版）），電氣工程師學會，2004。
Gongmin, Yan，“Research on Vehicle Autonomous Positioning and Orientation System”（車輛自主定位定向系統(tǒng)研究），博士論文，中國西北工業(yè)大學，2006。
Marins, João Luís，“An Extended Kalman Filter for Quaternion-Based Orientation Estimation Using MARG Sensors”（面向基于MARG傳感器的四元數(shù)方向估算應(yīng)用的擴展卡爾曼濾波器），IEEE，2001。
Prikhodko、Igor P.和Brock Bearss，“Towards Self-Navigating Cars Using MEMS IMU：Challenges and Opportunities”（使用MEMS IMU邁向自動駕駛汽車：挑戰(zhàn)與機遇），IEEE，2018。
RM3100.PNI傳感器公司，2018。
Woodman, Oliver J. “An Introduction to Inertial Navigation.” （慣性 導航簡介），劍橋大學，2007年8月。
推薦閱讀：
