晃動(dòng)基座捷聯(lián)慣導(dǎo)系統(tǒng)初始對(duì)準(zhǔn)迭代方法

侯淑華，梁 康，紀(jì)文濤，趙 龍

（1.北京航天控制儀器研究所，北京100039；2.中國(guó)人民解放軍國(guó)防大學(xué)聯(lián)合勤務(wù)學(xué)院，北京100039）

0 引言

捷聯(lián)慣性導(dǎo)航系統(tǒng)在船舶、航空、航天、兵器等領(lǐng)域有廣泛的應(yīng)用［1?4］， 其中初始對(duì)準(zhǔn)是慣性導(dǎo)航系統(tǒng)中的關(guān)鍵技術(shù)之一［5?6］。 在進(jìn)行導(dǎo)航工作前，慣性導(dǎo)航系統(tǒng)必須首先進(jìn)行初始對(duì)準(zhǔn)。初始對(duì)準(zhǔn)的精度直接影響導(dǎo)航精度［7?8］， 初始對(duì)準(zhǔn)時(shí)間直接關(guān)系到系統(tǒng)快速反應(yīng)能力，這兩項(xiàng)結(jié)果是衡量初始對(duì)準(zhǔn)效果優(yōu)劣的主要指標(biāo)［9?10］。

捷聯(lián)慣性導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)的原理是：根據(jù)相應(yīng)的敏感器件數(shù)據(jù)，通過(guò)算法實(shí)現(xiàn)姿態(tài)與方位角的解算［11?13］。捷聯(lián)慣導(dǎo)初始對(duì)準(zhǔn)的常用算法是根據(jù)加速度計(jì)對(duì)重力加速度的測(cè)量值和陀螺對(duì)地球旋轉(zhuǎn)角速度的測(cè)量值計(jì)算出基座的姿態(tài)矩陣，但這只適用于基座處于靜止或者微幅晃動(dòng)狀態(tài)［6，14］。在實(shí)際應(yīng)用環(huán)境下，各種環(huán)境干擾使得載體在對(duì)準(zhǔn)時(shí)常常產(chǎn)生大幅度搖擺運(yùn)動(dòng)，例如船舶?？吭诖a頭，飛機(jī)在停機(jī)坪下有風(fēng)，車載發(fā)動(dòng)機(jī)啟動(dòng)等條件。傳統(tǒng)的靜基座對(duì)準(zhǔn)已不能滿足此類環(huán)境需要，需要提出一種基于晃動(dòng)基座下捷聯(lián)慣性導(dǎo)航系統(tǒng)高精度的對(duì)準(zhǔn)算法［2，15?16］。 近年來(lái)， 一些研究者提出了一種以慣性坐標(biāo)系作為參考基準(zhǔn)進(jìn)行晃動(dòng)干擾基座初始對(duì)準(zhǔn)的方法（搖擺對(duì)準(zhǔn)），雖然滿足了晃動(dòng)干擾基座初始對(duì)準(zhǔn)的要求，但不能滿足在快速機(jī)動(dòng)情況下的對(duì)準(zhǔn)需要［5?6］。 本文針對(duì)晃動(dòng)基座初始對(duì)準(zhǔn)設(shè)計(jì)了一種迭代對(duì)準(zhǔn)算法，在發(fā)動(dòng)機(jī)開(kāi)啟情況下，針對(duì)相同的對(duì)準(zhǔn)時(shí)間，進(jìn)行了靜基座對(duì)準(zhǔn)、搖擺對(duì)準(zhǔn)和迭代對(duì)準(zhǔn)的對(duì)準(zhǔn)精度比較。

1 SINS迭代對(duì)準(zhǔn)原理

