CNS/SINS姿態(tài)組合導(dǎo)航系統(tǒng)仿真

（海軍航空工程學(xué)院電子信息工程系，山東煙臺264000）

慣性導(dǎo)航系統(tǒng)（Inertial Navigation System，INS）以其短時(shí)精度高，可以連續(xù)地輸出位置、速度、姿態(tài)信息，以及完全自主等突出優(yōu)點(diǎn)，已被各種類型的軍用飛行器普遍采用[1-5]。由于陀螺等慣性測量單元（Inertial Measurement Unit，IMU）的漂移或誤差使導(dǎo)航輸出誤差隨時(shí)間積累難以長時(shí)間獨(dú)立工作，因而需要增加其他外部量測信息，利用組合的方法提高綜合導(dǎo)航性能。

天文導(dǎo)航系統(tǒng)（Celestial Navigation System，CNS）主要特點(diǎn)有：被動探測、隱蔽性好、不受電磁干擾、可靠性高、自主工作、不依賴其他導(dǎo)航手段、精度高、在慣性坐標(biāo)系下，測量誤差也不隨時(shí)間而積累。其中，定向精度最高，而定位精度也僅次于全球定位系統(tǒng)（Global Positioning System，GPS），可全天候工作。天文導(dǎo)航作為一種隱蔽、無源、被動的導(dǎo)航手段，不存在無線電導(dǎo)航系統(tǒng)在惡劣戰(zhàn)場電磁環(huán)境條件下易受到干擾的問題，在戰(zhàn)時(shí)將具有極高的軍事應(yīng)用價(jià)值[6]。采用CNS/INS組合克服了各自缺點(diǎn)，取長補(bǔ)短，構(gòu)成了一種比較理想的組合導(dǎo)航系統(tǒng)。

目前，CNS/INS組合導(dǎo)航系統(tǒng)有多種工作模式，根據(jù)星敏感器和慣性器件安裝方式的不同，可分為全平臺式、平臺慣性導(dǎo)航系統(tǒng)與捷聯(lián)星敏感器模式、捷聯(lián)慣性導(dǎo)航系統(tǒng)與平臺星敏感器模式以及全捷聯(lián)模式4種[7]。由于制造成本、可靠性、測量精度、未來發(fā)展趨勢等因素，全捷聯(lián)模式的天文/慣性組合導(dǎo)航系統(tǒng)更有發(fā)展前景，是天文/慣性組合導(dǎo)航系統(tǒng)的重要發(fā)展方向之一，為此本文的以下內(nèi)容都將以全捷聯(lián)模式為例。

星敏感器測量輸出的慣性系下姿態(tài)信息，在其轉(zhuǎn)換為地理系下的載體姿態(tài)信息之前，無法直接應(yīng)用于實(shí)際的地理坐標(biāo)系導(dǎo)航。

本文對地理系下姿態(tài)觀測原理進(jìn)行了充分闡述，分析并建立了地理系下姿態(tài)線性化量測方程及狀態(tài)方程。

1 天文/慣性姿態(tài)組合導(dǎo)航算法研究

1.1 地理系下姿態(tài)觀測原理

天文/慣性組合導(dǎo)航系統(tǒng)比純慣導(dǎo)系統(tǒng)的精度高是因?yàn)樵趹T性空間里恒星的方位基本保持不變，盡管星敏感器的像差、地球極軸的進(jìn)動和章動以及視差等因素使恒星方向有微小的變化，但是它們所造成的資態(tài)誤差小于1″，因而星敏感器就相當(dāng)于沒有漂移的陀螺，所以可以用天文量測信息修正慣性器件誤差[8]。但是，現(xiàn)在大部分導(dǎo)航平臺使用地理坐標(biāo)系進(jìn)行導(dǎo)航，天文導(dǎo)航在地理坐標(biāo)系下姿態(tài)解算依賴導(dǎo)航位置而容易發(fā)生精度發(fā)散，為此需要研究地理坐標(biāo)系下天文/慣性姿態(tài)組合導(dǎo)航系統(tǒng)，來提高導(dǎo)航精度。

由于慣性組合導(dǎo)航系統(tǒng)中與姿態(tài)誤差相關(guān)的狀態(tài)誤差通常選擇為平臺誤差角(φE,φN,φU)，但其并不等同于姿態(tài)誤差角。為實(shí)現(xiàn)姿態(tài)誤差觀測量到平臺誤差角之間的轉(zhuǎn)換，目前有2種主要方法：一種是采用近似的方式，將姿態(tài)誤差和平臺誤差看為等價(jià)關(guān)系；另一種則是將姿態(tài)測量誤差轉(zhuǎn)換為平臺誤差角，然后再進(jìn)行組合濾波。但就這2種方法本質(zhì)而言，均具有其不足之處：如第1種方法具有建模的近似性，會帶來相應(yīng)的建模誤差；而第2種方法將姿態(tài)誤差測量轉(zhuǎn)換為平臺誤差角測量，也會帶來相應(yīng)的轉(zhuǎn)換誤差。

