簡(jiǎn)介

隨著服務(wù)機(jī)器人市場(chǎng)和技術(shù)的發(fā)展，導(dǎo)航已成為研究和應(yīng)用中的一個(gè)熱點(diǎn)。與車(chē)輛、船舶或飛機(jī)相比，服務(wù)機(jī)器人體積小，成本低，因此它們的導(dǎo)航系統(tǒng)應(yīng)該具有捷聯(lián)和低成本的特點(diǎn)。傳統(tǒng)的穩(wěn)定平臺(tái)導(dǎo)航系統(tǒng)通常要采用獨(dú)立的加速度計(jì)和光纖或激光陀螺儀，所有傳感器都機(jī)械且剛性地安裝在與正在移動(dòng)的車(chē)輛隔離的穩(wěn)定平臺(tái)上。這導(dǎo)致了尺寸大、可靠性差、成本高的缺點(diǎn)。相反，在捷聯(lián)慣導(dǎo)系統(tǒng)中，慣性傳感器直接固定在車(chē)輛本體上，這意味著傳感器會(huì)與車(chē)輛一起旋轉(zhuǎn)。這種捷聯(liá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°/時(shí)的陀螺儀偏置，1μg的加速器偏置）或軍用級(jí)（0.005°/時(shí)的陀螺儀偏置，30μg的加速器偏置），而多數(shù)SINS只能到達(dá)導(dǎo)航級(jí)（0.01°/時(shí)的陀螺儀偏置，50μg的加速器偏置）或戰(zhàn)術(shù)級(jí)（10°/時(shí)的陀螺儀偏置，1mg的加速器偏置）。對(duì)于大多數(shù)服務(wù)機(jī)器人或AGV導(dǎo)航應(yīng)用，這一精度足夠了。

導(dǎo)航方法很多，包括機(jī)器視覺(jué)、GPS、UWB、SLAM型激光雷達(dá)等?；贗MU的慣性導(dǎo)航始終是導(dǎo)航的重要組成部分。然而，由于這種傳感器的限制——例如偏置誤差、軸間誤差、噪聲，特別是零偏不穩(wěn)定性——慣性導(dǎo)航通常需要采用一個(gè)伙伴傳感器，定期為它提供參考或校準(zhǔn)，本文將這種情況稱為傳感器融合。許多傳感器都可以與IMU融合，例如攝像頭和里程表，但在這些傳感器中，地磁傳感器是一種低成本的方案，可與IMU配合獲得姿態(tài)信息。

在本文中，我們使用ADI的IMU ADIS16470和地磁傳感器來(lái)開(kāi)發(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ì)。其陀螺儀零偏穩(wěn)定性為8°/時(shí)，加速計(jì)零偏穩(wěn)定性為13μg 其關(guān)鍵參數(shù)都經(jīng)過(guò)出廠校準(zhǔn)。此外，ADIS16470的 低價(jià)格在同級(jí)產(chǎn)品中具有吸引力，得到了許多客戶的廣泛使用。在本文中，我們使用微控制器與ADIS16470通過(guò)SPI接口進(jìn)行通信。

地磁傳感器介紹

地磁傳感器是用于測(cè)量羅盤(pán)體坐標(biāo)（即坐標(biāo)系）中的地磁場(chǎng)的傳感器，可為航向提供絕對(duì)參考。其x、y和z分量值由本地地磁場(chǎng)投影而來(lái)。這種傳感器有兩個(gè)主要缺點(diǎn)——一是精度和分辨 率不高——例如，常用的霍尼韋爾羅盤(pán)傳感器HMC5883L的分辨率僅為12位。另一個(gè)缺點(diǎn)是傳感器容易受到周?chē)h(huán)境的干擾，因?yàn)榈卮艌?chǎng)非常弱，強(qiáng)度范圍為毫高斯到8高斯。

盡管有這些缺點(diǎn)，仍然可以在許多情況下使用，例如戶外、低EMI環(huán)境等。將地磁傳感器與IMU進(jìn)行松耦合，就可以在大多數(shù)環(huán)境中使用這類傳感器。

在本文中，我們使用PNI傳感器公司的高性能電子羅盤(pán)傳感器RM3100，它提供了24位分辨率。PNI使用主動(dòng)激勵(lì)法來(lái)提高抗噪聲能力。

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

在使用羅盤(pán)傳感器之前，需要對(duì)其進(jìn)行校準(zhǔn)以消除兩個(gè)主要誤差。一個(gè)是失調(diào)誤差，這原本是由傳感器和電路的失調(diào)誤差引起的。另一個(gè)是標(biāo)度誤差。這兩種誤差都容易受到周?chē)怒h(huán)境的干擾。例如，如果有一個(gè)x軸向的外部磁場(chǎng)施加到傳感器上，就會(huì)給出外部x軸失調(diào)誤差。同時(shí)，x軸標(biāo)度也將與y軸和z軸不同。

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

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

