基于递推最小二乘算法的惯导姿态误差动态标定方法✴

张同双，钟德安，李晓勇，冯鸿奎，杨磊

（中国卫星海上测控部，江苏江阴214431）

基于递推最小二乘算法的惯导姿态误差动态标定方法✴

张同双，钟德安，李晓勇，冯鸿奎，杨磊

（中国卫星海上测控部，江苏江阴214431）

在分析最小二乘算法原理的基础上，提出了一种基于递推最小二乘算法的惯导姿态误差动态标定方法，建立了计算模型，进行了仿真分析。仿真结果表明，该方法具有较高的解算精度和计算效率，航向误差解算精度优于3.5″，水平误差解算精度优于0.15″。该方法解决了动态条件下惯导姿态误差实时标定的技术难题，对提高惯导姿态测量精度和测量船外测精度具有重要意义。

航天测量船；惯性导航系统；姿态误差；递推最小二乘；动态标定

1 引 言

惯性导航系统的姿态测量精度直接影响航天测量船船载测量设备的外测精度，任务中必须对其进行精确标定。工程实践中，惯性导航系统的航向误差可以通过标校经纬仪测星解决；而水平姿态测量误差由于缺乏高精度的水平姿态测量基准，是一个困扰多年的技术难题。

文献［1］在总结常用水平姿态求解算法的基础上，提出了一种基于标校经纬仪俯仰脱靶量的惯导水平姿态标定算法。该方法可对惯性导航系统的水平姿态误差进行动态标定，但不能标定惯性导航系统的航向误差。文献［2］提出了最小二乘算法（Least Squares，LS），可以同时解算惯性导航系统的航向与水平姿态误差，但存在计算量大、实时性差等局限性，很难推广应用。

本文提出了一种基于递推最小二乘算法（Recursive Least Squares，RLS）［3］的惯导姿态误差分离算法。该方法克服了最小二乘算法数据存储量大、计算时间长等局限性，具有计算量小、精度高、收敛速度快等优点，可实时解算全参数惯导姿态误差。

2 LS算法原理及不足

文献［1，2］给出了利用标校经纬仪解算惯导姿态角误差的数学模型：

式中，ΔAi、ΔEi分别为i时刻由惯导姿态误差所引起的方位、俯仰测角误差，Δk、Δψ和Δθ分别为惯导航向、纵摇及横摇误差，Aci、Eci分别为标校经纬仪方位、俯仰编码器读数，e1i和e2i分别为方位、俯仰角度随机误差。

LS算法就是利用上述测角误差方程进行解算的，其解为

式中，＾X为未知参量X的估计值，L为观测向量，A为待估计参数的系数矩阵，上标T表示矩阵转置，对应表达式如下：

式中，¯Aci、¯Eci表示第i颗星的标校经纬仪测角平均值，n为测星总数。

该法的优点是可同时解算惯导航向与水平姿态角误差，但存在以下不足：

（1）由于船摇因素的影响，标校经纬仪的方位角和俯仰角是不断变化的，用每颗星的标校经纬仪测角平均值进行解算必然带来较大的误差；

（2）LS算法是一种增长记忆估计算法，解算过程中需要存储并利用所有历史数据，因而随着测星数量的增多，计算量和资源占用将越来越大，因而不能满足实时解算要求。

3 RLS算法

为了解决以上不足，20世纪中叶出现了RLS算法，它具有计算量小、收敛速度快、实时性强及不需求解逆矩阵等优点。

RLS算法过程如下：

式中，K（i）为增益矩阵，P（i）为状态估计协方差矩阵，Φ（i）为状态一步转移矩阵。

4 算 法设计与实现

4.1 标校经纬仪测角误差的获取

4.1.1 标校经纬仪测星

惯导开机并稳定工作，在时统的配合下，标校经纬仪进入测星程序。

4.1.2 脱靶量修正

对标校经纬仪实测甲板角进行脱靶量修正，修正公式如下：

式中，Ajit、Ejit分别为经脱靶量修正后的方位、俯仰角度值，δAi、δEi分别为标校经纬仪方位、俯仰脱靶量。

4.1.3 轴系误差与零位误差修正

对经脱靶量修正后的实测甲板角度值进行轴系、零位误差修正，修正公式如下：

式中，Aji、Eji为经轴系和零位误差修正后的方位、俯仰角度值，g为方位零位，c为照准差，I为方位转盘最大倾斜量，Am为最大倾斜方向，b为横轴差，h为俯仰零位。

4.1.4 蒙气差修正