由以上原因，本文為克服上述2種姿態(tài)組合算法的不足，進(jìn)一步提高組合系統(tǒng)精度，采用直接建立姿態(tài)誤差觀測量和系統(tǒng)狀態(tài)量之間的線性化量測方程的方法。

在全捷聯(lián)工作模式下，由星敏感器輸出的姿態(tài)信息可以得到載體的三軸姿態(tài)信息（俯仰角θS、航向角ψS、滾轉(zhuǎn)角γS），而慣性導(dǎo)航系統(tǒng)通過慣導(dǎo)解算也會給出載體的三軸姿態(tài)信息（俯仰角θI、航向角ψI、滾轉(zhuǎn)角γI）,因此，將兩者相減可得到載體的三軸姿態(tài)誤差角Δε為式（1）。為直接獲得線性化的姿態(tài)觀測方程，做如下理論推導(dǎo)：

1.1.1 姿態(tài)觀測關(guān)系分析

式（2）是根據(jù)姿態(tài)角度定義，導(dǎo)航坐標(biāo)系下的理想姿態(tài)轉(zhuǎn)換矩陣[9]，上標(biāo)n表示導(dǎo)航坐標(biāo)系，下標(biāo)b表示載體本體坐標(biāo)系；γ、θ、ψ分別表示載體相對于地理系下的滾動、俯仰和航向理想姿態(tài)信息。定義由慣性導(dǎo)航模擬的導(dǎo)航平臺坐標(biāo)系東向、北向和方位平臺誤差角分別為φE、φN、φU。則利用慣性導(dǎo)航計(jì)算輸出姿態(tài)和平臺誤差角，也可以獲得相對于導(dǎo)航坐標(biāo)系下的理想姿態(tài)轉(zhuǎn)換矩陣：

式（3）中，上標(biāo)c1表示通過慣性導(dǎo)航計(jì)算獲得的導(dǎo)航平臺坐標(biāo)系。由式（2）和式（3）可以獲得上面計(jì)算等式（4）。式（4）反映了慣性導(dǎo)航計(jì)算輸出姿態(tài)角、平臺誤差角和理想姿態(tài)之間的相互關(guān)系，可以利用上述等式關(guān)系獲得組合觀測量Δγ、Δθ、Δψ的計(jì)算表達(dá)式。但由于其為非線性方程，而為采用線性卡爾曼濾波器，還必須直接獲得組合系統(tǒng)的線性化量測方程，則必須利用上述關(guān)系求解出組合觀測量Δγ、Δθ、Δψ，并建立其和狀態(tài)量之間的線性量測關(guān)系。

1.1.2 角度觀測方程建立

利用在小角度姿態(tài)偏差情況下，Δγ、Δθ、Δψ為小量的假設(shè)以及三角函數(shù)關(guān)系，則有：

此外，對式（3）中的Cc1b可做如下簡化表達(dá)：

由式（6）和（4）化簡得式（7）。利用式（7）中矩陣對應(yīng)元素相等的關(guān)系，即可獲得姿態(tài)角度觀測量Δγ、Δθ、Δψ。式（7）中部分元素與計(jì)算無關(guān)，用“…”表示。由前文提到的，星敏感器提供的姿態(tài)信息誤差非常小，可近似為載體的姿態(tài)，則有如下的矩陣元素對應(yīng)關(guān)系式（8）。將式（5）代入式（8），經(jīng)過簡化，可以獲得最終的姿態(tài)角度線性化量測方程為式（9）：

1.2 系統(tǒng)狀態(tài)方程

采用線性卡爾曼濾波器進(jìn)行組合，系統(tǒng)的狀態(tài)方程為慣性導(dǎo)航系統(tǒng)的誤差方程，通過對慣性導(dǎo)航系統(tǒng)的性能及誤差源的分析獲得慣導(dǎo)系統(tǒng)的誤差方程為：

式中系統(tǒng)狀態(tài)變量定義參考文獻(xiàn)[9-10]。可對上述連續(xù)形式的系統(tǒng)狀態(tài)方程和量測方程進(jìn)行離散化處理，采用線性離散卡爾曼濾波器進(jìn)行濾波估計(jì)[9]。

