慣性/天文/衛星組合導航誤差在線標定方法

蹤 華,齊建宇,熊 攀,汪 渤,劉 準 ,周志強

(1.宇航智能控制技術國家級重點實驗室(北京航天自動控制研究所)，北京 100854；2.北京理工大學 自動化學院，北京100081；3.哈爾濱工業大學 航天學院，哈爾濱150001)

慣性/天文/衛星組合導航誤差在線標定方法

蹤 華1,齊建宇1,熊 攀3,汪 渤2,劉 準1,周志強2

(1.宇航智能控制技術國家級重點實驗室(北京航天自動控制研究所)，北京 100854；2.北京理工大學 自動化學院，北京100081；3.哈爾濱工業大學 航天學院，哈爾濱150001)

為提高慣性/天文/衛星(INS/CNS/GNSS )組合導航系統長期工作時的精度，推導了基于四元數誤差描述的組合導航系統誤差狀態方程和量測方程，利用分段定常系統(PWCS)定理，分析了不同輸入條件下系統狀態變量的可觀測性和可觀測度.結合可觀測性和可觀測度分析結果,提出了利用星敏感器及衛星導航系統提供的姿態、位置矢量在線標定陀螺儀、加速度計和星敏感器誤差參數的方法.可觀測性分析和仿真試驗結果表明,該方法可以精確在線估計出陀螺儀、加速度計、星敏感器的標度因數、零偏和安裝誤差，提高慣性/天文/衛星組合導航系統長期在軌工作的精度.

組合導航；四元數誤差；Kalman濾波；在線標定；可觀測度；星敏感器；標度因數；安裝誤差

長期自主導航技術在運載火箭上面級、軌道轉移飛行器、地球衛星等軌道飛行器上具有廣闊的應用前景.未來先進的火箭上面級、軌道轉移飛行器等需要拓展功能，適應在多星部署、在軌服務等領域的任務需求，需要在飛行時間長、空間跨度大等條件下具備長期在軌精確自主導航的能力[1-2].另外，隨著進入軌道的衛星數量的增多、地面測控站的工作量逐漸增大，為降低地面壓力，節省成本，也需要進一步發展地球衛星自主導航技術[3-5].因此，為滿足軌道飛行器長時間自主導航任務，慣性/天文/衛星組合導航已成為國內外軌道飛行器研制機構首選方案.精度和可靠性是慣性/天文/衛星組合導航系統兩項重要的技術指標.發射時的沖擊、空間環境的變換、導航器件重復性性能等因素，都會造成組合導航系統器件誤差天地不一致的問題，誤差在線分離技術已經成為慣性/天文/衛星組合導航系統實現高精度、高可靠性長期導航的關鍵技術.該技術具有重要的工程應用價值：1) 受火箭發射時刻沖擊、空間環境影響，星敏感器等天文姿態器件安裝誤差變化較大，安裝誤差分離與補償技術可提高星敏感器在線使用精度；2) 慣性器件參數誤差和一些緩變故障表現類似，通過器件誤差的在線估計與補償，可降低參數誤差的影響，有利于故障診斷方法的設計，從而提高系統的可靠性[6]；3) 天文導航和衛星導航更新周期低，誤差在線標定技術可提高當天文導航和衛星導航無法修正期間的慣性導航的精度.因此，導航器件誤差分離技術受到各國科研人員的重視，得到了持續研究.文獻[7]整理分析了用四元數描述飛行器姿態動力學方程的方法及其優點.文獻[8]建立了飛行器姿態四元數誤差、陀螺儀和星敏感器誤差模型，利用EKF和UF濾波方法進行誤差在線估計，分析了這兩種濾波方法對誤差估計性能的影響.文獻[9]研究了衛星上陀螺誤差在軌標定方法.文獻[10]利用四元數建立了姿態誤差模型，采用Kalman濾波器對慣性/天文組合導航系統陀螺在線標定技術進行了研究.文獻[11]研究了利用MPF-KF估計陀螺誤差的方法.文獻[7]采用歐拉角建立系統誤差模型，利用EKF在線估計加速度計和陀螺零偏、星敏感器安裝誤差和刻度因數誤差.

