萬曉帆，呂耀文，劉偉

（長春理工大學(xué) 光電工程學(xué)院，長春 130022）

隨著導(dǎo)航技術(shù)的迅猛發(fā)展，多傳感器信息融合技術(shù)逐漸成為了組合導(dǎo)航系統(tǒng)的關(guān)鍵技術(shù)［1-2］。目前應(yīng)用最廣泛、效率最高的融合方法是基于Kalman濾波技術(shù)的聯(lián)邦濾波算法［3］。賈沛然等［4］通過跑車實(shí)驗(yàn)驗(yàn)證了聯(lián)邦Kalman濾波技術(shù)應(yīng)用于組合導(dǎo)航系統(tǒng)進(jìn)行數(shù)據(jù)融合的可行性。解可新等［5］在GPS/SINS/OD組合導(dǎo)航系統(tǒng)中單一傳感器信號(hào)缺失時(shí)，利用聯(lián)邦卡爾曼濾波器增強(qiáng)了系統(tǒng)的容錯(cuò)性。雷克萊狄斯GV等［6］通過仿真實(shí)驗(yàn)驗(yàn)證了聯(lián)邦卡爾曼濾波技術(shù)應(yīng)用于導(dǎo)航數(shù)據(jù)融合中，導(dǎo)航精度仍有很大的提升空間?；诼?lián)邦Kalman濾波技術(shù)的數(shù)據(jù)融合方法包括各子濾波器估計(jì)相關(guān)時(shí)的融合方法和各子濾波器估計(jì)不相關(guān)時(shí)的融合方法，吳延昌等［7］采用方差上界方法將各子濾波器的最優(yōu)估計(jì)值的相關(guān)狀態(tài)轉(zhuǎn)化成不相關(guān)狀態(tài)，對(duì)于各不相關(guān)的子系統(tǒng)的估計(jì)值，傳統(tǒng)的聯(lián)邦濾波器將時(shí)間序列中的每一項(xiàng)數(shù)據(jù)做等權(quán)重處理，而組合導(dǎo)航系統(tǒng)實(shí)際運(yùn)行過程中每個(gè)子濾波器對(duì)下一時(shí)刻的全局最優(yōu)估計(jì)值影響并不相同［8］。

本文設(shè)計(jì)了基于Carlson聯(lián)邦濾波器的混合聯(lián)邦濾波器，主濾波器中通過對(duì)一個(gè)時(shí)間序列的每項(xiàng)數(shù)據(jù)進(jìn)行加權(quán)處理，實(shí)現(xiàn)權(quán)重值的自適應(yīng)更新，對(duì)全局最優(yōu)估計(jì)值影響較大的子濾波器分配更大的權(quán)重值，進(jìn)而提高了組合導(dǎo)航系統(tǒng)的導(dǎo)航精度、容錯(cuò)性等指標(biāo)。

1 數(shù)據(jù)采集平臺(tái)

數(shù)據(jù)采集平臺(tái)以S5PV210為數(shù)據(jù)處理核心，S5PV210可以實(shí)現(xiàn)2000DMIPS的高性能運(yùn)算能力，能夠滿足數(shù)據(jù)采集系統(tǒng)高效、實(shí)時(shí)的要求。設(shè)計(jì)中采用MPU6050傳感器進(jìn)行慣導(dǎo)數(shù)據(jù)（INS）采集；GPS數(shù)據(jù)采集以T326三星六頻高精度定位定向板卡模塊為核心，通過串行通信協(xié)議與S5PV210處理器進(jìn)行數(shù)據(jù)傳輸；使用OV5640圖像傳感器進(jìn)行圖像采集，由于相機(jī)不同的拍攝角度會(huì)引起復(fù)雜的圖像畸變，進(jìn)而會(huì)增大數(shù)據(jù)處理的難度，所以本文相機(jī)采用水平拍攝，即相機(jī)和特征點(diǎn)在兩個(gè)平行的平面內(nèi)，利用Harris角點(diǎn)檢測(cè)法對(duì)特征點(diǎn)進(jìn)行提取，利用特征點(diǎn)周圍的的灰度信息進(jìn)行焦點(diǎn)匹配。數(shù)據(jù)采集平臺(tái)如圖1所示。

圖1 組合導(dǎo)航系統(tǒng)數(shù)據(jù)采集平臺(tái)

組合導(dǎo)航數(shù)據(jù)采集精度是實(shí)現(xiàn)高效數(shù)據(jù)融合的前提，慣導(dǎo)（INS）和GPS采用數(shù)字傳感器模塊，進(jìn)而保證了數(shù)據(jù)采集精度能夠滿足系統(tǒng)要求。單目視覺傳感器對(duì)速度的解算是整個(gè)數(shù)據(jù)采集平臺(tái)數(shù)據(jù)輸出精度的關(guān)鍵技術(shù)。單目視覺導(dǎo)航系統(tǒng)速度解算示意圖如圖2所示，導(dǎo)航參考系選擇用當(dāng)?shù)貣|北天坐標(biāo)系（地理坐標(biāo)系），導(dǎo)航系用n表示，導(dǎo)航載體系用b表示，單目系用c表示，a是空間中特征點(diǎn)，At1和At2是空間中特征點(diǎn)a對(duì)應(yīng)的在同一水平面內(nèi)的兩個(gè)像點(diǎn)。