1.3 IMU輸出信息的仿真模型

載體飛行軌跡發(fā)生器作為組合導(dǎo)航仿真系統(tǒng)研究的基礎(chǔ)[11-14]，用來產(chǎn)生IMU 在地理坐標(biāo)系下的原始輸出。本文根據(jù)設(shè)定的軌跡信息（初始位置、速度和姿態(tài)及不同時(shí)段內(nèi)速度和姿態(tài)變化率），由軌跡方程得到地理坐標(biāo)系下的位置、速度、姿態(tài)；在地理坐標(biāo)系下，根據(jù)運(yùn)動學(xué)關(guān)系反推到IMU輸出的比力和角速率信息。而實(shí)際中慣性器件的輸出總是存在誤差的，IMU 誤差主要包括隨機(jī)誤差和固定誤差。陀螺與加速度計(jì)的隨機(jī)誤差均包括白噪聲、一階馬爾科夫過程隨機(jī)噪聲誤差、等效常值漂移誤差；陀螺的固定誤差包括：陀螺標(biāo)度因數(shù)誤差，陀螺安裝誤差。加速度計(jì)的固定誤差包括：加速度計(jì)標(biāo)度因數(shù)誤差，加速度計(jì)安裝誤差，加速度計(jì)二次項(xiàng)系數(shù)誤差[2，14]。

本文仿真中，卡爾曼濾波算法采用間接法反饋校正濾波器結(jié)構(gòu)，算法遞推公式參考文獻(xiàn)[9，10，15]。建立仿真流程框圖如圖1所示。

圖1 天文/慣性姿態(tài)組合導(dǎo)航仿真流程圖Fig.1 Simulation flow chart of CNS/SINS posture integrated navigation

在本文的仿真流程圖中的各模塊功能：

1）航跡解算模塊輸出載體在載體坐標(biāo)系下，隨時(shí)間變化的姿態(tài)、速度、加速度。

2）SINS 數(shù)據(jù)解算模塊根據(jù)給定的IMU 誤差和航跡信息，算出地理坐標(biāo)系下的載體位置、速度和姿態(tài)。

3）天文姿態(tài)的計(jì)算利用了慣導(dǎo)給出的位置與航跡解算出的位置、姿態(tài)信息。

考慮到由格林尼治恒星時(shí)角GAST可獲得慣性坐標(biāo)系到地球固聯(lián)坐標(biāo)系的變換矩陣為Cei；再利用慣導(dǎo)提供的地理經(jīng)緯度信息，可獲得地球固聯(lián)坐標(biāo)系到導(dǎo)航坐標(biāo)系的變換矩陣Cen。其中，Cie和Cen的計(jì)算公式如式（11）和（12）所示：

式（11）中，下標(biāo)i表示慣性坐標(biāo)系，上標(biāo)e表示地球固連坐標(biāo)系。則有載機(jī)相對于導(dǎo)航坐標(biāo)系的姿態(tài)變換矩陣：

上式即機(jī)體系相對于導(dǎo)航系的3個姿態(tài)角，其中Ci,j表示矩陣中下標(biāo)為(i,j)的元素。

4）卡爾曼濾波反饋不但能夠調(diào)整IMU的誤差修正值，還能修正導(dǎo)航輸出的位置，速度和姿態(tài)信息。

2 天文/慣性姿態(tài)組合導(dǎo)航系統(tǒng)仿真

2.1 仿真條件設(shè)置

假設(shè)飛機(jī)做機(jī)動飛行，其飛行航跡如圖2所示，含爬升、變速、平飛和轉(zhuǎn)彎等各種飛行狀態(tài)。仿真過程中慣導(dǎo)系統(tǒng)的采樣周期為0.02 s；CNS 數(shù)據(jù)采樣周期為1 s；Kalman濾波信息融合周期為1 s。慣導(dǎo)系數(shù)誤差參數(shù)見表1所示。

圖2 仿真飛行航跡Fig.2 Simulation flying track

表1 慣導(dǎo)誤差參數(shù)Tab.1 Error parameters of SINS

2.2 仿真結(jié)果與分析

對相同的航跡、慣導(dǎo)誤差仿真數(shù)據(jù)，本文進(jìn)行了純慣導(dǎo)和天文/慣性組合導(dǎo)航的仿真，其中圖3～5分別給出了純慣導(dǎo)與天文/慣性導(dǎo)航的位置誤差、速度誤差、姿態(tài)誤差的比較。圖中，SINS 所指曲線為純慣導(dǎo)誤差曲線；CNS/SINS 所指曲線為天文/慣性組合導(dǎo)航誤差曲線。

