一種支持傳感器即插即用的模塊化信息融合方法

楊斌，李恒，羅治斌

（1.中國電子科技集團(tuán)公司第十研究所，四川 成都 610036；2.哈爾濱工程大學(xué) 智能科學(xué)與工程學(xué)院，黑龍江 哈爾濱 150001）

0 引言

隨著信息化、智能化時(shí)代的到來，無人機(jī)（Unmanned Aerial Vehicle，UAV）和無人車（Unmanned Ground Vehicle，UGV）等逐漸進(jìn)入大眾視野。無人平臺具有風(fēng)險(xiǎn)小、成本低、環(huán)境適應(yīng)能力強(qiáng)等優(yōu)點(diǎn)，在軍事、民用方面得到了廣泛的應(yīng)用。在無人系統(tǒng)的工作過程中，感知、定位、決策、控制等四個(gè)方面相輔相成，共同決定無人系統(tǒng)的工作效能。其中，定位是無人系統(tǒng)精確、可靠地完成任務(wù)的必要前提。目前，導(dǎo)航定位技術(shù)種類繁多，每種類型都有優(yōu)勢和局限。由于無人系統(tǒng)的工作環(huán)境復(fù)雜多變，單一的導(dǎo)航傳感器已無法滿足無人系統(tǒng)對復(fù)雜環(huán)境下精確導(dǎo)航定位的需求，而基于多源信息融合的綜合導(dǎo)航技術(shù)是滿足這一需求的重要手段。隨著無人平臺裝載的導(dǎo)航傳感器種類不斷增加，導(dǎo)航系統(tǒng)對環(huán)境的適應(yīng)性越來越強(qiáng)，無人系統(tǒng)能夠獲取更精確的定位信息，但同時(shí)也增加了無人平臺導(dǎo)航系統(tǒng)設(shè)計(jì)的復(fù)雜度。當(dāng)前，綜合導(dǎo)航系統(tǒng)設(shè)計(jì)屬于“單點(diǎn)式”解決方案，各類導(dǎo)航方案獨(dú)立設(shè)計(jì)，若接入系統(tǒng)傳感器發(fā)生變化，需要對整個(gè)導(dǎo)航方案重新設(shè)計(jì)、驗(yàn)證，尚不能實(shí)現(xiàn)傳感器“即插即用”的能力，從而增加了導(dǎo)航方案投入使用的時(shí)間。本文基于模塊化信息融合架構(gòu)的全源導(dǎo)航技術(shù)來解決這一問題。

1 研究現(xiàn)狀

為滿足精確獲取導(dǎo)航定位信息的需求，導(dǎo)航系統(tǒng)經(jīng)歷了從單傳感器導(dǎo)航到多傳感器組合導(dǎo)航，再到全源導(dǎo)航系統(tǒng)的發(fā)展歷程，通過先進(jìn)的信息融合架構(gòu)實(shí)現(xiàn)系統(tǒng)組件的即插即用、敏捷開發(fā)。針對全源導(dǎo)航中傳感器的即插即用問題，部分研究者從信息融合架構(gòu)的角度對多傳感器信息融合方法進(jìn)行了研究。其中，文獻(xiàn)[3]為了實(shí)現(xiàn)全條件環(huán)境下的厘米級導(dǎo)航定位精度，分別對GPS、INS、偽衛(wèi)星、超寬帶、藍(lán)牙等定位技術(shù)進(jìn)行研究；同時(shí)為了保證各導(dǎo)航系統(tǒng)的自動(dòng)切換能力，提出一種包含過程噪聲管理器、狀態(tài)管理器、量測管理器、預(yù)測、更新的即插即用濾波器框架，在理論上實(shí)現(xiàn)了不同情況下導(dǎo)航系統(tǒng)的切換。

Groves針對惡劣環(huán)境下對導(dǎo)航定位精度和可靠性的需求，提出一種用于多傳感器導(dǎo)航定位系統(tǒng)的模塊化解決方案，但并未說明各部分的詳細(xì)實(shí)現(xiàn)，也未通過仿真驗(yàn)證其可行性。2020年，Kauffman等人為削弱導(dǎo)航系統(tǒng)對GPS的依賴性，提出了一種用于互補(bǔ)導(dǎo)航傳感器的模塊化傳感器融合套件，并將其命名為Scorpion。該開放性框架分為三個(gè)部分：模塊化導(dǎo)航濾波器、傳感器組合策略、完好性算法。其中模塊化濾波器被分為可插拔時(shí)間更新、量測更新兩部分。最后，通過仿真實(shí)驗(yàn)驗(yàn)證了該框架的可插拔性。當(dāng)前，國內(nèi)尚無利用此架構(gòu)進(jìn)行導(dǎo)航系統(tǒng)設(shè)計(jì)的公開報(bào)道，且該論文在系統(tǒng)數(shù)學(xué)建模方面存在技術(shù)細(xì)節(jié)不完備的問題。

