一種實用的飛行器捷聯慣性導航算法

陳柯勛，邱 偉

（1.太原理工大學 信息與計算機學院，太原030600；2.北京強度環境研究所，北京100076）

隨著我國航天事業的蓬勃發展，飛行器定位導航的精度和效率不斷提升。其中，捷聯式慣性導航系統由于成本低、便于安裝、無需天線等特點，常用于小型飛行器導航系統中。捷聯式慣性導航系統是把慣性敏感裝置直接安裝在飛行器上來獲取飛行器的初始姿態信息、初始速度信息和初始位置信息，然后通過捷聯慣性導航解算算法確定飛行器方位、速度和位置的自主式解算導航系統［1］。

捷聯慣性導航系統的效率和可靠性取決于捷聯慣性導航解算算法，目前已有較多學者在該領域取得了較多的研究成果。例如，尹劍等［2］開展了捷聯慣導飛行器旋轉矢量姿態優化相關的研究，通過對旋轉矢量的姿態算法進行優化來提高姿態計算的精度；陳凱等［3］針對臨近空間飛行高度，研究了發射慣性坐標系和當地水平坐標系下慣性導航解算算法的等價性；溫永智等［4］則提出了衛星載波相位與捷聯慣導組合方法對高軌機動飛行器進行自主導航。總體來看，針對飛行器捷聯慣性導航系統解算算法，國內外的研究成果大多是針對基礎理論，沒有給出一種實用性較強的解算算法以及相應的慣性系統安裝與實現方案［1-8］。

因此，本文提出了一種可實現的捷聯慣性導航解算算法，并在試驗過程中通過安裝示意圖描述了慣性系統在飛行器中的安裝與實現方法。本文所述算法的有效性和可靠性經過了實際試驗驗證，具有一定的理論意義和現實意義。

1 捷聯慣導解算算法

1.1 算法原理

捷聯式慣性導航系統是把慣性敏感裝置（陀螺儀和加速度計）直接安裝在運載體上，利用慣性敏感裝置、初始姿態信息（偏航角、俯仰角和橫滾角）、初始速度信息（東向速度、北向速度和垂向速度）和初始位置信息（地理經緯度）確定載體方位、速度和位置的自主式解算導航系統。捷聯慣性導航解算是將慣性敏感裝置的信息從載體坐標系轉換到導航坐標系（地理坐標系）的計算過程，現對這兩個坐標系作介紹［9-10］。

1.1.1 導航坐標系（OXnYnZn）

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

1.1.2 載體坐標系（OXbYbZb）

載體坐標系與飛行器本體固聯，并同飛行器一起運動（包括移動和轉動）。坐標系的原點O取飛行器的質心，OXb軸沿飛行器的橫軸指向右，OYb軸沿飛行器的縱軸指向前，OZb軸垂直于OXb軸指向上方。三個坐標軸構成右手正交直角坐標系。載體坐標系相對慣性坐標系的方位為飛行器的姿態。飛行器的姿態定義如下。

航向角：預定的航行方向為航向，在水平面內，用北向基準線與航行線之間的夾角表示航向角，以北向基準為0度角，順時針為正，逆時針為負。定義域為［0，360）度。

俯仰角：飛行器縱軸與水平面之間的夾角為俯仰角，向上為正，向下為負，定義域為［-90，90］度。

橫滾角：飛行器縱軸與水平面之間的夾角為俯仰角，右傾為正，左傾為負，定義域為［-180，180］度。

1.2 算法總體設計

本文所設計的捷聯慣性導航解算算法如圖1所示。

圖1 捷聯慣導解算算法原理框圖Fig.1 Block diagram of strapdown inertial navigation algorithm

根據圖1所示原理，捷聯慣導解算算法流程通過以下步驟實現：

1）確定導航坐標系。包括載體坐標系和導航坐標系的建立，沿著飛行器前進方向，載體系OXbYbZb對應右-前-上坐標系方向，導航坐標系OXnYnZn采用東-北-天坐標系［5］。

2）確定初始狀態，根據初始姿態計算出初始位置矩陣Cnb.初始狀態作為捷聯解算的時間起始點，包括飛行器的姿態信息（航向角、俯仰角和滾動角）、速度信息（東向、北向和垂向速度）、位置信息（飛行器的經緯度）。

3）將初始姿態信息用四元數法構造初始姿態矩陣（捷聯矩陣），用初始經緯度信息構造初始位置矩陣。

4）由初始時刻T的捷聯矩陣、陀螺輸出角速率信息及初始位置速度信息計算，同時根據位置信息計算，根據地球自轉速率計算ωnie，進而根據捷聯矩陣更新公式下一個采樣點T＋1時刻的捷聯矩陣，最終根據四元數的運動學微分方程從而計算出飛行器在該時刻的姿態角和位置［6］。

5）根據（4）得到的捷聯矩陣和加速度及輸出加速度信息解算出飛行器該時刻在東北天三個方向的加速度分量。

6）重復步驟（3）至步驟（5），直到采樣數據解算結束。

7）最后得到每個采樣時刻的姿態矩陣、位置矩陣、速度矩陣從而計算出姿態、位置和速度信息。

按照以上步驟重復進行就可以對捷聯慣導數據進行解算。其中，采用四元數法進行姿態計算，用畢卡近似法求解微分方程。上面解算過程，共使用了三個基本計算過程，即姿態更新、速度更新、位置更新。

1.3 姿態更新

