劉 春，周發(fā)根

（1.同濟(jì)大學(xué) 測量與國土信息工程系，上海200092；2.現(xiàn)代工程測量國家測繪局重點(diǎn)實(shí)驗(yàn)室，上海200092）

慣性導(dǎo)航系統(tǒng)（inertial navigation system，INS）是20世紀(jì)初發(fā)展起來的一種自主式導(dǎo)航系統(tǒng)，其基本原理是根據(jù)牛頓提出的慣性空間的力學(xué)定律，利用陀螺儀和加速度計(jì)等慣性敏感元件來感知運(yùn)載體的角速度和加速度信息，然后通過積分運(yùn)算，得出導(dǎo)航參數(shù)以確定載體位置［1］.因此慣性導(dǎo)航系統(tǒng)是一種既不依賴于外部信息，又不發(fā)射能量的自主導(dǎo)航系統(tǒng)，具有極強(qiáng)的抗干擾能力，能夠?qū)崟r地提供十分完備的導(dǎo)航參數(shù)，除了能提供運(yùn)載體的位置和速度外，還能給出高精度的姿態(tài)信息.

一個完整的慣性導(dǎo)航系統(tǒng)主要由慣性測量裝置（inertial measurement unit，IMU）、導(dǎo)航計(jì)算機(jī)、控制顯示器等3個部分組成.目前應(yīng)用中的慣性導(dǎo)航系統(tǒng)主要分為兩類：機(jī)械平臺式慣性導(dǎo)航系統(tǒng)（gimbaled inertial navigation system，GINS）與捷聯(lián)式慣性導(dǎo)航系統(tǒng)（strapdown inertial navigation system，SINS）.捷聯(lián)式慣導(dǎo)系統(tǒng)沒有實(shí)體平臺，陀螺和加速度計(jì)直接固連在載體上，由計(jì)算機(jī)完成慣性平臺的功能，因此有時也稱作數(shù)學(xué)平臺，其主要功能是坐標(biāo)變換矩陣和姿態(tài)角的計(jì)算［2］.

由于采用數(shù)學(xué)平臺代替了實(shí)體平臺，捷聯(lián)式系統(tǒng)具有結(jié)構(gòu)簡單、體積小、質(zhì)量輕、成本低、初始對準(zhǔn)快等優(yōu)點(diǎn).因此隨著計(jì)算機(jī)技術(shù)和慣性傳感器技術(shù)的發(fā)展，捷聯(lián)式系統(tǒng)已經(jīng)廣泛應(yīng)用于火箭、導(dǎo)彈、戰(zhàn)斗機(jī)和其他飛行器上［3］.近年來，在民用領(lǐng)域，捷聯(lián)式系統(tǒng)更是占據(jù)統(tǒng)治地位，在地下勘探、機(jī)器人技術(shù)、汽車導(dǎo)航等方面發(fā)揮了重要作用.捷聯(lián)慣導(dǎo)與GPS構(gòu)成的組合系統(tǒng)已廣泛應(yīng)用于地面移動測圖、輔助航空攝影測量、航空遙感等領(lǐng)域.

捷聯(lián)慣導(dǎo)系統(tǒng)在短時間內(nèi)能提供高精度的導(dǎo)航參數(shù)，但隨著導(dǎo)航時間的增長，誤差積累很快.因此，大量學(xué)者致力于捷聯(lián)導(dǎo)航算法的研究，并作出了重要貢獻(xiàn).其中最具代表性的是Paul G.Savage，他對捷聯(lián)慣導(dǎo)的姿態(tài)更新、速度更新、位置更新等算法都做了深入研究，取得了杰出成果［4］.秦永元等人也對以增量形式輸出測量值的捷聯(lián)慣導(dǎo)系統(tǒng)，研究了誤差補(bǔ)償算法，對姿態(tài)解算的圓錐效應(yīng)、速度解算的劃槳效應(yīng)、位置解算的渦卷效應(yīng)，都給出了相應(yīng)的補(bǔ)償方法［1］.然而，與目前許多介紹慣導(dǎo)算法的參考文獻(xiàn)一樣，文獻(xiàn)［1，4］都側(cè)重捷聯(lián)導(dǎo)航算法，沒有給出實(shí)用的捷聯(lián)導(dǎo)航計(jì)算模型.

