摘要：設(shè)計(jì)一種嵌入式衛(wèi)星/MEMS組合導(dǎo)航模塊。該模塊采用北京時(shí)代民芯科技有限公司的國產(chǎn)化GPS/BD多模導(dǎo)航接收機(jī)，實(shí)現(xiàn)與MEMS慣性器件的軟硬件融合?；诠こ袒瘧?yīng)用，提出一種高可靠擴(kuò)展卡爾曼濾波器濾波算法，實(shí)時(shí)校準(zhǔn)各種誤差項(xiàng)，改善組合導(dǎo)航模塊性能。在靜態(tài)情況下，模塊姿態(tài)角誤差小于0.2°，航向角誤差小于0.5°。在動(dòng)態(tài)情況下，姿態(tài)角與航向角誤差小于1°，如果衛(wèi)星導(dǎo)航突然中斷，將能持續(xù)30秒，定位精度維持在20米內(nèi)。

引言

隨著北斗導(dǎo)航應(yīng)用的不斷推廣，國產(chǎn)導(dǎo)航芯片和模塊持續(xù)升級(jí)，各領(lǐng)域應(yīng)用對(duì)北斗導(dǎo)航產(chǎn)品性能提出新的要求：低成本、低功耗、小型化、高精度。

其中，GPS/BD多模導(dǎo)航的實(shí)現(xiàn)，很大程度滿足國內(nèi)外需求，集合MEMS慣性傳感器的組合導(dǎo)航模塊，進(jìn)一步豐富多層面用戶應(yīng)用。與傳統(tǒng)導(dǎo)航系統(tǒng)相比，衛(wèi)星/MEMS組合導(dǎo)航模塊，采用較低成本的MEMS慣性器件，極大降低導(dǎo)航系統(tǒng)成本、功耗和體積，與此同時(shí)，MEMS慣性器件具有瞬間較高精度的特點(diǎn)，彌補(bǔ)衛(wèi)星導(dǎo)航不連續(xù)、輸出頻率不高、易失鎖等不足[1~3]。

由于MEMS-IMU(IMU，慣性測(cè)量單元)精度仍處于較低水平，無法單獨(dú)實(shí)現(xiàn)導(dǎo)航。通常采用MEMS-IMU與衛(wèi)星導(dǎo)航接收機(jī)、磁強(qiáng)計(jì)等相結(jié)合的方式，構(gòu)建組合導(dǎo)航模塊，實(shí)現(xiàn)較高精度姿態(tài)控制與導(dǎo)航定位[2]。

為了適合特定導(dǎo)航應(yīng)用，提出一種衛(wèi)星/MEMS組合導(dǎo)航模塊用工程化濾波算法。并在嵌入式平臺(tái)上實(shí)現(xiàn)。

衛(wèi)星/MEMS組合導(dǎo)航，通過算法實(shí)現(xiàn)角速度校準(zhǔn)，加速度校準(zhǔn)，以及磁校準(zhǔn);估計(jì)載體姿態(tài)角度，位置和速度信息，并以較高的輸出速率實(shí)時(shí)更新。該系統(tǒng)中采用擴(kuò)展卡爾曼濾波器(EKF)估算并校準(zhǔn)系統(tǒng)狀態(tài)量。通過融合衛(wèi)星導(dǎo)航信息和磁強(qiáng)計(jì)信息，實(shí)時(shí)校準(zhǔn)系統(tǒng)姿態(tài)、位置、速度和傳感器誤差。該EKF融合算法，考慮到傳感器主要特性：零偏、標(biāo)度因數(shù)誤差、正交耦合誤差等。此外，由于磁強(qiáng)計(jì)感測(cè)地磁場(chǎng)強(qiáng)度時(shí)，會(huì)受到硬鐵和軟鐵干擾，因此在濾波器中對(duì)其進(jìn)行估計(jì)。在靜態(tài)情況下，系統(tǒng)姿態(tài)角誤差小于0.2°，航向角誤差小于0.5°。在動(dòng)態(tài)情況下，姿態(tài)角與航向角誤差小于1°，如果衛(wèi)星導(dǎo)航突然中斷，將能持續(xù)30秒，定位精度維持在20米內(nèi)。

模塊設(shè)計(jì)

綜合考慮成本、功耗、體積、可靠性等因素，衛(wèi)星/MEMS組合導(dǎo)航模塊采用嵌入式平臺(tái)開發(fā)方案[6]，如圖1所示。系統(tǒng)由處理器、MEMS-IMU、GPS/BD多模導(dǎo)航接收機(jī)、磁強(qiáng)計(jì)等重要部件組成。

姿態(tài)與航向校準(zhǔn)算法

理想情況下，將陀螺感測(cè)的角速度信息融入姿態(tài)處理器，在獲悉載體初始姿態(tài)情況下，同時(shí)認(rèn)為陀螺的輸出比較精準(zhǔn)，一般的解算足以獲得夠用的姿態(tài)信息。然而，通常初始姿態(tài)無法準(zhǔn)確獲得，陀螺和加速度計(jì)都遭受隨機(jī)漂移、失準(zhǔn)角誤差、加速度敏感誤差、標(biāo)度因數(shù)誤差及其非線性等因素的影響，磁強(qiáng)計(jì)存在磁感應(yīng)失真等。如圖2所示，通過工裝將模塊安裝于測(cè)試設(shè)備上，設(shè)計(jì)合理標(biāo)定流程和算法，便可獲得陀螺和加速度計(jì)常值零偏、標(biāo)度因數(shù)、失準(zhǔn)角誤差量等關(guān)鍵參數(shù)。

