基于遞推最小二乘估計(jì)的CNS/INS組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)

周凌峰，趙小明，趙 帥，姚 琪，楊 琳

基于遞推最小二乘估計(jì)的CNS/INS組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)

周凌峰1,2，趙小明1,2，趙 帥2，姚 琪2，楊 琳2

（1. 哈爾濱工程大學(xué) 自動(dòng)化學(xué)院，哈爾濱 150001；2. 天津航海儀器研究所，天津 300131）

針對(duì)CNS/INS組合導(dǎo)航系統(tǒng)中縮短初始對(duì)準(zhǔn)時(shí)間的問(wèn)題，設(shè)計(jì)了一種CNS/INS組合導(dǎo)航系統(tǒng)組合對(duì)準(zhǔn)新方法。在CNS/INS姿態(tài)四元數(shù)組合算法的基礎(chǔ)上，推導(dǎo)CNS/INS組合系統(tǒng)線(xiàn)性化狀態(tài)方程，分析了INS和CNS姿態(tài)四元數(shù)差值構(gòu)建量測(cè)方程。利用遞推最小二乘原理實(shí)現(xiàn)了對(duì)該組合系統(tǒng)的信息融合，設(shè)計(jì)了基于該估計(jì)原理的組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)方法，考慮到大氣層內(nèi)動(dòng)基座條件下對(duì)于星敏感器造成的干擾因素增加了加權(quán)處理環(huán)節(jié)，最后通過(guò)仿真實(shí)驗(yàn)驗(yàn)證了遞推加權(quán)最小二乘法在處理組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中的有效性。仿真結(jié)果表明在微晃基座條件下，與傳統(tǒng)的濾波方法相比較該估計(jì)方法能夠有效地縮短約25%的對(duì)準(zhǔn)時(shí)間。

CNS/INS組合導(dǎo)航；星敏感器；初始對(duì)準(zhǔn)；遞推最小二乘

隨著大視場(chǎng)天文導(dǎo)航技術(shù)的更新發(fā)展，以星敏感器為代表的天文導(dǎo)航設(shè)備其獨(dú)立自主特點(diǎn)和不隨時(shí)間誤差積累的優(yōu)勢(shì)，成為了重要的輔助導(dǎo)航手段。近年來(lái)不少學(xué)者和工程技術(shù)人員在基于星敏感器的CNS/INS組合導(dǎo)航技術(shù)方面進(jìn)行了大量的理論分析和工程實(shí)際研究，并在空天探測(cè)領(lǐng)域中取得了較為廣泛的運(yùn)用。文獻(xiàn)[1]考慮了不同觀星條件下CNS/INS組合的可行性；文獻(xiàn)[2]為避免復(fù)雜非線(xiàn)性濾波運(yùn)算針對(duì)旋轉(zhuǎn)四元數(shù)建模的非線(xiàn)性問(wèn)題，分析了基于加性四元數(shù)的CNS/INS姿態(tài)組合算法；在CNS/INS姿態(tài)匹配算法方面，文獻(xiàn)[3]～[5]討論了星敏感器安裝誤差的估計(jì)方法，以及根據(jù)組合導(dǎo)航系統(tǒng)模型利用星敏感器觀測(cè)量估計(jì)慣性導(dǎo)航系統(tǒng)元件的常值誤差項(xiàng)等。在對(duì)星敏感器安裝誤差的估計(jì)過(guò)程中，有學(xué)者針對(duì)工程上星敏感器安裝誤差高精度快速標(biāo)定的需求，采用的最小二乘法推導(dǎo)的標(biāo)定算法，取得了較好的實(shí)際效果[6]。本文考慮星敏感器在大氣層內(nèi)的動(dòng)態(tài)使用條件，根據(jù)遞推最小二乘法的原理，設(shè)計(jì)了基于星敏感器為測(cè)量器件的CNS/INS組合導(dǎo)航快速對(duì)準(zhǔn)方法。