綜上所述，文獻[7-11]主要圍繞基于四元數據描述的飛行器姿態誤差建模和濾波方法展開研究，并沒有對誤差變量的可觀測性和精確在線分離的條件進行分析.文獻[12]建立了基于歐拉角描述的飛行器姿態誤差、速度和位置誤差模型，通過濾波方法在線估計導航器件的誤差，但是，基于歐拉角的姿態誤差模型為非線性模型，且對于一些旋轉的角度，無法用歐拉角描述[12]，在使用Kalman濾波器的時候會出現問題. 鑒于四元數具有非奇異性，基于四元數建立的姿態誤差模型為線性模型的優點，本文將在文獻[7]及上述其他相關研究成果的基礎上，沿用由四元數矢量部分描述姿態誤差方程的方法，并進一步進行拓展，用四元數矢量部分建立了完整的姿態、速度誤差方程和量測方程，采用Kalman濾波方法實現導航器件誤差的在線估計.另外，本文對系統誤差模型進行可觀測性和可觀測度分析，給出了輸入條件對可觀測度的影響，為導航器件誤差的精確在線分離提供了設計依據.結合可觀測性分析結果，設計了有效的慣性/天文/衛星組合導航系統誤差在線標定策略，獲得了理想的試驗效果.

1 在線標定算法

1.1 導航器件誤差模型

完成慣組標定后，若不對陀螺、加速度計進行重新拆裝，陀螺和加速度計的安裝誤差角逐次重復精度高，但陀螺和加速度計零偏、刻度因數誤差項逐次通電重復性精度較低，因此，忽略安裝誤差角，只考慮零偏和刻度因數誤差，建立陀螺儀和加速度計的誤差模型.

1.1.1 陀螺儀輸出誤差模型

陀螺儀輸出記為

(1)

由式(1)可得到陀螺的輸出誤差為

(2)

1.1.2 加速度計誤差模型

加速度計的輸出記為

(3)

由式(3)可得到加速度計的輸出誤差為

1.1.3 星敏感器誤差模型

星敏感器測量星光確定的姿態四元數表示為

1.2 組合導航系統誤差模型

1.2.1 姿態誤差方程

(4)

由于四元數運動滿足如下關系：

(5)

(6)

對式(4)求導數，并將式 (5)，(6)代入式(4)可得

其中，δqφ?δω可展開為

又因為δqφ0≈1，忽略二階項可得

因此，δqφ的導數為

將矢量和標量部分分開寫，可得:

(7)

(8)

將陀螺的輸出誤差模型式(2)代入式(7)，得到四元數誤差模型：

1.2.2 速度誤差方程

對于捷聯安裝的加速度計來說，速度測量方程可以描述為

(9)

理想的速度方程為

(10)

忽略引力誤差，式(10)減式(9)，可得:

(11)

(12)

(13)

忽略二階小量得到速度誤差方程：

(14)

將加速度計的輸出誤差式(3)代入式 (14)得到：

1.2.3 位置誤差方程

位置誤差方程為

1.3 組合導航系統誤差模型

1.3.1 狀態方程

導航坐標系為發射慣性系，組合導航系統的狀態方程為捷聯慣導的誤差方程，狀態方程為

式中：A(t)為系統的狀態轉移矩陣；G(t)為系統的噪聲系數矩陣；W(t)為系統噪聲向量;X(t)為t時刻的系統狀態矢量.結合導航器件、組合導航系統的誤差模型，系統狀態誤差變量取

(15)

式中：狀態變量為24個，分別為數學平臺失準角誤差、位置誤差、速度誤差、陀螺零偏誤差、加速度計零偏誤差、星敏感器安裝誤差(用四元數的矢量部分表示)、陀螺刻度因數誤差、加速度計刻度因數.

1.3.2 觀測方程

(16)

考慮到星敏感器安裝誤差和慣組數學平臺誤差角，得到：

(17)

(18)

由式(13)可知失準角和四元數誤差存在如下關系：

(19)

將式(19)代入式(18)，可以得到：

所以

(20)

以姿態四元數誤差和位置誤差作為觀測量，慣性/天文/衛星組合導航系統測量方程為

根據式(20)，得到：

1.3.3Kalman濾波方程

上述各物理量在應用過程中實際上都不是連續的模擬量，而是離散量，需要把上述Kalman濾波方程離散化.離散化公式為:

式中：Δ為Kalman濾波周期，n為離散化階數.離散化的系統狀態方程和量測方程可表示為

