基于準(zhǔn)北東地系地面靜態(tài)對(duì)準(zhǔn)算法

王獻(xiàn)忠 張 肖

1.上海航天技術(shù)研究院，上海 201109；2.上海航天控制技術(shù)研究所，上海 201109；3.空間智能控制技術(shù)重點(diǎn)實(shí)驗(yàn)室，上海 201109

0 引言

地面運(yùn)載和武器系統(tǒng)發(fā)射前需要進(jìn)行初始對(duì)準(zhǔn)，一般基于捷聯(lián)慣組測(cè)量的本體系下地球自轉(zhuǎn)角速率和重力加速度確定飛行器初始姿態(tài)。

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

本文基于粗對(duì)準(zhǔn)姿態(tài)建立準(zhǔn)北東地系，通過(guò)準(zhǔn)北東地系的建立使角度誤差小且三軸近似解耦；估算準(zhǔn)北東地系重力加速度和地球自轉(zhuǎn)角速率，通過(guò)不斷迭代修正，提高了角速度的測(cè)量精度；基于高精度的角速度測(cè)量信息，提出了一種易于工程實(shí)現(xiàn)的解析式精對(duì)準(zhǔn)算法，在保證穩(wěn)定收斂的前提下精度優(yōu)于0.05°。

1 地面粗對(duì)準(zhǔn)

1.1 計(jì)算重力加速度

基于對(duì)準(zhǔn)點(diǎn)緯度和高度計(jì)算重力加速度：

(1)

其中：

g0=9.7803267714m/s2為地面標(biāo)準(zhǔn)重力加速度；k1=0.00193185138639和k2=0.00669437999013為重力加速度修正系數(shù)；B為地理緯度；Re為地球半徑；h為對(duì)準(zhǔn)點(diǎn)地面高度。

1.2 計(jì)算本體系重力加速度和地球自轉(zhuǎn)角速率

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

(2)

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

基于陀螺輸出的角速率ωs計(jì)算本體系地球自轉(zhuǎn)角速率ωb：

(3)

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

1.3 計(jì)算本體相對(duì)北東地系粗對(duì)準(zhǔn)姿態(tài)

北東地系如圖1所示，在靜態(tài)對(duì)準(zhǔn)點(diǎn)基于重力加速度方向建立北東地(NED)坐標(biāo)系，NED系加速度計(jì)比力如下：

(4)

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

本體相對(duì)NED系3-2-1轉(zhuǎn)序姿態(tài)轉(zhuǎn)換陣Abn[7]：

(5)

其中：ψ為偏航姿態(tài)；θ為俯仰姿態(tài)；γ為滾動(dòng)姿態(tài)。

(6)

求得：

(7)

(8)

(9)

圖2 北東地系地球自轉(zhuǎn)角速度

(10)

(11)

(12)

否則：

(13)

2 估算準(zhǔn)北東地系重力加速度和地球自轉(zhuǎn)角速率

2.1 本體系重力加速度和地球自轉(zhuǎn)角速率轉(zhuǎn)換到準(zhǔn)北東地系

基于粗對(duì)準(zhǔn)姿態(tài)建立準(zhǔn)北東地系，將本體系比力fb轉(zhuǎn)換到準(zhǔn)北東地系：

(14)

其中：Αbn′為準(zhǔn)北東地系到本體系轉(zhuǎn)換矩陣。

將本體系地球自轉(zhuǎn)角速率ωb轉(zhuǎn)換到準(zhǔn)北東地系：

(15)

其中：Αbn′為準(zhǔn)北東地系到本體系轉(zhuǎn)換矩陣。

2.2 準(zhǔn)北東地系比力fn′和地球自轉(zhuǎn)角速率ωn′濾波

準(zhǔn)北東地系比力fn′和地球自轉(zhuǎn)角速率ωn′濾波：

(16)

(17)

3 基于準(zhǔn)北東地系精對(duì)準(zhǔn)

3.1 估計(jì)水平失準(zhǔn)角

基于準(zhǔn)北東地系比力北向和東向分量估計(jì)水平失準(zhǔn)角：

(18)

(19)

