捷联航姿系统航行状态下的组合对准算法研究

ＶｏＩ．３５　Ｎｏ．２　６０　舰船电子工程　Ｓｈｉｐ　Ｅｌｅｃｔｒｏｎｉｃ　Ｅｎｇｉｎｅｅｒｉｎｇ　总第２４８期　

２０１５年第２期　

捷联航姿系统航行状态下的组合对准算法研究　李莹　杨新。李波。　（１．９２９４１部队９５分队葫芦岛１２５００１）（２．北京西三环中路１９号北京１００８４１）　（３．天津市公安边防总队天津３００４５７）　

摘要由于捷联航姿设备无纬度解算值，导致跨纬度航行时航向误差过大，引入计程仪速度与航姿设备输出姿态、航　向一起对纬度值进行计算，航行状态引入计程仪信息后建立数学模型进行初始对准，经航行实验验证，７２小时内航向角误　差得到了极大的缩小，验证了该数学模型的可行性，可提高设备的精度，具有实际使用意义。　关键词捷联航姿；姿态矩阵；数学模型；计程仪速度；卡尔曼滤波器　中图分类号Ｕ６６６．１　ＤＯＩ：１０．３９６９／ｊ．ｉｓｓｎ１６７２—９７３０．２０１５．０２．０１６　

Ｉｎｔｅｇｒａｔｅｄ　Ａｌｇｎｍｅｎｔ　Ａｌｇｏｒｉｔｈｍ　ｏｆ　ｔｈｅ　Ｓｔｒａｐｄｏｗｎ　Ａｔｔｉｔｕｄｅ　Ｓｙｓｔｅｍ　ｉｎ　Ｓａｉｌｉｎｇ　

ＬＩ　Ｙｉｎｇ　ＹＡＮＧ　Ｘｉｎ　ＬＩ　Ｂｏ。　（１．Ｕｎｉｔ　９５，Ｎｏ．９２９４１　Ｔｒｏｏｐｓ　ｏｆ　ＰＬＡ，Ｈｕｌｕｄａｏ　１２５００１）（２．Ｎｏ．１９　Ｃｅｎｔｒａｌ　Ｘｉｓａｎｈｕａｎ　Ｒｏａｄ，Ｂｅｉｊｎｇ　１００８４１）　（３．Ｂｏｒｄｅｒ　Ｃｏｎｔｒｏｌ　Ｂｕｒｅａｕ　ｏｆ　Ｔｉａｎｊｉｎ　Ｐｕｂｌｉｃ　Ｓｅｃｕｒｉｔｙ。Ｔｉａｎｊｉｎ　３００４５７）　