本文基于Scorpion架構(gòu)，提出了可支持傳感器即插即用的模塊化信息融合架構(gòu)具體設(shè)計(jì)方法，并通過組合導(dǎo)航仿真驗(yàn)證了系統(tǒng)組件“即插即用”的能力。

2 模塊化信息融合架構(gòu)

考慮到全源導(dǎo)航系統(tǒng)對傳感器“即插即用”的要求，本文提出了一種由狀態(tài)模塊、量測模塊、濾波器模塊組成的支持傳感器“即插即用”的模塊化信息融合架構(gòu)，以實(shí)現(xiàn)全源導(dǎo)航系統(tǒng)的增量式開發(fā)。設(shè)計(jì)過程中將系統(tǒng)狀態(tài)模塊進(jìn)行解耦，每個(gè)狀態(tài)模塊的傳播僅與狀態(tài)模塊自身相關(guān)，不依賴于其他的狀態(tài)模塊；每個(gè)量測模塊和指定的狀態(tài)模塊相關(guān)聯(lián)，所有狀態(tài)模塊通過一個(gè)狀態(tài)向量合并成組合導(dǎo)航系統(tǒng)總的狀態(tài)，量測模塊根據(jù)量測更新的需要進(jìn)行調(diào)用；濾波器作為一個(gè)單獨(dú)的模塊，并不指定具體的狀態(tài)和量測，以實(shí)現(xiàn)其通用性，此外，濾波器模塊具有狀態(tài)、量測的添加和移除功能，可根據(jù)系統(tǒng)需要接入特定的狀態(tài)和量測。

2.1 狀態(tài)模塊

狀態(tài)模塊用于表示某一特定的狀態(tài)，它的傳播不依賴于所給定狀態(tài)模塊以外的任何狀態(tài)。狀態(tài)模塊的大小可以從單個(gè)狀態(tài)（一個(gè)傳感器的偏置）到多個(gè)狀態(tài)（組合導(dǎo)航系統(tǒng)中的誤差狀態(tài)）。狀態(tài)模塊內(nèi)部的具體信息流動(dòng)過程如圖1所示。

圖1 狀態(tài)模塊內(nèi)部的具體信息流動(dòng)過程

2.2 量測模塊

量測模塊用于表示傳感器量測和狀態(tài)向量之間的關(guān)系，其主要由傳感器量測值、量測函數(shù)、量測矩陣和量測噪聲協(xié)方差矩陣構(gòu)成。量測模塊內(nèi)部的具體信息流動(dòng)過程如圖2所示。

圖2 量測模塊內(nèi)部的具體信息流動(dòng)過程

2.3 濾波器模塊

在組合導(dǎo)航系統(tǒng)中，濾波器是融合傳感器量測信息的重要工具。由于不同傳感器的量測模型、噪聲特性不同，其所適用的濾波器也不相同，但濾波器可大致分成時(shí)間更新、量測更新兩部分，因此可將濾波器模塊分為時(shí)間更新函數(shù)、量測更新函數(shù)以及操作狀態(tài)、狀態(tài)協(xié)方差的一些基本函數(shù)。在時(shí)間更新階段，濾波器向狀態(tài)模塊請求狀態(tài)模型、狀態(tài)噪聲協(xié)方差矩陣等相關(guān)量，以執(zhí)行時(shí)間更新。在量測更新階段，濾波器向量測模塊請求量測模型、量測噪聲協(xié)方差矩陣等相關(guān)量，以執(zhí)行量測更新。

濾波器模塊的具體信息流動(dòng)過程如圖3所示。

圖3 濾波器模塊的具體信息流動(dòng)過程

3 組合導(dǎo)航系統(tǒng)及其誤差模型

