一種實(shí)用的飛行器捷聯(lián)慣性導(dǎo)航算法

陳柯勛，邱 偉

（1.太原理工大學(xué) 信息與計(jì)算機(jī)學(xué)院，太原030600；2.北京強(qiáng)度環(huán)境研究所，北京100076）

隨著我國(guó)航天事業(yè)的蓬勃發(fā)展，飛行器定位導(dǎo)航的精度和效率不斷提升。其中，捷聯(lián)式慣性導(dǎo)航系統(tǒng)由于成本低、便于安裝、無(wú)需天線等特點(diǎn)，常用于小型飛行器導(dǎo)航系統(tǒng)中。捷聯(lián)式慣性導(dǎo)航系統(tǒng)是把慣性敏感裝置直接安裝在飛行器上來(lái)獲取飛行器的初始姿態(tài)信息、初始速度信息和初始位置信息，然后通過(guò)捷聯(lián)慣性導(dǎo)航解算算法確定飛行器方位、速度和位置的自主式解算導(dǎo)航系統(tǒng)［1］。

捷聯(lián)慣性導(dǎo)航系統(tǒng)的效率和可靠性取決于捷聯(lián)慣性導(dǎo)航解算算法，目前已有較多學(xué)者在該領(lǐng)域取得了較多的研究成果。例如，尹劍等［2］開展了捷聯(lián)慣導(dǎo)飛行器旋轉(zhuǎn)矢量姿態(tài)優(yōu)化相關(guān)的研究，通過(guò)對(duì)旋轉(zhuǎn)矢量的姿態(tài)算法進(jìn)行優(yōu)化來(lái)提高姿態(tài)計(jì)算的精度；陳凱等［3］針對(duì)臨近空間飛行高度，研究了發(fā)射慣性坐標(biāo)系和當(dāng)?shù)厮阶鴺?biāo)系下慣性導(dǎo)航解算算法的等價(jià)性；溫永智等［4］則提出了衛(wèi)星載波相位與捷聯(lián)慣導(dǎo)組合方法對(duì)高軌機(jī)動(dòng)飛行器進(jìn)行自主導(dǎo)航。總體來(lái)看，針對(duì)飛行器捷聯(lián)慣性導(dǎo)航系統(tǒng)解算算法，國(guó)內(nèi)外的研究成果大多是針對(duì)基礎(chǔ)理論，沒(méi)有給出一種實(shí)用性較強(qiáng)的解算算法以及相應(yīng)的慣性系統(tǒng)安裝與實(shí)現(xiàn)方案［1-8］。

因此，本文提出了一種可實(shí)現(xiàn)的捷聯(lián)慣性導(dǎo)航解算算法，并在試驗(yàn)過(guò)程中通過(guò)安裝示意圖描述了慣性系統(tǒng)在飛行器中的安裝與實(shí)現(xiàn)方法。本文所述算法的有效性和可靠性經(jīng)過(guò)了實(shí)際試驗(yàn)驗(yàn)證，具有一定的理論意義和現(xiàn)實(shí)意義。

1 捷聯(lián)慣導(dǎo)解算算法

1.1 算法原理

捷聯(lián)式慣性導(dǎo)航系統(tǒng)是把慣性敏感裝置（陀螺儀和加速度計(jì)）直接安裝在運(yùn)載體上，利用慣性敏感裝置、初始姿態(tài)信息（偏航角、俯仰角和橫滾角）、初始速度信息（東向速度、北向速度和垂向速度）和初始位置信息（地理經(jīng)緯度）確定載體方位、速度和位置的自主式解算導(dǎo)航系統(tǒng)。捷聯(lián)慣性導(dǎo)航解算是將慣性敏感裝置的信息從載體坐標(biāo)系轉(zhuǎn)換到導(dǎo)航坐標(biāo)系（地理坐標(biāo)系）的計(jì)算過(guò)程，現(xiàn)對(duì)這兩個(gè)坐標(biāo)系作介紹［9-10］。

1.1.1 導(dǎo)航坐標(biāo)系（OXnYnZn）

導(dǎo)航坐標(biāo)系如下：以地球表面為XOY平面，以本初子午線與赤道交點(diǎn)（即經(jīng)緯度均為0的點(diǎn)）為坐標(biāo)原點(diǎn)O，沿緯線方向定義X軸（向東為正），X坐標(biāo)范圍為（-180，180）；沿經(jīng)線方向定義Y軸（向北為正），Y坐標(biāo)范圍為（-90，90）；Z軸垂直XOY平面，向上為正，構(gòu)成右手坐標(biāo)系，通常也叫做東北天坐標(biāo)系。對(duì)于導(dǎo)航坐標(biāo)系，坐標(biāo)軸還有不同的取法，如北西天、北東地坐標(biāo)系。在此坐標(biāo)系下，地球表面點(diǎn)的Z坐標(biāo)均為0，而X和Y坐標(biāo)分別以經(jīng)度值L和緯度值B來(lái)表示。