這樣就可以按照下列離散Kalman濾波方程進行濾波解算, 開環Kalman濾波方程參見文獻[14].

2 慣性/天文/衛星組合導航誤差可觀測性分析

在線標定算法建立了慣性/天文/衛星組合導航系統的狀態方程和量測方程，該系統為線性時變系統，系統的可觀測性和狀態變量的可觀測度與誤差變量可分離的數量和精度直接相關.

2.1 PWCS可觀測性分析定理

不考慮系統噪聲，第j個時間段離散系統模型為

(21)

系統的總可觀測矩陣TOM為

其中

定理1 如果Fjx=x,?x∈null{Qj(r)},1≤j≤r,則null{Q(r)}=null{Qs(r)}，rank{Q(r)}= rank{Qs(r)}.

基于奇異值分解理論的系統可觀測性分析方法參見文獻[15].

2.2 基于奇異值分解理論的系統可觀測性分析方法在本系統上的適用性證明

將第j個時間段組合導航系統連續的狀態方程和量測方程記為

(22)

提取可觀測矩陣為

(23)

(24)

2.3 可觀測性分析結果

飛行器在軌運動可以分為3種基本類型：1)無動力飛行狀態；2)進行姿態機動；3)線加減速運動.針對這3種基本類型，設計的可觀測性分析的輸入條件為：狀態Ⅰ為無動力飛行狀態，陀螺測量飛行器相對慣性空間的轉動角速率，通常該角速率較小，這里取三軸向陀螺敏感的角速率均為2.4×10-4(°)/s；狀態Ⅱ為角運動狀態下，x，y，z軸向角速率分別為0,0,10(°)/s；狀態Ⅲ為加減速運動條件下，x，y，z軸向加速度分別為，0, 20, 0m/s2.利用基于奇異值分解理論的系統可觀測性分析方法對系統的可觀測性進行了分析，可觀測性分析結果見表1，表1中為在3種運動條件下，狀態變量對應的奇異值，奇異值越大，可觀測度越大，變量的可觀測性越強，奇異值小于5×10-8時，對應的狀態不可觀測[16].在狀態Ⅰ條件下，有15個狀態變量的奇異值較大，其他的狀態變量的奇異值基本為0.可觀測的變量為數學平臺失準角誤差、位置誤差、速度誤差、陀螺零偏誤差、加速度計零偏誤差.在狀態Ⅱ條件下，與狀態Ⅰ相比，星敏感器安裝誤差和y軸加速度計刻度因數誤差可觀測，加表零偏變為了不可觀測.在狀態Ⅲ條件下，與狀態Ⅰ結果相比，星敏感器安裝誤差和z軸陀螺刻度因數誤差可觀測，z軸陀螺零偏不可觀測.因此，加入線加速度機動或角速率機動，可提高星敏感器器安裝誤差的可觀測性;加入線加速度機動，可提高對應軸向上的加表的刻度因數誤差的可觀測度;加入角速率機機動，可提高對應軸向上的陀螺刻度因數誤差的可觀測度.也就是在一定的機動條件下，本文24個誤差變量均可觀測.

表1 不同運動狀態下的奇異值

3 仿真試驗

3.1 仿真條件

利用在線標定的算法，編寫了組合導航系統誤差在線估計仿真程序，其中狀態變量為24維，初值為0，觀測變量6維，采用標準的開環Kalman濾波方法進行誤差估計，濾波方程系數矩陣離散化處理取3階.

仿真試驗中假設三軸向的陀螺隨機噪聲均為0.2(°)/h, 三軸向的陀螺零偏誤差為ωx=1×10-4rad/s,ωy=2×10-4rad/s,ωz=3×10-4rad/s, 陀螺刻度因數誤差均為3×10-5；三軸向加速度計隨機噪聲均為1×10-4m/s2, 陀螺和加速度計的更新周期2 ms； 星敏感器的安裝誤差分別為φx=1×10-3rad,φy=2×10-3rad,φz=3×10-3rad； 星敏感器定姿精度為10″(1σ) , 衛星導航的定位精度為10 m(1σ) .

3.2 仿真試驗結果

