你的位置:首頁(yè) > 傳感技術(shù) > 正文

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

發(fā)布時(shí)間:2019-11-29 來(lái)源:Joel Li 和 Van Yang 責(zé)任編輯:wenwei

【導(dǎo)讀】本文旨在介紹我們使用ADI公司的慣性測(cè)量單元(IMU)傳感器 ADIS16470 和PNI的地磁傳感器RM3100構(gòu)建的捷聯(lián)慣性導(dǎo)航系統(tǒng)(SINS)。實(shí)現(xiàn)了基于磁力、角速率和重力(MARG)的SINS的一些基本過(guò)程,包括電磁羅盤(pán)(地磁傳感器)校準(zhǔn)、使用擴(kuò)展卡爾曼濾波器(EKF)的姿態(tài)和航向參考系統(tǒng)(AHRS)和航跡跟蹤。還實(shí)現(xiàn)了使用最小平方誤差(MSE)方法的松耦合傳感器融合技術(shù)。文章展示了每個(gè)過(guò)程步驟使用的算法和實(shí)驗(yàn)設(shè)置。本文最后討論了結(jié)果分析和用于提高導(dǎo)航準(zhǔn)確性的方法。
 
簡(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),本文將這種情況稱(chēng)為傳感器融合。許多傳感器都可以與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)境中使用這類(lèi)傳感器。
 
在本文中,我們使用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ù)擬合為橢球面。
 
橢球方程可以表示為
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
其中,X、Y和Z是羅盤(pán)輸出在三個(gè)方向上的地磁分量。將這些值擬合為橢球面意味著,我們需要得到一組最優(yōu)系數(shù)解。我們將系數(shù)定義為:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
在擬合時(shí),我們定義向量:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
所以我們需要計(jì)算最優(yōu)σ,并使用公式2來(lái)找出最小值:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
這樣我們就可以得到圖1所示的擬合結(jié)果。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖1. 原始羅盤(pán)數(shù)據(jù)分布(左)和使用橢球擬合后的羅盤(pán)數(shù)據(jù)(右)。
 
為了校準(zhǔn)傳感器,我們需要拉伸或壓縮擬合的橢球面并將其移至以零為中心的球面上。我們使用矩陣奇異值分解(SVD)方法來(lái)進(jìn)行這種校準(zhǔn)。校準(zhǔn)后的球體如圖2所示。1,2
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖2. 用SVD方法進(jìn)行球體校準(zhǔn)后的羅盤(pán)數(shù)據(jù)。
 
校準(zhǔn)后,我們可以看到,測(cè)得的磁場(chǎng)強(qiáng)度(球半徑)幾乎恒定不變,如圖3所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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)。這種融合也被稱(chēng)為磁、角速率和重力(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)的映射(投影)矩陣定義為
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
姿態(tài)定義
 
與歐拉角或方向余弦矩陣(DCM)不同,我們?cè)谶@里使用四元數(shù),定義為
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
常用于導(dǎo)航以避免奇異性。
 
用卡爾曼濾波器更新姿態(tài)
 
我們?cè)诒疚闹惺褂玫倪\(yùn)動(dòng)學(xué)方程(即狀態(tài)轉(zhuǎn)移方程)是非線性微分方程,因此需要使用一個(gè)EKF,用于對(duì)該微分方程進(jìn)行一階近似。對(duì)于EKF設(shè)計(jì),我們定義
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
一個(gè)1×7向量作為狀態(tài)變量,其中
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
為角速率;
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
為姿態(tài)四元數(shù)。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
一個(gè)1×7向量作為觀測(cè)變量,與狀態(tài)變量具有相同的分量。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
一個(gè)7×7矩陣作為狀態(tài)轉(zhuǎn)移矩陣,其中,A的第一部分是角速率的數(shù)字化微分方程,第二部分是數(shù)字化四元數(shù)更新方程,后者從運(yùn)動(dòng)學(xué)方程推導(dǎo)而來(lái)。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
一個(gè)7×7矩陣作為觀察矩陣。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
為誤差協(xié)方差矩陣,這是一個(gè)7×7矩陣,其中
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
估計(jì)向量 x?真實(shí)值xx之間的誤差我們?cè)跍y(cè)試中將初始誤差設(shè)為相對(duì)較小的值。該值會(huì)自動(dòng)收斂到一個(gè)小值。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
被設(shè)為狀態(tài)轉(zhuǎn)移噪聲和觀測(cè)噪聲的協(xié)方差矩陣。我們得到它們的初始值,
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
在保持IMU和羅盤(pán)處于靜止?fàn)顟B(tài)的同時(shí),通過(guò)測(cè)量陀螺儀和加速器的交流均方根值的平方得到。我們?cè)O(shè)
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
根據(jù)以上定義,卡爾曼濾波器將通過(guò)以下五個(gè)步驟完成:
 
