初始方位信息辅助下潜航器快速传递对准算法

张 鹭，吴文启，王 林，铁俊波

（国防科技大学机电工程与自动化学院，长沙410073）

初始方位信息辅助下潜航器快速传递对准算法

张 鹭，吴文启，王 林，铁俊波

（国防科技大学机电工程与自动化学院，长沙410073）

针对多潜航器连续快速布放需求，提出基于初始方位信息辅助下快速传递对准算法。潜航器惯导系统只在对准初始时刻由移动基准惯导提供准确的方位信息和概略的水平姿态信息，布放入水后以初始时刻水平姿态误差和当前速度误差为滤波状态，以水下多普勒测速仪的速度为观测量，通过卡尔曼滤波进行惯性系下的航行中对准。实验验证以优于0.01（°）/h的激光陀螺惯导系统/卫星组合导航的姿态解算值为参考基准，基于实际数据的仿真计算表明，在50s内即可实现快速对准，方位精度达到1密位（1σ）。

捷联惯性导航；快速传递对准；方位信息辅助；多普勒测速仪

0　引言

潜航器布放时其惯性导航系统的初始对准可以选择采用组合导航初始对准的方式进行自对准［1］，也可以以舰船惯导系统或移动基准惯导系统为主惯导系统进行传递对准［2-3］。

采用组合导航自对准时不需要舰船惯导系统提供数据，但动基座环境下自对准通常需要较长时间才能完成初始对准过程，难以达到快速对准的要求。以主惯导或移动的基准惯导系统为信息源辅助子惯导系统进行传递对准，需要主惯导系统实时与子惯导系统进行一段时间的测量参数匹配或者导航参数匹配，按传统传递对准方式逐一进行传递对准时，仍难以实现短时间同时布放多个潜航器的任务。

本文提出一种初始方位信息辅助下潜航器快速传递对准新算法，由移动基准惯导在初始对准开始时刻提供潜航器惯导系统航向信息及概略的水平姿态信息后，潜航器随即布放入水，其惯导系统在水下多普勒测速仪的速度信息辅助下，在惯性系中估计初始水平姿态误差和当前的速度误差，进行组合导航自对准。由于通过移动基准惯导提供潜航器惯导系统开始时刻航向信息及概略的水平姿态信息的过程仅需不到10s，而入水后水平姿态误差和速度误差可通过卡尔曼滤波快速收敛，因此可实现短时间投放较多数量潜航器的快速对准要求。

1　坐标系及符号定义

1）地球坐标系（e系）:原点位于地心，ez轴沿地球自转轴方向，ex轴在赤道平面内且指向格林威治子午线，三轴构成右手正交坐标系。

2）导航坐标系:选取当地水平地理坐标（n系）为NED坐标系。

3）载体坐标系（b系）:轴向取为前右下。

4）多普勒坐标系（d系）:轴向依次为载体的前右下。

5）对准初始时刻导航坐标系（n0系）:对准初始时刻的地理坐标系，相对地球表面固定不动，不随导航系统在地球表面运动而运动。

6）对准初始时刻惯性导航系（in0系）:对准时刻n系与惯性空间固联得到，相对惯性空间不动。

7）对准初始时刻捷联惯组惯性坐标系（ib0系）:对准初始时刻b系与惯性空间固联得到，相对惯性空间固定不动。

2　基本思路

为实现潜航器能够快速初始对准的要求，移动基准惯导提供初始姿态信息即准确的方位信息（φ0）、存在误差的俯仰角）与横滚角快速对准原理如图1所示。由于在初始对准开始时，惯导解算速度误差大，因此直接用多普勒速度进行位置解算。对准结束后用组合导航速度进行位置解算。

图1　传递对准原理图Fig.1 Transfer alignment schematic diagram

3　导航解算

3.1潜航器入水后的姿态更新

潜航器惯导系统姿态矩阵Cnb的更新可表示为:

其中，ωie为地球自转角速度；t为初始时刻到当前时刻的时间；λ0、L0为初始对准时刻经纬度；λ、L为当前时刻经纬度。可由潜航器惯导（SINS）初始经纬度和t确定，求解可将其分解为:

其中，

由于对准时间短，忽略陀螺的常值零偏误差。

3.2速度更新

求解滤波器观测值需要惯导解算出当前时刻速度，由下列公式求解:

3.3位置更新

对准过程中惯性导航速度波动较大，因此采用多普勒速度进行航位推算及位置更新。取1s更新一次的多普勒速度信息，设为多普勒测速仪得到的载体对水底速度在n系中的投影，可表示为:

载体经纬度信息通过航迹推算进行更新:

对准过程结束后再用组合导航速度进行位置更新。

4　滤波模型

4.1滤波误差状态选取

根据导航解算可知造成纯惯导解算误差的主要原因是初始水平姿态误差和速度误差，选取横滚角误差、俯仰角误差、北向速度误差、东向速度误差为系统状态即

4.2误差状态微分方程

首先由于初始水平欧拉角误差为常值，可得:

下面推导速度误差方程。

定义姿态矩阵失准角为φ，其与欧拉角的关系为:

由式（14）～式（17）计算可得:

