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

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

基于遞推最小二乘算法的慣導(dǎo)姿態(tài)誤差動(dòng)態(tài)標(biāo)定方法?

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

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

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

航天測(cè)量船；慣性導(dǎo)航系統(tǒng)；姿態(tài)誤差；遞推最小二乘；動(dòng)態(tài)標(biāo)定

1 引 言

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

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

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

2 LS算法原理及不足

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

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

LS算法就是利用上述測(cè)角誤差方程進(jìn)行解算的，其解為

式中，＾X為未知參量X的估計(jì)值，L為觀測(cè)向量，A為待估計(jì)參數(shù)的系數(shù)矩陣，上標(biāo)T表示矩陣轉(zhuǎn)置，對(duì)應(yīng)表達(dá)式如下：

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

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

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

（2）LS算法是一種增長(zhǎng)記憶估計(jì)算法，解算過(guò)程中需要存儲(chǔ)并利用所有歷史數(shù)據(jù)，因而隨著測(cè)星數(shù)量的增多，計(jì)算量和資源占用將越來(lái)越大，因而不能滿(mǎn)足實(shí)時(shí)解算要求。

3 RLS算法

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

RLS算法過(guò)程如下：

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

4 算 法設(shè)計(jì)與實(shí)現(xiàn)

4.1 標(biāo)校經(jīng)緯儀測(cè)角誤差的獲取

4.1.1 標(biāo)校經(jīng)緯儀測(cè)星

慣導(dǎo)開(kāi)機(jī)并穩(wěn)定工作，在時(shí)統(tǒng)的配合下，標(biāo)校經(jīng)緯儀進(jìn)入測(cè)星程序。

4.1.2 脫靶量修正

對(duì)標(biāo)校經(jīng)緯儀實(shí)測(cè)甲板角進(jìn)行脫靶量修正，修正公式如下：

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

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

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

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

4.1.4 蒙氣差修正

蒙氣差是光學(xué)測(cè)量設(shè)備主要誤差源［4］，蒙氣差主要影響俯仰角度測(cè)量值，工程應(yīng)用中常采用式（8）近似公式進(jìn)行修正：

式中，Δρi為蒙氣差：

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

4.1.5 將標(biāo)校經(jīng)緯儀甲板角度值轉(zhuǎn)換至慣導(dǎo)地平坐標(biāo)系

對(duì)式（7）、（8）分別得到的Aji和Eji進(jìn)行船搖修正，轉(zhuǎn)換至慣導(dǎo)地平坐標(biāo)系，結(jié)果如式（10）～（14）所示［5］。

式中，Adi、Edi分別為慣導(dǎo)地平坐標(biāo)系中標(biāo)校經(jīng)緯儀方位、俯仰角度值，（xgi，ygi，zgi）為標(biāo)校經(jīng)緯儀甲板實(shí)測(cè)角度值轉(zhuǎn)換至慣導(dǎo)地平坐標(biāo)系后的直角坐標(biāo)值。xgi、ygi、zgi計(jì)算方法見(jiàn)式（12）～（14）。

式中，ψi、θi、ki分別為i時(shí)刻慣導(dǎo)縱搖角、橫搖角與航向角輸出值。

4.1.6 方位、俯仰測(cè)角誤差計(jì)算

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

4.2 RLS算法流程

RLS解算時(shí)采用如下兩個(gè)方程依次進(jìn)行迭代：式中，Z1（i）=ΔAi，Z2（i）=ΔEi，R1（i）=e1i，

具體流程如下。

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

（2）計(jì)算方位、俯仰測(cè)角誤差：按式（6）～（15）計(jì)算方位、俯仰測(cè)角誤差。

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

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

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

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

5 算 法效果分析

5.1 RLS算法性能分析

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

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

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

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

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

5.2 精度分析

5.2.1 航向解算精度

表1列出了兩套慣導(dǎo)航向誤差的實(shí)測(cè)值和RLS解算結(jié)果。

表1 慣導(dǎo)航向誤差解算結(jié)果Table 1 Calculation resultof course error

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

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

5.2.2 水平誤差解算精度

表2和表3列出了兩套慣導(dǎo)水平姿態(tài)誤差解算結(jié)果，由于動(dòng)態(tài)情況下無(wú)比對(duì)基準(zhǔn)，采用水平作差法進(jìn)行評(píng)估。

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

表3 橫搖誤差解算結(jié)果Table 3 Calculation resultof roll error

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

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

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

6 結(jié)論

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

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

［1］周朝猛，朱偉康，張同雙，等.基于星體測(cè)量的慣導(dǎo)水平姿態(tài)標(biāo)定技術(shù)［J］.中國(guó)慣性技術(shù)學(xué)報(bào)，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］姚兆寧，孫小昶，李成文.艦載精密測(cè)量雷達(dá)星體標(biāo)校方法及應(yīng)用［J］.現(xiàn)代雷達(dá)，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ù)字信號(hào)處理［M］.武漢：華中理工大學(xué)出版社，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］李哲，蘇秀琴，喬永明，等.基于船姿測(cè)量的艦載光電標(biāo)校經(jīng)緯儀視軸穩(wěn)定方法研究［J］.光子學(xué)報(bào)，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年獲碩士學(xué)位，現(xiàn)為高級(jí)工程師，主要研究方向?yàn)闇y(cè)量船標(biāo)校技術(shù)；

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)為高級(jí)工程師，主要研究方向?yà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年獲學(xué)士學(xué)位，現(xiàn)為高級(jí)工程師，主要研究方向?yàn)榇舜粶y(cè)量系統(tǒng)技術(shù)；

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—），男，江蘇無(wú)錫人，2007年獲碩士學(xué)位，現(xiàn)為工程師，主要研究方向?yàn)閿?shù)據(jù)處理技術(shù)。

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年獲碩士學(xué)位，現(xiàn)為高級(jí)工程師，主要研究方向?yàn)榇舜粶y(cè)量系統(tǒng)、標(biāo)校與校飛技術(shù)；

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

2011-04-07；

2011-05-31