1.1.2 載體坐標(biāo)系（OXbYbZb）

載體坐標(biāo)系與飛行器本體固聯(lián)，并同飛行器一起運(yùn)動(dòng)（包括移動(dòng)和轉(zhuǎn)動(dòng)）。坐標(biāo)系的原點(diǎn)O取飛行器的質(zhì)心，OXb軸沿飛行器的橫軸指向右，OYb軸沿飛行器的縱軸指向前，OZb軸垂直于OXb軸指向上方。三個(gè)坐標(biāo)軸構(gòu)成右手正交直角坐標(biāo)系。載體坐標(biāo)系相對(duì)慣性坐標(biāo)系的方位為飛行器的姿態(tài)。飛行器的姿態(tài)定義如下。

航向角：預(yù)定的航行方向?yàn)楹较?，在水平面?nèi)，用北向基準(zhǔn)線與航行線之間的夾角表示航向角，以北向基準(zhǔn)為0度角，順時(shí)針為正，逆時(shí)針為負(fù)。定義域?yàn)椋?，360）度。

俯仰角：飛行器縱軸與水平面之間的夾角為俯仰角，向上為正，向下為負(fù)，定義域?yàn)椋?90，90］度。

橫滾角：飛行器縱軸與水平面之間的夾角為俯仰角，右傾為正，左傾為負(fù)，定義域?yàn)椋?180，180］度。

1.2 算法總體設(shè)計(jì)

本文所設(shè)計(jì)的捷聯(lián)慣性導(dǎo)航解算算法如圖1所示。

圖1 捷聯(lián)慣導(dǎo)解算算法原理框圖Fig.1 Block diagram of strapdown inertial navigation algorithm

根據(jù)圖1所示原理，捷聯(lián)慣導(dǎo)解算算法流程通過(guò)以下步驟實(shí)現(xiàn)：

1）確定導(dǎo)航坐標(biāo)系。包括載體坐標(biāo)系和導(dǎo)航坐標(biāo)系的建立，沿著飛行器前進(jìn)方向，載體系OXbYbZb對(duì)應(yīng)右-前-上坐標(biāo)系方向，導(dǎo)航坐標(biāo)系OXnYnZn采用東-北-天坐標(biāo)系［5］。

2）確定初始狀態(tài)，根據(jù)初始姿態(tài)計(jì)算出初始位置矩陣Cnb.初始狀態(tài)作為捷聯(lián)解算的時(shí)間起始點(diǎn)，包括飛行器的姿態(tài)信息（航向角、俯仰角和滾動(dòng)角）、速度信息（東向、北向和垂向速度）、位置信息（飛行器的經(jīng)緯度）。

3）將初始姿態(tài)信息用四元數(shù)法構(gòu)造初始姿態(tài)矩陣（捷聯(lián)矩陣），用初始經(jīng)緯度信息構(gòu)造初始位置矩陣。

4）由初始時(shí)刻T的捷聯(lián)矩陣、陀螺輸出角速率信息及初始位置速度信息計(jì)算，同時(shí)根據(jù)位置信息計(jì)算，根據(jù)地球自轉(zhuǎn)速率計(jì)算ωnie，進(jìn)而根據(jù)捷聯(lián)矩陣更新公式下一個(gè)采樣點(diǎn)T＋1時(shí)刻的捷聯(lián)矩陣，最終根據(jù)四元數(shù)的運(yùn)動(dòng)學(xué)微分方程從而計(jì)算出飛行器在該時(shí)刻的姿態(tài)角和位置［6］。

5）根據(jù)（4）得到的捷聯(lián)矩陣和加速度及輸出加速度信息解算出飛行器該時(shí)刻在東北天三個(gè)方向的加速度分量。

6）重復(fù)步驟（3）至步驟（5），直到采樣數(shù)據(jù)解算結(jié)束。

7）最后得到每個(gè)采樣時(shí)刻的姿態(tài)矩陣、位置矩陣、速度矩陣從而計(jì)算出姿態(tài)、位置和速度信息。

按照以上步驟重復(fù)進(jìn)行就可以對(duì)捷聯(lián)慣導(dǎo)數(shù)據(jù)進(jìn)行解算。其中，采用四元數(shù)法進(jìn)行姿態(tài)計(jì)算，用畢卡近似法求解微分方程。上面解算過(guò)程，共使用了三個(gè)基本計(jì)算過(guò)程，即姿態(tài)更新、速度更新、位置更新。

1.3 姿態(tài)更新