圖3 純慣導(dǎo)與天文/慣性組合導(dǎo)航位置誤差比較Fig.3 Comparison of position error between SINS with CNS/SINS

圖4 純慣導(dǎo)與天文/慣性組合導(dǎo)航速度誤差比較Fig.4 Comparison of velocity error between SINS with CNS/SINS

圖5 純慣導(dǎo)與天文/慣性組合導(dǎo)航姿態(tài)誤差比較Fig.5 Comparison of posture error between SINS with CNS/SINS

從仿真結(jié)果可以得出如下結(jié)論：

1）純慣導(dǎo)與天文/慣性組合導(dǎo)航在導(dǎo)航的初始階段，誤差都比較低。由于IMU的漂移和誤差隨時(shí)間積累，純慣導(dǎo)的精度發(fā)散比較快，而組合導(dǎo)航比較緩慢，證實(shí)了天文/慣性姿態(tài)組合導(dǎo)航的效果。

2）當(dāng)慣導(dǎo)與天文導(dǎo)航進(jìn)行姿態(tài)組合后在短期內(nèi)可認(rèn)為是收斂的，但當(dāng)仿真時(shí)間較長后，各導(dǎo)航參數(shù)呈現(xiàn)出非常慢的發(fā)散趨勢，原因是CNS的姿態(tài)的解算依賴于導(dǎo)航的位置。

3）經(jīng)過CNS 與SINS組合后，各導(dǎo)航參數(shù)都得到了明顯提高，但高度通道改善的效果有待提高。

4）濾波器能有效估計(jì)陀螺及加速度計(jì)各項(xiàng)誤差。

3 結(jié)論

本文首先在分析CNS/SINS組合導(dǎo)航系統(tǒng)的基礎(chǔ)上建立了系統(tǒng)各模塊的數(shù)學(xué)模型，并基于Matlab軟件開發(fā)出仿真系統(tǒng)。該系統(tǒng)能夠?qū)崿F(xiàn)設(shè)定軌跡的IMU和CNS數(shù)據(jù)的仿真，并且利用原始數(shù)據(jù)進(jìn)行姿態(tài)組合導(dǎo)航運(yùn)算，本文的仿真結(jié)果驗(yàn)證了該仿真系統(tǒng)的實(shí)用性；然后，介紹了CNS/SINS 姿態(tài)組合系統(tǒng)的融合算法；最后，根據(jù)濾波實(shí)際設(shè)定組合導(dǎo)航系統(tǒng)各項(xiàng)參數(shù)并獲得仿真數(shù)據(jù)。

純慣導(dǎo)與天文/慣性組合導(dǎo)航在導(dǎo)航的初始階段，誤差都比較低。由于IMU的漂移和誤差隨時(shí)間積累，純慣導(dǎo)的精度發(fā)散比較快，而組合導(dǎo)航比較緩慢，證實(shí)了天文/慣性姿態(tài)組合導(dǎo)航的效果。經(jīng)過CNS 與SINS組合后，位置精度、速度精度與姿態(tài)精度都得到了明顯提高，但改變不了其發(fā)散的趨勢，原因是CNS的姿態(tài)的解算依賴于導(dǎo)航的位置；同時(shí)，高度通道改善的效果有待提高。為了進(jìn)一步提高位置及速度的濾波精度，可研究精度更高的量測方程，或加入GPS參與組合。

[1]曾偉，林訓(xùn)超，曾友州，等.組合導(dǎo)航技術(shù)的發(fā)展趨勢[J].成都航空職業(yè)技術(shù)學(xué)報(bào)，2011，27（2）：41-44.

ZENG WEI，LIN XUNCHAO，ZENG YOUZHOU，et al.The development tendency of the integrated navigation technology[J].Chengdu Aeronautical Vocational and Technical Journals，2011，27（2）：41-44.（in Chinese）

[2]張光天，王秀萍，王麗霞.捷聯(lián)慣性導(dǎo)航技術(shù)[M].北京：國防工業(yè)出版社，2010：9-10，106-109.

ZHANG GUANGTIAN，WANG XIUPING，WANG LIXIA.Strapdown inertial navigation technology[M].Beijing：National Defence Industry Press，2010：9-10，106-109.（in Chinese）

[3]PHILIPS CRAIG A.Guidance algorithm for range maximization and time-of-flight control of guided projectile[J].Journal of Guidance，Control，and Dynamics，2008，31（5）：1447.

[4]曹巖，趙家勝，王偉，等.彈載捷聯(lián)慣性導(dǎo)航系統(tǒng)算法及仿真[J].西安工業(yè)大學(xué)學(xué)報(bào)，2011，31（3）：231-235.

