CNS+GNSS+INS船載高精度實時定位定姿算法改進研究

程 風，李海霞，常 樂，王 力，牛小驥

(1. 武漢大學衛星導航定位技術研究中心，湖北 武漢 430079； 2. 清華大學導航技術工程中心，北京 100089)

天文導航(CNS)是一種利用光學探測設備測得恒星、近天體的信息來解算獲得載體精確姿態的自主導航技術，具有測量精度高、無誤差累積等優點，已成為船載高精度姿態測量的一種主要手段[1]。但是其容易受天氣環境的影響，在多云或缺星環境下通常無法獨立完成導航定姿的任務。因此,將GNSS+INS組合導航系統與CNS結合起來，可提供無縫的高精度定位定姿結果，并有效保障天文導航在具備可觀測天文條件下的搜星速度和實時輸出星敏定姿結果，大大提高了航行過程中導航結果的精度和穩健性[2-4]。

目前，基于CNS+GNSS+INS的數據融合算法研究較多，但是大部分都是基于仿真試驗數據進行的，沒有進行實物驗證[5-7]，更沒有考慮實際工程中存在以下難點：①INS長時間姿態誤差累積導致其提供給CNS的粗姿態誤差較大，影響星敏感器搜星快速性和天文導航精度[8-9]；②系統硬件傳輸存在不可忽略的時間延遲。如IMU輸出由于激光陀螺的去抖濾波等原因而帶有毫秒級的延遲等，尤其在惡劣海況下載體處于較大機動時，這種延遲會導致天文丟星及無法搜星的情況，從而影響艦船長時間航行時導航定姿的精度和穩定性。本文針對以上實際工程問題，提出一種降低系統定姿延遲影響的CNS+GNSS+INS高精度實時導航框架和算法[10]，并通過高精度轉臺測試，驗證本文方法的精度和有效性。

1 CNS+GNSS+INS三組合濾波算法

1.1 CNS+GNSS+INS三組合狀態方程

由于系統對姿態精度要求較高，因此使用集中式卡爾曼融合濾波器進行處理，并將狀態估計結果反饋至慣性導航系統內部。本文設計了狀態量維數為21維的卡爾曼濾波器，其最優估計參數由IMU的誤差狀態構成[11]，表示如下

(1)

式中，δrn、δvn和ψ分別為慣性導航位置誤差、速度誤差和姿態旋轉誤差在導航坐標系下的投影；bg、sg分別表示陀螺零偏和比例因子；ba、sa分別表示加速度計零偏和比例因子。

采用Psi角模型[12]建立慣性導航誤差模型如下

(2)

式中，n表示導航坐標系；i表示慣性坐標系；e表示地心地固坐標系；b系表示載體坐標系。

將高精度激光慣導的零偏和比例因子誤差模型考慮為一階高斯-馬爾科夫過程[13]

(3)

式中，wba和wsa分別表示加速度計零偏和比例因子的驅動白噪聲；wbg和wsg分別表示陀螺的零偏和比例因子的驅動白噪聲；τba和τsa分別表示加速度計零偏和比例因子誤差的相關時間；τbg和τsg分別表示陀螺零偏和比例因子誤差的相關時間。

通過上述式(2)和式(3)，結合估計的誤差狀態量式(1)，建立卡爾曼濾波的離散化狀態方程如下

xk=Φk,k-1xk-1+Γk-1Wk-1

(4)

式中，Φk,k-1為k-1時刻到k時刻的狀態轉移矩陣，表達式可以參考文獻[14]；xk-1和xk分別為k-1到k時刻的狀態估計向量；Γk-1為系統的噪聲驅動陣；Wk-1為系統狀態的噪聲向量。

1.2 GNSS和CNS觀測方程

若GNSS數據可用時，以INS位置結果和GNSS位置結果之差δzr作為觀測向量，er為位置觀測噪聲向量。當CNS數據可用時，有INS預測的姿態與CNS輸出姿態結果之差δzΦ，eΦ為姿態觀測噪聲，得到基于Psi角模型的位置和姿態量測方程[15]

(5)

由式(5)可得其位置與姿態的觀測模型為

(6)

1.3 針對CNS解算中INS數據時間延遲的處理

在船載基準系統運行過程中，天文系統搜星所需要的姿態是實時姿態估計，不應有延時。而由于系統設計實現的多種相關原因，提供給天文系統的姿態信息帶有不可忽略的時間延遲，包括慣導的IMU輸出由于激光陀螺的去抖濾波等原因而帶有大約10 ms的延遲、慣導數據的解算延遲等，實測總延遲約20.5 ms。在海況較為惡劣時，假設船體的搖擺幅值為6°，周期15 s，此時由時間延遲造成CNS接收到的慣組姿態誤差最大可以達到3′，使星跟蹤器接收到的用于隔離載體姿態運動的角度跟蹤指令不能有效反映當前時刻載體的運動情況，無法實現有效的載體運動隔離，星跟蹤器跟蹤到位后，恒星無法穩定地出現在相機的視場范圍內，造成天文系統無法搜星。

因此，在解算得到GNSS+INS組合姿態、位置結果時，對導航結果作外推處理后再發送給天文系統實現星敏相機對某個恒星的觀測。以航向姿態φ為例，具體外推采用線性外推方案如下(等角速度外推)

(7)

2 系統框架設計

本文設計的CNS+GNSS+INS信息融合定姿框架如圖1所示。