文獻(xiàn)［5］分析了捷聯(lián)式慣導(dǎo)系統(tǒng)的動態(tài)誤差特性，分別討論了陀螺漂移、加速度計(jì)零偏、初始對準(zhǔn)誤差對捷聯(lián)慣導(dǎo)系統(tǒng)的影響，具有重要的借鑒作用.然而在實(shí)際導(dǎo)航過程中，通常受到上述各種誤差源的共同影響.因此本文在前人工作的基礎(chǔ)上，設(shè)計(jì)了一套完整的捷聯(lián)導(dǎo)航算法，使整個導(dǎo)航過程，從數(shù)據(jù)采集到最后輸出完備的導(dǎo)航參數(shù)，得以清晰地實(shí)現(xiàn).并利用機(jī)載GPS／SINS系統(tǒng)的實(shí)測數(shù)據(jù)進(jìn)行導(dǎo)航計(jì)算，探討實(shí)際導(dǎo)航過程中捷聯(lián)慣導(dǎo)系統(tǒng)在各種誤差源共同影響下的誤差特性.采用卡爾曼濾波進(jìn)行GPS／SINS松散組合解算，結(jié)合卡爾曼平滑，分析SINS在組合系統(tǒng)中的作用和精度情況.

1 捷聯(lián)慣性導(dǎo)航系統(tǒng)的組成

捷聯(lián)慣性導(dǎo)航系統(tǒng)沒有實(shí)體平臺，慣性敏感元件（陀螺和加速度計(jì)）直接安裝在載體上，其敏感軸通常在載體坐標(biāo)系的三軸方向上.陀螺儀測定載體相對于慣性參照系的運(yùn)動角速度，并由此計(jì)算載體坐標(biāo)系至導(dǎo)航坐標(biāo)系的變換矩陣，通過此矩陣，把加速度計(jì)測得的加速度值變換到導(dǎo)航坐標(biāo)系，然后進(jìn)行積分運(yùn)算，得到所需要的導(dǎo)航定位參數(shù)，同時，利用坐標(biāo)變換矩陣提取姿態(tài)信息［1］.

1.1 捷聯(lián)慣導(dǎo)坐標(biāo)轉(zhuǎn)換關(guān)系

坐標(biāo)系統(tǒng)在慣性導(dǎo)航定位中具有重要的地位，具體的導(dǎo)航計(jì)算都是在一定的坐標(biāo)系統(tǒng)中進(jìn)行的.圖1給出了慣性導(dǎo)航系統(tǒng)中常用的坐標(biāo)系統(tǒng).

從圖1中可以看出，捷聯(lián)慣導(dǎo)系統(tǒng)中常涉及到5類不同的坐標(biāo)系，分別為地心慣性系（i系）、地固系（e系）、當(dāng)?shù)厮阶鴺?biāo)系（L 系）、游移方位坐標(biāo)系（w系）、載體坐標(biāo)系（b系）.值得注意的是，L 系和w 系的原點(diǎn)一般選在載體質(zhì)心，而圖中將L 系和w 系的原點(diǎn)選在地球表面，是為了更清楚地反映它們的三軸指向.以上5類坐標(biāo)系之間可以通過相應(yīng)的旋轉(zhuǎn)矩陣進(jìn)行轉(zhuǎn)換，它們之間的轉(zhuǎn)換關(guān)系可以參閱文獻(xiàn)［2］，這里不再列出.

圖1 捷聯(lián)慣導(dǎo)系統(tǒng)中的坐標(biāo)系Fig.1 Coordinate system of strapdown inertial navigation system

1.2 數(shù)據(jù)采集

在捷聯(lián)慣性導(dǎo)航系統(tǒng)中，IMU 直接固聯(lián)在運(yùn)載體上，陀螺和加速度計(jì)分別測量載體的角運(yùn)動信息和線運(yùn)動信息，結(jié)果以二進(jìn)制文件輸出，供后期處理.

IMU 的輸出內(nèi)容包含了采樣時刻、載體沿3個軸向的角速度、沿3 個軸向的比力，環(huán)境溫度等信息，見表1.為了減小誤差，有的IMU 以角度增量和速度增量形式輸出.