通常在組合模塊安裝好之后，對(duì)磁強(qiáng)計(jì)的誤差和干擾進(jìn)行校準(zhǔn)。姿態(tài)與航向解算中，陀螺的漂移引起的誤差最大，如果沒有濾波算法，解姿信息將不斷偏離真實(shí)數(shù)值。該卡爾曼濾波器提供在線陀螺漂移校準(zhǔn)，加速度計(jì)提供重力軸系參考，磁強(qiáng)計(jì)通過與加速度計(jì)配合，提供航向參考。

姿態(tài)估計(jì)算法中，提供穩(wěn)定的三維歐拉角roll、pitch、yaw，為了提高精度并避免奇異，采用四元數(shù)法實(shí)時(shí)更新方向余弦矩陣。MEMS陀螺感測(cè)到載體角速度，通過差分方程實(shí)時(shí)更新姿態(tài)四元數(shù)，同時(shí)獲得更新后的方向余弦矩陣，從而獲得姿態(tài)角的更新。

卡爾曼濾波器姿態(tài)校準(zhǔn)的實(shí)現(xiàn)，之所以能夠改善性能，主要在于它能夠準(zhǔn)確估計(jì)出陀螺的漂移和姿態(tài)誤差。這種方式的優(yōu)點(diǎn)是：濾波器估計(jì)了絕對(duì)姿態(tài)誤差，因而無論是哪一部分誤差污染了姿態(tài)角，都可以直接用其來校準(zhǔn)姿態(tài)角輸出。姿態(tài)與航向校準(zhǔn)模塊，采用EKF，包含兩個(gè)部分：線性姿態(tài)誤差與陀螺漂移模型，非線性姿態(tài)四元數(shù)誤差模型。狀態(tài)模型基于陀螺輸出數(shù)據(jù)，預(yù)測(cè)姿態(tài)誤差和陀螺漂移，量測(cè)模型采用真實(shí)世界的姿態(tài)誤差量測(cè)值校準(zhǔn)預(yù)測(cè)部分，該姿態(tài)誤差量測(cè)值由加速度計(jì)與磁強(qiáng)計(jì)獲得。這兩個(gè)參考數(shù)據(jù)源向卡爾曼濾波器提供適當(dāng)?shù)闹眯潘絒4]。

航姿模塊路測(cè)試驗(yàn)

為了能夠正確推算模塊的姿態(tài)、速度、位置等信息，準(zhǔn)確對(duì)準(zhǔn)初始姿態(tài)是十分必要的。由于低精度MEMS陀螺不能感測(cè)到地球自轉(zhuǎn)角速率，因此不能采用傳統(tǒng)的自對(duì)準(zhǔn)方法實(shí)現(xiàn)初始化對(duì)準(zhǔn)。基于系統(tǒng)方案，將磁強(qiáng)計(jì)與MEMS加速度計(jì)進(jìn)行組合，構(gòu)成測(cè)姿模塊，實(shí)現(xiàn)初始化對(duì)準(zhǔn)。如圖2所示，單片磁強(qiáng)計(jì)由三軸正交磁阻傳感器與數(shù)字化ASIC接口構(gòu)成，磁強(qiáng)計(jì)不能單獨(dú)確定航向角，需要MEMS-IMU模塊中的加速度計(jì)配合，輔助磁強(qiáng)計(jì)精確確定航向角[2]。

衛(wèi)星/MEMS組合濾波

導(dǎo)航解算

由于MEMS陀螺不同于傳統(tǒng)高精度陀螺，其不能感測(cè)地球相對(duì)于慣性空間的旋轉(zhuǎn)速率，因而無法使用傳統(tǒng)的導(dǎo)航解算公式去實(shí)時(shí)推算速度和位置。如圖4所示，為捷聯(lián)解算算法流程。

組合濾波算法

衛(wèi)星/MEMS組合導(dǎo)航模塊實(shí)現(xiàn)方案如圖5所示，MEMS-SINS模塊與衛(wèi)星導(dǎo)航模塊是彼此獨(dú)立的兩個(gè)部分，此框圖顯示了以MEMS-SINS和衛(wèi)星導(dǎo)航接收機(jī)的速度/位置信息做差作為量測(cè)量。松耦合組合導(dǎo)航通常有兩種形式：開環(huán)與閉環(huán)，如圖5所示，該技術(shù)方案采用閉環(huán)形式，將卡爾曼濾波估計(jì)出的各種誤差反饋至MEMS-SINS模塊，改善系統(tǒng)性能[5]。[!--empirenews.page--]

組合導(dǎo)航路測(cè)試驗(yàn)

