張彥濤,趙宏明,譚立龍,王 鵬,鐘 山

(火箭軍工程大學(xué), 西安 710025)

0 引言

SINS以數(shù)字遞推的方式實(shí)現(xiàn)載體定位,慣性測(cè)量組件的漂移誤差在積分環(huán)節(jié)中逐漸積累,導(dǎo)致定位系統(tǒng)出現(xiàn)較大偏差[1]。縮小誤差的方法一般分為兩類:一是與衛(wèi)星導(dǎo)航系統(tǒng)(GPS、GNSS、北斗等)組合構(gòu)成組合導(dǎo)航系統(tǒng)[2],二是借助外部傳感器的測(cè)量信息[3]。

增加外部傳感器系統(tǒng)輔助SINS可以有效的避免輔助定位信息丟失,完成對(duì)車輛的精確定位[4]。傳統(tǒng)的GPS/INS組合導(dǎo)航系統(tǒng)易受外界環(huán)境及器件漂移誤差影響,難以準(zhǔn)確建立模型實(shí)現(xiàn)最優(yōu)濾波[5],激光多普勒測(cè)速儀(laser Doppler velocimeter,LDV)和里程計(jì)(odometer,OD)是SINS常用的外部輔助傳感器[6],文中提出采用LDV與OD的不同測(cè)速原理和誤差特性,二者之間相互配合,對(duì)載體精密測(cè)量,實(shí)現(xiàn)高精度實(shí)時(shí)定位。

1 速度測(cè)量原理

載體速度是導(dǎo)航系統(tǒng)需要精確測(cè)量的關(guān)鍵參數(shù)之一。載體速度的測(cè)量方法主要有4種:GPS、LDV、OD和加速度計(jì)[7]。LDV和OD直接安裝在車輛上,自主性強(qiáng),可靠性高。

1.1 LDV測(cè)速原理

LDV是利用多普勒效應(yīng)直接測(cè)量載體的速度,通過(guò)測(cè)量發(fā)射激光與反射激光之間頻率的差值,確定物體的速度。具有反應(yīng)快、測(cè)速精度高等優(yōu)點(diǎn)。

(1)

(2)

1.2 OD測(cè)速原理

OD用來(lái)進(jìn)行位置導(dǎo)航,在以速度為觀測(cè)量建立卡爾曼濾波觀測(cè)方程時(shí)[8],OD測(cè)速誤差會(huì)降低濾波的修正效果。

航位推算模型:

(3)

誤差源有兩個(gè):安裝角誤差和刻度系數(shù)誤差。φO經(jīng)過(guò)標(biāo)定精確后可以當(dāng)成一個(gè)常數(shù)。δKO可以用一階馬爾科夫過(guò)程來(lái)表述:

(4)

式中：τ為馬爾科夫過(guò)程相關(guān)時(shí)間;T為輸出周期。

OD航位推算誤差:

(5)

實(shí)際航位推算方程:

(6)

DR誤差方程:

(7)

2 聯(lián)邦卡爾曼濾波基本原理

卡爾曼濾波一般可以分為集中式濾波和分散式濾波[9]。集中式濾波計(jì)算量大,不利于實(shí)時(shí)計(jì)算,Li等[10]提出魯棒性好的卡爾曼濾波器,但濾波器的容錯(cuò)性能差,不易診斷故障,造成導(dǎo)航系統(tǒng)輸出可靠性差。文中采用分散式濾波,即聯(lián)邦卡爾曼濾波來(lái)構(gòu)建組合導(dǎo)航系統(tǒng)的濾波器[11]。

2.1 聯(lián)邦濾波器算法

假設(shè)濾波過(guò)程中的總噪聲Q-1分配到各個(gè)子濾波和主濾波器中,則有:

(8)

狀態(tài)的初始信息按照信息分配原則進(jìn)行分配:

(9)

因此信息總量在分配前后保持不變,若各子濾波器和主濾波器之間相互獨(dú)立,則全局最優(yōu)估計(jì)可表示為:

(10)

2.2 聯(lián)邦卡爾曼濾波算法最優(yōu)性

運(yùn)用聯(lián)邦卡爾曼濾波器進(jìn)行估計(jì)時(shí),子濾波器將濾波結(jié)果輸出與主濾波器融合,再用融合的結(jié)果對(duì)子濾波器進(jìn)行校正,得到最優(yōu)的濾波結(jié)果[12]。