在CNS/INS組合導(dǎo)航系統(tǒng)中的初始對(duì)準(zhǔn)方法中，比較常見(jiàn)的初始對(duì)準(zhǔn)方法采用卡爾曼濾波器進(jìn)行數(shù)據(jù)融合的方式，認(rèn)為星敏感器的天文導(dǎo)航精度足夠高，將其輸出的姿態(tài)四元數(shù)理解為真值，視INS輸出的姿態(tài)四元數(shù)為估計(jì)值，由此建立導(dǎo)航系統(tǒng)的狀態(tài)方程。這種傳統(tǒng)的方法在建立狀態(tài)方程時(shí)假設(shè)了星敏感器的姿態(tài)四元數(shù)是沒(méi)有受到大氣層環(huán)境及載體動(dòng)態(tài)環(huán)境干擾的條件的理想值，但實(shí)際情況中星敏感器處于大氣層內(nèi)外不同使用環(huán)境中時(shí)受到的干擾因素差別較大，可能將導(dǎo)致組合導(dǎo)航系統(tǒng)的姿態(tài)誤差濾波收斂時(shí)間較長(zhǎng)，有的情況甚至發(fā)散。本文提出的基于遞推最小二乘估計(jì)的組合對(duì)準(zhǔn)方法中，形式上類(lèi)似于卡爾曼濾波器，根據(jù)遞推回歸原理，通過(guò)加權(quán)處理環(huán)節(jié)，實(shí)現(xiàn)對(duì)星敏感器觀測(cè)量權(quán)值分析，使得組合導(dǎo)航系統(tǒng)在滿(mǎn)足精度的條件下，實(shí)現(xiàn)快速對(duì)準(zhǔn)。

1 星敏感器坐標(biāo)系與導(dǎo)航坐標(biāo)系關(guān)系

基于星敏感器技術(shù)的CNS設(shè)備，其導(dǎo)航原理是以恒星為參照物，利用攝像探測(cè)單元對(duì)天空進(jìn)行拍攝直接捕獲星圖，經(jīng)過(guò)數(shù)據(jù)處理單元對(duì)星圖進(jìn)行星圖識(shí)別、質(zhì)點(diǎn)提取、星圖跟蹤等一系列的計(jì)算，確定出星體跟蹤器光軸在慣性空間的瞬時(shí)指向。它不需要任何外部信息自主地確定運(yùn)載體相對(duì)于慣性空間的姿態(tài)；通過(guò)外部提供水平基準(zhǔn)信息或地理位置信息，可相應(yīng)地確定運(yùn)載體的地理位置或載體姿態(tài)[6]。CNS的工作原理如圖1所示。

圖1 CNS工作原理圖Fig.1 Principle of CNS

CNS的安裝誤差在安裝時(shí)可通過(guò)光學(xué)方法標(biāo)定出來(lái)，即認(rèn)為從星敏感器坐標(biāo)系（s系）到載體坐標(biāo)系（b系）的轉(zhuǎn)換矩陣在CNS安裝的時(shí)候已確定[7]。假設(shè)s系、b系基本重合，安裝誤差為小角度θs（θs=(θsx,θsy,θsz)），則有

其中s()×θ為各元素組成的叉乘反對(duì)稱(chēng)陣，即

CNS通過(guò)星圖拍攝、星圖識(shí)別、質(zhì)點(diǎn)提取和導(dǎo)航解算等一系列的計(jì)算之后，可得到星光矢量r在天球坐標(biāo)系下的投影ri=(lx,ly,lz)T以及在星敏感器像空間的投影rs=(hx,hy,hz)T，兩者之間的關(guān)系如下：

其中，v為星敏感器系統(tǒng)本身的測(cè)量誤差，可考慮為零均值的白噪聲過(guò)程。

由式(2)可知，如果能同時(shí)觀測(cè)至少三顆星體，就可唯一確定坐標(biāo)轉(zhuǎn)換矩陣，而目前隨著大視場(chǎng)星體快速檢測(cè)技術(shù)的發(fā)展，天文導(dǎo)航設(shè)備已經(jīng)完全具備在某一刻完成多顆星的同步檢測(cè)的能力[2]。

進(jìn)一步由坐標(biāo)轉(zhuǎn)換矩陣之間的關(guān)系，可得到載體坐標(biāo)系（b系）到慣性坐標(biāo)系（i系）的轉(zhuǎn)換矩陣：