载体相对于地球速度在n系下的速度微分方程为:

估算速度微分方程为:

相减可得速度误差微分方程:

将式（18）带入式（21）中并选取北向、东向速度误差项:

由姿态误差方程以及速度误差方程，卡尔曼滤波器系统方程为:

4.3观测方程

在考虑载体姿态误差的情况下，含有误差的多普勒输出为:

以惯导解算北向、东向速度与多普勒北向、东向速度之差为滤波器观测量:

由式（27）、式（28）可得卡尔曼滤波器的观测方程为:

其中，v为观测噪声，至此，由式（24）（状态方程）和式（30）、式（31）（观测方程）构成了基于方向信息辅助对准滤波器模型，得以精确地估计出俯仰角、横滚角误差。

5　实验结果与分析

使用水下潜航器数据进行算法验证，实验中，主惯导系统传递给子惯导的姿态信息为初始基准值为潜航器姿态基准值为利用原始数据进行INS/GPS组合导航解算得到。潜航器惯导系统为激光捷联惯导系统，陀螺精度为0.01（°）/h，加表精度为5×10-5g。实验截取潜航器运行前10min进行算法验证。

图2中初始对准方式为以多普勒速度为观测量，潜航器在无初始方位信息下的自对准。方位误差在600s时才收敛。

图2　无初始方位角信息辅助下传统对准过程中航向角误差曲线Fig.2 Azimuth angle error graph during traditional alignment without initial azimuthinformation

图3中采用初始方位角信息辅助下的传递对准方案，方位误差角到50s时收敛到1密位。

图3　初始方位角信息辅助下传递对准过程中航向角误差曲线Fig.3 Azimuth angle error graph during transfer alignment with initial azimuth information

6　结论

通过实验验证了算法的有效性，实现了快速传递对准的要求。利用移动基准惯导的方位角信息确定子惯导系统的方位信息，缩短了与子惯导系统信息传输时间，使得子惯导系统可以实现与主惯导系统分离的快速性，且入水后50s内完成潜航器初始对准，精度达到1密位。在此基础上，可以实现短时间投放大量潜航器的实际目标。

［1］ Li W ，Wu W，Wang J，et al.A fast SINS initial alignment scheme for underwater vehicle applications［J］. Journal of Navigation，2012，66（2）：181-198.

［2］ 曹洁，孙枫，陈勤，刘柱.舰载水下潜器捷联惯导初始对准技术研究［J］.导航与控制，2003，2（2）：27-31. CAO Jie，SUN Feng，CHEN Qin，LIU Zhu.The research on ship-borne underwater vehicles SINS initial alignment technology［J］.Navigation and Control，2003，2（2）：27-31.

［3］ 王勇军，徐景硕，王晓飞，罗恬颖.一种改进的舰载机快速传递对准方法［J］.中国惯性技术学报，2014，22 （5）：597-600. WANG Yong-jun，XU Jing-shuo，WANG Xiao-fei，LUO Tian-ying.Improved rapid transfer alignment method for SINS of carrier plane［J］.Journal of Chinese Inertial Technology，2014，22（5）：597-600.

［4］ Xu J S，Wang Y J，Xiao Z C.Rapid transfer alignment for SINS of carrier craft［J］.Journal of Systems Engineer and Electronics，2013，24（2）：303-308.

［5］ 严恭敏，翁浚，白亮，等.基于惯性参考系的动基座初始对准与定位导航［J］.系统工程与电子技术，2011，33 （3）：618-621. YAN Gong-min，WENG Jun，BAI Liang，et al.Initial inmovement alignment and position determination based on inertial reference frame［J］.Systems Engineering and E-lectronics，2011，33（3）：618-621.

［6］ 高贤志，刘红光，陈伟，邹毅.舰载武器惯导传递对准技术分析［J］.导航与控制，2012，11（3）：16-20. GAO Xian-zhi，LIU Hong-guang，CHEN Wei，ZOU Yi. Analysis on transfer alignment method of INS on shipborne weapon systems［J］.Navigation and Control，2012，11（3）：16-20.

A Fast Transfer Alignment for Underwater Vehicles Based on Initial Azimuth Information

ZHANG Lu，WU Wen-qi，WANG Lin，TIE Jun-bo
（College of Mechatronics and Automation，National University of Defense Technology，Changsha 410073）

Considering the requirements for underwater vehicles to be deployed in quick succession，a fast transfer alignment algorithm based on azimuth information is presented.First a moving base INS provides accurate azimuth information and inaccurate horizontal information at the beginning of the initial alignment，then the alignment of navigation in the inertial system is carried out by Kalman filter using initial horizontal angles error and velocity error as system states and Doppler velocity measurements as observables.Simulation results based on experimental data shows that the algorithm can achieve fast alignment within 50s，and the azimuth accuracy reaches 1 mil（1σ）.

strapdown inertial navigation system（SINS）；fast transfer alignment；azimuth angle information；Doppler velocty log（DVL）

U666.12

A

1674-5558（2016）01-01128

10.3969/j.issn.1674-5558.2016.03.003

2015-06-02

张鹭，男，硕士，研究方向为惯性导航算法。