受慣性器件本身性能和外界環(huán)境的影響，IMU的輸出值通常都含有誤差，需要進(jìn)行補(bǔ)償.一般來說，在慣導(dǎo)系統(tǒng)工作之前要對IMU 的各項(xiàng)誤差進(jìn)行檢校測定，并加以補(bǔ)償.實(shí)際過程中，IMU 的輸出數(shù)據(jù)可能出現(xiàn)斷裂等異常情況，需要對測量值進(jìn)行內(nèi)插和重采樣.

表1 IMU 測量數(shù)據(jù)Tab.1 IMU measured data

值得一提的是，所有的測量值都是沿IMU 本身3個軸線方向的.由于IMU 本身的軸線定義與載體坐標(biāo)系不能完全一致，因此要將IMU 的輸出值轉(zhuǎn)換到載體坐標(biāo)系中.加速度計(jì)無法測量重力，其輸出值是沿IMU軸線的每單位質(zhì)量所受到的非慣性力，通常稱為比力［1］.在后續(xù)的導(dǎo)航計(jì)算中，要進(jìn)行重力補(bǔ)償.

2 捷聯(lián)慣性導(dǎo)航系統(tǒng)計(jì)算模型

2.1 導(dǎo)航方程

導(dǎo)航方程的推導(dǎo)基于牛頓第二定律和哥氏定理，在不同的導(dǎo)航坐標(biāo)系中有不同的表現(xiàn)形式.對于工作在非極區(qū)的捷聯(lián)慣導(dǎo)系統(tǒng)，為了簡化計(jì)算，導(dǎo)航坐標(biāo)系一般選取當(dāng)?shù)厮阶鴺?biāo)系.這樣，捷聯(lián)慣導(dǎo)系統(tǒng)完全等效于指北方位的平臺系統(tǒng)［1］.

選取L系作為導(dǎo)航坐標(biāo)系時，取位置向量為rL，速度向量為vL.

式中：φ，λ，h分別為緯度、經(jīng)度、大地高；ve，vn，vu分別為東向、北向、天向的速度.矢量形式的導(dǎo)航方程如下所示：

式中：

M，N分別為子午圈半徑和卯酉圈半徑為b系到L系的旋轉(zhuǎn)矩陣；fb為比力觀測值；Ωie，ΩeL，ΩiL分別為ωie，ωeL，ωiL的三維分量構(gòu)成的反對稱陣，其中ωie為地球自轉(zhuǎn)角速度，ωeL為導(dǎo)航坐標(biāo)系相對于地球的轉(zhuǎn)移速率，ωiL為導(dǎo)航坐標(biāo)系相對于慣性系的旋轉(zhuǎn)角速度為角速度觀測值.

2.2 導(dǎo)航計(jì)算

實(shí)踐中為了避免高緯度地區(qū)數(shù)值計(jì)算的不穩(wěn)定性，導(dǎo)航計(jì)算是在游移方位坐標(biāo)系中（w 系）進(jìn)行的［6］，然后將得到的導(dǎo)航參數(shù)再變換到L系.

圖2給出了捷聯(lián)慣性導(dǎo)航系統(tǒng)的計(jì)算流程圖.從圖中可以看出，導(dǎo)航計(jì)算的內(nèi)容主要包括：姿態(tài)計(jì)算、比力分解、導(dǎo)航處理.首先根據(jù)上一時刻的姿態(tài)信息，利用陀螺的輸出值，計(jì)算本時刻的姿態(tài)矩陣Rnb，稱為姿態(tài)更新或者旋轉(zhuǎn)矩陣更新，這個過程中涉及到地球自轉(zhuǎn)角速度和導(dǎo)航坐標(biāo)系的轉(zhuǎn)移速率；接著利用旋轉(zhuǎn)矩陣將比力觀測值分解到導(dǎo)航坐標(biāo)系中，并進(jìn)行哥氏修正和重力補(bǔ)償；最后積分求取速度和位置，并獲取載體的姿態(tài)信息.下面給出w 系導(dǎo)航計(jì)算的關(guān)鍵步驟和主要公式.

圖2 捷聯(lián)慣導(dǎo)系統(tǒng)的導(dǎo)航計(jì)算模型Fig.2 Navigation calculation model for strapdown inertial navigation system

2.2.1 旋轉(zhuǎn)矩陣更新

2.2.2 加速度計(jì)算

（1）哥氏加速度的計(jì)算哥氏改正項(xiàng)為