通過(guò)前面的分析可知，理論上CNS可在不需要任何外部先驗(yàn)信息條件下即可確定載體坐標(biāo)系（b系）到慣性系（i系）的姿態(tài)轉(zhuǎn)換矩陣。矩陣間的轉(zhuǎn)換關(guān)系有

由式(4)分析可知，當(dāng)CNS能獲取外部提供的水平基準(zhǔn)信息或地理位置信息時(shí)，相應(yīng)地可計(jì)算得到載體位置矩陣或姿態(tài)矩陣，最后推算出載體的經(jīng)、緯度值或n系下的姿態(tài)角。

INS的慣性測(cè)量單元（IMU）中，正交安裝三個(gè)

其中，(,)φλ為載體的地理位置，0λ為初始經(jīng)度，tΔ可由星敏感器自身攜帶的世界標(biāo)準(zhǔn)時(shí)間系統(tǒng)準(zhǔn)確得到。敏感載體角速度的激光陀螺儀和三個(gè)敏感線(xiàn)加速度的加速度計(jì)。加速度計(jì)測(cè)得載體沿坐標(biāo)系各軸向的加速度分量，利用陀螺測(cè)得的角速度信息計(jì)算出姿態(tài)矩陣并提取姿態(tài)角，利用姿態(tài)矩陣將加速度計(jì)的測(cè)量信息變換到導(dǎo)航坐標(biāo)系上，然后在導(dǎo)航坐標(biāo)系中進(jìn)行加速度到速度的積分以及速度到位置的積分。INS解算的機(jī)械編排原理如圖2所示。

圖2 INS工作示意圖Fig.2 Principle of INS

INS實(shí)際輸出的姿態(tài)轉(zhuǎn)換矩陣為Cn′，與計(jì)算導(dǎo)航

若要將INS、CNS的輸出信息進(jìn)行融合，首先必須將兩者的輸出信息統(tǒng)一到同一坐標(biāo)系下（如i系）。根據(jù)矩陣變換之間的關(guān)系，式(7)可將INS的輸出信息轉(zhuǎn)換到i系：

2 CNS/INS組合導(dǎo)航系統(tǒng)對(duì)準(zhǔn)方法

將CNS與INS進(jìn)行組合，利用CNS慣性系下高精度的姿態(tài)信息輔助INS進(jìn)行對(duì)準(zhǔn)，實(shí)現(xiàn)組合導(dǎo)航系統(tǒng)快速對(duì)準(zhǔn)的目的。在組合數(shù)據(jù)處理過(guò)程中，本文采用的是遞推最小二乘估計(jì)器的方式進(jìn)行數(shù)據(jù)融合的。

2.1遞推最小二乘擬合

當(dāng)應(yīng)用典型的最小二乘法時(shí)，測(cè)量信息建立的方程聯(lián)立如下：

其中：x1,x2,…xk是要估計(jì)的“k”個(gè)參數(shù)；y1,y2,…,yk是n次觀測(cè)；e1,e2,…,en是n次觀測(cè)的誤差。實(shí)現(xiàn)這些參數(shù)的“最佳”估計(jì)，即選用x1,x2,…xk的值，使得最小。

函數(shù)關(guān)系認(rèn)為能夠線(xiàn)性化，則有

聯(lián)立式(8)由可得

其中n≥k 。式(10)改為矩陣形式為

其中：

為了得到X的最小二乘估計(jì)，令

假設(shè)多進(jìn)行一次觀測(cè)，則有：Mn+1X=yn+1-en+1。對(duì)于狀態(tài)量X用?n+1表示的估計(jì)值可由式(12)給出：

為避免在引入新觀測(cè)量時(shí)出現(xiàn)kk×的矩陣求逆，

應(yīng)用Sherman-Morisson和Bartlett公式，令

可得

2.2加權(quán)處理

在2.1節(jié)的基礎(chǔ)上，增加測(cè)量信息的權(quán)值分析處理環(huán)節(jié)，可以在動(dòng)態(tài)干擾條件下避免CNS/INS組合系統(tǒng)中測(cè)量噪聲帶來(lái)的濾波振蕩現(xiàn)象.考慮在測(cè)量值進(jìn)行權(quán)值處理，即期望nQ最小。

