張桓瑞,劉向龍,邵洪峰
(1. 北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院, 北京 100191;2.交通運(yùn)輸部科學(xué)研究院 城市公共交通智能化交通運(yùn)輸行業(yè)重點(diǎn)實(shí)驗(yàn)室,北京 100029;3.北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院, 北京 100191)
隨著微機(jī)電系統(tǒng)(MEMS)傳感技術(shù)不斷發(fā)展,基于MEMS技術(shù)的慣性測(cè)量單元(IMU) 已在小型無(wú)人機(jī)及戰(zhàn)術(shù)武器等領(lǐng)域得到了廣泛應(yīng)用。目前,MEMS慣性器件精度不高,一般MEMS陀螺精度大多在每小時(shí)幾十度到100°,因此,微慣性測(cè)量單元(MIMU)進(jìn)行空中對(duì)準(zhǔn)時(shí)常處于大失準(zhǔn)角狀態(tài)下。目前對(duì)于大失準(zhǔn)角下的空中對(duì)準(zhǔn)研究主要有:
1) 對(duì)大失準(zhǔn)角下的姿態(tài)誤差模型進(jìn)行研究[1-6]。
2) 基于最優(yōu)估計(jì)的初始對(duì)準(zhǔn)法(OBA)將初始對(duì)準(zhǔn)轉(zhuǎn)換為通過(guò)使用量測(cè)信息對(duì)初始姿態(tài)的最優(yōu)估計(jì)問題。
2013年,吳等[7]提出了一種基于最優(yōu)估計(jì)的空中粗對(duì)準(zhǔn)法,但此方法對(duì)使用慣性器件精度要求較高。2017年,CHANG等[8]提出了一種基于間接卡爾曼濾波的OBA算法,先使用姿態(tài)估計(jì)的方法將姿態(tài)誤差縮小到小角度,然后使用線性卡爾曼濾波進(jìn)行精對(duì)準(zhǔn),此方法可用于MEMS慣導(dǎo)系統(tǒng),但對(duì)準(zhǔn)時(shí)間較長(zhǎng)。CUI等[9]基于OBA法構(gòu)建了非線性的量測(cè)模型來(lái)估計(jì)代表初始姿態(tài)轉(zhuǎn)換矩陣的羅德里格參數(shù),并通過(guò)全球定位系統(tǒng)(GPS)提供的速度位置信息實(shí)時(shí)計(jì)算當(dāng)前時(shí)刻的載體姿態(tài)。此方法使用二階擴(kuò)展卡爾曼濾波(2nd-EKF)將非線性量測(cè)模型線性化,需要計(jì)算二階雅克比矩陣,計(jì)算量較大。
本文采用CUI等提出的濾波模型,引進(jìn)了平方根容積卡爾曼濾波來(lái)處理非線性量測(cè)模型,得到了更快的收斂速度,并保證濾波過(guò)程的穩(wěn)定。
載體坐標(biāo)系(b系):坐標(biāo)系原點(diǎn)在載體的質(zhì)心,y軸為載體縱軸,x軸指向載體右側(cè)與y軸垂直,z軸和x,y軸組成右手坐標(biāo)系。
導(dǎo)航坐標(biāo)系(n系):坐標(biāo)系原點(diǎn)在載體的質(zhì)心,x軸指向地理北向,y軸指向地理東向,z軸指向天向。
初始導(dǎo)航坐標(biāo)系(in系):慣性坐標(biāo)系與初始時(shí)刻的導(dǎo)航坐標(biāo)系重合。
初始載體坐標(biāo)系(ib系):慣性坐標(biāo)系與初始時(shí)刻的載體坐標(biāo)系重合。
根據(jù)矩陣鏈?zhǔn)匠朔ǚ▌t,t時(shí)刻載體到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣可分解成3個(gè)部分:
(1)
(2)
(3)
(4)
式中:L0,λ0分別為初始位置緯度、經(jīng)度;Lk,λk分別為當(dāng)前時(shí)刻緯度、經(jīng)度;δλ為t時(shí)間內(nèi)經(jīng)度變化值,且δλ=λk-λ0+ωiet,ωie為地球自轉(zhuǎn)角速度。以上數(shù)據(jù)均可從GPS獲取。
(5)
式中Vin,Vib分別為in、ib系下的比力積分所得值,且
(6)
(7)
考慮誤差可得:
(8)
(9)
式中δVib(tk)為誤差項(xiàng)。
將初始姿態(tài)轉(zhuǎn)換矩陣用羅德里格參數(shù)l表示可得:
(10)
將式(10)代入式(9)整理可得:
l+l×δVib(tk)+wtk
(11)
Dtk=Stk×l+l×δVib(tk)+wtk
(12)
式(12)即為關(guān)于初始羅德里格參數(shù)的量測(cè)方程,此方程為非線性方程。估計(jì)出最優(yōu)的羅德里格參數(shù)后,可通過(guò)式(10)求得初始姿態(tài)轉(zhuǎn)換矩陣,從而求出當(dāng)前時(shí)刻的姿態(tài)轉(zhuǎn)換矩陣。
(13)
假定陀螺加速度計(jì)的誤差模型分別如下:
(14)
(15)
(16)
由式(6)、(8)可得:
(17)
將式(17)左、右兩邊對(duì)t求導(dǎo),代入式(13)、(15)可得:
(18)
(19)
慣組陀螺加速度計(jì)的常值誤差微分為0有:
(20)
(21)
(22)
一般離散非線性系統(tǒng)為
(23)
式中:xk為k時(shí)刻估計(jì)狀態(tài)量;f為k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移函數(shù);Γk為噪聲驅(qū)動(dòng)函數(shù);wk-1為k-1時(shí)刻的隨機(jī)噪聲;zk為k時(shí)刻量測(cè)量;h為量測(cè)函數(shù);vk為量測(cè)噪聲。
(24)
式(24)積分可使用Spherical-Radial容積準(zhǔn)則來(lái)進(jìn)行計(jì)算,對(duì)于一般高斯分布有[10]:
(25)
平方根容積卡爾曼濾波算法過(guò)程如下:
1) 初始化。
Sk-1=chol(Pk-1)
(26)
2) 時(shí)間更新。
a. 構(gòu)造容積點(diǎn):
Xi,k-1|k-1=Sk-1ξi+xk-1
(27)
b. 用狀態(tài)方程傳播容積點(diǎn):
(28)
c. 估計(jì)k時(shí)刻的狀態(tài)一步預(yù)測(cè)值:
(29)
d. 計(jì)算誤差協(xié)方差平方根的一步預(yù)測(cè):
(30)
(31)
3) 量測(cè)更新。
a. 使用誤差協(xié)方差平方根的一步預(yù)測(cè)構(gòu)造容積點(diǎn):
Xi,k|k-1=Sk|k-1ξi+xk|k-1
(32)
b. 使用量測(cè)方程傳播容積點(diǎn):
(33)
c. 計(jì)算量測(cè)量的一步預(yù)測(cè)值:
(34)
d. 計(jì)算量測(cè)自相關(guān)協(xié)方差的平方根:
(35)
(36)
e. 計(jì)算狀態(tài)和量測(cè)互相關(guān)協(xié)方差的平方根:
(37)
(38)
f. 計(jì)算k時(shí)刻的濾波增益:
Wk=Pxz,k|k-1/[Szz,k|k-1·(Szz,k|k-1)T]
(39)
g. 計(jì)算k時(shí)刻的狀態(tài)估計(jì)值:
xk=xk|k-1+Wk(zk-zk|k-1)
(40)
h. 計(jì)算k時(shí)刻的誤差協(xié)方差平方根:
(41)
圖1 方法流程圖
為驗(yàn)證此方法對(duì)低成本戰(zhàn)術(shù)武器和民用車輛導(dǎo)航的有效性,本文對(duì)以上兩種情況分別進(jìn)行了半實(shí)物仿真。采集了MTI-3 MEMS慣導(dǎo)的隨機(jī)噪聲數(shù)據(jù),并將其加入預(yù)設(shè)軌跡數(shù)據(jù)中。MTI-3 MEMS慣導(dǎo)參數(shù)如表1所示。
表1 MTI-3 MEMS慣導(dǎo)參數(shù)(g=9.8 m/s2)
仿真過(guò)程中的參數(shù)設(shè)置如下:
(42)
通常,短距離制導(dǎo)武器的飛行時(shí)間在60 s內(nèi),飛行距離在20 km內(nèi)。根據(jù)短距離制導(dǎo)武器軌跡特性,半實(shí)物仿真使用的軌跡姿態(tài)與速度變化如圖2、3所示。初始姿態(tài)估計(jì)結(jié)果如圖4、5所示。
圖2 制導(dǎo)武器仿真軌跡姿態(tài)變化
圖3 制導(dǎo)武器仿真軌跡速度變化
圖4 制導(dǎo)武器初始姿態(tài)估計(jì)結(jié)果
圖5 制導(dǎo)武器姿態(tài)誤差
由圖4可看出,對(duì)3個(gè)初始姿態(tài)角的估計(jì)均在10 s內(nèi)完成收斂。其中航向角估計(jì)誤差在0.1°內(nèi);俯仰角估計(jì)誤差在0.2°內(nèi);橫滾角誤差在0.5°內(nèi)。由圖5可看出,本文方法可在25 s內(nèi)完成姿態(tài)角的估計(jì)航向角和俯仰角的誤差均在0.1°內(nèi),橫滾角誤差在0.3°內(nèi)。
對(duì)于低成本民用車輛導(dǎo)航,設(shè)置了如下半實(shí)物仿真軌跡進(jìn)行驗(yàn)證。圖6為車輛東向、北向速度及航向角變化示意圖。其中15~25 s為勻加速直線運(yùn)動(dòng),故東、北向速度發(fā)生變化但航向角未發(fā)生改變。
圖6 車輛仿真速度航向變化
圖7為車輛仿真初始姿態(tài)估計(jì)結(jié)果。由圖可知,對(duì)3個(gè)初始姿態(tài)角的估計(jì)均在15 s內(nèi)完成收斂。其中航向角估計(jì)誤差在1°內(nèi);俯仰角估計(jì)誤差在0.5°內(nèi);橫滾角誤差在0.5°內(nèi)。圖8 車輛仿真姿態(tài)誤差。由圖可看出,在30 s內(nèi),航向角誤差在0.2°內(nèi),俯仰角和橫滾角誤差在1°內(nèi),可滿足低成本民用車輛的對(duì)準(zhǔn)需求。
圖7 車輛仿真初始姿態(tài)估計(jì)結(jié)果
圖8 車輛仿真姿態(tài)誤差
本文提出了基于平方根容積卡爾曼濾波器和姿態(tài)估計(jì)法的SINS/GPS在線對(duì)準(zhǔn)方法,并針對(duì)短距離制導(dǎo)武器及低成本車載導(dǎo)航系統(tǒng)進(jìn)行了半實(shí)物仿真。該方法可在25 s左右完成在線對(duì)準(zhǔn),其中短距離制導(dǎo)武器仿真結(jié)果航向角及俯仰角誤差在0.1°內(nèi),橫滾角誤差在0.3°內(nèi);低成本車載導(dǎo)航系統(tǒng)仿真結(jié)果航向角誤差在0.2°內(nèi),俯仰角及橫滾角誤差在1°內(nèi),可以滿足制導(dǎo)武器及低成本民用車輛的對(duì)準(zhǔn)需求。