（2）正常重力的計(jì)算

重力補(bǔ)償量gw應(yīng)根據(jù)重力場模型來計(jì)算.對于WGS84橢球，可采用下面的公式：

（3）比力分解

將 比 力fb分 解 到w 系

2.2.3 速度計(jì)算

移動車體沿著軌道行走的過程中，導(dǎo)向機(jī)構(gòu)的導(dǎo)向輪與軌道之間的作用關(guān)系均包含三個階段：1)導(dǎo)向輪全部夾著直線軌道；2)導(dǎo)向輪部分夾著直線軌道，部分夾著彎曲軌道；3)導(dǎo)向輪全部夾著彎曲軌道，如圖5。當(dāng)轉(zhuǎn)向架處于過彎狀態(tài)時，彈簧被壓縮或者拉伸，使得導(dǎo)向輪能夠時刻壓緊軌道并且避免卡死。階段1情況下可以很容易看出導(dǎo)向輪與軌道之間均始終保持壓緊狀態(tài)，彈簧形變量保持不變，所以階段1屬于穩(wěn)定行駛階段。通過幾何法對階段2和階段3情況下導(dǎo)向輪和軌道之間的接觸關(guān)系進(jìn)行分析。

2.2.4 位置坐標(biāo)計(jì)算

式中：vz為高度方向的速度分量的表達(dá)式中含游移方位角α，計(jì)算公式為

2.2.5 地速計(jì)算

游移方位坐標(biāo)系中的速度vw需分解到當(dāng)?shù)厮阶鴺?biāo)系中

2.2.6 姿態(tài)角提取

式如下：

在實(shí)際導(dǎo)航應(yīng)用中，常出現(xiàn)y趨于90°或者r趨于90°的情況，這時采用上式計(jì)算將會出現(xiàn)“奇點(diǎn)”，不但精度受影響，而且計(jì)算不安全.對此，文獻(xiàn)［2］提供了更好的計(jì)算方法.以上計(jì)算過程中涉及到的公式及具體變量可以參閱文獻(xiàn)［1，6－7］.

3 解算實(shí)例

3.1 數(shù)據(jù)來源

3.2 處理流程及結(jié)果

首先使用差分處理軟件進(jìn)行GPS 動態(tài)差分定位處理，可以達(dá)到厘米級的定位精度.然后以GPS差分定位結(jié)果作為標(biāo)準(zhǔn)，分析SINS的誤差特性和精度情況，實(shí)驗(yàn)分以下兩類.

（1）為了分析SINS的誤差特性，將實(shí)驗(yàn)數(shù)據(jù)人為地刪掉了長達(dá)10min的GPS信號.刪掉GPS信號的時段內(nèi)，SINS獨(dú)立進(jìn)行導(dǎo)航計(jì)算.以GPS定位結(jié)果為標(biāo)準(zhǔn)，衡量SINS導(dǎo)航計(jì)算結(jié)果的誤差情況.表2列出了SINS在10min內(nèi)的坐標(biāo)和距離誤差漂移情況，圖3和圖4給出了誤差情況的直觀表達(dá).

圖3 SINS獨(dú)立導(dǎo)航計(jì)算的坐標(biāo)漂移Fig.3 Coordinate drift of SINS

圖4 SINS獨(dú)立導(dǎo)航計(jì)算的距離漂移Fig.4 Distance drift of SINS

（2）為了分析SINS在組合系統(tǒng)中的作用和精度情況，共4 次人為刪掉GPS 信號，每次刪除的時長為1min.采用卡爾曼濾波進(jìn)行差分GPS／SINS松散組合解算，并對組合結(jié)果進(jìn)行卡爾曼平滑.圖5給出了兩段軌跡，其中一段為正常情況下GPS差分定位結(jié)果表示的飛行軌跡，另一段是人為刪除GPS信號后GPS／SINS 松散組合定位結(jié)果表示的飛行軌跡.圖6輸出的是以GPS差分定位結(jié)果為標(biāo)準(zhǔn)衡量的GPS／SINS組合系統(tǒng)誤差情況.

