陣列式多MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)與算法

沈 凱，左健文，左思琪，李岳倫，劉雅萱，郭文君

（北京理工大學(xué)自動(dòng)化學(xué)院，北京 100081）

目前無(wú)人車輛上廣泛使用的組合導(dǎo)航系統(tǒng)是以全球衛(wèi)星導(dǎo)航系統(tǒng)（Global Navigation Satellite System,GNSS）和慣性導(dǎo)航系統(tǒng)（Inertial Navigation System,INS）為基礎(chǔ)的導(dǎo)航定位系統(tǒng)。實(shí)踐表明，GNSS/INS組合導(dǎo)航系統(tǒng)在開(kāi)闊無(wú)遮擋的環(huán)境中表現(xiàn)良好，可以為無(wú)人車輛提供高精度和高可靠性的導(dǎo)航定位信息，但當(dāng)車輛行駛到反介入/區(qū)域拒止環(huán)境下,全球衛(wèi)星導(dǎo)航系統(tǒng)能力被嚴(yán)重削弱[1]，此時(shí)通過(guò)慣性導(dǎo)航系統(tǒng)[2,3]實(shí)現(xiàn)無(wú)人車輛高精度的自主導(dǎo)航。與全球衛(wèi)星導(dǎo)航系統(tǒng)相比，慣性導(dǎo)航系統(tǒng)是一種不依賴于外部信息、也不向外部輻射能量的自主式導(dǎo)航系統(tǒng)，其精確度、可靠性等性能直接影響著極端環(huán)境下系統(tǒng)部署和導(dǎo)航任務(wù)的完成。隨著微機(jī)電系統(tǒng)（Micro-Electro-Mechanical System, MEMS）技術(shù)的快速發(fā)展，成本低、體積小、重量輕的微慣性測(cè)量單元（MEMS-IMU）精確度、可靠性不斷提升，逐步在智能車輛、無(wú)人機(jī)和機(jī)器人導(dǎo)航與定位等領(lǐng)域應(yīng)用[4-6]。

在全球衛(wèi)星導(dǎo)航系統(tǒng)拒止環(huán)境下，MEMS-IMU誤差隨時(shí)間不斷累積。為了抑制MEMS-IMU誤差發(fā)散，一方面從硬件上（即硬件方式）改良慣性敏感元件的生產(chǎn)工藝或使用新材料，但研制周期長(zhǎng)、成本高、難度大；另一方面從系統(tǒng)和算法設(shè)計(jì)上（即算法方式）來(lái)補(bǔ)償誤差，從而提升MEMS-IMU的精度。與硬件方式相比，采用算法方式能夠在很大程度上縮短研制周期，具有成本低、見(jiàn)效快、效果好等優(yōu)點(diǎn)。

當(dāng)前，為了實(shí)現(xiàn)慣導(dǎo)誤差補(bǔ)償，主要采取以下兩種算法方式：一是旋轉(zhuǎn)調(diào)制技術(shù)，通過(guò)采用旋轉(zhuǎn)或者翻轉(zhuǎn)等物理方式，周期性地改變IMU相對(duì)于導(dǎo)航系的方位，從而將IMU的常值誤差調(diào)制成周期性變化的物理量，促使其在一個(gè)周期內(nèi)積分為零或一個(gè)極小的值，達(dá)到抑制IMU誤差發(fā)散的目的[7]；二是協(xié)同導(dǎo)航技術(shù)，通過(guò)聯(lián)合利用多個(gè)IMU的測(cè)量信息，采用互補(bǔ)濾波或信息融合算法[8]，精確地估計(jì)IMU的各項(xiàng)誤差，并對(duì)IMU測(cè)量誤差進(jìn)行補(bǔ)償，從而提高IMU的導(dǎo)航定位性能。

為了實(shí)現(xiàn)不依賴衛(wèi)星的高精度位置、速度和姿態(tài)等導(dǎo)航信息解算，本文從微慣性器件自身工作機(jī)理出發(fā)，分析影響其精度和可靠性的關(guān)鍵因素，綜合利用MEMS-IMU、旋轉(zhuǎn)調(diào)制和協(xié)同導(dǎo)航技術(shù)，分析對(duì)比了基于旋轉(zhuǎn)調(diào)制技術(shù)的雙MEMS-IMU協(xié)同導(dǎo)航方案，并設(shè)計(jì)了適用于自主無(wú)人系統(tǒng)導(dǎo)航的陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)，從而提升了MEMS-IMU的在線使用精度、可靠性等性能。