橢球方程可以表示為

其中，X、Y和Z是羅盤(pán)輸出在三個(gè)方向上的地磁分量。將這些值擬合為橢球面意味著，我們需要得到一組最優(yōu)系數(shù)解。我們將系數(shù)定義為：

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

所以我們需要計(jì)算最優(yōu)σ，并使用公式2來(lái)找出最小值：

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

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

為了校準(zhǔn)傳感器，我們需要拉伸或壓縮擬合的橢球面并將其移至以零為中心的球面上。我們使用矩陣奇異值分解（SVD）方法來(lái)進(jìn)行這種校準(zhǔn)。校準(zhǔn)后的球體如圖2所示。1，2

圖2. 用SVD方法進(jìn)行球體校準(zhǔn)后的羅盤(pán)數(shù)據(jù)。

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

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

使用ADIS16470和羅盤(pán)的姿態(tài)和航向參考系統(tǒng)

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

在介紹我們的方法之前，有必要首先解釋為什么確定姿態(tài)需要進(jìn)行融合。事實(shí)上，我們的系統(tǒng)現(xiàn)在有三種傳感器：陀螺儀、加速器和羅盤(pán)（地磁傳感器）。

陀螺儀提供圍繞各軸的旋轉(zhuǎn)角速度。通過(guò)角速率積分計(jì)算，我們可以得到旋轉(zhuǎn)角度。如果我們知道初始航向，通過(guò)角度就始終能夠得到航向姿態(tài)。積分將累積陀螺儀的不穩(wěn)定零偏，這將導(dǎo)致角度誤差。此外，來(lái)自陀螺儀的高斯分布噪聲將積分成一個(gè)布朗運(yùn)動(dòng)過(guò)程，并導(dǎo)致隨機(jī)游走誤差。因此，我們很難長(zhǎng)時(shí) 間使用陀螺儀，陀螺儀需要定期校準(zhǔn)。

加速度計(jì)提供每個(gè)軸方向的移動(dòng)加速度。在靜態(tài)狀態(tài)下，我們可以得到每個(gè)軸與重力加速度之間的角度。由于重力加速度在方向和值上恒定不變，我們可以獲得相對(duì)于重力方向的航向姿態(tài)。然而，該方法使用重力加速度作為參考，因此不能解出圍繞重力加速度旋轉(zhuǎn)的角度。

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

從這一解釋中，我們可以看到，很難靠一個(gè)傳感器來(lái)找到姿態(tài)，我們需要組合使用兩個(gè)或三個(gè)傳感器并把信息融合起來(lái)。本文用加速度計(jì)、陀螺儀和地磁羅盤(pán)查找姿態(tài)。這種融合也被稱為磁、角速率和重力（MARG）系統(tǒng)。

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

有多種方法可以將IMU和羅盤(pán)數(shù)據(jù)融合起來(lái)，例如互補(bǔ)濾波器、統(tǒng)計(jì)學(xué)ARMA濾波器，卡爾曼濾波器等。我們?cè)诒疚闹惺褂玫氖菙U(kuò)展卡爾曼濾波器。

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

坐標(biāo)定義

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

姿態(tài)定義

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

常用于導(dǎo)航以避免奇異性。

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

我們?cè)诒疚闹惺褂玫倪\(yùn)動(dòng)學(xué)方程（即狀態(tài)轉(zhuǎn)移方程）是非線性微分方程，因此需要使用一個(gè)EKF，用于對(duì)該微分方程進(jìn)行一階近似。對(duì)于EKF設(shè)計(jì)，我們定義

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

為角速率；

為姿態(tài)四元數(shù)。

一個(gè)1×7向量作為觀測(cè)變量，與狀態(tài)變量具有相同的分量。

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

一個(gè)7×7矩陣作為觀察矩陣。

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

估計(jì)向量 x?真實(shí)值xx之間的誤差我們?cè)跍y(cè)試中將初始誤差設(shè)為相對(duì)較小的值。該值會(huì)自動(dòng)收斂到一個(gè)小值。

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

在保持IMU和羅盤(pán)處于靜止?fàn)顟B(tài)的同時(shí)，通過(guò)測(cè)量陀螺儀和加速器的交流均方根值的平方得到。我們?cè)O(shè)

根據(jù)以上定義，卡爾曼濾波器將通過(guò)以下五個(gè)步驟完成：

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

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

