基于準北東地系地面靜態對準算法

王獻忠 張 肖

1.上海航天技術研究院，上海 201109；2.上海航天控制技術研究所，上海 201109；3.空間智能控制技術重點實驗室，上海 201109

0 引言

地面運載和武器系統發射前需要進行初始對準，一般基于捷聯慣組測量的本體系下地球自轉角速率和重力加速度確定飛行器初始姿態。

國內外學者對捷聯慣導初始對準進行了廣泛的研究，文獻[1]通過將無跡卡爾曼濾波算法(UKF)和魯棒環節引入到粒子濾波(PF)的重要性密度函數中，得到了RUPF 算法，提高了算法的魯棒性。文獻[2]采用改進的UKF算法進行捷聯慣導初始對準研究，減少了UT變換中Sigma點的計算數量，提高了運算效率。文獻[3]基于四元數自適應卡爾曼濾波進行捷聯慣導初始對準研究，直接以初始對準時刻的四元數作為估計狀態，建立卡爾曼濾波模型，同時通過對觀測模型中的觀測矩陣進行改造，有效加快了算法的收斂速度。文獻[4]利用以慣性系為參考基準的解析對準法和卡爾曼濾波精對準方法，對高精度全自主對準技術和其在運載火箭上的應用展開了詳細論述。綜上述文獻，對于靜態對準算法的研究，目前的研究重點在于提高對準的精度、算法的魯棒性及收斂速度。文獻[5]針對微慣組測量噪聲大，不能辨別地球自轉角速度，造成捷聯慣性導航系統不能實現自對準的問題，采用GPS輔助微慣組進行初始對準。針對大失準角初始對準問題，文獻[6]在游動方位角坐標系下建立了方位大失準角條件下的SINS初始對準誤差模型；文獻[7]基于二階非線性量測方程的二階泰勒級數展開的濾波算法進行捷聯慣導大失準角初始對準研究。受啟發于上述文獻，本文針對利用本身測量精度并不是很高的陀螺如何實現高精度初始對準，及適應任意姿態和無初值條件下的初始對準和參數估計問題進行研究。

本文基于粗對準姿態建立準北東地系，通過準北東地系的建立使角度誤差小且三軸近似解耦；估算準北東地系重力加速度和地球自轉角速率，通過不斷迭代修正，提高了角速度的測量精度；基于高精度的角速度測量信息，提出了一種易于工程實現的解析式精對準算法，在保證穩定收斂的前提下精度優于0.05°。

1 地面粗對準

1.1 計算重力加速度

基于對準點緯度和高度計算重力加速度：

(1)

其中：

g0=9.7803267714m/s2為地面標準重力加速度；k1=0.00193185138639和k2=0.00669437999013為重力加速度修正系數；B為地理緯度；Re為地球半徑；h為對準點地面高度。

1.2 計算本體系重力加速度和地球自轉角速率

基于加速度計輸出的比力fs計算本體系比力fb：

(2)

其中：Αsb,a為加速度計在本體系安裝矩陣。

基于陀螺輸出的角速率ωs計算本體系地球自轉角速率ωb：

(3)

其中：Αsb,g為陀螺在本體系安裝矩陣。

1.3 計算本體相對北東地系粗對準姿態

北東地系如圖1所示，在靜態對準點基于重力加速度方向建立北東地(NED)坐標系，NED系加速度計比力如下：

(4)

圖1 北東地坐標系(NED)示意圖

本體相對NED系3-2-1轉序姿態轉換陣Abn[7]：

(5)

其中：ψ為偏航姿態；θ為俯仰姿態；γ為滾動姿態。

(6)

求得：

(7)

(8)

(9)

圖2 北東地系地球自轉角速度

(10)

(11)

(12)

否則：

(13)

2 估算準北東地系重力加速度和地球自轉角速率

2.1 本體系重力加速度和地球自轉角速率轉換到準北東地系

基于粗對準姿態建立準北東地系，將本體系比力fb轉換到準北東地系：

(14)

其中：Αbn′為準北東地系到本體系轉換矩陣。