系統(tǒng)狀態(tài)向量:

(11)

系統(tǒng)狀態(tài)方程:

(12)

增廣系統(tǒng)協(xié)方差矩陣:

(13)

其中,時(shí)間更新和量測(cè)更新之間相互獨(dú)立。

1)時(shí)間更新

由式(12)得到:

(14)

則有:

(15)

由于公共噪聲Q的存在,Pji≠0始終成立,即時(shí)間更新引入的各子濾波器狀態(tài)相關(guān)?！胺讲钌辖纭狈梢韵鸬淖訛V波器狀態(tài)相關(guān)問(wèn)題。

初始協(xié)方差矩陣上界設(shè)置:

(16)

采用“方差上界”法處理后,各個(gè)子濾波器的量測(cè)更新和時(shí)間更新都可以獨(dú)立,各子濾波器之間的狀態(tài)估計(jì)也是獨(dú)立的。

2)量測(cè)更新

系統(tǒng)量測(cè)更新方程:

(17)

當(dāng)Pjl(-)增加γi倍時(shí),Pjl(+)不會(huì)相應(yīng)增加γi倍。解決此問(wèn)題可以采用系統(tǒng)重置法:用全局濾波來(lái)重置濾波協(xié)方差和局部濾波。

經(jīng)過(guò)重置后,協(xié)方差最優(yōu)融合估計(jì)在任何情況下都是成立的。Pg(+)可以按照下式:

(18)

上述合成Pg(+)用到了主濾波器和子濾波器協(xié)方差陣之逆。局部估計(jì)結(jié)果雖是次優(yōu)的,但全局濾波結(jié)果是最優(yōu)的。

2.3 聯(lián)邦濾波器結(jié)構(gòu)與性能分析

在設(shè)計(jì)聯(lián)邦卡爾曼濾波器的過(guò)程中,將全局總噪聲Q-1分配至主濾波器和子濾波器中,βi取不同值可以得到不同類型的濾波器結(jié)構(gòu)[13]。典型的聯(lián)邦卡爾曼濾波器結(jié)構(gòu)有6種,如表1所示。表中FDI是指系統(tǒng)故障檢測(cè)隔離能力,FR是指系統(tǒng)故障恢復(fù)能力。

表1 六種聯(lián)邦濾波結(jié)構(gòu)性能分析表

3 自主定位系統(tǒng)濾波器設(shè)計(jì)

SINS精度隨時(shí)間增加逐漸降低。通過(guò)運(yùn)用聯(lián)邦卡爾曼濾波技術(shù),將LDV/OD測(cè)速傳感器與車輛輔助信息應(yīng)用到車載導(dǎo)航中,構(gòu)建基于車輛約束模型的SINS/LDV/OD自主定位系統(tǒng)。

3.1 自主定位系統(tǒng)結(jié)構(gòu)設(shè)計(jì)

SINS作為公共參考系統(tǒng),添加LDV和OD測(cè)速傳感器,構(gòu)建聯(lián)邦卡爾曼濾波器,如圖1所示。

3個(gè)子濾波器采用集中濾波的方式估計(jì)導(dǎo)航誤差,主濾波器融合各子濾波器結(jié)果,獲得定位結(jié)果的最優(yōu)估計(jì),反饋到各子濾波器和SINS進(jìn)行濾波器自校正,并輸出濾波結(jié)果校正定位誤差。[14]

3.2 子濾波器設(shè)計(jì)

根據(jù)SINS誤差模型,選取位置、速度和姿態(tài)誤差,并與陀螺和加速度計(jì)漂移構(gòu)成15維狀態(tài)矢量,構(gòu)建SINS誤差方程[4]。

圖1 自主定位系統(tǒng)卡爾曼濾波器原理圖

(19)

式中,狀態(tài)向量XS有15個(gè)狀態(tài)的列向量:

(20)

系數(shù)矩陣:

(21)

設(shè)Re=6 378 137.0 m,f=1/298.275,

ωie=15°/h,Rh=Re+h,R1=Re(1-2f+3fsin2L)+h,

R2=Re(1+fsin2L)+h。

系統(tǒng)噪聲:

(22)

噪聲矩陣的系數(shù)矩陣:

(23)

1)SINS/LDV子濾波器

