基于遞推最小二乘算法的慣導姿態(tài)誤差動態(tài)標定方法?

張同雙，鐘德安，李曉勇，馮鴻奎，楊磊

（中國衛(wèi)星海上測控部，江蘇江陰214431）

基于遞推最小二乘算法的慣導姿態(tài)誤差動態(tài)標定方法?

張同雙，鐘德安，李曉勇，馮鴻奎，楊磊

（中國衛(wèi)星海上測控部，江蘇江陰214431）

在分析最小二乘算法原理的基礎上，提出了一種基于遞推最小二乘算法的慣導姿態(tài)誤差動態(tài)標定方法，建立了計算模型，進行了仿真分析。仿真結果表明，該方法具有較高的解算精度和計算效率，航向誤差解算精度優(yōu)于3.5″，水平誤差解算精度優(yōu)于0.15″。該方法解決了動態(tài)條件下慣導姿態(tài)誤差實時標定的技術難題，對提高慣導姿態(tài)測量精度和測量船外測精度具有重要意義。

航天測量船；慣性導航系統(tǒng)；姿態(tài)誤差；遞推最小二乘；動態(tài)標定

1 引 言

慣性導航系統(tǒng)的姿態(tài)測量精度直接影響航天測量船船載測量設備的外測精度，任務中必須對其進行精確標定。工程實踐中，慣性導航系統(tǒng)的航向誤差可以通過標校經(jīng)緯儀測星解決；而水平姿態(tài)測量誤差由于缺乏高精度的水平姿態(tài)測量基準，是一個困擾多年的技術難題。

文獻［1］在總結常用水平姿態(tài)求解算法的基礎上，提出了一種基于標校經(jīng)緯儀俯仰脫靶量的慣導水平姿態(tài)標定算法。該方法可對慣性導航系統(tǒng)的水平姿態(tài)誤差進行動態(tài)標定，但不能標定慣性導航系統(tǒng)的航向誤差。文獻［2］提出了最小二乘算法（Least Squares，LS），可以同時解算慣性導航系統(tǒng)的航向與水平姿態(tài)誤差，但存在計算量大、實時性差等局限性，很難推廣應用。

本文提出了一種基于遞推最小二乘算法（Recursive Least Squares，RLS）［3］的慣導姿態(tài)誤差分離算法。該方法克服了最小二乘算法數(shù)據(jù)存儲量大、計算時間長等局限性，具有計算量小、精度高、收斂速度快等優(yōu)點，可實時解算全參數(shù)慣導姿態(tài)誤差。

2 LS算法原理及不足

文獻［1，2］給出了利用標校經(jīng)緯儀解算慣導姿態(tài)角誤差的數(shù)學模型：

式中，ΔAi、ΔEi分別為i時刻由慣導姿態(tài)誤差所引起的方位、俯仰測角誤差，Δk、Δψ和Δθ分別為慣導航向、縱搖及橫搖誤差，Aci、Eci分別為標校經(jīng)緯儀方位、俯仰編碼器讀數(shù)，e1i和e2i分別為方位、俯仰角度隨機誤差。

LS算法就是利用上述測角誤差方程進行解算的，其解為

式中，＾X為未知參量X的估計值，L為觀測向量，A為待估計參數(shù)的系數(shù)矩陣，上標T表示矩陣轉置，對應表達式如下：

式中，ˉAci、ˉEci表示第i顆星的標校經(jīng)緯儀測角平均值，n為測星總數(shù)。

該法的優(yōu)點是可同時解算慣導航向與水平姿態(tài)角誤差，但存在以下不足：

（1）由于船搖因素的影響，標校經(jīng)緯儀的方位角和俯仰角是不斷變化的，用每顆星的標校經(jīng)緯儀測角平均值進行解算必然帶來較大的誤差；

（2）LS算法是一種增長記憶估計算法，解算過程中需要存儲并利用所有歷史數(shù)據(jù)，因而隨著測星數(shù)量的增多，計算量和資源占用將越來越大，因而不能滿足實時解算要求。

