基于遞推最小二乘估計的CNS/INS組合導航系統初始對準

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

基于遞推最小二乘估計的CNS/INS組合導航系統初始對準

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

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

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

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

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

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

1 星敏感器坐標系與導航坐標系關系

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

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

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

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

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

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

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

進一步由坐標轉換矩陣之間的關系，可得到載體坐標系（b系）到慣性坐標系（i系）的轉換矩陣：

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

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

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

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

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

INS實際輸出的姿態轉換矩陣為Cn′，與計算導航

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

2 CNS/INS組合導航系統對準方法

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

2.1遞推最小二乘擬合

當應用典型的最小二乘法時，測量信息建立的方程聯立如下：

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

函數關系認為能夠線性化，則有

聯立式(8)由可得

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

其中：

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

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

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

應用Sherman-Morisson和Bartlett公式，令

可得

2.2加權處理

在2.1節的基礎上，增加測量信息的權值分析處理環節，可以在動態干擾條件下避免CNS/INS組合系統中測量噪聲帶來的濾波振蕩現象.考慮在測量值進行權值處理，即期望nQ最小。

由式(12)可得

當利用加權值得到額外的信息時，有

利用式(14)得到

2.3組合系統初始對準流程

建立CNS/INS組合導航系統的狀態方程。CNS/INS組合系統的狀態方程采用INS的誤差方程，方程如下：

式中，系統狀態變量的具體定義為

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

建立CNS/INS組合系統的量測方程。若直接利用CNS和INS解算出來的姿態矩陣對應元素相減作為量測量，建立的量測方程維數較高，計算相對較為復雜。根據姿態矩陣與姿態四元數q=[q q q q]i/b0123對應的關系

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

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

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

3 仿真分析

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

圖4 靜態下橫搖誤差對比Fig.4 Static roll errors

圖5 靜態下縱搖誤差對比Fig.5 Static pitch errors

圖6 靜態下航向誤差對比Fig.6 Static heading errors

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

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

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

圖7 動態下橫搖誤差對比Fig.7 Dynamic roll errors

圖8 動態下縱搖誤差對比Fig.8 Dynamic pitch errors

圖9 動態下航向誤差對比Fig.9 Dynamic heading errors

4 結 論

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

（Reference）

[1] 熊智, 劉建業, 郁豐, 等. 基于天文角度觀測的機載CNS/INS組合濾波算法研究[J]. 宇航學報, 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] 屈薔, 劉建業, 熊智. 基于加性四元數誤差方程的CNS/INS姿態組合算法[J]. 中國慣性技術學報, 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/星敏感器組合導航姿態匹配算法[J]. 西北工業大學學報, 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] 張金亮, 秦永元, 吳楓. 捷聯慣導基于星體跟蹤器的高精度初始對準算法[J]. 中國慣性技術學報, 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

國家重大科學儀器設備開發專項（2013YQ310799）

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

聯 系 人：趙小明（1961—），男，研究員，博士生導師，主要從事導航、制導與控制系統研究。

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