以INS作為核心，采用集中卡爾曼濾波器進行三組合導航測量。衛星、天文導航結果用于抑制慣性導航誤差積累，同時實時修正慣導的誤差，以提高系統的穩健性。當天文導航因觀測條件限制而無法使用時，通過衛星和慣性組合的方式保障短時間段內航姿測量精度和方位測量精度，實現可持續性的定位測姿。另外，由于系統硬件(激光陀螺的去抖濾波)和傳輸處理所帶來的延遲，GNSS+INS子濾波器輸出給CNS的姿態有延時，本文對該姿態進行等角速度外推補償后再發送給CNS使用，這樣可以有效解決在艦船處于較大動態時CNS容易丟星不工作的情況，從而增加CNS系統的抗干擾能力。CNS實現高精度姿態測量后，在卡爾曼主濾波器中與INS數據進行融合，從而輸出三組合的高精度姿態估計值。

3 試驗結果與分析

本文試驗中采用的INS設備為高精度激光陀螺(性能參數參見表1)，采樣率為1000 Hz，輸出載體的加速度和角速度信息；GNSS設備選用Trimble AG332 GPS接收機，采樣率為1 Hz，輸出天文的位置信息；CNS設備由光學鏡頭、測星相機組件等組成(性能參數參見表2)，輸出被測星體脫靶量數據及Camlink數字視頻圖像。

表1 高精度激光陀螺的主要性能參數

表2 星敏感器的主要性能參數

為了驗證該系統的實時定姿精度，實測試驗需要將設備安裝在姿態精度在角秒級別的高精度轉臺上進行，轉臺外環轉動對應著設備的航向角變化，內環轉動對應著設備的橫滾角變化。測試現場如圖2所示。

3.1 算法有效性驗證

為驗證CNS+GNSS+INS三組合算法的有效性，將設備安裝到高精度轉臺上進行測試，試驗測試時間為7700 s，測試環境為開闊且晴朗的天空，在此期間GNSS和CNS均正常工作。設置搖擺臺外環以幅值6°，周期15 s。為保障試驗環境的一致性，在組合導航解算程序中設置一個獨立的進程來實時解算GNSS+INS組合的姿態結果，并以高精度轉臺的姿態輸出作為參考真值，來評估三組合算法高精度實時定姿的性能。由于轉臺外環主要針對設備的Z軸進行旋轉測試，測試過程中設備的橫滾角與俯仰角動態變化過小，GNSS+INS組合與CNS+GNSS+INS組合解算結果差異不明顯，因此本文試驗主要對設備輸出的航向姿態誤差進行分析，兩種組合方式得到的航向姿態誤差如圖3所示。

從圖3中可以看出，使用CNS+GNSS+INS組合方式的姿態誤差曲線范圍明顯穩定在±10″以內，航向姿態沒有漂移。使用GNSS+INS組合方式的姿態誤差曲線有明顯的漂移，在2 h后約漂移30″。分析表3中統計結果可知，三組合方式中CNS的加入有效抑制了航向發散，航向誤差平均值為-0.619″，均方根為5.572″，明顯優于GNSS+INS組合結果。

表3 誤差統計結果 (″)

3.2 驗證INS延遲處理后CNS+GNSS+INS的姿態精度

為驗證INS延遲處理的效果，本文利用高精度轉臺進行了兩次測試。第一次測試使用無INS延遲處理的程序(方案1)，測試時間為16 000 s，測試環境為開闊且晴朗的天空，先靜止設備約15 min之后搖擺臺外環一直以幅值6°，周期10 s進行大角速率搖擺來模擬載體復雜機動環境。由于搖擺測試時間較長，截取測試開始后850～1150 s的轉臺外環搖擺角度變化圖，如圖4所示。第二次測試使用本文給出的改進INS延遲的程序(方案2)，其余測試條件與第一次測試保持一致。兩次測試試驗各以每次試驗高精度轉臺的姿態輸出作為參考真值，來評估三組合算法進行INS延遲處理后實時定姿的性能，得到兩者結果對比如圖5所示。圖5(a)為未進行延遲處理(方案1)的CNS+GNSS+INS航向姿態誤差結果，(b)為進行延遲處理后(方案2)的CNS+GNSS+INS航向姿態誤差結果，表4為方案1與方案2的航向誤差統計結果。

116.11640.40119.4882-4.76222.3466.587

分析以上圖表可以看出：未進行延遲處理的三組合在開始轉臺靜止的15 min內航向誤差穩定，轉臺外環轉動后則出現了航向姿態發散，誤差均值高達16.116″，均方根為19.488″。這是因為在大角速率搖擺下，慣導提供的姿態角延時不能忽略不計，此時星敏感器接收到的用于隔離載體姿態運動的角度跟蹤指令不能有效反映當前時刻載體的運動情況，無法實現有效的載體運動隔離，導致恒星無法穩定地出現在相機的視場范圍內，CNS無法穩定正常的工作。此時，無外推的三組合退化為GNSS+INS兩組合結果，航向姿態誤差逐漸積累。而在同樣的搖擺條件下，加入延遲處理的三組合航向姿態誤差得到了有效的控制，誤差一直穩定在10″以內，均值為-4.762″，均方根為6.587″，滿足高精度姿態測量需求。

4 結 語

本文在GNSS+INS組合導航基礎上，加入CNS的星敏姿態信息對慣導進行姿態修正，設計和實現了一套CNS+GNSS+INS三組合定位定姿架構和算法，以滿足船載測量任務對高精度姿態信息的需求。針對惡劣海況下CNS+GNSS+INS三組合系統中，因INS的硬件延遲、計算延遲等造成的CNS搜星和跟星不穩定問題，提出了一種對INS預測姿態進行等角速度外推的處理方法。最后利用高精度轉臺模擬載體復雜運動環境，對該系統進行了實測驗證評估。試驗結果表明：引入INS姿態外推方法能夠有效維持星敏相機連續穩定工作，保障了三組合定位定姿系統的姿態精度和穩定性。