組合導(dǎo)航系統(tǒng)中所使用的誤差模型對整個(gè)系統(tǒng)的性能有很大的影響。由于導(dǎo)航傳感器的多樣性，組合導(dǎo)航系統(tǒng)的誤差模型被構(gòu)建為一個(gè)模型復(fù)合體，其中包括INS、全球?qū)Ш叫l(wèi)星系統(tǒng)（Global Navigation Satellite System，GNSS）、里 程 計(jì)（Odometer，ODO）、磁 力 計(jì)（Magnetometer，Mag）、視 覺 相 機(jī)（Vision Inertial Odometer，VIO）等。因?yàn)镮NS頻率較高，能夠提供連續(xù)的導(dǎo)航信息，所以將其誤差模型作為系統(tǒng)模型的主要部分，其中INS的姿態(tài)、速度、位置誤差模型構(gòu)成了組合導(dǎo)航系統(tǒng)狀態(tài)模型的核心。

3.1 狀態(tài)誤差模型

本文選取東北天（E?N?U）地理坐標(biāo)系作為導(dǎo)航坐標(biāo)系。系統(tǒng)狀態(tài)方程可以表示為：

式中：()表示狀態(tài)誤差向量，由姿態(tài)失準(zhǔn)角、速度誤差、位置誤差、陀螺儀常值偏置、加速度計(jì)常值零偏組成；()表示狀態(tài)轉(zhuǎn)移矩陣，其詳細(xì)表示可見參考文獻(xiàn)[6]；()表示輸入過程噪聲向量；()表示過程噪聲驅(qū)動(dòng)陣。

為了便于計(jì)算機(jī)處理，將連續(xù)狀態(tài)空間模型離散化為如下形式：

式中：表示與狀態(tài)向量同維度的單位矩陣；表示INS的采樣時(shí)間；Φ ，Γ分別表示離散化后的狀態(tài)轉(zhuǎn)移矩陣和噪聲驅(qū)動(dòng)陣。

3.2 傳感器量測模型

傳感器量測模型可表達(dá)為：

式中：Ζ表示量測向量；H表示量測矩陣；v表示量測噪聲向量。為了保證無人系統(tǒng)能夠?qū)崟r(shí)獲取高精度的導(dǎo)航定位信息，本文利用INS作為基本導(dǎo)航系統(tǒng)，并將其與GNSS、ODO、Mag、VIO組合使用，從而抑制INS的發(fā)散，確保導(dǎo)航的連續(xù)性和可靠性。在使用分布式濾波架構(gòu)的前提下，可構(gòu)建4組量測方程。

1）INS與GNSS組合導(dǎo)航子系統(tǒng)，其量測向量包括位置誤差和速度誤差，它們分別是INS解算所得到的位置以及速度與GNSS量測所得到的位置和速度的差值。INS和GNSS組合導(dǎo)航系統(tǒng)的量測方程可表示為：

式中：表示INS和GNSS組合的量測矩陣；表示GNSS的量測噪聲，一般建模為零均值的高斯白噪聲。

2）INS和ODO組合導(dǎo)航子系統(tǒng)，里程計(jì)利用編碼器提供運(yùn)載器的位置增量或瞬時(shí)速度信息。本文以瞬時(shí)速度為里程計(jì)的原始輸出數(shù)據(jù)，考慮到INS解算速度投影在地理坐標(biāo)系下，INS和ODO組合導(dǎo)航系統(tǒng)的量測向量被選擇為地理坐標(biāo)系下的速度誤差，其是由INS解算的速度和經(jīng)過姿態(tài)變換的里程計(jì)速度作差得到的?？紤]到里程計(jì)測量的是無人系統(tǒng)的前向速度，根據(jù)非完整運(yùn)動(dòng)約束，可以假定無人系統(tǒng)的另外兩個(gè)方向上的速度為0。量測向量的具體表示形式如下所示：

式中：表示時(shí)刻INS解算的速度；表示時(shí)刻里程計(jì)的量測噪聲。

3）INS和Mag組合導(dǎo)航子系統(tǒng)，INS解算的位置所對應(yīng)的地磁總強(qiáng)度與磁力計(jì)量測到的地磁總強(qiáng)度的差值作為量測值，具體表示形式如下：

式中：Ζ表示時(shí)刻INS解算的位置所對應(yīng)的地磁總強(qiáng)度；Ζ表示時(shí)刻磁力計(jì)測得的地磁總強(qiáng)度。由于地磁量測模型是一個(gè)強(qiáng)非線性模型，所以INS和Mag組合導(dǎo)航系統(tǒng)可表示為：

式中：(X)表示地磁量測模型所對應(yīng)的量測非線性函數(shù)；表示時(shí)刻磁力計(jì)的量測噪聲，一般被建模為零均值的高斯白噪聲。

