基于SCKF和姿態(tài)估計的SINS/GPS在線對準(zhǔn)方法

張桓瑞，劉向龍，邵洪峰

(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ù)的慣性測量單元(IMU) 已在小型無人機(jī)及戰(zhàn)術(shù)武器等領(lǐng)域得到了廣泛應(yīng)用。目前，MEMS慣性器件精度不高，一般MEMS陀螺精度大多在每小時幾十度到100°，因此，微慣性測量單元(MIMU)進(jìn)行空中對準(zhǔn)時常處于大失準(zhǔn)角狀態(tài)下。目前對于大失準(zhǔn)角下的空中對準(zhǔn)研究主要有：

1) 對大失準(zhǔn)角下的姿態(tài)誤差模型進(jìn)行研究[1-6]。

2) 基于最優(yōu)估計的初始對準(zhǔn)法(OBA)將初始對準(zhǔn)轉(zhuǎn)換為通過使用量測信息對初始姿態(tài)的最優(yōu)估計問題。

2013年，吳等[7]提出了一種基于最優(yōu)估計的空中粗對準(zhǔn)法，但此方法對使用慣性器件精度要求較高。2017年，CHANG等[8]提出了一種基于間接卡爾曼濾波的OBA算法，先使用姿態(tài)估計的方法將姿態(tài)誤差縮小到小角度，然后使用線性卡爾曼濾波進(jìn)行精對準(zhǔn)，此方法可用于MEMS慣導(dǎo)系統(tǒng),但對準(zhǔn)時間較長。CUI等[9]基于OBA法構(gòu)建了非線性的量測模型來估計代表初始姿態(tài)轉(zhuǎn)換矩陣的羅德里格參數(shù)，并通過全球定位系統(tǒng)(GPS)提供的速度位置信息實(shí)時計算當(dāng)前時刻的載體姿態(tài)。此方法使用二階擴(kuò)展卡爾曼濾波(2nd-EKF)將非線性量測模型線性化，需要計算二階雅克比矩陣,計算量較大。

本文采用CUI等提出的濾波模型，引進(jìn)了平方根容積卡爾曼濾波來處理非線性量測模型，得到了更快的收斂速度，并保證濾波過程的穩(wěn)定。

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

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)系與初始時刻的導(dǎo)航坐標(biāo)系重合。

初始載體坐標(biāo)系(ib系)：慣性坐標(biāo)系與初始時刻的載體坐標(biāo)系重合。

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

根據(jù)矩陣鏈?zhǔn)匠朔ǚ▌t，t時刻載體到導(dǎo)航坐標(biāo)系的姿態(tài)轉(zhuǎn)換矩陣可分解成3個部分：

(1)

(2)

(3)

(4)

式中：L0，λ0分別為初始位置緯度、經(jīng)度；Lk，λk分別為當(dāng)前時刻緯度、經(jīng)度;δλ為t時間內(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ù)的量測方程，此方程為非線性方程。估計出最優(yōu)的羅德里格參數(shù)后，可通過式(10)求得初始姿態(tài)轉(zhuǎn)換矩陣，從而求出當(dāng)前時刻的姿態(tài)轉(zhuǎn)換矩陣。

(13)

假定陀螺加速度計的誤差模型分別如下：

(14)

(15)

(16)

由式(6)、(8)可得：

(17)

將式(17)左、右兩邊對t求導(dǎo)，代入式(13)、(15)可得：

(18)

(19)

慣組陀螺加速度計的常值誤差微分為0有：

(20)

(21)

(22)

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

一般離散非線性系統(tǒng)為

(23)

式中：xk為k時刻估計狀態(tài)量；f為k-1時刻到k時刻的狀態(tài)轉(zhuǎn)移函數(shù)；Γk為噪聲驅(qū)動函數(shù)；wk-1為k-1時刻的隨機(jī)噪聲；zk為k時刻量測量；h為量測函數(shù)；vk為量測噪聲。

(24)

式(24)積分可使用Spherical-Radial容積準(zhǔn)則來進(jìn)行計算，對于一般高斯分布有[10]：

(25)

平方根容積卡爾曼濾波算法過程如下：

1) 初始化。

Sk-1=chol(Pk-1)

(26)