值得說明的是，考慮到圖5 的重點(diǎn)是清晰表達(dá)出GPS信號失鎖時段兩個軌跡的差異，因此輸出的僅僅是飛行過程中10min內(nèi)的軌跡，刪除GPS信號的時間為第1，4，7，10min.實(shí)際上，在GPS信號連續(xù)跟蹤時，兩條軌跡幾乎重合，因此沒必要將2h的飛行軌跡全部畫出.另外，由于該圖只是直觀表達(dá)兩軌跡的差異，并不反映具體的坐標(biāo)信息，因此為了清晰起見，兩條軌跡均以每6s一個點(diǎn)輸出，實(shí)驗(yàn)證明，這樣的輸出間隔使得圖形表達(dá)效果最清晰.

圖5 部分實(shí)驗(yàn)飛行軌跡Fig.5 Part of experimental flight path

基于同樣的考慮，圖6中輸出的僅僅是10min內(nèi)的誤差情況.然而不同的是，該圖能定量反映誤差情況，因此嚴(yán)格按照GPS的頻率每秒一個點(diǎn)輸出.

數(shù)據(jù)處理過程中，卡爾曼濾波方程取15階誤差向量Xk＝［δr，δv，ε，d，b］T作為狀態(tài).其中δr為位置誤差；δv為速度誤差；ε為姿態(tài)誤差；d為陀螺漂移，考慮成一階馬爾科夫過程；b為加速度計(jì)偏置，也考慮成一階馬爾科夫過程.取SINS 與GPS 獨(dú)立解算的坐標(biāo)差值作為濾波器的部分輸入［2］.

卡爾曼平滑采用的是雙向?yàn)V波再平滑的固定區(qū)間平滑方式，可以參閱文獻(xiàn)［2，7－8］.

圖6 GPS／SINS組合導(dǎo)航計(jì)算的坐標(biāo)誤差Fig.6 Coordinate error of GPS／SINS integrated navigation

3.3 精度分析