4）INS和視覺組合導(dǎo)航子系統(tǒng)，當(dāng)環(huán)境中存在豐富的特征點(diǎn)時(shí)，視覺能提供精確的位置信息。故而將INS解算位置與VIO輸出位置的差值作為組合導(dǎo)航系統(tǒng)的量測信息，可表示為：

式中：表示時(shí)刻INS解算得到的位置信息；表示時(shí)刻VIO系統(tǒng)輸出的位置信息。INS和視覺組合系統(tǒng)的量測方程可表示為：

式中：表示時(shí)刻INS和視覺組合系統(tǒng)的量測矩陣；表示時(shí)刻視覺相機(jī)的量測噪聲，一般假定為零均值的高斯白噪聲。

4 聯(lián)邦濾波算法

本文選擇聯(lián)邦濾波算法作為多源信息融合算法。聯(lián)邦濾波算法在組合導(dǎo)航系統(tǒng)的多傳感器信息融合中被廣泛應(yīng)用，其內(nèi)部包含一個(gè)或多個(gè)子濾波器和一個(gè)主濾波器。下面將給出聯(lián)邦濾波算法的具體實(shí)現(xiàn)。

為了能夠清晰地表達(dá)聯(lián)邦濾波器中子濾波器的信息處理過程，使用如下狀態(tài)方程：

離散時(shí)間量測表達(dá)式為：

式中=1,2,,表示局部濾波器的序列號。

聯(lián)邦濾波的計(jì)算流程為：

1）子濾波器時(shí)間更新。時(shí)間更新是一個(gè)狀態(tài)預(yù)測與誤差協(xié)方差預(yù)測的過程，它可表示為：

式中Q表示輸入過程噪聲w的協(xié)方差矩陣。

2）子濾波器量測更新。量測更新是融合量測信息，修正狀態(tài)估計(jì)值、狀態(tài)協(xié)方差的一個(gè)過程。局部濾波器的最優(yōu)卡爾曼增益可以表示為：

式中R表示時(shí)刻量測噪聲v的協(xié)方差矩陣。基于此，后驗(yàn)狀態(tài)估計(jì)值可以表示為：

后驗(yàn)狀態(tài)估計(jì)的誤差協(xié)方差矩陣可以表示為：

3）信息融合。信息融合的主要目的是通過融合所有子濾波器的狀態(tài)信息得到系統(tǒng)的全局最優(yōu)估計(jì)，具體融合算法如下所示：

4）信息共享。根據(jù)以下信息共享原則，將最優(yōu)狀態(tài)估計(jì)、狀態(tài)誤差協(xié)方差矩陣、過程噪聲矩陣分配給各子濾波器：

式中β表示標(biāo)量形式的信息共享因子。另外，根據(jù)信息分配原則，信息共享因子應(yīng)該滿足信息守恒定理：

5 仿真分析

本節(jié)以車載組合導(dǎo)航系統(tǒng)設(shè)計(jì)為例，通過仿真實(shí)驗(yàn)驗(yàn)證所設(shè)計(jì)的模塊化信息融合方法。仿真中所使用的傳感器包括慣性測量單元（Inertial Measurement Unit，IMU）、GNSS、ODO、Mag、視覺相機(jī)，其中IMU的陀螺偏置為1(°)h，加速度計(jì)零偏為500 μ；GNSS的位置量測噪聲為5 m，速度量測噪聲為0.1 m/s；ODO的速度量測噪聲為0.5 m/s；Mag的量測噪聲為1 nT。模擬的運(yùn)動(dòng)軌跡包含直線運(yùn)動(dòng)、轉(zhuǎn)彎、加減速等。

5.1 狀態(tài)模塊可插拔性驗(yàn)證

以GNSS的桿臂估計(jì)為例，驗(yàn)證模塊化信息融合方法中的狀態(tài)模塊的可插拔性。假設(shè)GNSS接收機(jī)的軸、軸、軸分別有5 m的桿臂誤差，然后利用所設(shè)計(jì)的慣導(dǎo)誤差狀態(tài)模塊、GNSS量測模塊、卡爾曼濾波器模塊設(shè)計(jì)組合導(dǎo)航系統(tǒng)，可得圖4所示未估計(jì)桿臂時(shí)的位置誤差結(jié)果。由圖可知，在不建模桿臂效應(yīng)的前提下，會(huì)引起三軸5 m左右的定位誤差。

圖4 未估計(jì)桿臂時(shí)的位置誤差