根據系統的可觀測度分析結果，導航器件誤差在線標定策略為：1)依次在慣組3個軸向加入100(°)/s的角速率機動，取加入角速率機動對應時間段的陀螺刻度因數誤差濾波估計結果，得到陀螺刻度因數誤差數據.在該條件下，也可以同時估計出其他21個誤差變量，但是由表1中的數據可知，其他變量可觀測度相對較差，故不作為最終的分離結果；2)依次在慣組3個軸向加入50 m/s2的加速度機動，分離出加表刻度因數誤差，同時估計出星敏感器安裝誤差；3)在慣組三軸向角速率均為2.4×10-4(°)/s，加速度均為0 m/s2時，估計出陀螺零偏誤差、加速度計零偏誤差.

導航器件參數誤差估計結果如圖1～圖5所示，圖中結果表明，15項參數誤差均能收斂到穩定值.濾波穩定后，陀螺刻度因數誤差的精度優于1×10-6，加速度計的刻度因數誤差優于2×10-6，星敏感器安裝誤差的精度優于5×10-6rad，陀螺零偏誤差的精度1.5×10-6rad/s，加速度計零偏誤差優于2.75×10-4m/s2，該試驗結果表明，利用本文的在線估計方法，各參數誤差的在線標定精度均能提高一個數量級，仿真試驗結果與可觀測性分析結果一致，因此本文的誤差標定模型和標定正確，通過本方法可有效提高導航精度.

圖1 陀螺刻度因數誤差

圖2 加速度計刻度因數誤差

圖3 星敏感器安裝誤差

圖4 陀螺零偏誤差

圖5 加速度計零偏誤差

4 結 論

1)為了提高慣性/天文/衛星組合導航系統長期在軌工作的精度，完善了基于四元數描述的慣性/天文/衛星組合導航誤差和各慣性器件誤差模型，利用分段定常系統(PWCS)定理，分析了不同輸入條件下系統狀態變量的可觀測性和可觀測度.為各項誤差的有效分離提供了理論依據。

2)結合慣性/天文/衛星組合導航系統可觀測性和可觀測度分析結果，提出了利用Kalman濾波技術在線估計慣性/天文/衛星組合導航誤差和導航器件誤差的標定策略，仿真試驗驗證了該策略的有效性和精度，該在線標定方法、可觀測性分析結果和試驗結果對在線標定技術在工程上的應用具有重要的參考價值.

[1] DRESSLER G，MATUSZAK L，STEPHENSON D D. Study of a high-energy upper stage for future shuttle missions[C]//Proceedings of the 39th AIAA/ASME/SAE/ASEE Joint Propulsion Conference and Exhibit. Huntsville, Alabama: AIAA, 2003:1-10. DOI: 10.2514/6.2003-5128 .

[2] LEESON J. Feasibility study of the multipurpose orbitally deployed upper stage (MODUS) platform[C]//Proceedings of the 65th International Astronautical Congress. Toronto, Canada:[s.n].2014.

[3] WHITE R L, GOUNLEY R B. Satellite autonomous navigation with SHAD[R].CSDL-R-1982, Cambride: The Charles Stark Draper Laboratory, Inc, 1987.

[4] HICKS K D, IESEL W E. Autonomous orbit determination system for earth satellites[J]. Journal of Guidance, Control and Dynamics,1992, 15(3):562-566. DOI: 10.2514/3.20876.

[5] GOTTZEIN E, FICHTER W, JABLONSKI A, et al. Challenges in control and autonomous of communications Satellites[J]. Control Engineering Practice, 2000, 8(4): 409-427. DOI: 10.1016/S0967-0661(99)00171-9.

[6] ZONG Hua, WANG Bo, LIU Zhun, et al. Fault detection in SINS/CNS/GPS integrated system based on federated filter [C]//Proceedings of the 33nd Chinese Control Conference. Nan Jing:IEEE, 2014: 608-612. DOI: 10.1109/ChiCC.2014.6896694.

[7] LEFFERTS E J, MARKLEY F L, SHUSTER M D.Kalman filtering for spacecraft attitude estimation[J].Journal of Guidance, Control, and Dynamics, 1982,5(5): 417-429. DOI: 10.2514/3.56190.

[8] KOKLAM LAI, JOHN L, CRASSIDIS, et al. In-space spacecraft alignment calibration using the unscented filter [C]//AIAA Guidance, Navigation, and Control Conference and Exhibit. Austin, Texas: AIAA, 2003:1-11. DOI: 10.2514/6.2003-5563.