記載體系OXbYbZb為b系，記“東-北-天（E-NU）”地理坐標(biāo)系（n系）作為捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航參考坐標(biāo)系，則以n系作為參考系的姿態(tài)微分方程為：

式中：ωnin表示導(dǎo)航系相對(duì)于慣性系的旋轉(zhuǎn)，它包含地球自轉(zhuǎn)引起的導(dǎo)航系旋轉(zhuǎn)，以及系統(tǒng)在地球表面附近移動(dòng)因地球表面彎曲引起的導(dǎo)航系旋轉(zhuǎn)，即有

根據(jù)矩陣鏈乘規(guī)則，有：

式中：角標(biāo)括號(hào)中的符號(hào)m表示tm時(shí)刻。由于i系是絕對(duì)不動(dòng)的慣性參考坐標(biāo)系，它與時(shí)間無(wú)關(guān)，不需標(biāo)注時(shí)刻；而n系和b系相對(duì)于i系都是動(dòng)坐標(biāo)系，均跟時(shí)間有關(guān)，需標(biāo)注時(shí)刻。

將式（6）和（7）代入式（5），得：

通常在導(dǎo)航更新周期［tm-1，tm］內(nèi)，可以認(rèn)為由速度和位置引起的變化很小，即可視為常值，記為，則有：

式（8）～（11）即為捷聯(lián)慣導(dǎo)數(shù)值遞推姿態(tài)更新算法。

在實(shí)際工程應(yīng)用中，首先需要根據(jù)慣性敏感裝置給出的初始姿態(tài)角，依據(jù)初始姿態(tài)角求取四元數(shù)：

式中：θ，γ，φ分別為俯仰角、橫滾角和偏航角。

進(jìn)而根據(jù)四元數(shù)求方向余弦矩陣：

姿態(tài)角與姿態(tài)矩陣的關(guān)系：

式中：θ，γ，φ分別為俯仰角，橫滾角和偏航角。如果記：

則由以上兩式即可解算出姿態(tài)角：

1.4 速度更新

慣導(dǎo)比力方程一般形式如下［10-13］：

在比力方程（21）中將vnen簡(jiǎn)寫為vn，并明確標(biāo)注出各量時(shí)間參數(shù)，如下：

上式兩邊同時(shí)在時(shí)間段［tm-1，tm］內(nèi)積分，得：

即

將式（24）移項(xiàng)，可改寫成遞推形式：

速度更新變?yōu)橹饕懻?Δvnsf（m）和 Δvncor／g（m）的數(shù)值積分算法。實(shí)際工程應(yīng)用解算中，地理坐標(biāo)系相對(duì)地球坐標(biāo)系的角速度為：

加速度計(jì)獲得的比力信息fibb為載體坐標(biāo)系中各個(gè)軸向的比力，而我們需要的比力finn為地理坐標(biāo)系中各個(gè)軸向的比力，它們之間應(yīng)用矩陣Cnb做變換：

根據(jù)比力信息可以求出各個(gè)方向上的加速度：

因此可以求得速度為：

1.5 位置更新

捷聯(lián)慣導(dǎo)系統(tǒng)的位置（緯度、經(jīng)度和高度）微分方程式可以表示如下：

將它們改寫成矩陣形式，為

其中，記：

與捷聯(lián)慣導(dǎo)姿態(tài)和速度更新算法相比，位置更新算法引起的誤差一般比較小，可采用比較簡(jiǎn)單的梯形積分法對(duì)式（33）離散化，得：

上式移項(xiàng)，便得位置更新算法：

其中，Mpv（m-1／2）可 采 用 所 示 的 線 性 外 推 算 法［13-16］。

可對(duì)矩陣整體Mpv進(jìn)行外推，亦可對(duì)矩陣元素中的位置變量L，h外推，再構(gòu)造矩陣Mpv.

實(shí)際工程應(yīng)用中，載體所在位置的地理緯度L、經(jīng)度λ可由下列方程求得：

其中，Rxt＝Re·（1＋esin2φ）為地球參考橢球 WGS-84的子午圈曲率半徑，Ryt＝Re·（1-2e＋3esin2φ）為相應(yīng)的卯酉圈曲率半徑，e＝1／298.257為相應(yīng)的橢圓度。

2 捷聯(lián)慣導(dǎo)算法應(yīng)用實(shí)例

本文通過(guò)試驗(yàn)水箱模擬飛行器，通過(guò)試驗(yàn)水箱空投試驗(yàn)來(lái)對(duì)本文所述算法的有效性進(jìn)行驗(yàn)證。

2.1 試驗(yàn)條件

試驗(yàn)前，為了保證測(cè)量前端的準(zhǔn)確性，選擇了進(jìn)口的陀螺儀和加速度計(jì)。陀螺儀選用了美國(guó)NXP公司的FXAS21002C三軸陀螺儀，具體指標(biāo)如下：