利用模塊化信息融合架構(gòu)，在系統(tǒng)中添加所設(shè)計(jì)的GNSS桿臂狀態(tài)模塊，使其與慣導(dǎo)狀態(tài)模塊結(jié)合，共同構(gòu)成系統(tǒng)的狀態(tài)，可得圖5、圖6的估計(jì)結(jié)果。

圖5 桿臂估計(jì)效果圖

從圖6可以看出，在系統(tǒng)中加入桿臂狀態(tài)模塊，可正確估計(jì)出GNSS的桿臂。通過對比圖4和圖6的位置誤差圖可知，桿臂的估計(jì)可以很好地消除位置估計(jì)偏差。仿真過程中，僅通過狀態(tài)模塊的添加或移除即可改變系統(tǒng)的狀態(tài)，系統(tǒng)將自適應(yīng)調(diào)整濾波計(jì)算相關(guān)矩陣，而不必因?yàn)橄到y(tǒng)狀態(tài)模型的改變而重新設(shè)計(jì)整個(gè)系統(tǒng)，提升了系統(tǒng)設(shè)計(jì)的靈活性和工程開發(fā)效率。

圖6 估計(jì)桿臂時(shí)的位置誤差

5.2 傳感器即插即用能力驗(yàn)證

為了驗(yàn)證傳感器的即插即用能力，本文借助模塊化信息融合架構(gòu)，設(shè)計(jì)多傳感器組合導(dǎo)航系統(tǒng)。在該仿真中，為了充分體現(xiàn)所設(shè)計(jì)模塊化信息融合架構(gòu)的靈活性，假設(shè)不同時(shí)段具有不同數(shù)量的傳感器處于工作狀態(tài)，具體設(shè)計(jì)如下：0~150 s為INS/GNSS/ODO/Mag/vision組合導(dǎo)航模式；150~300 s為INS/ODO組合導(dǎo)航模式；300~400 s為INS/ODO/Mag組合導(dǎo)航模式；400~500 s為INS/Mag組合導(dǎo)航模式；500~600 s為INS/ODO/Mag/vision組合導(dǎo) 航 模 式；600~900 s為INS/GNSS組 合 導(dǎo)航模式。在150 s，300 s，400 s，500 s，600 s時(shí)進(jìn)行了傳感器切換。

圖7展示了組合導(dǎo)航系統(tǒng)的位置誤差圖。由圖可知，系統(tǒng)開始時(shí)使用4個(gè)傳感器能使得位置快速收斂，隨著傳感器切換依然保持運(yùn)行穩(wěn)定性，驗(yàn)證了模塊化信息融合架構(gòu)下傳感器的即插即用能力。另外，由圖7可知，組合切換系統(tǒng)過程中，位置估計(jì)具有很好的一致性。由此可知，模塊化信息融合架構(gòu)能根據(jù)傳感器可用性，自適應(yīng)調(diào)整系統(tǒng)模型和相關(guān)計(jì)算矩陣，減小傳感器改變對系統(tǒng)運(yùn)行的擾動(dòng)，具有較好的環(huán)境適應(yīng)性。

圖7 組合導(dǎo)航系統(tǒng)位置誤差

6 結(jié) 語

本文提出了一種支持傳感器即插即用的模塊化信息融合方法，通過將系統(tǒng)分為狀態(tài)模塊、量測模塊、濾波器模塊，實(shí)現(xiàn)綜合導(dǎo)航系統(tǒng)各個(gè)組件的獨(dú)立開發(fā)和高效復(fù)用，有效提高了開發(fā)效率和靈活性?；谠撔畔⑷诤霞軜?gòu)，設(shè)計(jì)基于INS/GNSS/ODO/Mag/vision傳感器的組合導(dǎo)航系統(tǒng)，通過GNSS的桿臂估計(jì)驗(yàn)證了狀態(tài)模塊的可插拔性，通過車載組合導(dǎo)航系統(tǒng)工作模式無縫切換驗(yàn)證了傳感器的即插即用能力。仿真結(jié)果表明，模塊化信息融合架構(gòu)在系統(tǒng)設(shè)計(jì)階段具備使用靈活、開發(fā)效率較高的優(yōu)勢，在系統(tǒng)運(yùn)行階段可適應(yīng)異類傳感器接入和斷開，具有較好的環(huán)境適應(yīng)性。本文的研究工作對未來組合導(dǎo)航系統(tǒng)開發(fā)具有較強(qiáng)的指導(dǎo)意義。