Ａｂｓｔｒａｃｔ　Ｔｈｅ　ｓｔｒａｐｄｏｗｎ　ａｔｔｉｔｕｄｅ　ｓｙｓｔｅｍ　ｈａｓ　ｎｏ　ｌａｔｉｔｕｄｅ　ｔｏ　ｂｅ　ｃａｌｃｕｌａｔｅｄ，ｃａｕｓｉｎｇ　ｈｅａｄｉｎｇ’Ｓ　ｅｒｒｏｒｓ　ｔｏｏ　ｌａｒｇｅ　ｗｈｅｎ　ｔｈｅ　ｗａｒｓｈｉｐ　ｉｓ　ｃｒｏｓｓ—ｌａｔｉｔｕｄｅｓ　ｓａｉｌｉｎｇ．Ｌｏｇ’Ｓ　ｖｅｌｏｃｉｔｙ　ｗｉｔｈ　ａｔｔｉｔｕｄｅ　ａｎｄ　ｃｏｕｒｓｅ　ｏｆ　ｔｈｅ　ｓｔｒａｐｄｏｗｎ　ａｔｔｉｔｕｄｅ　ｓｙｓｔｅｍ　ｉｓ　ｉｍｐｏｒｔｅｄ　ｔｏ　ｃａｌ—　ｃｕｌａｔｅ　ｌａｔｉｔｕｄｅ，ｔｈｅｎ　ｍａｔｈｅｍａｔｉｃｓ　ｍｏｄｅｌ　ｉｓ　ｅｓｔａｂｌｉｓｈｅｄ　ｔｏ　ｐｒｏｃｅｓｓ　ｔｈｅ　ｉｎｉｔｉａｌ　ａｌｉｇｎｍｅｎｔ．Ｔｈｒｏｕｇｈ　ｓａｉｌｉｎｇ　ｔｅｓｔ，ｃｏｕｒｓｅ　ｅｒｒｏｒ’Ｓ　ｓｗｉｎｇ　ｒｅｄｕｃｅｄ　ｍｕｃｈ　ｉｎ　７２ｈ，ｔｈｅ　ｐｏｓｓｉｂｉｌｉｔｙ　ｏｆ　ｔｈｅ　ｍａｔｈｅｍａｔｉｃｓ’ｍｏｄｅｌ　ｉｓ　ｖｅｒｉｆｉｅｄ　ｔｈａｔ　ｉｔ　ｃａｎ　ｒａｉｓｅ　ｔｈｅ　ａｃｃｕｒａｃｙ　ｏｆ　ｔｈｅ　ｓｙｓｔｅｍ　ａｎｄ　ｈａｓ　ａｃｔｕａｌ　ｕｓａｇｅ　ｍｅａｎｉｎｇ．　Ｋｅｙ　Ｗｏｒｄｓ　ｓｔｒａｐｄｏｗｎ　ａｔｔｉｔｕｄｅ，ａｔｔｉｔｕｄｅ　ｍａｔｒｉｘ，ｍａｔｈｅｍａｔｉｃｓ　ｍｏｄｅｌ，ｌｏｇ’Ｓ　ｖｅｌｏｃｉｔｙ，Ｋａｌｍａｎ　ｆｉｌｔｅｒ　Ｃｌａｓｓ　Ｎｕｍｂｅｒ　ＩＪ６６６．】　

１　引言　惯性导航系统是根据测得的运载体的加速度，　经过积分运算求得速度与位置的，为此，必须知道　初始速度和初始位置。此外，以地理坐标系为导航　坐标系的惯导系统（包括平台式和捷联式），物理平　台和数学平台都是测量加速度的基准，而且平台必　须准确地对准和跟踪地理坐标系，以避免由平台误　差引起加速度测量误差。在惯导系统加电启动后，　平台的三轴指向是任意的，可不在水平面内，又没　有确定的方位，因此在系统进入导航工作状态前，　必须将平台的指向对准，此过程便称为惯导系统的　初始对准＿１￣３］。初始对准的精度直接关系到惯导　系统的工作精度，初始对准的时间是惯导系统的重　要战术技术指标。由于航姿设备无纬度解算值，需　要引入外部位置信息或速度信息，同时，在几乎所　有舰艇上均有计程仪设备，本文基于此引入计程仪　速度与航姿设备输出姿态、航向一起对跨纬度值进　行计算，经实际使用证明可大幅提高航姿设备的精　度。　

２精对准方程的建立　对于捷联系统，由于捷联矩阵起到了平台的作　用，因此导航工作一开始就需要获得捷联矩阵的初　始值，显然捷联系统的初始对准就是确定捷联矩阵　的初始值。捷联式的姿态矩阵实际上是运载体坐　