圖2 單目視覺導(dǎo)航系統(tǒng)示意圖

像點(diǎn)隨著載體的運(yùn)動(dòng)而變化，由單目系的像點(diǎn)坐標(biāo)到導(dǎo)航系的轉(zhuǎn)移方程如式（1）所示。

為提高單目速度解算的精度，簡(jiǎn)化數(shù)據(jù)計(jì)算量，單目運(yùn)動(dòng)平面為水平二維運(yùn)動(dòng)平面，當(dāng)攝像機(jī)水平向前拍攝時(shí)，根據(jù)式（1）單目系的像點(diǎn)坐標(biāo)到導(dǎo)航系的轉(zhuǎn)移方程，導(dǎo)航系下單目解算的速度為：

2 混合聯(lián)邦濾波器器模型

Carlson聯(lián)邦濾波器由多個(gè)子濾波器和一個(gè)主濾波器構(gòu)成，對(duì)數(shù)據(jù)進(jìn)行并行、分級(jí)處理。本設(shè)計(jì)中混合聯(lián)邦濾波器模型將INS作為公共參考系，分別與GPS和單目進(jìn)行數(shù)據(jù)融合，主濾波器利用兩個(gè)子濾波器的濾波結(jié)果對(duì)INS信息進(jìn)行更新。組合導(dǎo)航濾波器結(jié)構(gòu)圖如圖3所示。

圖3 組合導(dǎo)航濾波器結(jié)構(gòu)圖

基于INS構(gòu)建捷聯(lián)慣導(dǎo)系統(tǒng)作為公共參考系，狀態(tài)變量為：

式中，δLIEδλIN為緯度、經(jīng)度誤差；δvIE、δvIN為東向、北向速度誤差；φIE、φIN、φIU為平臺(tái)航向角、橫滾角和俯仰角誤差。?E、?N為東向加速度計(jì)零偏和北向加速度計(jì)零偏；εE、εN、εU為東向陀螺儀漂移，北向陀螺儀漂移和方位陀螺儀漂移。

2.1 INS/GPS組合導(dǎo)航系統(tǒng)

GPS狀態(tài)變量為：

式中，δLG為GPS輸出的緯度誤差和經(jīng)度誤差；δvEG、δvGN為東向速度誤差和北向速度誤差；δψG為航向角誤差，δθG為俯仰角誤差，δγG為橫滾角誤差。

INS/GPS組合導(dǎo)航系統(tǒng)狀態(tài)方程為：

式中，W為均值為零、方差為Q的白噪聲，F(xiàn)INS為12×12矩陣［9］，F(xiàn)GPS為7×7矩陣，

其中相關(guān)時(shí)間τLG,τλG,τνEG,τνNG,τψG,τθG,τγG,在100～200s［10］之間選取。以上假設(shè)GPS的位置誤差、速度誤差、姿態(tài)誤差均為一階馬爾科夫過程。

傳統(tǒng)的INS/GPS組合導(dǎo)航系統(tǒng)采用位置、速度組合方式，隨著GPS技術(shù)的發(fā)展，GPS模塊可以實(shí)時(shí)輸出高精度的姿態(tài)數(shù)據(jù)，在本設(shè)計(jì)中將姿態(tài)也作為系統(tǒng)觀測(cè)量。INS/GPS組合導(dǎo)航系統(tǒng)量測(cè)方程為：

量測(cè)噪聲V是零均值，方差為R的白噪聲，且V、W互不相關(guān)。

2.2 INS/單目組合導(dǎo)航系統(tǒng)

INS/單目視覺組合導(dǎo)航采用速度組合模式，INS和單目視覺解算出的載體速度之差作為觀測(cè)

式中δLk2,δλk2為載體緯度誤差、經(jīng)度誤差，δvNk2,δvEk2為載體北向和東向速度，δφNk2,δφEk2和δφEk2分別為載體北東和天向的姿態(tài)誤差角度，δk為攝像頭標(biāo)度因數(shù)誤差。δk包括常值誤差和隨機(jī)誤差，本系統(tǒng)中用隨機(jī)常數(shù)代替常值誤差，而隨機(jī)誤差用作一階馬爾科夫過程表示，反時(shí)間相關(guān)常數(shù)設(shè)為1τc。系統(tǒng)狀態(tài)方程為：量。選取狀態(tài)量為：

式中FSINS為7×7的矩陣［9］。取INS系統(tǒng)解算的載體速度和單目解算的載體速度的差值為量測(cè)向量，系統(tǒng)的量測(cè)方程為：

