作者：Joel Li 和 Van Yang

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

介紹

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

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

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

ADIS16470 IMU簡(jiǎn)介

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

地磁傳感器簡(jiǎn)介

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

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

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

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

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

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

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

橢球方程可以表示為

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

擬合時(shí)，我們將向量定義為：

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

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

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

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

圖2.使用SVD方法進(jìn)行球體校準(zhǔn)后的指南針數(shù)據(jù)。

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

圖 3.校準(zhǔn)前和校準(zhǔn)后校準(zhǔn)之間的磁場(chǎng)比較。

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

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

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

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

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

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

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

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

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

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

坐標(biāo)定義

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

態(tài)度定義

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

這通常用于導(dǎo)航以避免爭(zhēng)論。3

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

我們?cè)诒疚闹惺褂玫倪\(yùn)動(dòng)學(xué)方程，即狀態(tài)轉(zhuǎn)移方程是一種偏差格式，它不是線性的，因此我們需要使用具有一階線性近似的 EKF 來表示偏差方程。對(duì)于 EKF 設(shè)計(jì)，我們定義

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

是角速率，并且

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

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

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

7×7矩陣是觀測(cè)矩陣。

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

是從實(shí)數(shù) x 估計(jì)的誤差向量 x?。我們?cè)跍y(cè)試中將初始誤差設(shè)置為相對(duì)較小的值。它將自動(dòng)收斂到一個(gè)小值。

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

和

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

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

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

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

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

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

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

該過程可以簡(jiǎn)單地描述為圖4中的框圖。

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

基于MSE的傳感器融合

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

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

我們定義變量如下：

mb和一個(gè)b：體框內(nèi)的羅盤磁力值和加速度值。

mn和一個(gè)n：導(dǎo)航框中的羅盤磁值和加速度值。

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

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

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

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

通過最小化公式8：

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

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

通過結(jié)合角速率，我們將得到觀測(cè)變量，

在卡爾曼濾波器中融合了指南針數(shù)據(jù)和 IMU 數(shù)據(jù)。

該過程可以通過圖5中的框圖簡(jiǎn)單描述。

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

松耦合

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

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

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

使用ADIS16470的航位推算（DR）

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

航位推算方法介紹

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

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

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

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

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

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

加速器輸出總是有偏置，偏置與重力相結(jié)合，因此很難在公式10中減去，因此更準(zhǔn)確的表達(dá)式應(yīng)該是：

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

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

方法一：

（零階持有者）

方法2：

（線性插值）

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

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

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

我們不使用地球坐標(biāo)作為導(dǎo)航框架。相反，正如我們?cè)趯ふ蚁惹暗膽B(tài)度時(shí)所做的那樣，我們使用最初的態(tài)度

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



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

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

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

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

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

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

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

所有轉(zhuǎn)彎都在停止后發(fā)生。如果在移動(dòng)時(shí)發(fā)生轉(zhuǎn)彎，姿態(tài)解析將受到干擾，因?yàn)閷⑸婕邦~外的加速度。

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

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

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

圖8.導(dǎo)航框架中三個(gè)方向的加速度。

基于第三個(gè)運(yùn)動(dòng)學(xué)假設(shè)，我們可以使用ZUPT來消除這個(gè)誤差。ZUPT后的積分速度如圖9所示。

圖9.導(dǎo)航框架中三個(gè)方向的速度。

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

基線移位取消

盡管使用了ZUPT，但有時(shí)仍可能無法達(dá)到零加速度。這導(dǎo)致兩個(gè)因素：

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

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

以圖 10 為例。圖10中的左圖是ADIS16470的原始數(shù)據(jù)（主體框架），圖10中的右圖是導(dǎo)航幀中預(yù)測(cè)的加速度。可以看出，當(dāng)它停止運(yùn)動(dòng)時(shí)，預(yù)計(jì)加速度不為零。由于它總是在變化，我們稱之為基線轉(zhuǎn)移。

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

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

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

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

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

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

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

實(shí)驗(yàn)結(jié)果及結(jié)論

實(shí)驗(yàn)結(jié)果

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

圖 13.實(shí)驗(yàn)平臺(tái)設(shè)置。

該實(shí)驗(yàn)是通過將電路板和計(jì)算機(jī)放在轉(zhuǎn)椅上并在我們的實(shí)驗(yàn)室中將轉(zhuǎn)椅推一圈來實(shí)現(xiàn)的。

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

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

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

圖 15.倉(cāng)位計(jì)算結(jié)果。

結(jié)論

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

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

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

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

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

我們?cè)趯?shí)驗(yàn)中發(fā)現(xiàn)的最后一點(diǎn)，也是最重要的一點(diǎn)是，INS與應(yīng)用或運(yùn)動(dòng)學(xué)模式密切相關(guān)。例如，我們?cè)趦蓚€(gè)地方進(jìn)行了實(shí)驗(yàn)：一個(gè)沒有地毯的實(shí)驗(yàn)室和一個(gè)鋪有地毯的辦公室。如果我們使用相同的參數(shù)集，DR 結(jié)果顯示出巨大的差異。因此，無論是哪種應(yīng)用，如患者跟蹤、AGV導(dǎo)航、停車定位，還是針對(duì)同一應(yīng)用中的不同工況，我們都需要全面了解其運(yùn)動(dòng)學(xué)模型。

審核編輯：郭婷