＊收稿日期：２０１４年８月３日，修回日期：２０１４年９月１１日　作者简介：李莹，女，工程师，研究方向：惯性导航系统试验与数据处理。　２０１５年第２期　舰船电子工程　标系和地理坐标系之间的方向余弦矩阵。由于粗　对准确定的姿态矩阵还有较大误差，因此实际建立　的导航坐标系　与理想导航坐标系　仍存在偏差　角，即失准角　］。精对准就是估计出失准角，对　粗对准确定的姿态矩阵做一次性修正。　在组合滤波精对准中采用Ｋａｌｍａｎ滤波器进　行状态估计，需要建立航姿设备的对准阶段误差模　型［　～　。　采用地理坐标系作为导航坐标系时简化的误　差模型为　一一　＋（　ｓｉｎＬ￣　（ｃｏ￣ｃｏｓｇ　）丸　ｎＬ＋　）　一　一ｔａｎＬ　＋（　ｃ。ｓＬ＋　Ｖｅ　１＿　Ｖ．　＋ｅ　＝＝＝　ｇｎ　ｔａｎＬ　＋（２　ｓｉｎＬ＋￣ｔａｎＬ）３Ｖ．　＋丸－厂　一　厂　＋Ｖ　一一（２　ｓｉｎＬ＋　ｔａｎＬ）　＋｛５　ｆ　－－￣　厂　＋Ｖ　（１）　式中：　，　，丸为计算导航坐标系和导航坐标系之　间的失准角；　，　为载体东向和北向速度误差；　，　为载体的东向速度和北向速度；Ｒ　，Ｒ　为载　体所在点的子午曲率半径和卯酉曲率半径；Ｌ，　分别为当地地理纬度和地球自转角速度；　，　，ｆｕ　为比力在导航坐标系下的投影　，ｅ　，ｅ　为陀螺的　器件误差，￡　，ｅ　，ｅ。在导航坐标系下的投影有　［ｅ　ｅ　ｅ　］　一　［ｅ　￡　￡　］　，　，Ｖ　为加速度计器件误差，Ｖ　，　，　在导　航坐标系下的投影有　［ｖ　ｖ　ｖ　］　一　［　Ｖ　Ｖ　］　在对准阶段，捷联航姿设备的误差模型中包括　陀螺和加速度计的器件误差。器件误差包含三种　分量：随机常值误差、一阶马尔可夫过程和白噪声　误差。马尔可夫过程的相关时间一般大于１小时，　所以这种误差可近似视为随机常数，且与随机常值　漂移相比小１～２个数量级。所以，陀螺和加速度　计误差模型可简化为　如一０　ｅ—ｅ６＋　Ｖ６—０　Ｖ＝Ｖ＾＋ａ　（２）　式中，ｅ一［ｅ　ｅ　ｅ　］　，Ｖ＿－Ｅｖ　Ｖ　Ｖ　］　都是投　影在载体坐标系中。￡　，ｖ６为器件随机常值误差，　，ａ为器件白噪声误差。　将陀螺仪和加速度计随机常值误差ｅ。，Ｖ　扩充　为状态变量。此时，十阶的系统状态方程可以写成　如下形式：　（　）一Ａ（　）Ｘ（￡）４－Ｂ（　）Ｖ　（　）　（３）　式中的状态向量和系统噪声分别为　ｘ（　）一［　Ｖ　≠　声　Ｖｋ　Ｖ　￡ｋ　ｅ　ｅｋ］　Ｗ（￡）一Ｅａ　ａ　叫　０　０　０　０　Ｏ］　（４）　

３航向状态下组合对准模型的建立　由于航姿设备无纬度解算值，需要引入外部位　置信息或速度信息［　卅］，同时，在几乎所有舰艇上　均有计程仪设备，可引入计程仪速度与航姿设备输　出姿态、航向一起对跨纬度值进行计算，舰艇跨纬　度值由舰艇航行情况确定。　外部计程仪提供准确的参考水平速度信息，航　姿设备工作在捷联惯性导航阶段，实时解算出水平　速度，解算出水平速度与外部参考速度之差作为观　测量，则观测方程为　Ｚ（　）一Ｈ（￡）Ｘ（￡）＋　（￡）　（５）　式中，系统观测矩阵Ｈｃ　一［　；　：　，　

量测噪声　（　）一［　］　，其方差为Ｒ（　）。　连续的动态系统需离散化后才能在机上运行，　式（３）与式（５）所描述的系统的离散化形式为　ｆ　一　１　一１＋ｗ　…　【Ｚｋ＝＝＝Ｈ　Ｘ　＋　＋　式中　为一步转移矩阵；Ｗ　为系统激励白噪声　序列；　为量测白噪声序列，　为不确定性输入　（外界环境所造成的随机干扰输入）。同时Ｗ　、　满足：　ｆＥ［　］＝＝：０　Ｅ［Ｗ　］＝＝＝Ｑ　ｆ　Ｅ［　］一０　Ｅ　ｒ－Ｖｋ　］一尺　，　（７）　ｌＥｒＷ　Ｖ　］一０　下面给出离散卡尔曼滤波方程：　１）状态一步预测：　Ｘ＾，　一１一　．　１　ｘ　一１；　２）状态估计：　