1 基于旋轉(zhuǎn)調(diào)制技術(shù)的微慣性測(cè)量單元

1.1 單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU系統(tǒng)

旋轉(zhuǎn)調(diào)制型慣導(dǎo)是在IMU的外部加裝轉(zhuǎn)動(dòng)機(jī)構(gòu)和測(cè)角裝置，并采用一定的旋轉(zhuǎn)方案而構(gòu)成的系統(tǒng)。旋轉(zhuǎn)調(diào)制型慣導(dǎo)按轉(zhuǎn)軸數(shù)目的多少可以分為單軸系統(tǒng)、雙軸系統(tǒng)和三軸系統(tǒng)等類型[9,10]，而每種類型又可以采用不同的轉(zhuǎn)動(dòng)方案，例如連續(xù)旋轉(zhuǎn)、多位置轉(zhuǎn)停方案等[11,12]。

為了提高M(jìn)EMS-IMU長(zhǎng)時(shí)間工作的導(dǎo)航精度，采用旋轉(zhuǎn)調(diào)制技術(shù)將慣導(dǎo)誤差調(diào)制成周期性變化信號(hào)，能夠有效抑制MEMS-IMU誤差發(fā)散。圖1展示了單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU，其主要由三部分組成，即MEMS-IMUA、轉(zhuǎn)臺(tái)以及電機(jī)。MEMS-IMUA與轉(zhuǎn)臺(tái)固聯(lián)，電機(jī)內(nèi)部集成了光電編碼器作為測(cè)角機(jī)構(gòu)，并采取繞平臺(tái)y軸正反180 °旋轉(zhuǎn)調(diào)制方案。

在圖1所示的單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU中，影響MEMS-IMU測(cè)量誤差的主要因素包括：陀螺儀漂移、加速度計(jì)零偏誤差、標(biāo)度因數(shù)誤差、安裝誤差以及測(cè)量噪聲等。為了建立單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU模型，需要建立載體坐標(biāo)系、平臺(tái)坐標(biāo)系與傳感坐標(biāo)系之間的轉(zhuǎn)換關(guān)系。

圖1 單軸旋轉(zhuǎn)調(diào)制型MEMS-IMUFig.1 Single-axis rotary modulation MEMS-IMU

在單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU中，定義載體坐標(biāo)系b的原點(diǎn)在載體的重心位置，坐標(biāo)系與載體固聯(lián)，Xb軸指向載體的右方，Yb軸指向載體的前方，Zb軸指向載體的上方，構(gòu)成右手系；定義平臺(tái)坐標(biāo)系p與轉(zhuǎn)動(dòng)機(jī)構(gòu)固聯(lián)，其旋轉(zhuǎn)時(shí)與載體系在平面上的夾角定義為旋轉(zhuǎn)機(jī)構(gòu)的旋轉(zhuǎn)角度；定義傳感器坐標(biāo)系s與MEMS-IMU 固聯(lián)，其三個(gè)軸的方向分別與MEMS-IMU傳感器的三個(gè)敏感軸方向相重合，但與平臺(tái)系之間存在安裝誤差。

單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU中平臺(tái)坐標(biāo)系p到傳感器坐標(biāo)系s的轉(zhuǎn)換矩陣為：

單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU中載體坐標(biāo)系b到平臺(tái)坐標(biāo)系p的轉(zhuǎn)換矩陣為：

式中：φA為MEMS-IMUA的旋轉(zhuǎn)角。

此時(shí)，傳感器坐標(biāo)系s下的角速率和加速度的量測(cè)模型為：

考慮MEMS-IMU的誤差通常都是較小的值，我們可以忽略安裝誤差與標(biāo)度因數(shù)的乘積，作式(4)所示的近似處理，陀螺漂移誤差和測(cè)量誤差同理。

因此，平臺(tái)坐標(biāo)系p下的角速率和加速度的量測(cè)模型為：

由于慣性導(dǎo)航系統(tǒng)的導(dǎo)航信息需要在導(dǎo)航坐標(biāo)系下進(jìn)行解算，為了方便討論，本文假設(shè)導(dǎo)航坐標(biāo)系與載體坐標(biāo)系重合。因此，式(5)所示的量測(cè)模型可以通過(guò)坐標(biāo)變換矩陣轉(zhuǎn)化到導(dǎo)航坐標(biāo)系下：