步驟1:使用公式3計(jì)算卡爾曼增益K
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
步驟2:計(jì)算誤差協(xié)方差矩陣,P:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
步驟3:輸出估算狀態(tài)x?:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
步驟4:更新?tīng)顟B(tài)x?–:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
步驟5:更新誤差協(xié)方差矩陣P–:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
該過(guò)程可以簡(jiǎn)單地描述為圖4中的框圖。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖4. 用于更新姿態(tài)的卡爾曼濾波器流程圖。
 
基于MSE的傳感器融合
 
在上一節(jié)中,觀測(cè)變量是
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
其中沒(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)磁值和加速度值。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
為從體坐標(biāo)系到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣,用四元數(shù)表示,可以寫(xiě)成
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
其給出了導(dǎo)航坐標(biāo)系中初始值與實(shí)時(shí)從體坐標(biāo)系映射到導(dǎo)航坐標(biāo)系的值之間的誤差ε。
 
根據(jù)之前的定義,MSE方法可用于求取最優(yōu)值。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
通過(guò)求方程8的最小值:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
對(duì)f(q)求導(dǎo)并使其等于零,
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
我們將獲得方差意義上的最優(yōu)q。我們使用高斯-牛頓方法,用一階梯度收斂來(lái)求解以上非線性方程。
 
通過(guò)組合角速率,我們將得到觀測(cè)變量
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
其中融合了卡爾曼濾波器中的羅盤(pán)數(shù)據(jù)和IMU數(shù)據(jù)。
 
該過(guò)程可以簡(jiǎn)單地描述為圖5中的框圖。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
其中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)系的加速度映射,如格式
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
第二個(gè)等式將加速度積分或累加為速度;然而,由于測(cè)量的加速度包含了重力分量,所以需要減去重力。
 
與等式11類(lèi)似,等式12將速度積分成距離。
 
傳統(tǒng)方法存在幾個(gè)問(wèn)題。
 
● 加速度計(jì)輸出總是有偏置,與重力相結(jié)合后,難以從公式10中減去,因此更準(zhǔn)確的表達(dá)式應(yīng)為:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
除非是用一些專(zhuān)業(yè)設(shè)備來(lái)測(cè)量該偏置,例如分度頭。
 
● 基于數(shù)值積分的實(shí)現(xiàn)方式,通常使用零階保持器方法(前一個(gè)值)進(jìn)行積分。但是,對(duì)于連續(xù)移動(dòng),這將帶來(lái)重大的誤差。例如,我們來(lái)比較以下方法:
 
方法1:
 
 
(零階保持器)
 
方法2:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
(線性插值)
 
在5秒內(nèi)加速度為為0.5 m/s2時(shí),位移最高將相差4m。仿真結(jié)果如圖7所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖7. 速度計(jì)算中的零階保持與一階積分方法比較。
 
基于前面的討論,基于應(yīng)用,我們修改了傳統(tǒng)比例方程中的兩個(gè)地方:
 
X 我們不使用地球坐標(biāo)作為導(dǎo)航坐標(biāo)系。相反,正如我們?cè)谟?jì)算先前姿態(tài)時(shí)所做的那樣,我們用初始姿態(tài)
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
作為導(dǎo)航坐標(biāo)系。通過(guò)這種方式,偏置和重力都可以輕松取消,如公式14所示:
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
雖然姿態(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分量。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖8. 導(dǎo)航坐標(biāo)系中三個(gè)方向的加速度。
 
基于第三條運(yùn)動(dòng)學(xué)假設(shè),我們可以使用ZUPT來(lái)消除此誤差。經(jīng)ZUPT處理之后的積分速度如圖9所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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í),投影加速度不為零。由于它總是在變化,我們此處稱(chēng)之為基線漂移。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖10. 體坐標(biāo)系(左)和導(dǎo)航坐標(biāo)系(右)的加速度。
 