[9] 陳雪芹, 耿云海, 何蕊. 基于二階非線性濾波的星上陀螺在軌標定[J].哈爾濱工業大學學報, 2008, 40(11): 1690-1695. CHEN Xueqin, GENG Yunhai, HE Rui. On-orbit calibration of satellite gyro based on second-order nonlinear filtering[J]. Journal of Harbin Institute of Technology, 2008, 40(11): 1690-1695.

[10]肇慧, 沈繼紅, 陳濤. SINS/CNS組合導航系統陀螺在線標定技術[J]. 計算機工程與應用, 2013, 49(19): 1-4. DOI: 10.3778/j.issn.1002-8331.1304-0227. ZHAO Hui, SHEN Jihong, CHEN Tao. On-line calibration of gyro in SINS/CNS integrated navigation system[J]. Computer Engineering and Applications, 2013, 49(19): 1-4. DOI: 10.3778/j.issn.1002-8331.1304-0227.

[11]HAO Yanling, MU Hongwei, LIU Xintao. On-line calibration technology for SINS/CNS based on MPF-KF[C]// Proceedings of 2012 IEEE International Conference on Mechatronics and Automation. Cheng Du: IEEE, 2012: 1132-1136. DOI: 10.1109/ICMA.2012.6283409.

[12]VETH M M,RAQUET J. Alignment and calibration of optical and inertial sensors using stellar observations[C]//Proceedings of the 18th International Technical Meeting of the Satellite Division of the Institute of Navigation. Long Beach, CA: IEEE, 2005:2494-2503.

[13]秦永元. 慣性導航[M]. 北京:科學出版社, 2006:358-359.

[14]ZONG Hua, WANG Bo, LIU Zhun, et al. SINS/CNS/GNSS integrated navigation scheme for advanced upper stage[C]// Proceedings of the 2014 International Conference on Mechatronics and Control. Jinzhou: IEEE, 2014:2109-2113. DOI: 10.1109/ICMC.2014.7231938.

[15]萬德鈞, 房建成. 慣性導航初始對準[M]. 南京: 東南大學出版社, 1998: 100-106.

[16]周衛東,蔡佳楠,孫龍, 等. GPS/SINS超緊組合導航系統可觀測性分析[J]. 北京航空航天大學學報, 2013, 39(9): 1157-1162. ZHOU Weidong, CAI Jianan, SUN Long, et al. Observability analysis of GPS/SINS ultra-tightly coupled system [J]. Journal of Beijing University of Aeronautics and Astronautics, 2013, 39(9): 1157-1162.

(編輯 張 紅)

On-line errors calibration method for INS/CNS/GNSS integrated navigation

ZONG Hua1, QI Jianyu1, XIONG Pan3, WANG Bo2, LIU Zhun1, ZHOU Zhiqiang2

(1.State Key Laboratory of Science and Technology on Aerospace Intelligent Control (Beijing Aerospace Automatic Control Institute),Beijing 100854, China; 2. School of Automation, Beijing Institute of Technology, Beijing 100081, China;3.School of Astronautics， Harbin Institute of Technology, Harbin 150001, China)

To improve the accuracy of the integrated navigation system INS(Inertial Navigation System)/CNS(Celestial Navigation System)/GNSS(Global Navigation Satellite Systems) integrated navigation system for long-time working, this paper establishes the integrated navigation system state equation and the observation equation based on quaternion errors, and analyzes the observability and observable degree of the system states by applying Piece-Wise Constant System(PWCS) theory for different input conditions. Based on the analysis of the observability and observable degree, using the measurement information of attitude and position vectors from star sensor and GNSS, this paper proposes a method of on-line calibration for the error parameters of gyros, accelerometers and star sensor. The observability analysis and simulation results show that this method can precisely estimate the scale factor, zero,and installation error of gyros, accelerators and star sensor. This method can improve the long-term working accuracy of INS/CNS/GNSS integrated navigation system and has an important engineering application value.

integrated navigation; quaternion errors; Kalman filter; on-line calibration; observable degree; star sensor; scale factor; installation error

10.11918/j.issn.0367-6234.201509109

2015-09-23

國家高技術研究發展計劃(2015AA7026083)

蹤 華(1978—)，女，博士，高工； 汪 渤(1963—)，男，教授，博士生導師

蹤 華，zonghua3@sina.cn

V249.3

A

0367-6234(2017)04-0088-07