單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU是最簡(jiǎn)單、最易實(shí)現(xiàn)的系統(tǒng)之一，然而單軸旋轉(zhuǎn)調(diào)制型系統(tǒng)具有一個(gè)最大的缺點(diǎn)：旋轉(zhuǎn)軸方向上的誤差無(wú)法被調(diào)制，導(dǎo)致其誤差仍然隨時(shí)間累積而發(fā)散。

2.2 正交式旋轉(zhuǎn)調(diào)制型雙MEMS-IMU協(xié)同系統(tǒng)

為了解決單軸旋轉(zhuǎn)調(diào)制型MEMS-IMU系統(tǒng)旋轉(zhuǎn)軸方向上的誤差累積問(wèn)題，采用繞不同軸旋轉(zhuǎn)的雙MEMS-IMU協(xié)同導(dǎo)航方法[12]，以提高導(dǎo)航系統(tǒng)誤差狀態(tài)的可觀測(cè)度，從而在線標(biāo)定和補(bǔ)償各個(gè)MEMS-IMU的誤差。如圖2所示，MEMS-IMUA旋轉(zhuǎn)調(diào)制而MEMS-IMUB靜止（記為型系統(tǒng)），構(gòu)成型正交式旋轉(zhuǎn)調(diào)制雙MEMS-IMU系統(tǒng)。

圖2 型正交式旋轉(zhuǎn)調(diào)制雙MEMS-IMU系統(tǒng)Fig.2 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

為構(gòu)建正交式旋轉(zhuǎn)調(diào)制雙MEMS-IMU誤差模型，將系統(tǒng)中旋轉(zhuǎn)調(diào)制 MEMS-IMUA 和靜止MEMS-IMUB的量測(cè)模型相減，得到：

為了進(jìn)一步提高雙MEMS-IMU姿態(tài)誤差的可觀測(cè)度，可以采用MEMS-IMUA和B同時(shí)旋轉(zhuǎn)調(diào)制的正交式旋轉(zhuǎn)調(diào)制雙MEMS-IMU（記為型系統(tǒng)）方案。如圖3所示，MEMS-IMUA的旋轉(zhuǎn)調(diào)制方式為：繞其平臺(tái)坐標(biāo)系y軸進(jìn)行正反180 °旋轉(zhuǎn)，旋轉(zhuǎn)的角度為Aφ；MEMS-IMUB的旋轉(zhuǎn)調(diào)制方式為：繞其平臺(tái)坐標(biāo)系的x軸進(jìn)行正反180 °旋轉(zhuǎn)，旋轉(zhuǎn)的角度為Bφ。

圖3 型正交式旋轉(zhuǎn)調(diào)制雙MEMS-IMU系統(tǒng)Fig.3 Dual MEMS-IMU cooperative navigation system based onorthogonal rotary modulation

2 陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)及算法

2.1 陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)

針對(duì)無(wú)人車等自主無(wú)人系統(tǒng)的行駛特點(diǎn)，在利用雙MEMS-IMU獲取精準(zhǔn)姿態(tài)信息的基礎(chǔ)上，為了進(jìn)一步解算出更高精度的定位定向信息，本文提出了如圖4所示的陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)方案。

圖4 陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)Fig.4 Array four MEMS-IMU cooperative navigation system

為了充分發(fā)揮旋轉(zhuǎn)調(diào)制技術(shù)與協(xié)同導(dǎo)航系統(tǒng)技術(shù)的綜合優(yōu)勢(shì)，對(duì)定向和定位誤差進(jìn)行補(bǔ)償，最大限度地提升導(dǎo)航系統(tǒng)的精度和可靠性，把陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)劃分為3個(gè)子系統(tǒng)，每個(gè)子系統(tǒng)包含兩個(gè)MEMS-IMU，且子系統(tǒng)內(nèi)部可以獨(dú)立工作，其中子系統(tǒng)劃分如表1所示。

表1 陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)的子系統(tǒng)劃分Tab.1 Subsystem division of array four MEMS-IMU cooperative navigation system

2.2 聯(lián)邦卡爾曼濾波算法

為了高效準(zhǔn)確地處理陣列式四MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)中各子系統(tǒng)的導(dǎo)航信息，本文采用聯(lián)邦卡爾曼濾波算法（Federated Kalman Filter, FKF）融合各子系統(tǒng)信息，如圖5所示。