由式(12)可得

當(dāng)利用加權(quán)值得到額外的信息時(shí)，有

利用式(14)得到

2.3組合系統(tǒng)初始對(duì)準(zhǔn)流程

建立CNS/INS組合導(dǎo)航系統(tǒng)的狀態(tài)方程。CNS/INS組合系統(tǒng)的狀態(tài)方程采用INS的誤差方程，方程如下：

式中，系統(tǒng)狀態(tài)變量的具體定義為

其中：δqi/b為姿態(tài)四元數(shù)真值與估計(jì)值之間的誤差；εcx,εcy,εcz為陀螺隨機(jī)常值誤差；?ax,?ay,?az為加速度計(jì)常值零位誤差。

建立CNS/INS組合系統(tǒng)的量測(cè)方程。若直接利用CNS和INS解算出來(lái)的姿態(tài)矩陣對(duì)應(yīng)元素相減作為量測(cè)量，建立的量測(cè)方程維數(shù)較高，計(jì)算相對(duì)較為復(fù)雜。根據(jù)姿態(tài)矩陣與姿態(tài)四元數(shù)q=[q q q q]i/b0123對(duì)應(yīng)的關(guān)系

可求出CNS和INS相應(yīng)的四元數(shù)值。假設(shè)CNS解算的慣性系下姿態(tài)四元數(shù)為/ibq?，INS解算出慣性系下的姿態(tài)四元數(shù)為/?ibq[2,8]，由于CNS解算的作為外參考信息，因此將CNS和INS慣性系下的姿態(tài)四元數(shù)誤差作為量測(cè)值，建立量測(cè)方程為

CNS/INS組合對(duì)準(zhǔn)的工作流程如圖3所示。

圖3 CNS/INS組合對(duì)準(zhǔn)的工作流程圖Fig.3 Flowchart of initial alignment for CNS/INS navigation system

3 仿真分析

為了對(duì)比分析說(shuō)明遞推加權(quán)最小二乘法在處理組合導(dǎo)航系統(tǒng)中的特點(diǎn)，分別模擬靜態(tài)下和系泊狀態(tài)下的艦船姿態(tài)環(huán)境，利用傳統(tǒng)的卡爾曼濾波方法與之比較，選取中高精度慣組作為仿真對(duì)象。系統(tǒng)方案采用正反停轉(zhuǎn)的調(diào)制方式，水平陀螺采用中精度的陀螺，其陀螺漂移的零偏重復(fù)性為0.004 (°)/h，穩(wěn)定性0.006 (°)/h；方位陀螺采用高精度陀螺，其零偏重復(fù)性?xún)?yōu)于0.0007 (°)/h，穩(wěn)定性?xún)?yōu)于0.001 (°)/h；加速度計(jì)的分辨率為5×10-6g，加表零偏穩(wěn)定性?xún)?yōu)于1×10-5g；慣性元件的輸出頻率為400 Hz，星敏感器的輸出頻率為1 Hz。星敏感器姿態(tài)計(jì)算方法采用QUEST算法[9]，在靜態(tài)下設(shè)星敏感器的噪聲為5″，可將星敏感器的測(cè)量誤差考慮為均值為零的白噪聲[10]。為了更好地比較仿真結(jié)果，同時(shí)為了方便觀察其誤差關(guān)系，將狀態(tài)向量的四元數(shù)誤差關(guān)系，導(dǎo)入地理坐標(biāo)系利用水平姿態(tài)、航向誤差關(guān)系來(lái)表示誤差大小。

圖4 靜態(tài)下橫搖誤差對(duì)比Fig.4 Static roll errors

圖5 靜態(tài)下縱搖誤差對(duì)比Fig.5 Static pitch errors

圖6 靜態(tài)下航向誤差對(duì)比Fig.6 Static heading errors

