張桓瑞，劉向龍，邵洪峰

(1. 北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院, 北京 100191；2.交通運(yùn)輸部科學(xué)研究院 城市公共交通智能化交通運(yùn)輸行業(yè)重點(diǎn)實(shí)驗(yàn)室,北京 100029；3.北京航空航天大學(xué) 儀器科學(xué)與光電工程學(xué)院, 北京 100191)

0 引言

隨著微機(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)定。

1 姿態(tài)估計(jì)算法

1.1 坐標(biāo)系定義

載體坐標(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)系重合。

1.2 初始姿態(tài)估計(jì)方法

根據(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獲取。

1.3 基于羅德里格參數(shù)的誤差模型

(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)

2 平方根容積卡爾曼濾波器

一般離散非線性系統(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 方法流程圖

3 半實(shí)物仿真結(jié)果

為驗(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)誤差

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

本文提出了基于平方根容積卡爾曼濾波器和姿態(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)需求。