如圖1所示，迭代對(duì)準(zhǔn)算法共4個(gè)部分，包括水平粗對(duì)準(zhǔn)、校正、導(dǎo)航和校正量計(jì)算等。首先，通過(guò)采集的加速度計(jì)輸出水平粗對(duì)準(zhǔn)方法計(jì)算出姿態(tài)矩陣，由導(dǎo)航計(jì)算的水平速度和姿態(tài)誤差、航向誤差的關(guān)系，利用最小二乘算法計(jì)算出水平角速率誤差、水平速度誤差、姿態(tài)誤差和航向誤差計(jì)算校正量，并利用校正算法通過(guò)校正量校正導(dǎo)航計(jì)算所用的水平地球角速率、水平速度和姿態(tài)矩陣，最終取得導(dǎo)航時(shí)刻的初始姿態(tài)和航向。

2 SINS迭代對(duì)準(zhǔn)算法

本文中，b系為上前左載體坐標(biāo)系，p系為天北西地理坐標(biāo)系，即導(dǎo)航坐標(biāo)系；Ω為地球自轉(zhuǎn)角速率；φ為當(dāng)?shù)鼐暥?；g為當(dāng)?shù)刂亓铀俣?；fb為慣導(dǎo)加速度計(jì)輸出；Δt為導(dǎo)航周期；Re為地球半徑。

2.1 水平對(duì)準(zhǔn)算法

利用對(duì)準(zhǔn)時(shí)間內(nèi)的加速度計(jì)輸入，計(jì)算加速度計(jì)的均值，進(jìn)而計(jì)算姿態(tài)轉(zhuǎn)換矩陣。

其中，

2.2 校正算法

校正算法分為4步，算法如下。

（1）校正速度

由上次迭代計(jì)算的速度誤差估計(jì)值修正速度初值。

其中，vyp0、vzp0為上次迭代計(jì)算時(shí)的速度初值，δvyp0、δvzp0為上次迭代計(jì)算出的初始速度誤差估計(jì)值。

（2）校正姿態(tài)矩陣

根據(jù)上次迭代計(jì)算出的航向和水平姿態(tài)的估計(jì)值更新姿態(tài)矩陣。

其中，q＝1/（4＋α2＋β2＋γ2），α、β、γ分別為上次迭代計(jì)算的航向角誤差、俯仰角誤差、橫滾角誤差的估計(jì)值，在粗航向?qū)?zhǔn)階段航向α取0。

（3）校正水平角速率

在粗航向?qū)?zhǔn)階段，ωp為：

其中，ωp為上次迭代計(jì)算時(shí)的角速度初值，δωp為上次迭代計(jì)算出的角速度估計(jì)值。

在精航向?qū)?zhǔn)階段，ωp為：

（4）粗航向?qū)?zhǔn)轉(zhuǎn)換到精航向?qū)?zhǔn)

2.3 導(dǎo)航算法

利用慣組加速度計(jì)和陀螺的輸出計(jì)算當(dāng)前時(shí)刻的速度和姿態(tài)矩陣。

2.4 計(jì)算校正量

導(dǎo)航速度誤差的計(jì)算為：

在搖擺條件下，沒(méi)有線運(yùn)動(dòng)，在整個(gè)過(guò)程中，速度為0，可得：

由導(dǎo)航計(jì)算的速度v、導(dǎo)航計(jì)算總循環(huán)次數(shù)N1、導(dǎo)航周期Δt、當(dāng)前導(dǎo)航循環(huán)次數(shù)n分別計(jì)算速度v的累加和∑v、v·n的累加和∑（kv）、v·n·n的累加和∑（k2v），可構(gòu)造得出：

其中，粗航向?qū)?zhǔn)階段，α＝0；精航向?qū)?zhǔn)階段， 航向α＝－δωzp/（Ω·cosφ）。

3 車載試驗(yàn)結(jié)果及分析