步驟3：輸出估算狀態(tài)x?：

步驟4：更新?tīng)顟B(tài)x?–：

步驟5：更新誤差協(xié)方差矩陣P–：

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

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

基于MSE的傳感器融合

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

其中沒(méi)有來(lái)自羅盤(pán)的信息。由于ω是角速率，我們只能使用四 元數(shù)來(lái)導(dǎo)入羅盤(pán)數(shù)據(jù)q. 我們使用MSE方法獲得q， 即觀測(cè)變量 中的組分。

我們將各變量定義如下：

mb和ab： 體坐標(biāo)系里的羅盤(pán)磁值和加速度值。

mn和an： 導(dǎo)航坐標(biāo)系里的羅盤(pán)磁值和加速度值。

mn0和an0： 導(dǎo)航坐標(biāo)系里的初始靜態(tài)羅盤(pán)磁值和加速度值。

為從體坐標(biāo)系到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣，用四元數(shù)表示，可以寫(xiě)成

其給出了導(dǎo)航坐標(biāo)系中初始值與實(shí)時(shí)從體坐標(biāo)系映射到導(dǎo)航坐標(biāo)系的值之間的誤差ε。

根據(jù)之前的定義，MSE方法可用于求取最優(yōu)值。

通過(guò)求方程8的最小值：

對(duì)f（q）求導(dǎo)并使其等于零，

我們將獲得方差意義上的最優(yōu)q。我們使用高斯-牛頓方法，用一階梯度收斂來(lái)求解以上非線性方程。

通過(guò)組合角速率，我們將得到觀測(cè)變量

其中融合了卡爾曼濾波器中的羅盤(pán)數(shù)據(jù)和IMU數(shù)據(jù)。

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

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

松耦合

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

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

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

使用ADIS16470進(jìn)行航位推算（DR）

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

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

其中ae是地球坐標(biāo)系里的加速度，ab 是體坐標(biāo)系里的加速度，ve是地球坐標(biāo)系里的速度，se是地球坐標(biāo)系里的距離，ge 是 地球坐標(biāo)系里的重力加速為［0 0 1］，單位為 g。需要強(qiáng)調(diào)的是，地球坐標(biāo)系與導(dǎo)航坐標(biāo)系不同——地球坐標(biāo)系是基于NED的。該 δtt是解算周期。

用第一個(gè)等式可以得到從IMU體坐標(biāo)系到地球坐標(biāo)系的加速度映射，如格式

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

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

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

● 加速度計(jì)輸出總是有偏置，與重力相結(jié)合后，難以從公式10中減去，因此更準(zhǔn)確的表達(dá)式應(yīng)為：

除非是用一些專業(yè)設(shè)備來(lái)測(cè)量該偏置，例如分度頭。

● 基于數(shù)值積分的實(shí)現(xiàn)方式，通常使用零階保持器方法（前一個(gè)值）進(jìn)行積分。但是，對(duì)于連續(xù)移動(dòng)，這將帶來(lái)重大的誤差。例如，我們來(lái)比較以下方法：

方法1：

（零階保持器）

方法2：

（線性插值）

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

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

基于前面的討論，基于應(yīng)用，我們修改了傳統(tǒng)比例方程中的兩個(gè)地方：

X 我們不使用地球坐標(biāo)作為導(dǎo)航坐標(biāo)系。相反，正如我們?cè)谟?jì)算先前姿態(tài)時(shí)所做的那樣，我們用初始姿態(tài)

作為導(dǎo)航坐標(biāo)系。通過(guò)這種方式，偏置和重力都可以輕松取消，如公式14所示：

雖然姿態(tài)中同時(shí)包含了偏置和重力分量，但這樣我們就不需要將它們分開(kāi)成單獨(dú)的分量，而是直接一起減去它們。

基于零階保持器與一階插值之間的比較，我們使用一階方法來(lái)獲得更準(zhǔn)確的積分結(jié)果。

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

通過(guò)使用IMU的初始值作為導(dǎo)航坐標(biāo)系，我們可以消除一部分加速度的偏置影響。然而，即使我們?cè)谑褂迷O(shè)備之前能用分度頭準(zhǔn)確測(cè)量偏置，仍然很難取消，除非使用另一個(gè)精確的傳感器來(lái)定期校準(zhǔn)它。這主要是由兩個(gè)原因引起的：一是偏置不穩(wěn)定，這意味著我們之前測(cè)量的偏置不是現(xiàn)在的實(shí)際偏置。二是速度隨機(jī)游走，由加速度噪聲積分而來(lái)。前面提到的不良特性會(huì)使我們計(jì)算的距離顯著漂移。即使我們停止移動(dòng)并保持靜止，從加速度積分而來(lái)的速度仍然存在，距離仍會(huì)增加。

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

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

