CNS+GNSS+INS船載高精度實(shí)時(shí)定位定姿算法改進(jìn)研究

程 風(fēng)，李海霞，常 樂，王 力，牛小驥

(1. 武漢大學(xué)衛(wèi)星導(dǎo)航定位技術(shù)研究中心，湖北 武漢 430079； 2. 清華大學(xué)導(dǎo)航技術(shù)工程中心，北京 100089)

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

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

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

1.1 CNS+GNSS+INS三組合狀態(tài)方程

由于系統(tǒng)對姿態(tài)精度要求較高，因此使用集中式卡爾曼融合濾波器進(jìn)行處理，并將狀態(tài)估計(jì)結(jié)果反饋至慣性導(dǎo)航系統(tǒng)內(nèi)部。本文設(shè)計(jì)了狀態(tài)量維數(shù)為21維的卡爾曼濾波器，其最優(yōu)估計(jì)參數(shù)由IMU的誤差狀態(tài)構(gòu)成[11]，表示如下

(1)

式中，δrn、δvn和ψ分別為慣性導(dǎo)航位置誤差、速度誤差和姿態(tài)旋轉(zhuǎn)誤差在導(dǎo)航坐標(biāo)系下的投影；bg、sg分別表示陀螺零偏和比例因子；ba、sa分別表示加速度計(jì)零偏和比例因子。

采用Psi角模型[12]建立慣性導(dǎo)航誤差模型如下

(2)

式中，n表示導(dǎo)航坐標(biāo)系；i表示慣性坐標(biāo)系；e表示地心地固坐標(biāo)系；b系表示載體坐標(biāo)系。

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

(3)

式中，wba和wsa分別表示加速度計(jì)零偏和比例因子的驅(qū)動(dòng)白噪聲；wbg和wsg分別表示陀螺的零偏和比例因子的驅(qū)動(dòng)白噪聲；τba和τsa分別表示加速度計(jì)零偏和比例因子誤差的相關(guān)時(shí)間；τbg和τsg分別表示陀螺零偏和比例因子誤差的相關(guān)時(shí)間。

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

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

(4)

式中，Φk,k-1為k-1時(shí)刻到k時(shí)刻的狀態(tài)轉(zhuǎn)移矩陣，表達(dá)式可以參考文獻(xiàn)[14]；xk-1和xk分別為k-1到k時(shí)刻的狀態(tài)估計(jì)向量；Γk-1為系統(tǒng)的噪聲驅(qū)動(dòng)陣；Wk-1為系統(tǒng)狀態(tài)的噪聲向量。

1.2 GNSS和CNS觀測方程

若GNSS數(shù)據(jù)可用時(shí)，以INS位置結(jié)果和GNSS位置結(jié)果之差δzr作為觀測向量，er為位置觀測噪聲向量。當(dāng)CNS數(shù)據(jù)可用時(shí)，有INS預(yù)測的姿態(tài)與CNS輸出姿態(tài)結(jié)果之差δzΦ，eΦ為姿態(tài)觀測噪聲，得到基于Psi角模型的位置和姿態(tài)量測方程[15]

(5)

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

(6)

1.3 針對CNS解算中INS數(shù)據(jù)時(shí)間延遲的處理

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

因此，在解算得到GNSS+INS組合姿態(tài)、位置結(jié)果時(shí)，對導(dǎo)航結(jié)果作外推處理后再發(fā)送給天文系統(tǒng)實(shí)現(xiàn)星敏相機(jī)對某個(gè)恒星的觀測。以航向姿態(tài)φ為例，具體外推采用線性外推方案如下(等角速度外推)

(7)

2 系統(tǒng)框架設(shè)計(jì)

本文設(shè)計(jì)的CNS+GNSS+INS信息融合定姿框架如圖1所示。

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

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

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

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

表2 星敏感器的主要性能參數(shù)

為了驗(yàn)證該系統(tǒng)的實(shí)時(shí)定姿精度，實(shí)測試驗(yàn)需要將設(shè)備安裝在姿態(tài)精度在角秒級(jí)別的高精度轉(zhuǎn)臺(tái)上進(jìn)行，轉(zhuǎn)臺(tái)外環(huán)轉(zhuǎn)動(dòng)對應(yīng)著設(shè)備的航向角變化，內(nèi)環(huán)轉(zhuǎn)動(dòng)對應(yīng)著設(shè)備的橫滾角變化。測試現(xiàn)場如圖2所示。

3.1 算法有效性驗(yàn)證

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

從圖3中可以看出，使用CNS+GNSS+INS組合方式的姿態(tài)誤差曲線范圍明顯穩(wěn)定在±10″以內(nèi)，航向姿態(tài)沒有漂移。使用GNSS+INS組合方式的姿態(tài)誤差曲線有明顯的漂移，在2 h后約漂移30″。分析表3中統(tǒng)計(jì)結(jié)果可知，三組合方式中CNS的加入有效抑制了航向發(fā)散，航向誤差平均值為-0.619″，均方根為5.572″，明顯優(yōu)于GNSS+INS組合結(jié)果。

表3 誤差統(tǒng)計(jì)結(jié)果 (″)

3.2 驗(yàn)證INS延遲處理后CNS+GNSS+INS的姿態(tài)精度

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

116.11640.40119.4882-4.76222.3466.587

分析以上圖表可以看出：未進(jìn)行延遲處理的三組合在開始轉(zhuǎn)臺(tái)靜止的15 min內(nèi)航向誤差穩(wěn)定，轉(zhuǎn)臺(tái)外環(huán)轉(zhuǎn)動(dòng)后則出現(xiàn)了航向姿態(tài)發(fā)散，誤差均值高達(dá)16.116″，均方根為19.488″。這是因?yàn)樵诖蠼撬俾蕮u擺下，慣導(dǎo)提供的姿態(tài)角延時(shí)不能忽略不計(jì)，此時(shí)星敏感器接收到的用于隔離載體姿態(tài)運(yùn)動(dòng)的角度跟蹤指令不能有效反映當(dāng)前時(shí)刻載體的運(yùn)動(dòng)情況，無法實(shí)現(xiàn)有效的載體運(yùn)動(dòng)隔離，導(dǎo)致恒星無法穩(wěn)定地出現(xiàn)在相機(jī)的視場范圍內(nèi)，CNS無法穩(wěn)定正常的工作。此時(shí)，無外推的三組合退化為GNSS+INS兩組合結(jié)果，航向姿態(tài)誤差逐漸積累。而在同樣的搖擺條件下，加入延遲處理的三組合航向姿態(tài)誤差得到了有效的控制，誤差一直穩(wěn)定在10″以內(nèi)，均值為-4.762″，均方根為6.587″，滿足高精度姿態(tài)測量需求。

4 結(jié) 語

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