針對(duì)晃動(dòng)干擾基座的環(huán)境要求，設(shè)計(jì)了在發(fā)動(dòng)機(jī)啟動(dòng)條件下的車載試驗(yàn)，來(lái)驗(yàn)證本文提出的迭代對(duì)準(zhǔn)方法的可行性，并進(jìn)行了多種對(duì)準(zhǔn)算法的比較。將高精度激光慣組放于晃動(dòng)汽車（發(fā)動(dòng)機(jī)啟動(dòng)條件下）內(nèi)，同時(shí)以5ms采樣周期存儲(chǔ)慣組數(shù)據(jù)，按照快速機(jī)動(dòng)對(duì)準(zhǔn)時(shí)間要求（2min和5min）將采集數(shù)據(jù)拆分為相同長(zhǎng)度的13組，分別進(jìn)行了靜態(tài)對(duì)準(zhǔn)、慣性系搖擺對(duì)準(zhǔn)和迭代對(duì)準(zhǔn)計(jì)算，得到了導(dǎo)航時(shí)刻的初始姿態(tài)和航向，試驗(yàn)數(shù)據(jù)的計(jì)算結(jié)果如圖2～圖5所示。

車載試驗(yàn)結(jié)果的統(tǒng)計(jì)方差如表1所示。

從表1可以看出，對(duì)準(zhǔn)時(shí)間為2min時(shí)，靜態(tài)對(duì)準(zhǔn)俯仰角方差為0.00626°，橫滾角方差為0.00361°，航向角方差為 0.39244°；搖擺對(duì)準(zhǔn)俯仰角方差為 0.00516°，橫滾角方差為 0.00386°，航向角方差為0.03331°；迭代對(duì)準(zhǔn)俯仰角方差為0.00519°，橫滾角方差為 0.00381°，航向角方差為0.00883°。對(duì)準(zhǔn)時(shí)間5min時(shí)，靜態(tài)對(duì)準(zhǔn)俯仰角方差為0.00582°，橫滾角方差為0.00349°，航向角方差為 0.20414°；搖擺對(duì)準(zhǔn)俯仰角方差為0.00522°，橫滾角方差為 0.00381°，航向角方差為0.00901°；迭代對(duì)準(zhǔn)俯仰角方差為 0.00519°，橫滾角方差為0.00379°，航向角方差為0.00544°。統(tǒng)計(jì)數(shù)據(jù)可以看出：靜態(tài)對(duì)準(zhǔn)、搖擺對(duì)準(zhǔn)和迭代對(duì)準(zhǔn)3種方法中，俯仰角和橫滾角的精度大體相當(dāng)，航向角存在不同，在同樣時(shí)間內(nèi)，采用迭代對(duì)準(zhǔn)的航向精度較高；在同等航向?qū)?zhǔn)精度要求下，采用迭代對(duì)準(zhǔn)時(shí)間較短，迭代對(duì)準(zhǔn)2min的航向精度同搖擺5min的航向精度相當(dāng)。

表1 半實(shí)物仿真對(duì)準(zhǔn)結(jié)果Table 1 Alignment results of semi physical simulation

4 結(jié)論

由于陣風(fēng)、海浪、發(fā)動(dòng)機(jī)振動(dòng)、裝載物資以及人員走動(dòng)等因素對(duì)運(yùn)載體的作用，安裝在其上面的慣性導(dǎo)航系統(tǒng)在啟動(dòng)時(shí)不可避免地遭受運(yùn)動(dòng)干擾，使得常用的初始對(duì)準(zhǔn)方法精度不高。本文提出了一種基于晃動(dòng)基座捷聯(lián)慣導(dǎo)系統(tǒng)迭代對(duì)準(zhǔn)算法，解決了捷聯(lián)慣導(dǎo)系統(tǒng)在晃動(dòng)基座下的高精度初始對(duì)準(zhǔn)問(wèn)題。由車載試驗(yàn)結(jié)果可以看出，相比于常用的靜態(tài)對(duì)準(zhǔn)方法和慣性系搖擺對(duì)準(zhǔn)方法，相同對(duì)準(zhǔn)時(shí)間下，迭代對(duì)準(zhǔn)算法航向?qū)?zhǔn)精度較高；在相同航向精度要求的前提下，迭代對(duì)準(zhǔn)算法對(duì)準(zhǔn)時(shí)間較短。