3 RLS算法

為了解決以上不足，20世紀中葉出現(xiàn)了RLS算法，它具有計算量小、收斂速度快、實時性強及不需求解逆矩陣等優(yōu)點。

RLS算法過程如下：

式中，K（i）為增益矩陣，P（i）為狀態(tài)估計協(xié)方差矩陣，Φ（i）為狀態(tài)一步轉移矩陣。

4 算 法設計與實現(xiàn)

4.1 標校經(jīng)緯儀測角誤差的獲取

4.1.1 標校經(jīng)緯儀測星

慣導開機并穩(wěn)定工作，在時統(tǒng)的配合下，標校經(jīng)緯儀進入測星程序。

4.1.2 脫靶量修正

對標校經(jīng)緯儀實測甲板角進行脫靶量修正，修正公式如下：

式中，Ajit、Ejit分別為經(jīng)脫靶量修正后的方位、俯仰角度值，δAi、δEi分別為標校經(jīng)緯儀方位、俯仰脫靶量。

4.1.3 軸系誤差與零位誤差修正

對經(jīng)脫靶量修正后的實測甲板角度值進行軸系、零位誤差修正，修正公式如下：

式中，Aji、Eji為經(jīng)軸系和零位誤差修正后的方位、俯仰角度值，g為方位零位，c為照準差，I為方位轉盤最大傾斜量，Am為最大傾斜方向，b為橫軸差，h為俯仰零位。

4.1.4 蒙氣差修正

蒙氣差是光學測量設備主要誤差源［4］，蒙氣差主要影響俯仰角度測量值，工程應用中常采用式（8）近似公式進行修正：

式中，Δρi為蒙氣差：

式中，p為大氣壓強，單位為hPa；t為環(huán)境溫度，單位為℃。

4.1.5 將標校經(jīng)緯儀甲板角度值轉換至慣導地平坐標系

對式（7）、（8）分別得到的Aji和Eji進行船搖修正，轉換至慣導地平坐標系，結果如式（10）～（14）所示［5］。

式中，Adi、Edi分別為慣導地平坐標系中標校經(jīng)緯儀方位、俯仰角度值，（xgi，ygi，zgi）為標校經(jīng)緯儀甲板實測角度值轉換至慣導地平坐標系后的直角坐標值。xgi、ygi、zgi計算方法見式（12）～（14）。

式中，ψi、θi、ki分別為i時刻慣導縱搖角、橫搖角與航向角輸出值。

4.1.6 方位、俯仰測角誤差計算

式中，ΔA0i、ΔE0i分別為i時刻慣導地平坐標系下某顆恒星相對標校經(jīng)緯儀的理論方位角和俯仰角。

4.2 RLS算法流程

RLS解算時采用如下兩個方程依次進行迭代：式中，Z1（i）=ΔAi，Z2（i）=ΔEi，R1（i）=e1i，

具體流程如下。

（1）初始化：初始化參數(shù)包括向量＾X（i-1）和狀態(tài)估計協(xié)方差矩陣P1（i-1）、P2（i-1）。計算時可設X（i-1）=0、P1（i-1）=P2（i-1）=100I3×3，其中I3×3為單位矩陣。

（2）計算方位、俯仰測角誤差：按式（6）～（15）計算方位、俯仰測角誤差。

（3）用方位測角誤差進行迭代求解＾X（i）：根據(jù)

式（16）按照式（3）～（5）分別計算方位增益矩陣K1（i）、狀態(tài)估計值＾X（i）及方位協(xié)方差矩陣P1（i）。

（4）用俯仰測角誤差進行迭代求解＾X（i）：將步驟3獲得的狀態(tài)估計值＾X（i）作為新的狀態(tài)初始值，并根據(jù)式（17）進行RLS迭代，獲得新的＾X（i）、K2（i）、P2（i）。

