基于載體約束的SINS/LDV/OD自主定位系統(tǒng)設(shè)計*

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

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

0 引言

SINS以數(shù)字遞推的方式實現(xiàn)載體定位,慣性測量組件的漂移誤差在積分環(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],二是借助外部傳感器的測量信息[3]。

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

1 速度測量原理

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

1.1 LDV測速原理

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

(1)

(2)

1.2 OD測速原理

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

航位推算模型:

(3)

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

(4)

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

OD航位推算誤差:

(5)

實際航位推算方程:

(6)

DR誤差方程:

(7)

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

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

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

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

(8)

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

(9)

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

(10)

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

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

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

(11)

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

(12)

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

(13)

其中,時間更新和量測更新之間相互獨立。

1)時間更新

由式(12)得到:

(14)

則有:

(15)

由于公共噪聲Q的存在,Pji≠0始終成立,即時間更新引入的各子濾波器狀態(tài)相關(guān)。“方差上界”法可以消除引起的子濾波器狀態(tài)相關(guān)問題。

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

(16)

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

2)量測更新

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

(17)

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

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

(18)

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

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

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

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

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

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

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

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

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

3.2 子濾波器設(shè)計

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

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

(19)

式中,狀態(tài)向量XS有15個狀態(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]。以速度差值作為該子濾波器的觀測量,建立子系統(tǒng)量測方程。

狀態(tài)方程:

(24)

量測方程:

(25)

(26)

2)SINS/OD子濾波器

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

狀態(tài)方程:

(27)

量測方程:

(28)

(29)

3.3 主濾波器設(shè)計

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

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

(30)

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

(31)

2)信息分配

(32)

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

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

(33)

3.4 系統(tǒng)容錯性設(shè)計

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

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

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

(34)

(35)

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

(36)

(37)

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

(38)

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

(39)

(40)

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

(41)

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

(42)

(43)

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

(44)

4 試驗檢驗

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

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

圖3 跑車試驗軌跡圖

圖4表示實際跑車中位置誤差。可以看出,試驗中經(jīng)度誤差不超過20 m,緯度誤差不超過20 m,高度誤差不超過10 m。

圖4 跑車試驗位置誤差

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

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

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

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

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

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

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

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

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

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

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

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

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

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

5 結(jié)束語

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