對(duì)水平失準(zhǔn)角進(jìn)行限幅處理，水平失準(zhǔn)角大時(shí)比例作用強(qiáng)一些，加快收斂；水平失準(zhǔn)角小時(shí)比例作用弱一些，避免振蕩。

比例系數(shù)ka進(jìn)行分段處理：

(20)

其中：ka1為水平失準(zhǔn)角未收斂時(shí)比例系數(shù)，ka2為水平失準(zhǔn)角收斂時(shí)比例系數(shù)，f0為比力水平方向分量判別閾值。

3.2 估計(jì)方位失準(zhǔn)角

基于準(zhǔn)北東地坐標(biāo)系地球自轉(zhuǎn)角速率東向分量估計(jì)方位失準(zhǔn)角：

(21)

對(duì)方位失準(zhǔn)角進(jìn)行限幅處理，方位失準(zhǔn)角大時(shí)比例作用強(qiáng)一些，加快收斂；方位失準(zhǔn)角小時(shí)比例作用弱一些，避免振蕩。

比例系數(shù)kω進(jìn)行分段處理：

(22)

其中：kω1為方位失準(zhǔn)角未收斂時(shí)比例系數(shù)，kω2為方位失準(zhǔn)角收斂時(shí)比例系數(shù)，ω0為地球自轉(zhuǎn)角速率東向分量判別閾值。

3.3 估計(jì)精對(duì)準(zhǔn)姿態(tài)四元數(shù)

基于水平和方位姿態(tài)失準(zhǔn)角估計(jì)精對(duì)準(zhǔn)姿態(tài)誤差四元數(shù)：

(23)

求得精對(duì)準(zhǔn)姿態(tài)四元數(shù)：

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

(24)

qbn,0=qbn′

(25)

其中：qbn,k為第k步推算的本體系相對(duì)NED系姿態(tài)四元數(shù)，qbn,k-1為第k-1步推算的本體系相對(duì)NED系姿態(tài)四元數(shù)，qbn′為基于粗對(duì)準(zhǔn)姿態(tài)確定的本體系相對(duì)NED系姿態(tài)四元數(shù)。

3.4 計(jì)算精對(duì)準(zhǔn)姿態(tài)

令：

(26)

基于精對(duì)準(zhǔn)姿態(tài)四元數(shù)qbn，求得Abnq[8]：

(27)

結(jié)合姿態(tài)按3-2-1轉(zhuǎn)序求得Abn，按3-2-1轉(zhuǎn)序求精對(duì)準(zhǔn)姿態(tài)：

如果|a13|≤0.99999：

(28)

否則：

(29)

4 算法精度及收斂性仿真驗(yàn)證

4.1 算法精度仿真驗(yàn)證

地球表面靜態(tài)對(duì)準(zhǔn)點(diǎn)地固系經(jīng)度121.2538°，緯度31.101985°，高度0m；3-2-1轉(zhuǎn)序標(biāo)稱姿態(tài)ψ=41.5°，θ=-87.99°、φ=41.0°。

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

圖3 高精度慣組三軸姿態(tài)對(duì)準(zhǔn)誤差曲線

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

圖4 一般慣組三軸姿態(tài)對(duì)準(zhǔn)誤差曲線

4.2 算法收斂性仿真驗(yàn)證

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

圖5 高精度慣組直接精對(duì)準(zhǔn)三軸姿態(tài)對(duì)準(zhǔn)誤差曲線

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

基于捷聯(lián)慣組確定飛行器在地面相對(duì)北東地系姿態(tài)，通過(guò)準(zhǔn)北東地系的建立使角度誤差小且三軸近似解耦；通過(guò)不斷迭代修正，提高了角速度的測(cè)量精度；進(jìn)而基于高精度的角速度測(cè)量信息，提出了一種易于工程實(shí)現(xiàn)的解析式精對(duì)準(zhǔn)算法。仿真驗(yàn)證了靜態(tài)對(duì)準(zhǔn)算法的有效性和收斂性。本文基于捷聯(lián)慣組確定飛行器在地面初始姿態(tài)算法簡(jiǎn)單，易于工程實(shí)現(xiàn)。