（5）狀態(tài)更新：將獲得的＾X（i）、P1（i）和P2（i）作為新的初始化參數(shù)，重復步驟2～4，直至迭代結束。

5 算 法效果分析

5.1 RLS算法性能分析

圖1～3分別為利用某船標校經(jīng)緯儀測星數(shù)據(jù)解算慣1、慣2的航向、縱搖及橫搖協(xié)方差估計結果。

圖1 航向估計協(xié)方差Fig.1 Covariance of course estimation

圖2 縱搖估計協(xié)方差Fig.2 Covariance of pitch estimation

圖3 橫搖估計協(xié)方差Fig.3 Covariance of roll estimation

由圖可見：經(jīng)過約200次迭代后，估計協(xié)方差急劇減小，說明RLS算法的收斂速度非常快；航向、縱搖及橫搖估計協(xié)方差分別為（0.022″）2、（0.023″）2和（0.02″）2，估計精度滿足慣性導航系統(tǒng)的姿態(tài)標定要求。

5.2 精度分析

5.2.1 航向解算精度

表1列出了兩套慣導航向誤差的實測值和RLS解算結果。

表1 慣導航向誤差解算結果Table 1 Calculation resultof course error

由表1可見：利用RLS解算與標校經(jīng)緯儀實測結果的差值最大為-6.9″，最小為-0.1″，平均解算誤差為-3.5″，兩者結果一致性非常好。慣導航向誤差曲線如圖4所示。

圖4 航向誤差解算結果Fig.4 Calculation result of course error

5.2.2 水平誤差解算精度

表2和表3列出了兩套慣導水平姿態(tài)誤差解算結果，由于動態(tài)情況下無比對基準，采用水平作差法進行評估。

表2 縱搖誤差解算結果Table 2 Calculation result of pitch error

表3 橫搖誤差解算結果Table 3 Calculation resultof roll error

由表2和表3可見：RLS解算縱搖最大誤差為1.3″，橫搖最大誤差為0.9″，縱搖平均誤差為0.15″，橫搖平均誤差為-0.15″，兩者一致性好。慣導縱搖、橫搖誤差曲線分別如圖5和圖6所示。

圖5 縱搖誤差解算結果Fig.5 Calculation result of pitch error

圖6 橫搖誤差解算結果Fig.6 Calculation result of roll error

6 結論

通過以上解算實例分析可知，RLS算法用于標定慣導姿態(tài)角誤差在技術上是可行的，與其它算法比較，具有計算量小、資源占用少、收斂速度快及估計精度高等優(yōu)點。該算法可同時得到兩套慣導的全參數(shù)姿態(tài)誤差信息，對提高慣導姿態(tài)測量精度進而提高測量船外測精度具有重要意義。

由于標校經(jīng)緯儀方位、俯仰測角誤差存在一定的相關性，因此，實際解算過程中，采用方位、俯仰測角誤差依次迭代的算法，理論上它是非最優(yōu)的，這點是以后需要解決的技術難題，但從工程應用角度看，其精度已足以滿足慣導姿態(tài)誤差標定的精度要求。

［1］周朝猛，朱偉康，張同雙，等.基于星體測量的慣導水平姿態(tài)標定技術［J］.中國慣性技術學報，2009（3）：253-257. ZHOU Chao-meng，ZHU Wei-kang，ZHANG Tongshuang，et al.Horizontal attitude calibration technology of inertial navigation based on star measurement［J］.Journal of Chinese Inertial Technology，2009，17（3）：253-257.（in Chinese）

［2］姚兆寧，孫小昶，李成文.艦載精密測量雷達星體標校方法及應用［J］.現(xiàn)代雷達，1999，21（4）：7-12. YAOZhao-ning，SUN Xiao-chang，LICheng-wen.Shipborne accurate instrumentation radar star checkoutmethod and application［J］.Modern Radar，1999，21（4）：7-12.（in Chinese）