與傳統(tǒng)高精度捷聯(lián)慣性導(dǎo)航系統(tǒng)不同的是，MEMS-SINS模塊無法感測(cè)地球自轉(zhuǎn)角速率，地球的自轉(zhuǎn)角速率完全淹沒在MEMS陀螺的噪聲之中，當(dāng)載體處于靜止?fàn)顟B(tài)時(shí)，認(rèn)為陀螺輸出角速率為0。MEMS-SINS誤差方程修正如下所示：

姿態(tài)誤差方程為：
速度誤差方程為：
位置誤差方程為：

其中，為姿態(tài)誤差， 為速度誤差， 為位置誤差。為輸出角速度誤差矢量， 為所測(cè)得的比力誤差矢量。

卡爾曼濾波器系統(tǒng)狀態(tài)向量由15項(xiàng)構(gòu)成，其中， 為姿態(tài)誤差角;分別為載體的東向、北向和天向速度誤差;分別為緯度誤差、經(jīng)度誤差和高度誤差;分別為陀螺隨機(jī)常值漂移和加速度計(jì)隨機(jī)常值零偏。具體卡爾曼濾波算法功能通過軟件代碼實(shí)現(xiàn)。

通過軟件實(shí)現(xiàn)所設(shè)計(jì)的捷聯(lián)算法與組合濾波算法。重點(diǎn)研究了采用EKF組合算法之后的系統(tǒng)誤差狀態(tài)向量的估計(jì)情況。并通過matlab軟件對(duì)衛(wèi)星/MEMS組合中的各種誤差向量估計(jì)做仿真觀測(cè)。并在實(shí)際路測(cè)環(huán)境下，獲得導(dǎo)航全參數(shù)。如圖6所示為路測(cè)試驗(yàn)專用車。

如圖7所示，紅色為衛(wèi)星導(dǎo)航，藍(lán)色為組合導(dǎo)航，當(dāng)試驗(yàn)車路過城市峽谷時(shí)，由于遮擋和多路徑效應(yīng)等，衛(wèi)星導(dǎo)航出現(xiàn)較大偏差，而組合導(dǎo)航表現(xiàn)良好。

如圖8所示，當(dāng)路測(cè)試驗(yàn)車進(jìn)入地下車庫時(shí)，衛(wèi)星導(dǎo)航立刻中斷，而組合導(dǎo)航依然能夠維持導(dǎo)航能力。

當(dāng)衛(wèi)星導(dǎo)航信號(hào)中斷，組合導(dǎo)航模塊的定位、定速性能將逐漸惡化，而姿態(tài)角受影響較小。表1給出了組合濾波算法在不同的衛(wèi)星導(dǎo)航中斷間隔內(nèi)的實(shí)測(cè)結(jié)果。在靜態(tài)情況下，姿態(tài)角誤差小于0.2°，航向角誤差小于0.5°。在動(dòng)態(tài)情況下，姿態(tài)角與航向角誤差小于1°，如果衛(wèi)星導(dǎo)航信號(hào)突然中斷，將能持續(xù)30秒，定位精度維持在20米內(nèi)。

硬件平臺(tái)

本文以GPS/BD多模導(dǎo)航接收機(jī)、MEMS-IMU、磁強(qiáng)計(jì)構(gòu)成的超低成本微小型組合導(dǎo)航模塊為研究對(duì)象，結(jié)合實(shí)際需求，設(shè)計(jì)組合導(dǎo)航架構(gòu);通過比較傳統(tǒng)SINS與現(xiàn)代MEMS-SINS，提出實(shí)用的MEMS-SINS的初始化方案和捷聯(lián)解算算法;基于MEMS-IMU，給出衛(wèi)星/MEMS組合導(dǎo)航模塊工程化的濾波算法，實(shí)現(xiàn)非線性連續(xù)系統(tǒng)的線性離散化。仿真與實(shí)測(cè)試驗(yàn)結(jié)果表明：在MEMS-IMU精度較低的情況下，通過相關(guān)算法的合理設(shè)計(jì)，仍可以實(shí)現(xiàn)較高精度導(dǎo)航。圖9展示了自主設(shè)計(jì)的衛(wèi)星/MEMS組合導(dǎo)航模塊和集成開發(fā)軟件平臺(tái)。

參考文獻(xiàn)

[1]G H Elkaim. Comparison of Low-Cost GPS/INS Sensors for Autonomous Vehicle Applications [J]. 2008, 1133: 1144.
[2]Michael J. Caruso Applications of Magnetic Sensors for Low Cost Compass Systems [R]. Honeywell Inc, 2000.
[3]David H Titterton, John L Weston. Strapdown Inertial Navigation Technology 2nd Edition [M]. The IEE, 2004.
[4]秦永元, 張洪鉞, 汪叔華. 卡爾曼濾波與組合導(dǎo)航原理[M]. 西安: 西北工業(yè)大學(xué)出版社, 1998.
[5]Di Li. Based Low-cost Inertial/GPS Integrated Navigation Platform: Design and Experiments [J]. Journal of GPS, 2007
[6]杜春雷. ARM體系結(jié)構(gòu)與編程[M]. 清華大學(xué)出版社，2003.[!--empirenews.page--]