基于SCKF和姿態估計的SINS/GPS在線對準方法

張桓瑞，劉向龍，邵洪峰

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

0 引言

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

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

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

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

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

1 姿態估計算法

1.1 坐標系定義

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

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

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

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

1.2 初始姿態估計方法

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

(1)

(2)

(3)

(4)

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

1.3 基于羅德里格參數的誤差模型

(5)

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

(6)

(7)

考慮誤差可得：

(8)

(9)

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

將初始姿態轉換矩陣用羅德里格參數l表示可得：

(10)

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

l+l×δVib(tk)+wtk

(11)

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

(12)

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

(13)

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

(14)

(15)

(16)

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

(17)

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

(18)

(19)

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

(20)

(21)

(22)

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

一般離散非線性系統為

(23)

式中：xk為k時刻估計狀態量；f為k-1時刻到k時刻的狀態轉移函數；Γk為噪聲驅動函數；wk-1為k-1時刻的隨機噪聲；zk為k時刻量測量；h為量測函數；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. 用狀態方程傳播容積點：

(28)

c. 估計k時刻的狀態一步預測值：

(29)

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

(30)

(31)

3) 量測更新。

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

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

(32)

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

(33)

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

(34)

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

(35)

(36)

e. 計算狀態和量測互相關協方差的平方根：

(37)

(38)

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

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

(39)

g. 計算k時刻的狀態估計值：

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

(40)

h. 計算k時刻的誤差協方差平方根：

(41)

圖1 方法流程圖

3 半實物仿真結果

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

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

仿真過程中的參數設置如下：

(42)

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

圖2 制導武器仿真軌跡姿態變化

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

圖4 制導武器初始姿態估計結果

圖5 制導武器姿態誤差

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

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

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

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

圖7 車輛仿真初始姿態估計結果

圖8 車輛仿真姿態誤差

4 結束語

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