該子濾波器采用集中濾波的方式,選取SINS的誤差方程(19)和LDV的誤差方程作為子濾波器的狀態(tài)方程[15]。以速度差值作為該子濾波器的觀測(cè)量,建立子系統(tǒng)量測(cè)方程。

狀態(tài)方程:

(24)

量測(cè)方程:

(25)

(26)

2)SINS/OD子濾波器

在將OD與SINS組合,采用集中濾波的方式建立子濾波器時(shí),選取SINS的誤差方程和DR的誤差方程作為子濾波器的狀態(tài)方程[16]。以SINS與DR之間的位置的差值作為該子濾波器的觀測(cè)量,建立子系統(tǒng)量測(cè)方程。

狀態(tài)方程:

(27)

量測(cè)方程:

(28)

(29)

3.3 主濾波器設(shè)計(jì)

1)系統(tǒng)離散化

卡爾曼濾波建立的狀態(tài)方程和量測(cè)方程是連續(xù)的,因此首先要對(duì)建立的狀態(tài)方程和量測(cè)方程進(jìn)行離散化處理。

(30)

F(t)化成Φk,k-1:

(31)

2)信息分配

(32)

3)主系統(tǒng)濾波

子系統(tǒng)采用集中式卡爾曼濾波法,濾波周期為1 s;主濾波器10 s鐘進(jìn)行一次信息融合計(jì)算,融合算法:

(33)

3.4 系統(tǒng)容錯(cuò)性設(shè)計(jì)

容錯(cuò)系統(tǒng)的具體結(jié)構(gòu)如圖2所示,可以看出該容錯(cuò)系統(tǒng)主要包括子系統(tǒng)故障的檢測(cè)、隔離和容錯(cuò)綜合算法。

檢測(cè)故障的子濾波器并進(jìn)行隔離,其余的子濾波器誤差估計(jì)值進(jìn)入主濾波器,進(jìn)行系統(tǒng)的誤差估計(jì),同時(shí)利用主濾波器的結(jié)果對(duì)故障子濾波器進(jìn)行故障修復(fù)和檢測(cè)。當(dāng)故障子濾波系統(tǒng)恢復(fù)正常后接入系統(tǒng),將子濾波結(jié)果輸入到主濾波器,系統(tǒng)繼續(xù)提供精確的導(dǎo)航信息。

圖2 基于載體約束的SINS/LDV/OD容錯(cuò)系統(tǒng)結(jié)構(gòu)

(34)

(35)

1)當(dāng)子濾波器1失效時(shí),主濾波器估計(jì)誤差:

(36)

(37)

估計(jì)失效子系統(tǒng)1:

(38)

2)當(dāng)子濾波器2失效時(shí),主濾波器估計(jì)誤差:

(39)

(40)

估計(jì)失效子系統(tǒng)2:

(41)

3)當(dāng)子濾波器3失效時(shí),主濾波器估計(jì)誤差:

(42)

(43)

估計(jì)失效子系統(tǒng)3:

(44)

4 試驗(yàn)檢驗(yàn)

為驗(yàn)證系統(tǒng)有效性及定位精度,在西安地區(qū)進(jìn)行跑車試驗(yàn)。同時(shí)將OD安裝在試驗(yàn)車輛上。OD刻度系數(shù)誤差是0.013 09,滾轉(zhuǎn)角誤差0.700 31′,偏航角誤差0.720 31′,俯仰角誤差-0.085 59′。SINS進(jìn)行5 min初始對(duì)準(zhǔn),起步2 min內(nèi)對(duì)OD進(jìn)行刻度系數(shù)誤差標(biāo)定。在固定時(shí)間內(nèi)特定傳感器停止采集數(shù)據(jù),其它傳感器正常工作。

跑車試驗(yàn)從經(jīng)緯度(108.77°,34.03°)處出發(fā),至經(jīng)緯度(108.96°,34.31°)處結(jié)束,總路程48.4 km,用時(shí)107 min,其中前420 s靜止,行駛路線如圖3所示。

圖3 跑車試驗(yàn)軌跡圖

圖4表示實(shí)際跑車中位置誤差?？梢钥闯?試驗(yàn)中經(jīng)度誤差不超過(guò)20 m,緯度誤差不超過(guò)20 m,高度誤差不超過(guò)10 m。

圖4 跑車試驗(yàn)位置誤差