1）采用緊湊型QFN封裝；

2）2.7mA工作電流（2.8μA待機(jī)電流）；

3）角速率分辨率為0.062 5dps／LSB；

4）動(dòng)態(tài)可選的全量程范圍（±250／±500／±1 000／±2 000°／s）；

5）32采樣FIFO；

6）輸出數(shù)據(jù)頻率（ODR）范圍：12.5～800Hz；

7）工作溫度范圍：-40～＋85℃.

加速度計(jì)選用了美國(guó)Endevco 2230E型三軸向壓電式加速度計(jì)，具體指標(biāo)如下。

1）靈敏度：3pC／g；

2）正弦振動(dòng)極限：1 000g；

3）沖擊極限：2 000g；

4）頻率響應(yīng)：1～10 000Hz；

5）溫度范圍：-55～＋260℃；

6）質(zhì)量：17g.

試驗(yàn)過(guò)程中，將空投數(shù)據(jù)記錄裝置安裝在水箱內(nèi)部的凹槽內(nèi)，慣性測(cè)量單元及氣壓高度計(jì)固定在水箱上蓋板內(nèi)側(cè)，具體安裝位置及方向如圖2所示。

圖2 數(shù)據(jù)記錄裝置、慣性測(cè)量單元及氣壓高度計(jì)安裝示意圖Fig.2 Installation diagram of data recording device，inertial measurement unit and barometric altimeter

試驗(yàn)水箱在機(jī)艙內(nèi)的安裝如圖3所示。

圖3 試驗(yàn)水箱在機(jī)艙內(nèi)的安裝示意圖Fig.3 Installation diagram of test water tank in engine room

飛機(jī)在落區(qū)投放時(shí)，試驗(yàn)水箱從飛機(jī)尾部的滑軌投下，投放時(shí)刻的參數(shù)如表1所示。

表1 空投試驗(yàn)參數(shù)表Table 1 Airdrop test parameter table

試驗(yàn)水箱落點(diǎn)的參數(shù)：方位 E：113°54＇41″，N：31°7＇38.4″，緯度為31°時(shí)的每秒約合26.4m，經(jīng)度的每秒約合31m.

2.2 試驗(yàn)結(jié)果分析

根據(jù)試驗(yàn)水箱中的數(shù)據(jù)記錄裝置導(dǎo)出的數(shù)據(jù)，可以得出陀螺儀輸出角速度時(shí)域波形如圖4所示。

加速度計(jì)過(guò)載時(shí)域波形如圖5所示。

通常情況下，捷聯(lián)慣導(dǎo)解算算法的運(yùn)算結(jié)果與多種因素有關(guān)，其中就包括陀螺儀和加速度計(jì)的精度和誤差模型，但從圖4和圖5中可以看出，空投試驗(yàn)所選用的陀螺儀和加速度計(jì)精度較高，測(cè)量數(shù)據(jù)沒(méi)有影響捷聯(lián)慣性導(dǎo)航解算算法的解算過(guò)程，因此可以不考慮該項(xiàng)因素的影響。

圖4 陀螺儀輸出角速度時(shí)域波形圖Fig.4 Waveform of gyroscope output angular velocity

圖5 加速度計(jì)輸出加速度時(shí)域波形圖Fig.5 Waveform of accelerometer output acceleration

使用本文所述算法對(duì)試驗(yàn)水箱中采集到的試驗(yàn)數(shù)據(jù)進(jìn)行解算，可以得到飛行器的三個(gè)姿態(tài)角變化曲線如圖6所示，飛行器的三向位置變化曲線如圖7所示。

圖6 本文算法解算后飛行器姿態(tài)變化曲線圖Fig.6 Attitude change curve of aircraft after the algorithm

從圖6和圖7可以看出，本文所述算法解算出來(lái)的姿態(tài)、位置結(jié)果與實(shí)際落地點(diǎn)的姿態(tài)和位置信息吻合。綜合分析整個(gè)試驗(yàn)結(jié)果，可以證明本文所述算法的有效性和可靠性。

圖7 本文算法解算后飛行器位置變化曲線圖Fig.7 Position change curve of aircraft after the algorithm

3 結(jié)論

本文所提出的捷聯(lián)慣性導(dǎo)航解算算法借助四元數(shù)法進(jìn)行飛行器姿態(tài)和位置信息解算，算法運(yùn)行過(guò)程簡(jiǎn)潔高效，可實(shí)施性強(qiáng)，試驗(yàn)設(shè)計(jì)切合實(shí)際應(yīng)用場(chǎng)景，可以為相關(guān)領(lǐng)域提供參考，具有一定的理論意義和實(shí)用價(jià)值。