圖5 聯(lián)邦卡爾曼濾波算法框架圖Fig.5 Framework of federated Kalman filter algorithm

以東-北-天地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系，在本課題中，導(dǎo)航坐標(biāo)系與載體坐標(biāo)系重合，在卡爾曼濾波算法框架下，各子濾波器的狀態(tài)空間模型為：

式中：xi(t)為子系統(tǒng)各項(xiàng)誤差組成的狀態(tài)變量，fi(t)為狀態(tài)轉(zhuǎn)移矩陣，ui(t)為控制量，gi(t)為輸入矩陣，wi(t)為過(guò)程噪聲；zi(t)為測(cè)量輸出，hi(t)為測(cè)量矩陣，vi(t)為測(cè)量噪聲；i表示第i個(gè)子系統(tǒng)。

同理，系統(tǒng)的量測(cè)方程為：

進(jìn)而，得到各個(gè)子濾波器的離散時(shí)間狀態(tài)方程和量測(cè)方程：

此時(shí)，聯(lián)邦卡爾曼濾波算法迭代過(guò)程如下。

聯(lián)邦卡爾曼濾波算法步驟一：子濾波器i初始化0 0 ? [ ]i i X =EX T 0 0 0 0 0 ? ?[()()]i i i i i P EX X X X= - -步驟二：子濾波器i時(shí)間更新| 1 | 1 1 ? ? i i i kk kk k X X-- -=Φ T T| 1 | 1 1 | 1 1 1 1 ?i i i i i i i kk kk k kk k k k P P-- - - - - -=Φ Φ+GQG步驟三：子濾波器i量測(cè)更新T T| 1 | 1[]i i i i i i i k kk k k kk k k K P H HP H R-1--=+| 1 | 1 ? ? ?()i i i i i i k kk k k k kk-X X KZ HX-= + -| 1 ? ?(I)i i i i k k k kk P KH P-= -步驟四：主濾波器信息融合1 1 1 ? ?[ ()]N g i k k i=P P --=∑ ； 1 ? ? ? ?[ () ]N g g i i k k k k i=1 X P P-X= ∑步驟五：信息共享與全局狀態(tài)估計(jì)? ? i g k k X=X；N i i=1∑β=1 1? i g k i k P=β-P； 1 i g k i k Q=β-Q

3 MEMS-IMU實(shí)驗(yàn)平臺(tái)搭建及仿真試驗(yàn)分析

3.1 旋轉(zhuǎn)調(diào)制型MEMS-IMU實(shí)驗(yàn)平臺(tái)

為了研究旋轉(zhuǎn)調(diào)制型MEMS-IMU的工作性能，基于ADIS16405型MEMS-IMU，搭建了如圖6所示的實(shí)驗(yàn)平臺(tái)。

圖6 旋轉(zhuǎn)調(diào)制型MEMS-IMU實(shí)驗(yàn)平臺(tái)圖Fig.6 Experimental platform of rotary modulation MEMS-IMU

該系統(tǒng)主要由MEMS-IMU旋轉(zhuǎn)調(diào)制平臺(tái)、電源穩(wěn)壓模塊、控制器模塊和數(shù)據(jù)采集與處理模塊等構(gòu)成。MEMS-IMU旋轉(zhuǎn)調(diào)制平臺(tái)測(cè)量旋轉(zhuǎn)角速率和加速度；電源穩(wěn)壓模塊為ADIS16405、控制器等提供工作電壓；控制器模塊提供所需的旋轉(zhuǎn)調(diào)制方案；數(shù)據(jù)采集與處理模塊用于讀取角速率、加速度、旋轉(zhuǎn)角等信息，并進(jìn)行導(dǎo)航解算。

通過(guò)將搭建好的旋轉(zhuǎn)調(diào)制型MEMS-IMU實(shí)驗(yàn)平臺(tái)分別設(shè)置為靜止?fàn)顟B(tài)和旋轉(zhuǎn)調(diào)制狀態(tài)，然后讀取加速度、旋轉(zhuǎn)角速率的位姿測(cè)量信息。最后，利用高通濾波器對(duì)所得的系統(tǒng)量測(cè)數(shù)據(jù)濾波，從而獲得微慣性測(cè)量單元ADIS16405的實(shí)際噪聲。

3.2 無(wú)人車輛運(yùn)動(dòng)仿真設(shè)置

本實(shí)驗(yàn)?zāi)M的無(wú)人車輛運(yùn)動(dòng)情景如圖7所示。