從表2可以看出，當(dāng)SINS獨(dú)立導(dǎo)航計(jì)算時，誤差增長很快，10min之后距離誤差達(dá)到了400m 以上.從圖3可以看出，坐標(biāo)誤差以拋物線形式的平滑曲線呈非線性增長.其中，高度通道的坐標(biāo)誤差最小，且變化平緩，是因?yàn)槠湔`差僅受高度方向的速度誤差影響，而高度方向的速度誤差主要是由正常重力計(jì)算誤差引起的，這個值相對較小.東向和北向的誤差隨時間推移增長越來越快，其中東向最顯著，其原因是東向坐標(biāo)誤差同時受到東、北、高度3個方向的速度誤差影響，且東向誤差無反饋.但值得說明的是，這3個方向的速度誤差影響并不一定是同向的，因此短時間內(nèi)東向坐標(biāo)誤差增長最快并非必然，從表2中可以看出，在2s以內(nèi)，東向和北向誤差在數(shù)值量級上很接近.

表2 SINS獨(dú)立導(dǎo)航計(jì)算的坐標(biāo)漂移和距離漂移Tab.2 Coordinate drift and distance drift of SINS

從圖4可以發(fā)現(xiàn)，距離誤差增長與坐標(biāo)誤差情況很相似，結(jié)合表2可以看到，1min內(nèi)距離誤差在2m 以內(nèi)，5min達(dá)到約80m，10min后誤差則超過400m，這種變化趨勢呈拋物線形狀，而且是極其平滑的曲線.

圖5中的兩段軌跡顯示，當(dāng)GPS 信號良好時，GPS定位結(jié)果與GPS／SINS組合系統(tǒng)的定位結(jié)果幾乎一致，這時SINS的作用主要表現(xiàn)為數(shù)據(jù)內(nèi)插，解決GPS輸出頻率低的問題，當(dāng)然，SINS的另一作用是輸出高精度的姿態(tài)參數(shù).當(dāng)GPS 信號失鎖時，組合系統(tǒng)的定位結(jié)果開始偏離GPS軌跡，當(dāng)GPS信號重新鎖定后，組合系統(tǒng)的軌跡又保持與GPS 一致.然而整個過程中，組合系統(tǒng)的軌跡始終是平滑的，沒有出現(xiàn)突變的情況，這說明卡爾曼平滑是有效的，特別在GPS 失鎖時段，卡爾曼平滑對于提高GPS／SINS組合導(dǎo)航結(jié)果的精度具有重要作用［2］，也體現(xiàn)出SINS能顯著提高組合系統(tǒng)的完整性和抗干擾性.

圖6定量地表示了GPS／SINS組合系統(tǒng)的誤差情況.從圖中可以看出，當(dāng)GPS信號良好時，組合系統(tǒng)的誤差很小；在GPS信號4 次失鎖的時段內(nèi)，出現(xiàn)了相似的誤差情況，都呈拋物線增長，且誤差最大值點(diǎn)近似出現(xiàn)在GPS信號失鎖時段的中央，東、北、高度3個通道的誤差均小于±0.3m，說明GPS信號失鎖的短時間內(nèi)，SINS仍能提供高精度的定位參數(shù)，充分體現(xiàn)了SINS的可靠性和抗干擾性.

4 結(jié)語

捷聯(lián)慣性導(dǎo)航系統(tǒng)因其獨(dú)特的優(yōu)勢，在軍事和民用領(lǐng)域得到越來越廣泛的應(yīng)用，如何提高精度，一直是慣性技術(shù)界孜孜追求的目標(biāo).本文在介紹捷聯(lián)導(dǎo)航算法之后，通過實(shí)測數(shù)據(jù)的計(jì)算與分析，得出SINS的坐標(biāo)誤差和距離誤差累計(jì)均呈拋物線形式增長，10min內(nèi)距離誤差達(dá)到400 m 以上的結(jié)論.SINS在短時間內(nèi)能提供高精度的導(dǎo)航參數(shù).實(shí)驗(yàn)結(jié)果表明，SINS與GPS的組合系統(tǒng)，不僅能提供豐富的導(dǎo)航信息，更能解決GPS輸出頻率低、信號易受干擾、動態(tài)可靠性差等問題，具有很好的應(yīng)用價(jià)值.

［1］ 秦永元.慣性導(dǎo)航［M］.北京：科學(xué)出版社，2006.QIN Yongyuan.Inertial navigation ［M］.Beijing：Science Press，2006.

［2］ 孫紅星.差分GPS／INS組合定位定姿及其在MMS 中的應(yīng)用［D］.武漢：武漢大學(xué)遙感信息工程學(xué)院，2004.SUN Hongxing.DGPS／INS integrated position and attitude determination and its application in MMS［D］.Wuhan：Wuhan University.School of Remote Sensing and Information Engineering，2004.

［3］ 許江寧，朱濤，卞鴻巍.慣性傳感技術(shù)發(fā)展與展望［J］.海軍工程大學(xué)學(xué)報(bào)，2007，19（3）：1.XU Jiangning，ZHU Tao，BIAN Hongwei.Development and prospects of inertial sensor technology［J］.Journal of Naval University of Engineering，2007，19（3）：1.

［4］ Paul G Savage.Strapdown inertial navigation integration algorithm design ［J］.Journal of Guidance，Control，and Dynamics，1998，21（1）：208.

［5］ 孫麗，秦永元，朱啟舉.捷聯(lián)式慣導(dǎo)系統(tǒng)動態(tài)誤差分析［J］.彈箭與制導(dǎo)學(xué)報(bào)，2007，27（4）：13.SUN Li，QIN Yongyuan，ZHU Qiju.Analysis on the dynamic error of strapdown inertial navigation system［J］.Journal of Projectiles，Rockets，Missiles and Guidance，2007，27（4）：13.

［6］ 董緒榮，張守信，華仲春.GPS／INS 組合導(dǎo)航定位及其應(yīng)用［M］.長沙：國防科技大學(xué)出版，1998.DONG Xurong，ZHANG Shouxin，HUA Zhongchun.GPS／INS integrated navigation and application［M］.Changsha：National University of Defense and Technology Press，1998.

［7］ 袁信，俞濟(jì)樣，陳哲.導(dǎo)航系統(tǒng)［M］.北京：航空工業(yè)出版社，1993.YUAN Xin，YU Jixiang，CHEN Zhe.Navigation system［M］.Beijing：Aviation Industry Press，1993.

［8］ 秦永元，張洪誡，汪叔華.卡爾曼濾波與組合導(dǎo)航原理［M］.西安：西北工業(yè)大學(xué)出版社，1998.QIN Yongyuan，ZHANG Hongjie，WANG Shuhua.Kalman filtering and integrated navigation theory［M］.Xi’an：Northwest Industrial University Press，1998.