2.3 全局濾波最優(yōu)估計(jì)

采用最小二乘迭代加權(quán)算法對(duì)兩個(gè)子濾波器的最優(yōu)估計(jì)值進(jìn)行融合。一般最小二乘法將時(shí)間序列中的各項(xiàng)數(shù)據(jù)的重要性同等看待，而事實(shí)上時(shí)間序列各項(xiàng)數(shù)據(jù)對(duì)未來的影響作用應(yīng)是不同的，因此比較合理的方法就是使用加權(quán)的方法，對(duì)相對(duì)重要的數(shù)據(jù)賦以較大的權(quán)數(shù)。兩個(gè)子濾波器更新的局部最優(yōu)估計(jì)值分別為，兩個(gè)子濾波器的權(quán)重分配系數(shù)關(guān)系如式（10）所示。

式中，a和b為需要確定的權(quán)值。取k個(gè)時(shí)刻的測(cè)量數(shù)據(jù)進(jìn)行最小二乘計(jì)算，公式如（11）所示。

求出K個(gè)時(shí)刻內(nèi)的參數(shù)a和b，進(jìn)而根據(jù)式（10），將兩個(gè)局部最優(yōu)估計(jì)值Xk1(t)和Xk2(t)發(fā)送到主濾波器得到最優(yōu)估計(jì)X(t)。

3 Matlab仿真結(jié)果與分析

導(dǎo)航系統(tǒng)數(shù)據(jù)采集平臺(tái)數(shù)據(jù)更新周期為0.1s，數(shù)據(jù)更新次數(shù)N=500，數(shù)據(jù)采集平臺(tái)靜止GPS初始北緯L0=43°50′12.97″，東經(jīng)λ0=125°18′17.02″，地球的自轉(zhuǎn)角速度ωie=7.27×10-5rad/s，INS中陀螺的一階馬爾科夫過程的白噪聲和相關(guān)時(shí)間分別為ωr=0.1、Tr=0.1s。INS中加速度計(jì)的一階馬爾科夫過程白噪聲和相關(guān)時(shí)間分別為ωa=0.01、Ta=0.01s。OV5640鏡頭分辨率2592×1944，視場(chǎng)角54.1H°×41.9V°。在PC端Matlab環(huán)境下，對(duì)數(shù)據(jù)采集平臺(tái)采集到數(shù)據(jù)進(jìn)行仿真分析，仿真分析流程圖4所示。

圖4 仿真分析流程圖

姿態(tài)誤差對(duì)比曲線如圖5所示，圖中實(shí)線表示應(yīng)用采用等權(quán)處理的Carlson聯(lián)邦濾波器得到的誤差曲線，虛線表示采用基于最小二乘加權(quán)處理的混合聯(lián)邦濾波器的誤差曲線。俯仰角、翻滾角和航向角誤差曲線均收斂于0值。

圖5 Carlson聯(lián)邦濾波器與混合聯(lián)邦濾波器姿態(tài)誤差對(duì)比曲線

位置誤差對(duì)比曲線如圖6所示，圖中實(shí)線表示采用等權(quán)處理的誤差曲線，虛線表示采用基于最小二乘加權(quán)處理的誤差曲線。等權(quán)處理的Carlson聯(lián)邦濾波算法位置誤差呈現(xiàn)發(fā)散狀態(tài)，而采用混合聯(lián)邦濾波器位置誤差穩(wěn)定在±2m范圍內(nèi)。

圖6 Carlson聯(lián)邦濾波器與混合聯(lián)邦濾波器位置誤差對(duì)比曲線

速度誤差對(duì)比曲線如圖7所示，圖中實(shí)線表示采用等權(quán)處理的誤差曲線，虛線表示采用基于最小二乘加權(quán)處理的誤差曲線。采用混合聯(lián)邦濾波器速度誤差曲線雖然波動(dòng)范圍較大，但是相比于等權(quán)處理的Carlson聯(lián)邦濾波器得到的速度誤差曲線并未出現(xiàn)明顯的發(fā)散現(xiàn)象，速度穩(wěn)定在±0.5m/s范圍內(nèi)。

圖7 Carlson聯(lián)邦濾波器與混合聯(lián)邦濾波器速度誤差對(duì)比曲線

4 結(jié)論

多傳感器信息融合方法是提高組合導(dǎo)航系統(tǒng)導(dǎo)航精度的關(guān)鍵技術(shù)，本文基于Carlson聯(lián)邦濾波器模型設(shè)計(jì)了混合聯(lián)邦濾波器，主濾波器中采用最小二乘加權(quán)方法給每個(gè)子濾波器分配動(dòng)態(tài)、自適應(yīng)系統(tǒng)的權(quán)重值。Matlab仿真結(jié)果表明，相比于等權(quán)重值Carlson聯(lián)邦濾波器，混合聯(lián)邦濾波器導(dǎo)航精度更高。為組合導(dǎo)航數(shù)融合方法的研究提供了參考。