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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

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

INS的慣性測量單元（IMU）中，正交安裝三個

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

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

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

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

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

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

2.1遞推最小二乘擬合

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

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

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

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

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

其中：

為了得到X的最小二乘估計，令

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

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

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

可得

2.2加權(quán)處理

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

由式(12)可得

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

利用式(14)得到

2.3組合系統(tǒng)初始對準流程

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

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

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

建立CNS/INS組合系統(tǒng)的量測方程。若直接利用CNS和INS解算出來的姿態(tài)矩陣對應(yīng)元素相減作為量測量，建立的量測方程維數(shù)較高，計算相對較為復(fù)雜。根據(jù)姿態(tài)矩陣與姿態(tài)四元數(shù)q=[q q q q]i/b0123對應(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ù)誤差作為量測值，建立量測方程為

CNS/INS組合對準的工作流程如圖3所示。

圖3 CNS/INS組合對準的工作流程圖Fig.3 Flowchart of initial alignment for CNS/INS navigation system

3 仿真分析

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

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

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

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

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

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

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

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

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

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

4 結(jié) 論

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

（Reference）

[1] 熊智, 劉建業(yè), 郁豐, 等. 基于天文角度觀測的機載CNS/INS組合濾波算法研究[J]. 宇航學(xué)報, 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]. 中國慣性技術(shù)學(xué)報, 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é)報, 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)基于星體跟蹤器的高精度初始對準算法[J]. 中國慣性技術(shù)學(xué)報, 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

國家重大科學(xué)儀器設(shè)備開發(fā)專項（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)研究。

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