一　１

＋Ｋ　（　一Ｈｋ　一１）；　

３）滤波增益：　Ｋ　一Ｐ　一１Ｈ　（Ｈ　Ｐ　一１Ｈ　＋Ｒ　）＿。　４）一步预测均方误差：　Ｐ　，＾一１一ｌ　，　一１　

Ｐ　一ｌ・　一１＋Ｂ　一１　ＱＢＬ１；　

５）估计均方误差：　（下转第１４５页）　２０１５年第２期　舰船电子工程　１４５　５５．　［６］郑连兴，倪育德．ＤＶＯＲ　ＶＲＢ－５１Ｄ多普勒全向信标　［Ｍ］．北京：中国民航出版社，１９９６：２３—４０．　［７］詹艳艳．基于ＤＤＳ的波形信号发生器的设计［Ｊ］．沈阳　理工大学学报，２００８，２７（３）：３５—３８．　［８］许朋，乔龙飞，何明．高性能ＤＤＳ芯片ＡＤ９９５４及其应　

用＿Ｊ］．新特器件应用，２０１１，１３（２）：１—６．　［９］江贤志，左传友，刘华章．基于Ｃ８０５１Ｆ０２０单片机的实　时测控装置设计［Ｊ］．现代电子技术，２０１３（２）：２０—２３．　［１Ｏ］赵玉峰，杨世彦，盖晓东．基于Ｃ８０５１Ｆ０２０单片机的智　能燃烧控制器设计［Ｊ］．电子器件，２００６（４）：１１５５—　１ｔ５７．　

开　舔　舔　不　乖　乖　乖　毋　不　不　乖　不　乔　不　不　．　钚　不　不　坏　矫　！坏　不　不　：　．　（２－接第６１页）　Ｐ　一（　—Ｋ　Ｈ　）Ｐ　一ｌ（　—Ｋ＾Ｈ　）　＋Ｋ　Ｒ　Ｋｌ　系统由于存在不确定性量测输入（随机干扰），　为了保证卡尔曼滤波的性能，滤波方程中用调谐后　的Ｒ　阵代替Ｒ阵，由于Ｒ　的调谐有一个相当大　的范围，因此可以通过仿真确定Ｒ　阵。　估计出状态　，　，　后，可以得到更为精确的　捷联矩阵　：　—　，ＣＺ一（Ｊ＋　×）　（８）　式中，　为粗对准确定的捷联矩阵；　×为　，　，≯　构成的反对称矩阵　４航行试验和仿真试验结果对比　辆　匠　＾　；　！Ｖ　ｉ　；　图１　组合计程仪信息的航行对准后　继续航行７２ｈ的航向一次差曲线　４Ｏ　３Ｏ　２Ｏ　１０　一　。０　摄一２Ｏ　一３Ｏ　一４Ｏ　厂　．、　■…ｌ／　，　＼　＼　／　、　．厂　＼　Ｉ　｜　－　Ｗ　。　０　１ｏ　２０　３０　４０　５０　６Ｏ　７０　时间，，Ｊ、时　图２　未组合计程仪信息的航行对准后　继续航行７２ｈ的航向一次差仿真曲线　论文进行了航行状态下的组合对准方法实验，　并继续航行了７２ｈ，航向一次差曲线如图１所示。　随后使用该实船录取的数据，进行了未组合计程仪　信息的事后仿真，时间同样为７２ｈ，一次差曲线如　图２所示。　５　结语　综上所述，对比图１和图２可以看出，航行状　态引人计程仪信息后，７２小时内航向角误差得到　了极大的缩小，极大的提高了设备精度，验证了该　数学模型的可行性。