2) 時間更新。

a. 構(gòu)造容積點(diǎn)：

Xi,k-1|k-1=Sk-1ξi+xk-1

(27)

b. 用狀態(tài)方程傳播容積點(diǎn)：

(28)

c. 估計k時刻的狀態(tài)一步預(yù)測值：

(29)

d. 計算誤差協(xié)方差平方根的一步預(yù)測：

(30)

(31)

3) 量測更新。

a. 使用誤差協(xié)方差平方根的一步預(yù)測構(gòu)造容積點(diǎn)：

Xi,k|k-1=Sk|k-1ξi+xk|k-1

(32)

b. 使用量測方程傳播容積點(diǎn)：

(33)

c. 計算量測量的一步預(yù)測值：

(34)

d. 計算量測自相關(guān)協(xié)方差的平方根：

(35)

(36)

e. 計算狀態(tài)和量測互相關(guān)協(xié)方差的平方根：

(37)

(38)

f. 計算k時刻的濾波增益：

Wk=Pxz,k|k-1/[Szz,k|k-1·(Szz,k|k-1)T]

(39)

g. 計算k時刻的狀態(tài)估計值：

xk=xk|k-1+Wk(zk-zk|k-1)

(40)

h. 計算k時刻的誤差協(xié)方差平方根：

(41)

圖1 方法流程圖

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

為驗(yàn)證此方法對低成本戰(zhàn)術(shù)武器和民用車輛導(dǎo)航的有效性，本文對以上兩種情況分別進(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)

仿真過程中的參數(shù)設(shè)置如下：

(42)

通常，短距離制導(dǎo)武器的飛行時間在60 s內(nèi)，飛行距離在20 km內(nèi)。根據(jù)短距離制導(dǎo)武器軌跡特性，半實(shí)物仿真使用的軌跡姿態(tài)與速度變化如圖2、3所示。初始姿態(tài)估計結(jié)果如圖4、5所示。

圖2 制導(dǎo)武器仿真軌跡姿態(tài)變化

圖3 制導(dǎo)武器仿真軌跡速度變化

圖4 制導(dǎo)武器初始姿態(tài)估計結(jié)果

圖5 制導(dǎo)武器姿態(tài)誤差

由圖4可看出，對3個初始姿態(tài)角的估計均在10 s內(nèi)完成收斂。其中航向角估計誤差在0.1°內(nèi)；俯仰角估計誤差在0.2°內(nèi)；橫滾角誤差在0.5°內(nèi)。由圖5可看出，本文方法可在25 s內(nèi)完成姿態(tài)角的估計航向角和俯仰角的誤差均在0.1°內(nèi)，橫滾角誤差在0.3°內(nèi)。

對于低成本民用車輛導(dǎo)航，設(shè)置了如下半實(shí)物仿真軌跡進(jìn)行驗(yàn)證。圖6為車輛東向、北向速度及航向角變化示意圖。其中15～25 s為勻加速直線運(yùn)動，故東、北向速度發(fā)生變化但航向角未發(fā)生改變。

圖6 車輛仿真速度航向變化

圖7為車輛仿真初始姿態(tài)估計結(jié)果。由圖可知，對3個初始姿態(tài)角的估計均在15 s內(nèi)完成收斂。其中航向角估計誤差在1°內(nèi)；俯仰角估計誤差在0.5°內(nèi)；橫滾角誤差在0.5°內(nèi)。圖8 車輛仿真姿態(tài)誤差。由圖可看出，在30 s內(nèi)，航向角誤差在0.2°內(nèi)，俯仰角和橫滾角誤差在1°內(nèi)，可滿足低成本民用車輛的對準(zhǔn)需求。

圖7 車輛仿真初始姿態(tài)估計結(jié)果

圖8 車輛仿真姿態(tài)誤差

4 結(jié)束語

本文提出了基于平方根容積卡爾曼濾波器和姿態(tài)估計法的SINS/GPS在線對準(zhǔn)方法，并針對短距離制導(dǎo)武器及低成本車載導(dǎo)航系統(tǒng)進(jìn)行了半實(shí)物仿真。該方法可在25 s左右完成在線對準(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)武器及低成本民用車輛的對準(zhǔn)需求。