兩種方法在靜態(tài)下的仿真沒(méi)有明顯的差別，無(wú)論是卡爾曼濾波還是遞推最小二乘估計(jì)器均能在較短時(shí)間內(nèi)收斂至穩(wěn)定。收斂后穩(wěn)態(tài)誤差的峰峰值振幅小于1′，說(shuō)明遞推最小二乘估計(jì)能夠?qū)崿F(xiàn)對(duì)組合系統(tǒng)初始對(duì)準(zhǔn)的功能，同時(shí)在較短時(shí)間內(nèi)都能夠滿(mǎn)足靜態(tài)對(duì)準(zhǔn)精度要求。

假設(shè)在系泊狀態(tài)下，艦船的搖擺姿態(tài)如下：航向角、縱搖角、橫搖角的周期和幅值變化分別為40′、35′、50′，周期分別為10 s、8 s、8 s?？紤]在大氣層內(nèi)晃動(dòng)條件下星敏感器的雜光處理能力將受到影響，其星敏感器姿態(tài)精度用一階馬爾科夫過(guò)程表示，輸出噪聲為45″，相關(guān)時(shí)間為130 s。對(duì)比兩種方法對(duì)初始對(duì)準(zhǔn)精度和收斂時(shí)間的影響如圖7～9。

從誤差收斂的曲線(xiàn)可以看出，組合系統(tǒng)在系泊狀態(tài)下，通過(guò)40 min的濾波時(shí)間無(wú)論是卡爾曼濾波方法還是遞推最小二乘方法都能夠估計(jì)出INS的初始對(duì)準(zhǔn)誤差來(lái)。對(duì)比二者的誤差曲線(xiàn)從收斂時(shí)間來(lái)看，遞推加權(quán)最小二乘估計(jì)器所用的時(shí)間更短，說(shuō)明在系泊的晃動(dòng)條件下，遞推加權(quán)最小二乘估計(jì)器有較好的適應(yīng)性。

圖7 動(dòng)態(tài)下橫搖誤差對(duì)比Fig.7 Dynamic roll errors

圖8 動(dòng)態(tài)下縱搖誤差對(duì)比Fig.8 Dynamic pitch errors

圖9 動(dòng)態(tài)下航向誤差對(duì)比Fig.9 Dynamic heading errors

4 結(jié) 論

針對(duì)CNS/INS組合導(dǎo)航系統(tǒng)的特點(diǎn)，以四元數(shù)運(yùn)動(dòng)學(xué)微分方程為基礎(chǔ)，以CNS和INS姿態(tài)四元數(shù)誤差為觀測(cè)量，聯(lián)立系統(tǒng)的量測(cè)方程。在組合導(dǎo)航系統(tǒng)初始對(duì)準(zhǔn)中提出了利用遞推最小二乘法估計(jì)慣性導(dǎo)航系統(tǒng)初始失調(diào)角，通過(guò)加權(quán)處理的方式，增加了遞推最小二乘法計(jì)算的魯棒性，使得能夠采用遞推最小二乘法實(shí)現(xiàn)CNS/INS的初始對(duì)準(zhǔn)過(guò)程。分別通過(guò)靜態(tài)和系泊條件的數(shù)學(xué)仿真驗(yàn)證了其算法的有效性。當(dāng)動(dòng)態(tài)條件下CNS定姿精度下降時(shí)，遞推最小二乘法有較強(qiáng)的適應(yīng)能力，相對(duì)于傳統(tǒng)的卡爾曼濾波器能夠縮短25%的對(duì)準(zhǔn)時(shí)間。雖然在仿真條件下驗(yàn)證了該方法的有效性，但是嘗試在工程實(shí)際中應(yīng)用需要對(duì)CNS/INS組合系統(tǒng)模型進(jìn)一步的深入研究，提高算法的可靠性。

（Reference）

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

Xiong Zhi, Liu Jian-ye, Yu Feng, et al. Research of airborne CNS/INS integrated filtering algorithm base on celestial angle observation[J]. Journal of Astronautics, 2010, 31(2): 397-403.

[2] 屈薔, 劉建業(yè), 熊智. 基于加性四元數(shù)誤差方程的CNS/INS姿態(tài)組合算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2011, 19(3): 316-319.

Qu Qiang, Liu Jian-ye, Xiong Zhi. Inertial/Celestial attitude integrated algorithm based on additive quaternion [J]. Journal of Chinese Inertial Technology, 2011, 31(2): 316-319.