［3］姚天任，孫洪.現(xiàn)代數(shù)字信號處理［M］.武漢：華中理工大學出版社，2000. YAO Tian-ren，SUN Hong.Modern digital signal processing［M］.Wuhan：Huazhong University of Science and Technology Press，2000.（in Chinese）

［4］MacMillan D S，Ma C.Evaluation of very long baseline interferometry atmosphericmodeling improvements［J］.Journal of Geophysical Research，1994，99（B1）：637-651.

［5］李哲，蘇秀琴，喬永明，等.基于船姿測量的艦載光電標校經(jīng)緯儀視軸穩(wěn)定方法研究［J］.光子學報，2009（6）：1552-1556. LIZhe，SU Xiu-qin，QIAO Yong-ming，et al.Study LOS Stabilization of Warship-based Electro-optical Theodolite Based on Measure by Sensors of Vessel［J］.Acta Photonica Sinica，2009，38（6）：1552-1556.（in Chinese）

ZHANG Tong-shuangwas born in Suqian，Jiangsu Province，in 1968.He received the M.S.degree in 2006.He isnow a senior engineer.His research interests include attitude and position determination，calibration，flight test for TT＆C ship.

Email：zts-123＠163.com

鐘德安（1964—），男，江蘇江陰人，1990年獲碩士學位，現(xiàn)為高級工程師，主要研究方向為測量船標校技術；

ZHONGDe-an was born in Jiangyin，Jiangsu Province，in 1964. He received the M.S.degree in 1990.He is now a senior engineer. His research concerns calibration techniques for TT＆C ship.

李曉勇（1962—），男，湖南新寧人，現(xiàn)為高級工程師，主要研究方向為彈道數(shù)據(jù)處理、精度分析；

LIXiao-yong was born in Xinning，Hunan Province，in 1962. He is now a senior engineer.His research concerns ballistic data processing and accuracy analysis.

Email：xyli212＠163.com

馮鴻奎（1971—），男，江蘇興化人，1994年獲學士學位，現(xiàn)為高級工程師，主要研究方向為船姿船位測量系統(tǒng)技術；

FENG Hong-kui was born in Xinghua，Jiangsu Province，in 1971.He received the B.S.degree in 1994.He is now a senior engineer.His research concerns general technology of the attitude and position determination system of TT＆C ship.

Email：fhk103＠hotmail.com

楊磊（1981—），男，江蘇無錫人，2007年獲碩士學位，現(xiàn)為工程師，主要研究方向為數(shù)據(jù)處理技術。

YANG Leiwas born in Wuxi，Jiangsu Province，in 1981.He received theM.S.degree in 2007.He received theM.S.degree in 2007.He is now an engineer.His research concerns data processing technique.

Dynam ic Calibration M ethod of Attitude Error for Inertial Navigation System Based on RLS Algorithm

ZHANG Tong-shuang，ZHONGDe-an，LIXiao-yong，F(xiàn)ENGHong-kui，YANG Lei
（China Satellite Maritime Tracking and Controlling Department，Jiangyin 214431，China）

Based on analysing the principle of the Least Squares（LS）algorithm，a new dynamic calibration ofattitude error for inertialnavigation system（INS）based on the Recursive Least Squares（RLS）algorithm is presented，and a newmodel is proposed.Simulation results shows that thismethod can improve the estimation precision and efficiency，the course error is less than 3.5″，and the horizontal attitude error is less than 0.15″.It can solve the problem of real-time calibration of attitude error for INS in dynamic condition.It can also improve the attitudemeasurement precision of INS and the exterior ballistic measurement precision of the space tracking，

telemetering and command（TT＆C）ship.

space TT＆C ship；inertial navigation system；attitude error；RLS；dynamic calibration

V557；U666.1

A

10.3969／j.issn.1001-893x.2011.08.003

張同雙（1968—），男，江蘇宿遷人，2006年獲碩士學位，現(xiàn)為高級工程師，主要研究方向為船姿船位測量系統(tǒng)、標校與校飛技術；

1001-893X（2011）08-0011-05

2011-04-07；

2011-05-31