CAO YAN，ZHAO JIASHENG，WANG WEI，et al.Algorithm for strapdown inertial navigation system and its simulation[J].Journal of Xi'an Technological University，2011，31（3）：231-235.（in Chinese）

[5]劉莉娜，劉任慶.基于捷聯(lián)慣性導(dǎo)航的組合導(dǎo)航系統(tǒng)研究[J].現(xiàn)代電子技術(shù)，2009（3）：111-113.

LIU LINA，LIU RENQING.Reserch of integrated navigation system based on strapdown inertial navigation system[J].Modern Electronics Technique，2009（3）：111-113.（in Chinese）

[6]張宗麟.慣性導(dǎo)航與組合導(dǎo)航[M].北京：航空工業(yè)出版社，2000：20-24.

ZHANG ZONGLIN.Inertial navigation and integrated navigation[M].Beijing：Aviation Industry Press，2000：20-24.（in Chinese）

[7]全偉，劉百奇，宮曉琳，等.慣性/天文/衛(wèi)星組合導(dǎo)航技術(shù)[M].北京：國防工業(yè)出版社，2011：177-179.

QUAN WEI，LIU BAIQI，GONG XIAOLIN，et al.INS/CNS/GPS integrated navigation technique[M].Beijing：National Defense Industry Press，2011：177-179.（in Chinese）

[8]房建成，寧曉琳.天文導(dǎo)航原理及應(yīng)用[M].北京：北京航空航天大學(xué)出版社，2006：266-267.

FANG JIANCHENG，NING XIAOLIN.The principle and application of the celestial navigation[M].Beijing：Beihang University Press，2006：266-267.（in Chinese）

[9]袁信，俞濟(jì)祥，陳哲.導(dǎo)航系統(tǒng)[M].北京：航空工業(yè)出版社，1993：137-140.

YUAN XIN，YU JIXIANG，CHEN ZHE.Navigation system[M].Beijing：Aviation Industry Press，1993：137-140.（in Chinese）

[10]秦永元，張洪鉞，汪叔華.卡爾曼濾波與組合導(dǎo)航原理[M].西安：西北工業(yè)大學(xué)出版社，1998：33-56.

QIN YONGYUAN，ZHANG HONGYUE，WANG SHUHUA.Kalman filter and the principle of the integrated navigation[M].Xi’an：Northwestern Polytechnical University Press，1998：33-56.（in Chinese）

[11]卞鴻巍，李安，譚方君，等.現(xiàn)代信息融合技術(shù)在導(dǎo)航中的應(yīng)用[M].北京：國防工業(yè)出版社，2010：97-100.

BIAN HONGWEI，LI AN，TAN FANGJUN，et al.The application of modern information fusion technique in navigation[M].Beijing：National Defense Industry Press，2010：97-100.（in Chinese）

[12]賴際舟，于永軍，熊智，等.慣性/天文深組合非線性定位算法[J].控制與決策，2012，27（11）：1649-1652.

LAI JIZHOU，YU YONGJUN，XIONG ZHI，et al.SINS/CNS tightly integrated navigation positioning algorithm with nonlinear filter[J].Control and Decision，2012，27（11）：1649-1652.（in Chinese）

[13]屈薔，劉建業(yè)，熊智，等.機(jī)載天文/慣性位置組合導(dǎo)航[J].南京理工大學(xué)學(xué)報(bào)：自然科學(xué)版，2010，34（6）：729-732.

QU QIANG，LIU JIANYE，XIONG ZHI，et al.Airborne SINS/CNS location integrated system[J].Journal of Nanjing University of Science and Technology：Natural Science，2010，34（6）：729-732.（in Chinese）

[14]寧曉琳，蔡洪煒，吳偉仁，等.月球車的慣性/天文組合導(dǎo)航新方法[J].系統(tǒng)工程與電子技術(shù)，2011，33（8）：1837-1844.

NING XIAOLIN，CAI HONGWEI，WU WEIREN，et al.INS/CNS integrated navigation method for lunar rover[J].System Engineering and Electronics，2011，33（8）：1837-1844.（in Chinese）

[15]熊智，劉建業(yè)，郁豐，等.基于天文角度觀測的機(jī)載慣性/天文組合濾波算法研究[J].宇航學(xué)報(bào)，2010，31（2）：397-403.

XIONG ZHI，LIU JIANYE，YU FENG，et al.Research of airborne INS/CNS integrated filtering Algorithm based on celestial angle observation[J].Journal of Astronautics，2010，31（2）：397-403.（in Chinese）