為了消除基線漂移,我們需要實(shí)時(shí)連續(xù)獲得偏移偏置并從投影加速度中減去該值。結(jié)果如圖11所示
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
Figure圖11. 基線漂移消除之前(上)和之后(下)的加速度。
 
上圖是基線漂移消除前的加速度,下圖中的綠色軌跡是我們計(jì)算的基線偏移,紅色軌跡是基線偏移消除后的加速度。
 
可以使用圖12中的框圖簡(jiǎn)要描述航位推算過(guò)程。我們將體坐標(biāo)系加速度ab和姿態(tài)轉(zhuǎn)移矩陣(來(lái)自AHRS)輸入
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
 
到DR系統(tǒng)。完成后,我們將獲得導(dǎo)航坐標(biāo)系中的位置。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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í)行。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖13. 實(shí)驗(yàn)平臺(tái)設(shè)置。
 
將評(píng)估板和計(jì)算機(jī)放在轉(zhuǎn)椅上,并在實(shí)驗(yàn)室中推著轉(zhuǎn)椅繞圈。
 
● AHRS輸出:姿態(tài)以四元數(shù)格式和DCM格式表示,如圖14所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖14. 四元數(shù)格式(左)和DCM格式(右)的姿態(tài)。
 
● DR輸出:帶XYZ位置的航位推算結(jié)果和三維圖如圖15所示。
 
基于IMU和地磁傳感器的捷聯(lián)慣性導(dǎo)航系統(tǒng)
圖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é)模型。
 
參考電路
 
1 Long Li和Zhang He,“Automatic and Adaptive Calibration Method of Tri-axial Magnetometer”(三軸磁力計(jì)的自動(dòng)和自適應(yīng)校準(zhǔn)方法),《中國(guó)儀器儀表學(xué)報(bào)》,2013。
 
2 Timothy Sauer,Numerical Analysis(數(shù)值分析(第2版)),Pearson,2011。
 
3 David H. Titterton,Strapdown Inertial Navigation Technology(捷聯(lián)慣性導(dǎo)航技術(shù)(第2版)),電氣工程師學(xué)會(huì),2004。
 
Gongmin, Yan,“Research on Vehicle Autonomous Positioning and Orientation System”(車(chē)輛自主定位定向系統(tǒng)研究),博士論文,中國(guó)西北工業(yè)大學(xué),2006。
 
Marins, João Luís,“An Extended Kalman Filter for Quaternion-Based Orientation Estimation Using MARG Sensors”(面向基于MARG傳感器的四元數(shù)方向估算應(yīng)用的擴(kuò)展卡爾曼濾波器),IEEE,2001。
 
Prikhodko、Igor P.和Brock Bearss,“Towards Self-Navigating Cars Using MEMS IMU:Challenges and Opportunities”(使用MEMS IMU邁向自動(dòng)駕駛汽車(chē):挑戰(zhàn)與機(jī)遇),IEEE,2018。
 
RM3100.PNI傳感器公司,2018。
 
Woodman, Oliver J. “An Introduction to Inertial Navigation.” (慣性 導(dǎo)航簡(jiǎn)介),劍橋大學(xué),2007年8月。
 
 
推薦閱讀:
 
安靜且簡(jiǎn)單的Silent Switcher器件
∑-Δ型ADC時(shí)鐘—不僅僅是抖動(dòng)
帶故障注入功能的高精度旋轉(zhuǎn)變壓器仿真系統(tǒng)
小尺寸高功率密度
音頻創(chuàng)新:汽車(chē)、智能家居和專(zhuān)業(yè)音頻應(yīng)用的新風(fēng)向
特別推薦
技術(shù)文章更多>>
技術(shù)白皮書(shū)下載更多>>
熱門(mén)搜索
?

關(guān)閉

?

關(guān)閉