將本體系地球自轉角速率ωb轉換到準北東地系：

(15)

其中：Αbn′為準北東地系到本體系轉換矩陣。

2.2 準北東地系比力fn′和地球自轉角速率ωn′濾波

準北東地系比力fn′和地球自轉角速率ωn′濾波：

(16)

(17)

3 基于準北東地系精對準

3.1 估計水平失準角

基于準北東地系比力北向和東向分量估計水平失準角：

(18)

(19)

對水平失準角進行限幅處理，水平失準角大時比例作用強一些，加快收斂；水平失準角小時比例作用弱一些，避免振蕩。

比例系數ka進行分段處理：

(20)

其中：ka1為水平失準角未收斂時比例系數，ka2為水平失準角收斂時比例系數，f0為比力水平方向分量判別閾值。

3.2 估計方位失準角

基于準北東地坐標系地球自轉角速率東向分量估計方位失準角：

(21)

對方位失準角進行限幅處理，方位失準角大時比例作用強一些，加快收斂；方位失準角小時比例作用弱一些，避免振蕩。

比例系數kω進行分段處理：

(22)

其中：kω1為方位失準角未收斂時比例系數，kω2為方位失準角收斂時比例系數，ω0為地球自轉角速率東向分量判別閾值。

3.3 估計精對準姿態四元數

基于水平和方位姿態失準角估計精對準姿態誤差四元數：

(23)

求得精對準姿態四元數：

qbn,k=δqk?qbn,k-1

(24)

qbn,0=qbn′

(25)

其中：qbn,k為第k步推算的本體系相對NED系姿態四元數，qbn,k-1為第k-1步推算的本體系相對NED系姿態四元數，qbn′為基于粗對準姿態確定的本體系相對NED系姿態四元數。

3.4 計算精對準姿態

令：

(26)

基于精對準姿態四元數qbn，求得Abnq[8]：

(27)

結合姿態按3-2-1轉序求得Abn，按3-2-1轉序求精對準姿態：

如果|a13|≤0.99999：

(28)

否則：

(29)

4 算法精度及收斂性仿真驗證

4.1 算法精度仿真驗證

地球表面靜態對準點地固系經度121.2538°，緯度31.101985°，高度0m；3-2-1轉序標稱姿態ψ=41.5°，θ=-87.99°、φ=41.0°。

陀螺角速率常值漂移0.01(°)/h，隨機漂移0.001(°)/h；加速度計比力常值漂移1×10-5g，加速度計加速度隨機漂移1×10-6g。在粗對準基礎上進行精對準，高精度慣組三軸姿態對準誤差如圖3所示，對準精度約0.04°。

圖3 高精度慣組三軸姿態對準誤差曲線

陀螺角速率常值漂移0.1(°)/h，隨機漂移0.01(°)/h；加速度計比力常值漂移1×10-4g，加速度計加速度隨機漂移1×10-5g。在粗對準基礎上進行精對準，一般慣組三軸姿態對準誤差如圖4所示，對準精度約0.4°。

圖4 一般慣組三軸姿態對準誤差曲線

4.2 算法收斂性仿真驗證

高精度慣組陀螺角速率常值漂移0.01(°)/h，隨機漂移0.001(°)/h；加速度計比力常值漂移1×10-5g，加速度計加速度隨機漂移1×10-6g。不估計粗對準姿態，設初始姿態ψ=0.0°；θ=0.0°；φ=0.0°，直接進行精對準，高精度慣組三軸姿態對準誤差收斂后如圖5所示，對準精度約0.04°。

圖5 高精度慣組直接精對準三軸姿態對準誤差曲線

5 結束語

基于捷聯慣組確定飛行器在地面相對北東地系姿態，通過準北東地系的建立使角度誤差小且三軸近似解耦；通過不斷迭代修正，提高了角速度的測量精度；進而基于高精度的角速度測量信息，提出了一種易于工程實現的解析式精對準算法。仿真驗證了靜態對準算法的有效性和收斂性。本文基于捷聯慣組確定飛行器在地面初始姿態算法簡單，易于工程實現。