[3] 龍瑞, 秦永元, 夏家和. 一種新的SINS/星敏感器組合導(dǎo)航姿態(tài)匹配算法[J]. 西北工業(yè)大學(xué)學(xué)報(bào), 2011, 29(3): 476-479.

Long Rui, Qin Yong-yuan, Xia Jia-he. A new and effective algorithm of attitude matching for SINS/Star sensor integrated navigation system[J]. Journal of North western Polytechnical University, 2011, 29(3): 476-479.

[4] Lam Q M, Crassidis J L. Precision attitude determination using a multiple model adaptive estimation scheme[C]// 2007 IEEE Aerospace Conference. 2007: 1-20.

[5] Stuart R G. Applications of complex analysis to celestial navigation[J]. Navigation, Journal of the Institute of Navigation, 2009, 56(4): 221-227.

[6] Hu H D, Huang X L. SCNS/INS/GPS integrated navigation algorithm based on UKF[J]. Journal of systems Engineering and Electronics, 2010, 21(1): 102- 109.

[7] Wu X, Wang X L, A SINS/CNS tightly integrated navigation method based on mathematical horizon reference[J]. Aircraft Engineering and aerospace Technology, 2011, 83(1): 26-34.

[8] 張金亮, 秦永元, 吳楓. 捷聯(lián)慣導(dǎo)基于星體跟蹤器的高精度初始對(duì)準(zhǔn)算法[J]. 中國(guó)慣性技術(shù)學(xué)報(bào), 2013, 12(1): 22-25.

Zhang Jin-liang, Qin Yong-yuan, Wu Feng. Precise SINS initial alignment algorithm based on star tracker[J]. Journal of Chinese Inertial Technology, 2013, 12(1): 22-25.

[9] Quan Wei, Fang Jian-cheng, Xu Fan, et al. Hybrid simulation system study of SCNS/INS integrated navigation[J]. IEEE Aerospace & Electronic Systems Magazine, February, 2008: 17-24.

[10] Hua Bing, Yuan Jing, Liu Hai-ying, et al. Distributed attitude transfer alignment research based on INS/CNS integrated navigation system[C]//Proceedings of the 33rdChinese Control Conference. Nanjing, China, 2014.

Initial alignment of CNS/INS integrated navigation system based on recursive least square method

ZHOU Ling-feng1,2, ZHAO Xiao-ming1,2, ZHAO Shuai2, YAO Qi2, YANG Lin2
(1. College of Automation, Harbin Engineering University, Harbin 150001, China; 2. Tianjin Navigation Instrument Research Institute, Tianjin 300131, China)

In order to reduce the initial alignment time of INS, a new integrated alignment algorithm is designed for CNS/INS integrated navigation system. Based on an attitude quaternion algorithm, the linear state equation of CNS/INS system is deduced, and the measurement equation is formed from the attitude quaternion errors between INS and CNS. By using a recursive least square method, the CNS/INS system’s integrated alignment data fusion is realized. A weighted average method is added when fusing the INS and CNS’s information, taking into consideration the interference factors on moving base under aerosphere. At last, the validity of using recursive weighted least square method for data fusion is testified by simulation experiments, and the results shows that the initial alignment time is reduced by 25% compared with that of the traditional Kalman filter algorithm.

CNS/INS integrated navigation; star sensor; initial alignment; recursive least square method

U666.1

A

1005-6734(2015)03-0281-06

10.13695/j.cnki.12-1222/o3.2015.03.001

2015-01-21；

2015-05-09

國(guó)家重大科學(xué)儀器設(shè)備開(kāi)發(fā)專(zhuān)項(xiàng)（2013YQ310799）

周凌峰（1981—），女，博士研究生，主要從事導(dǎo)航、組合導(dǎo)航與系統(tǒng)控制研究。E-mail：zfl_325@sina.com

聯(lián) 系 人：趙小明（1961—），男，研究員，博士生導(dǎo)師，主要從事導(dǎo)航、制導(dǎo)與控制系統(tǒng)研究。

??編號(hào)：1005-6734(2015)03-0287-06 doi: 10.13695/j.cnki.12-1222/o3.2015.03.002