圖7 無(wú)人車輛運(yùn)動(dòng)模擬情景示意圖Fig.7 Simulation of unmanned vehicle driving environment

無(wú)人車輛從GNSS信號(hào)良好路段駛?cè)氤鞘袇^(qū)域，由于城市高樓建筑遮擋GNSS信號(hào)，因此無(wú)人駕駛車輛主要依靠純MEMS-IMU進(jìn)行慣性導(dǎo)航，以GNSS導(dǎo)航路段與MEMS-IMU導(dǎo)航路段交接處為起點(diǎn)，設(shè)置無(wú)人車輛的姿態(tài)角運(yùn)動(dòng)狀態(tài)為：

其中ψ為無(wú)人車輛的航向角，θ為俯仰角，γ為翻滾角，并設(shè)置無(wú)人車輛的東向和北向的運(yùn)動(dòng)狀態(tài)為：

在上述設(shè)置的無(wú)人車輛運(yùn)動(dòng)狀態(tài)條件下，慣性導(dǎo)航采用本文設(shè)計(jì)的陣列式多MEMS-IMU協(xié)同導(dǎo)航算法，并注入慣性器件ADIS16405采集的實(shí)際噪聲數(shù)據(jù)，然后仿真測(cè)試各導(dǎo)航方案的定向定位性能。

3.3 姿態(tài)解算實(shí)驗(yàn)及對(duì)比分析

圖8 定向性能及對(duì)比分析圖Fig.8 Orientation performance and comparative analysis

為了更直觀地對(duì)比分析不同導(dǎo)航系統(tǒng)所能達(dá)成的定向性能，在表2中列出了均方根（Root Mean Square,RMS）指標(biāo)下各方案姿態(tài)角解算誤差。

表2 導(dǎo)航系統(tǒng)姿態(tài)角解算誤差對(duì)比表Tab.2 Comparison of attitude angle calculation error of navigation system

3.4 位置解算實(shí)驗(yàn)及對(duì)比分析

圖9 定位性能及對(duì)比分析圖Fig.9 Positioning performance and comparative analysis

表3展示了各個(gè)方案的位置解算誤差。

表3 導(dǎo)航系統(tǒng)位置解算誤差對(duì)比表Tab.3 Comparison of positioning calculation error of navigation system

3.5 定向定位綜合性能分析

表4 各方案導(dǎo)航誤差的RMS指標(biāo)改善百分比Tab.4 Improvement percentage of navigation error of each system

4 結(jié) 論

在全球衛(wèi)星導(dǎo)航系統(tǒng)失效后，慣性導(dǎo)航是實(shí)現(xiàn)自主無(wú)人系統(tǒng)高精度自主導(dǎo)航與定位的重要途徑。MEMS-IMU作為慣性導(dǎo)航的一種低成本途徑，其長(zhǎng)時(shí)間工作的精確度和可靠性較差，為了解決這一問(wèn)題，本文綜合利用MEMS-IMU、旋轉(zhuǎn)調(diào)制和協(xié)同導(dǎo)航技術(shù)，設(shè)計(jì)了陣列式四MEMS-IMU協(xié)同導(dǎo)航方案，并利用慣性器件ADIS16405采集的實(shí)際噪聲數(shù)據(jù)進(jìn)行了仿真實(shí)驗(yàn)。實(shí)驗(yàn)結(jié)果表明：本文設(shè)計(jì)的陣列式四MEMS-IMU協(xié)同導(dǎo)航方案能夠有效抑制低成本MEMS-IMU的各項(xiàng)導(dǎo)航誤差；在自主無(wú)人系統(tǒng)運(yùn)行120 s內(nèi)，姿態(tài)角解算誤差不大于0.25 °；自主行駛1000 m時(shí)，東向和北向位置誤差不大于3.07%d和1.04%d。因此，陣列式四MEMS-IMU協(xié)同導(dǎo)航方案具有一定的實(shí)用性，能夠顯著提升低成本MEMS-IMU的導(dǎo)航性能，實(shí)現(xiàn)不依賴衛(wèi)星的高精度位置、速度和姿態(tài)等解算。未來(lái)研究工作將進(jìn)一步優(yōu)化多MEMS-IMU協(xié)同導(dǎo)航系統(tǒng)相關(guān)算法，開(kāi)展實(shí)驗(yàn)驗(yàn)證，致力于系統(tǒng)的具體實(shí)現(xiàn)。