試驗(yàn)過(guò)程中,模擬自主定位系統(tǒng)中LDV故障,其它設(shè)備正常工作。跑車試驗(yàn)1 500～1 800 s時(shí),不記錄LDV采集數(shù)據(jù)。子濾波器1的輸出位置誤差、速度誤差如圖5所示。

子濾波器1故障時(shí),對(duì)LDV誤差進(jìn)行估計(jì)。由結(jié)果可知經(jīng)度誤差不超過(guò)15 m,緯度誤差不超過(guò)20 m,高度誤差不超過(guò)10 m,可以看出東向和北向速度誤差不超過(guò)0.1 m/s,天向速度誤差不超過(guò)0.05 m/s。

圖5 子濾波器1位置誤差、速度誤差

模擬自主定位系統(tǒng)中OD故障,其它設(shè)備正常工作。跑車試驗(yàn)2 500～2 800 s時(shí),不記錄OD采集數(shù)據(jù)。子濾波器3的輸出結(jié)果如圖6所示。

圖6 子濾波器2位置誤差、速度誤差

子濾波器2故障時(shí),對(duì)航位推算誤差進(jìn)行估計(jì)?？芍?jīng)度誤差不超過(guò)15 m,緯度誤差不超過(guò)25 m,高度誤差不超過(guò)5 m,東向和北向速度誤差不超過(guò)0.1 m/s,天向速度誤差不超過(guò)0.05 m/s。

模擬自主定位系統(tǒng)中角度傳感器故障,其它設(shè)備正常。在3 500 s至3 800 s時(shí),不記錄角度傳感器采集數(shù)據(jù)。子濾波器3輸出的位置誤差、速度誤差如圖7所示。

圖7 子濾波器3位置誤差、速度誤差

子濾波器3故障時(shí),對(duì)其誤差進(jìn)行估計(jì)。經(jīng)度誤差不超過(guò)20 m,緯度誤差不超過(guò)25 m,高度誤差不超過(guò)3 m,東向和北向速度誤差不超過(guò)0.1 m/s,天向速度誤差不超過(guò)0.05 m/s。

模擬自主定位系統(tǒng)正常工作,主濾波器的位置誤差、速度誤差如圖8所示。

聯(lián)邦濾波器正常工作時(shí),對(duì)自主定位系統(tǒng)的定位誤差進(jìn)行估計(jì)。從圖8可以看出經(jīng)度誤差不超過(guò)15 m,緯度誤差不超過(guò)20 m,高度誤差不超過(guò)5 m,東向、北向和天向速度誤差不超過(guò)0.05 m/s。

在圖5～圖8中,子濾波器故障時(shí),信息融合技術(shù)對(duì)子濾波器的誤差進(jìn)行估計(jì),補(bǔ)償子濾波器的位置和速度誤差。自主定位系統(tǒng)誤差保持在一個(gè)合理的范圍內(nèi),確保了自主定位系統(tǒng)正常工作。

分析試驗(yàn)結(jié)果可知,LDV比OD故障引起的誤差小,對(duì)組合導(dǎo)航產(chǎn)生的影響也小。同時(shí),組合系統(tǒng)定位定向精度隨時(shí)間趨于穩(wěn)定,但誤差一直存在。分析設(shè)計(jì)的組合定位系統(tǒng),發(fā)現(xiàn)建立的融合算法誤差方程中存在缺陷,導(dǎo)致實(shí)際補(bǔ)償與理論計(jì)算存在偏差,融合算法需要進(jìn)一步改進(jìn)。

圖8 主濾波器位置誤差、速度誤差

5 結(jié)束語(yǔ)

文中主要闡述了LDV/OD測(cè)速原理,分析其誤差來(lái)源。根據(jù)LDV和OD的誤差源,建立SINS、LDV和OD的誤差方程,介紹了聯(lián)邦卡爾曼濾波器基本原理,設(shè)計(jì)了SINS/LDV/OD自主定位系統(tǒng)的聯(lián)邦卡爾曼濾波器,并給出其狀態(tài)方程和量測(cè)方程以及系統(tǒng)容錯(cuò)型設(shè)計(jì)。跑車試驗(yàn)結(jié)果表明經(jīng)度誤差不超過(guò)20 m,緯度誤差不超過(guò)20 m,高度誤差不超過(guò)10 m,誤差合理,設(shè)計(jì)有效。