記載體系OXbYbZb為b系，記“東-北-天（E-NU）”地理坐標系（n系）作為捷聯慣導系統的導航參考坐標系，則以n系作為參考系的姿態微分方程為：

式中：ωnin表示導航系相對于慣性系的旋轉，它包含地球自轉引起的導航系旋轉，以及系統在地球表面附近移動因地球表面彎曲引起的導航系旋轉，即有

根據矩陣鏈乘規則，有：

式中：角標括號中的符號m表示tm時刻。由于i系是絕對不動的慣性參考坐標系，它與時間無關，不需標注時刻；而n系和b系相對于i系都是動坐標系，均跟時間有關，需標注時刻。

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

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

式（8）～（11）即為捷聯慣導數值遞推姿態更新算法。

在實際工程應用中，首先需要根據慣性敏感裝置給出的初始姿態角，依據初始姿態角求取四元數：

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

進而根據四元數求方向余弦矩陣：

姿態角與姿態矩陣的關系：

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

則由以上兩式即可解算出姿態角：

1.4 速度更新

慣導比力方程一般形式如下［10-13］：

在比力方程（21）中將vnen簡寫為vn，并明確標注出各量時間參數，如下：

上式兩邊同時在時間段［tm-1，tm］內積分，得：

即

將式（24）移項，可改寫成遞推形式：

速度更新變為主要討論 Δvnsf（m）和 Δvncor／g（m）的數值積分算法。實際工程應用解算中，地理坐標系相對地球坐標系的角速度為：

加速度計獲得的比力信息fibb為載體坐標系中各個軸向的比力，而我們需要的比力finn為地理坐標系中各個軸向的比力，它們之間應用矩陣Cnb做變換：

根據比力信息可以求出各個方向上的加速度：

因此可以求得速度為：

1.5 位置更新

捷聯慣導系統的位置（緯度、經度和高度）微分方程式可以表示如下：

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

其中，記：

與捷聯慣導姿態和速度更新算法相比，位置更新算法引起的誤差一般比較小，可采用比較簡單的梯形積分法對式（33）離散化，得：

上式移項，便得位置更新算法：

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

可對矩陣整體Mpv進行外推，亦可對矩陣元素中的位置變量L，h外推，再構造矩陣Mpv.

實際工程應用中，載體所在位置的地理緯度L、經度λ可由下列方程求得：

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

2 捷聯慣導算法應用實例

本文通過試驗水箱模擬飛行器，通過試驗水箱空投試驗來對本文所述算法的有效性進行驗證。

2.1 試驗條件

試驗前，為了保證測量前端的準確性，選擇了進口的陀螺儀和加速度計。陀螺儀選用了美國NXP公司的FXAS21002C三軸陀螺儀，具體指標如下：

1）采用緊湊型QFN封裝；

2）2.7mA工作電流（2.8μA待機電流）；

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

4）動態可選的全量程范圍（±250／±500／±1 000／±2 000°／s）；

5）32采樣FIFO；

6）輸出數據頻率（ODR）范圍：12.5～800Hz；

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

加速度計選用了美國Endevco 2230E型三軸向壓電式加速度計，具體指標如下。

1）靈敏度：3pC／g；

2）正弦振動極限：1 000g；

3）沖擊極限：2 000g；

4）頻率響應：1～10 000Hz；

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

6）質量：17g.

試驗過程中，將空投數據記錄裝置安裝在水箱內部的凹槽內，慣性測量單元及氣壓高度計固定在水箱上蓋板內側，具體安裝位置及方向如圖2所示。

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

試驗水箱在機艙內的安裝如圖3所示。

圖3 試驗水箱在機艙內的安裝示意圖Fig.3 Installation diagram of test water tank in engine room

飛機在落區投放時，試驗水箱從飛機尾部的滑軌投下，投放時刻的參數如表1所示。

表1 空投試驗參數表Table 1 Airdrop test parameter table

試驗水箱落點的參數：方位 E：113°54＇41″，N：31°7＇38.4″，緯度為31°時的每秒約合26.4m，經度的每秒約合31m.

2.2 試驗結果分析

根據試驗水箱中的數據記錄裝置導出的數據，可以得出陀螺儀輸出角速度時域波形如圖4所示。

加速度計過載時域波形如圖5所示。

通常情況下，捷聯慣導解算算法的運算結果與多種因素有關，其中就包括陀螺儀和加速度計的精度和誤差模型，但從圖4和圖5中可以看出，空投試驗所選用的陀螺儀和加速度計精度較高，測量數據沒有影響捷聯慣性導航解算算法的解算過程，因此可以不考慮該項因素的影響。

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

圖5 加速度計輸出加速度時域波形圖Fig.5 Waveform of accelerometer output acceleration

使用本文所述算法對試驗水箱中采集到的試驗數據進行解算，可以得到飛行器的三個姿態角變化曲線如圖6所示，飛行器的三向位置變化曲線如圖7所示。

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

從圖6和圖7可以看出，本文所述算法解算出來的姿態、位置結果與實際落地點的姿態和位置信息吻合。綜合分析整個試驗結果，可以證明本文所述算法的有效性和可靠性。

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

3 結論

本文所提出的捷聯慣性導航解算算法借助四元數法進行飛行器姿態和位置信息解算，算法運行過程簡潔高效，可實施性強，試驗設計切合實際應用場景，可以為相關領域提供參考，具有一定的理論意義和實用價值。