蒙气差是光学测量设备主要误差源［4］，蒙气差主要影响俯仰角度测量值，工程应用中常采用式（8）近似公式进行修正：

式中，Δρi为蒙气差：

式中，p为大气压强，单位为hPa；t为环境温度，单位为℃。

4.1.5 将标校经纬仪甲板角度值转换至惯导地平坐标系

对式（7）、（8）分别得到的Aji和Eji进行船摇修正，转换至惯导地平坐标系，结果如式（10）～（14）所示［5］。

式中，Adi、Edi分别为惯导地平坐标系中标校经纬仪方位、俯仰角度值，（xgi，ygi，zgi）为标校经纬仪甲板实测角度值转换至惯导地平坐标系后的直角坐标值。xgi、ygi、zgi计算方法见式（12）～（14）。

式中，ψi、θi、ki分别为i时刻惯导纵摇角、横摇角与航向角输出值。

4.1.6 方位、俯仰测角误差计算

式中，ΔA0i、ΔE0i分别为i时刻惯导地平坐标系下某颗恒星相对标校经纬仪的理论方位角和俯仰角。

4.2 RLS算法流程

RLS解算时采用如下两个方程依次进行迭代：式中，Z1（i）=ΔAi，Z2（i）=ΔEi，R1（i）=e1i，

具体流程如下。

（1）初始化：初始化参数包括向量＾X（i-1）和状态估计协方差矩阵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）：根据

式（16）按照式（3）～（5）分别计算方位增益矩阵K1（i）、状态估计值＾X（i）及方位协方差矩阵P1（i）。

（4）用俯仰测角误差进行迭代求解＾X（i）：将步骤3获得的状态估计值＾X（i）作为新的状态初始值，并根据式（17）进行RLS迭代，获得新的＾X（i）、K2（i）、P2（i）。

（5）状态更新：将获得的＾X（i）、P1（i）和P2（i）作为新的初始化参数，重复步骤2～4，直至迭代结束。

5 算 法效果分析

5.1 RLS算法性能分析

图1～3分别为利用某船标校经纬仪测星数据解算惯1、惯2的航向、纵摇及横摇协方差估计结果。

图1 航向估计协方差Fig.1 Covariance of course estimation

图2 纵摇估计协方差Fig.2 Covariance of pitch estimation

图3 横摇估计协方差Fig.3 Covariance of roll estimation

由图可见：经过约200次迭代后，估计协方差急剧减小，说明RLS算法的收敛速度非常快；航向、纵摇及横摇估计协方差分别为（0.022″）2、（0.023″）2和（0.02″）2，估计精度满足惯性导航系统的姿态标定要求。

5.2 精度分析

5.2.1 航向解算精度

表1列出了两套惯导航向误差的实测值和RLS解算结果。

表1 惯导航向误差解算结果Table 1 Calculation resultof course error

由表1可见：利用RLS解算与标校经纬仪实测结果的差值最大为-6.9″，最小为-0.1″，平均解算误差为-3.5″，两者结果一致性非常好。惯导航向误差曲线如图4所示。

图4 航向误差解算结果Fig.4 Calculation result of course error

5.2.2 水平误差解算精度

表2和表3列出了两套惯导水平姿态误差解算结果，由于动态情况下无比对基准，采用水平作差法进行评估。

表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算法用于标定惯导姿态角误差在技术上是可行的，与其它算法比较，具有计算量小、资源占用少、收敛速度快及估计精度高等优点。该算法可同时得到两套惯导的全参数姿态误差信息，对提高惯导姿态测量精度进而提高测量船外测精度具有重要意义。

由于标校经纬仪方位、俯仰测角误差存在一定的相关性，因此，实际解算过程中，采用方位、俯仰测角误差依次迭代的算法，理论上它是非最优的，这点是以后需要解决的技术难题，但从工程应用角度看，其精度已足以满足惯导姿态误差标定的精度要求。

［1］周朝猛，朱伟康，张同双，等.基于星体测量的惯导水平姿态标定技术［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］.现代雷达，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］姚天任，孙洪.现代数字信号处理［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］.光子学报，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年获硕士学位，现为高级工程师，主要研究方向为测量船标校技术；

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—），男，湖南新宁人，现为高级工程师，主要研究方向为弹道数据处理、精度分析；

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年获学士学位，现为高级工程师，主要研究方向为船姿船位测量系统技术；

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年获硕士学位，现为工程师，主要研究方向为数据处理技术。

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，FENGHong-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年获硕士学位，现为高级工程师，主要研究方向为船姿船位测量系统、标校与校飞技术；

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

2011-04-07；

2011-05-31