● 對(duì)于航位推算，導(dǎo)航坐標(biāo)系中沒(méi)有z軸移動(dòng)。此限制僅適用于航位推算，不適用于姿態(tài)求解。顯然，我們是在二維空間中移動(dòng)系統(tǒng)。這有助于消除z軸誤差。

● 所有轉(zhuǎn)彎都發(fā)生在停止后。如果在移動(dòng)時(shí)發(fā)生轉(zhuǎn)彎，則會(huì)因?yàn)橐腩~外加速而干擾姿態(tài)求解。

● 如果系統(tǒng)正在移動(dòng)，加速度不能保持不變超過(guò)500毫秒。速度不能保持不變超過(guò)2秒。由于我們?cè)谕苿?dòng)或拉動(dòng)轉(zhuǎn)椅，因此很難手動(dòng)使力精確地保持不變超過(guò)500毫秒，并且個(gè)人很難以勻速持續(xù)推動(dòng)轉(zhuǎn)椅2秒以上。事實(shí)上，我們正是運(yùn)用這一規(guī)則來(lái)實(shí)施ZUPT。

● 加速度不能大于±1 m/s2。該規(guī)則用于一些噪音過(guò)濾，后者基于我們施加于椅子上、不會(huì)很大的拉力或推力。

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

圖8. 導(dǎo)航坐標(biāo)系中三個(gè)方向的加速度。

基于第三條運(yùn)動(dòng)學(xué)假設(shè)，我們可以使用ZUPT來(lái)消除此誤差。經(jīng)ZUPT處理之后的積分速度如圖9所示。

圖9. 導(dǎo)航坐標(biāo)系中三個(gè)方向的速度。

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

雖然使用了ZUPT，但有時(shí)仍然無(wú)法達(dá)到零速。這由兩個(gè)因素導(dǎo)致：

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

● 我們求出的姿態(tài)有一些誤差，結(jié)果將導(dǎo)致投影（從體坐標(biāo)系到導(dǎo)航坐標(biāo)系）后的加速度誤差。

以圖10為例。圖10中的左圖是ADIS16470的原始數(shù)據(jù)（體坐標(biāo)系），圖10中的右圖是投影到導(dǎo)航坐標(biāo)系的加速度?？梢钥闯觯Ｖ挂苿?dòng)時(shí)，投影加速度不為零。由于它總是在變化，我們此處稱之為基線漂移。

圖10. 體坐標(biāo)系（左）和導(dǎo)航坐標(biāo)系（右）的加速度。

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

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

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

可以使用圖12中的框圖簡(jiǎn)要描述航位推算過(guò)程。我們將體坐標(biāo)系加速度ab和姿態(tài)轉(zhuǎn)移矩陣（來(lái)自AHRS）輸入

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

圖12. 航位推算流程圖

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

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

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

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

將評(píng)估板和計(jì)算機(jī)放在轉(zhuǎn)椅上，并在實(shí)驗(yàn)室中推著轉(zhuǎn)椅繞圈。

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

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

● DR輸出：帶XYZ位置的航位推算結(jié)果和三維圖如圖15所示。

圖15. 位置計(jì)算結(jié)果。

結(jié)論

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

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

● 使用里程表或UWB距離測(cè)量方法與IMU中的加速度計(jì)融合，以在DR中獲得更準(zhǔn)確的距離值。

● 使用更復(fù)雜的運(yùn)動(dòng)學(xué)模型，從而在AHRS和DR中在傳感器和系統(tǒng)層次引入更多特性，例如系統(tǒng)的振動(dòng)、加速和減速模型、地面平整度等。這意味著為了提高導(dǎo)航結(jié)果的準(zhǔn)確性，需要給出更多的邊界條件。

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

最后但也是最重要的一點(diǎn)，我們?cè)谠囼?yàn)中發(fā)現(xiàn)INS與應(yīng)用或運(yùn)動(dòng)學(xué)模型緊密相關(guān)。例如，我們?cè)趦蓚€(gè)地方進(jìn)行了實(shí)驗(yàn)：未鋪地毯的實(shí)驗(yàn)室和鋪有地毯的辦公室。如果我們使用相同的參數(shù)集，DR結(jié)果會(huì)顯示出巨大的差異。因此，無(wú)論哪種應(yīng)用，例如患者跟蹤、AGV導(dǎo)航或停車(chē)定位，或者對(duì)于同一應(yīng)用中的不同條 件，我們都需要全面了解其運(yùn)動(dòng)學(xué)模型。
責(zé)任編輯；zl