張桓瑞，劉向龍，邵洪峰

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

0 引言

隨著微機電系統(tǒng)(MEMS)傳感技術不斷發(fā)展，基于MEMS技術的慣性測量單元(IMU) 已在小型無人機及戰(zhàn)術武器等領域得到了廣泛應用。目前，MEMS慣性器件精度不高，一般MEMS陀螺精度大多在每小時幾十度到100°，因此，微慣性測量單元(MIMU)進行空中對準時常處于大失準角狀態(tài)下。目前對于大失準角下的空中對準研究主要有：

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

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

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

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

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

1.1 坐標系定義

載體坐標系(b系)：坐標系原點在載體的質心，y軸為載體縱軸，x軸指向載體右側與y軸垂直，z軸和x,y軸組成右手坐標系。

導航坐標系(n系)：坐標系原點在載體的質心，x軸指向地理北向，y軸指向地理東向，z軸指向天向。

初始導航坐標系(in系)：慣性坐標系與初始時刻的導航坐標系重合。

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

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

根據(jù)矩陣鏈式乘法法則，t時刻載體到導航坐標系的姿態(tài)轉換矩陣可分解成3個部分：

(1)

(2)

(3)

(4)

式中：L0，λ0分別為初始位置緯度、經度；Lk，λk分別為當前時刻緯度、經度;δλ為t時間內經度變化值,且δλ=λk-λ0+ωiet,ωie為地球自轉角速度。以上數(shù)據(jù)均可從GPS獲取。

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

(5)

式中Vin，Vib分別為in、ib系下的比力積分所得值，且

(6)

(7)

考慮誤差可得：

(8)

(9)

式中δVib(tk)為誤差項。

將初始姿態(tài)轉換矩陣用羅德里格參數(shù)l表示可得：

(10)

將式(10)代入式(9)整理可得：

l+l×δVib(tk)+wtk

(11)

Dtk=Stk×l+l×δVib(tk)+wtk

(12)

式(12)即為關于初始羅德里格參數(shù)的量測方程，此方程為非線性方程。估計出最優(yōu)的羅德里格參數(shù)后，可通過式(10)求得初始姿態(tài)轉換矩陣，從而求出當前時刻的姿態(tài)轉換矩陣。

(13)

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

(14)

(15)

(16)

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

(17)

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

(18)

(19)

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

(20)

(21)

(22)

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

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

(23)

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

(24)

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

(25)

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

1) 初始化。

Sk-1=chol(Pk-1)

(26)

2) 時間更新。

a. 構造容積點：

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

(27)

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

(28)

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

(29)

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

(30)

(31)

3) 量測更新。

a. 使用誤差協(xié)方差平方根的一步預測構造容積點：

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

(32)

b. 使用量測方程傳播容積點：

(33)

c. 計算量測量的一步預測值：

(34)

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

(35)

(36)

e. 計算狀態(tài)和量測互相關協(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 半實物仿真結果

為驗證此方法對低成本戰(zhàn)術武器和民用車輛導航的有效性，本文對以上兩種情況分別進行了半實物仿真。采集了MTI-3 MEMS慣導的隨機噪聲數(shù)據(jù)，并將其加入預設軌跡數(shù)據(jù)中。MTI-3 MEMS慣導參數(shù)如表1所示。

表1 MTI-3 MEMS慣導參數(shù)(g=9.8 m/s2)

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

(42)

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

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

圖3 制導武器仿真軌跡速度變化

圖4 制導武器初始姿態(tài)估計結果

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

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

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

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

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

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

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

4 結束語

本文提出了基于平方根容積卡爾曼濾波器和姿態(tài)估計法的SINS/GPS在線對準方法，并針對短距離制導武器及低成本車載導航系統(tǒng)進行了半實物仿真。該方法可在25 s左右完成在線對準，其中短距離制導武器仿真結果航向角及俯仰角誤差在0.1°內，橫滾角誤差在0.3°內；低成本車載導航系統(tǒng)仿真結果航向角誤差在0.2°內，俯仰角及橫滾角誤差在1°內，可以滿足制導武器及低成本民用車輛的對準需求。