光纤罗经和卫星测姿的组合导航算法

杨 晔，刘乃道，孟凡彬，张文杰，董金发

（天津航海仪器研究所，天津 300131）

光纤罗经和卫星测姿的组合导航算法

杨 晔，刘乃道，孟凡彬，张文杰，董金发

（天津航海仪器研究所，天津 300131）

为了提高光纤罗经和卫星测姿的组合导航精度和系统稳定性，提出了一种光纤罗经/卫星测姿的组合导航算法。首先分别对光纤罗经、卫星测姿和组合系统的误差进行了数学建模，以姿态作为组合系统的观测量，建立了组合系统的状态方程和观测方程。然后，给出了一种改进的 Kalman滤波器，这种滤波器不但能够保证滤波器稳定可靠，防止滤波发散，还提高了准确度，有助于提高组合导航精度。最后，通过静态试验和动态跑车试验得出，光纤罗经/卫星测姿组合导航系统的航向角误差在0.1°以内；通过极区航行实验得出，船舶在高纬度航行中组合导航系统航向角误差在0.5°以内。

光纤罗经；卫星测姿；组合导航；卡尔曼滤波

与传统陀螺罗经产品相比，光纤陀螺罗经和卫星测姿系统具有体积小、启动快、精度高、寿命长、易安装、全寿命周期成本低等多方面优势。其中，卫星具有全球、全天候适用且无误差积累的特点，缺点是会受环境因素影响，在天气恶劣的情况下容易出现失锁现象，造成输出数据不连续。光纤罗经的主要缺点是随着纬度的升高，航向误差增大[1]。

为了克服上述缺陷，寻求最优设计，本文提出了一种新的光纤罗经/卫星姿态组合算法，利用光纤陀螺、卫星及其相关的融合算法来综合地测定航向，充分结合光纤罗经安装简单、使用方便、精度高以及卫星的全球、全天候、无误差积累等特点，克服光纤罗经和卫星测姿各自的缺点，达到优势互补的目的。最后利用静态实验、动态跑车实验和极区航行实验验证了组合算法对航向精度的提高所起的效果。

1 光纤罗经/卫星测姿组合导航系统建模

1.1 光纤罗经误差数学模型

利用惯性测量信息求解姿态信息的方法主要有方向余弦法和四元数法[2]。与方向余弦法相比，由于在相同舍位阶数下四元数法计算后得到的姿态漂移值要小于方向余弦法得到的结果，并且算法简单，计算量小，易于操作，因此，在本文的研究课题中，采用四元数法对光纤罗经的姿态信息进行求解。

在导航结算中组合导航状态方程选取各导航参数的误差为状态量，因此，首先确定惯导系统的误差方程，采用东北天地理坐标系作为导航解算的基本坐标系。在此坐标系下某量的投影均用n表示。

① 姿态误差方程为

推导得出姿态角误差分量表达式：

式中，φx、 φy、φz为姿态误差角， Vx、 Vy为东、北向速度，R为地球半径，ωie为地球自转角速率，εx、 εy、 εz为陀螺常值漂移[3]。

② 速度误差方程为

式中， fn为加表比力，▽n为加表常值零偏。

③ 位置误差方程为

式中，L、 h、λ分别为纬度、高度和经度。

1.2 卫星测姿误差数学模型

利用卫星测姿就是通过接收到的卫星定位信号，特定的算法计算出天线阵列或轨迹的空间指向，进一步处理成与传统罗经测量相当的参数，如方位角、俯仰角。其测向原理示意图如图1所示。

系统的两幅天线A和B分别布置在与运动载体轴线平行的连线上，接收同一时刻的卫星导航信号。射频前端和基带处理单元与卫导接收机一样，射频前端处理模块通过天线接收所有可见卫星信号，经滤波放大后，变频成中频信号，最后经模数转换输出数字中频信号。基带数字信号处理模块对数字中频信号进行载波解调和伪码解扩，从中获得伪距、多普勒频移、载波相位等测量值以及解调出导航电文。基于载波相位差分定位的核心技术就是整周模糊度的解算[4]。

与卫导接收机不同的是，导航姿态解算模块同时接收两路基带处理单元输出的伪距、多普勒频移、载波相位等测量值以及导航电文和整周模糊度，根据单差/双差的相对定位模型，采用双差法[5]（利用不同卫星和不同天线之间的载波相位观测量做二次差）解算出姿态量。

图1 卫星测姿原理示意图Fig.1 Principle of satellite compass

假设天线A的坐标已知并约定为主天线，天线B的坐标未知并约定为副天线。那么，天线A和B构成的基线矢量方向为从主天线指向副天线。首先利用双差法得到双差载波相位观测方程：

式中，l、m、n为天线B指向卫星的方向余弦参数，(Δ x,Δ y, Δ z)为概略坐标和真实坐标的偏差，即坐标修正量， ▽Δ N为整周模糊度双差值，其采用LAMDA方法[6]求解。

1.3 光纤罗经/卫星测姿组合系统的数学模型

1.3.1 组合系统的状态方程

根据对组合系统的性能和误差源分析，误差方程选用15维误差状态向量：

式中，下标x、y、z分别表示东北天地理坐标系的三个方向， φx、 φy、 φz为姿态误差角，δvx、 δ vy、δ vz为速度误差，δφ 、δ λ、 δ h 为位置误差；εx、 εy、 εz为陀螺常值漂移，▽x、 ▽y、 ▽z为加速度计常值零偏。其中，前9个为惯性基本误差，后6个为惯性器件的误差，共15个状态量。

系统的误差状态方程：

式中：G(t)是系统噪声驱动矩阵，A(t)为15×15维的系统阵，由分块矩阵构成；W(t)是噪声驱动向量，W (t) =[wgx,wgy,wgz,wax,way,waz]中 wgx、 wgy、 wgz、 wax、 way、 waz分别为陀螺和加速度计的白噪声。 AN9×9可由光纤罗经误差方程导出：

1.3.2 组合系统的量测方程

通过光纤罗经估计得到的基线矢量[7-9]的误差δD为

卫星测姿系统中，利用LAMDA方法求得整周模糊度后，则可以得到载波相位双差值残差δφ为：

式中， ▽Rˆik为第i颗卫星到光纤罗经估计的主天线距离与第j颗卫星到光纤罗经估计的主天线距离之间的差， ζφ为观测噪声向量。

结合以上分析，得到组合系统的观测方程：

1.3.3 光纤罗经/卫星测姿系统组合测姿系统的卡尔

曼滤波器设计

状态预测方程[10]：

状态估计方程：

滤波增益方程：

估计均方误差方程：

一步预测均方误差方程：

2 改进的卡尔曼滤波

光纤陀螺在惯性系统应用中存在以下几类误差：陀螺零偏、标度因数误差、频响宽度与延迟时间、光纤陀螺敏感轴失调角误差等。这些误差对测姿精度有一定的影响，同时这些误差造成传统的卡尔曼滤波无法准确建模，存在滤波发散，系统不稳定问题。

为了克服传统的Kalman滤波的发散性，本文提出了一种改进的卡尔曼滤波方法。在该方法中，速度加姿态组合模式需要对卫星测姿系统输出速度信息杆臂效应补偿，在卫星测姿系统输出处补偿杆臂速度至光纤罗经位置。具体算法如下：

按照前述算法模型，设计 Kalman滤波器。将杆臂效应[11]补偿后卫星测姿速度信息及输出三维姿态信息与光纤罗经速度信息、三维姿态信息作差，作为Kalman滤波器量测信息，通过滤波估计出姿态误差角φ和光纤罗经与卫星测姿系统的坐标失调角λ。

“速度+姿态”组合模式对时间延迟比较敏感，光纤罗经与卫星测姿通过时统信息进行同步，应使时间延迟量尽量小，并且需要通过时统信息计算出时间延迟量▽T。对准过程中用对齐后的t-▽T时刻光纤罗经与卫星测姿信息进行匹配，估计出t-▽T时刻姿态误差角 φ( t -▽ T)和光纤罗经与卫星测姿的坐标失调角λ（ t-▽ T）。通过以上改进的卡尔曼滤波器不但能够保证滤波器稳定可靠，防止滤波发散[12]，同时提高了准确度，有助于提高组合导航精度。

3 实验结果与验证

在通过仿真实验的前提下，分别进行了静态试验、动态试验，特别是在高纬度航行实验中表现出高精度和高稳定性的效果。

3.1 静态试验

在静态情况下，GPS基线矢量长度为3 m，测试时间为 3～4 h。从中选取了一段组合系统观测时间为7000～17000 s的历元信息，用于光纤罗经导航信息仿真与解算。利用Matlab进行图形化处理，得到如图2所示静态情况下的航向角误差曲线图。

图2 静态情况下的航向角误差Fig.2 Heading error in static case

从图 2可以看出，光纤罗经稳定航向角误差为0.2°，卫星测姿稳定航向角误差为 0.4°，组合系统稳定航向角误差小于0.1°。

3.2 动态试验

动态跑车实验时，将高精度的PHINS导航系统输出数据作为姿态角基准数据，对单光纤罗经、单卫星测姿和组合系统解算后的结果进行比较分析，利用Matlab进行图形化处理，得到如图3所示动态情况下的航向角误差曲线图。

实验跑车数据如图3所示。由图3中光纤罗经、卫星测姿与组合系统的对比可知，组合系统相对于单光纤罗经、单卫星测姿，航向误差明显降低，精度得到的极大的提高，光纤罗经航向角误差为0.4°，卫星测姿误差为0.5°，组合系统航向角误差为0.1°。

图3 动态情况下航向角误差Fig.3 Heading error in dynamic case

3.3 极区航行实验

光纤罗经系统随中远集团永盛轮成功往返北极航线，历时55 d，航程20 000 nm，到达北纬78°05′的北极航道最高点。

按照实验系统所工作地点纬度，将本次试验分为以下两个航行区域：

① 在低纬度区域（φ≤60°N）和高纬度区域（60°N＜φ≤77.5°N）范围内，系统采用罗经算法；

② 在高纬度区域（77.5°N＜φ＜90°N）范围内，系统采用极区算法（地理坐标系与格网坐标系的切换）。

考虑到卫星测姿艏向误差不发散，本次试验将该设备作为基准设备。人工读取并记录组合系统、光纤罗经、电罗经在各纬度的艏向，并与卫星测姿系统的数据进行对比。鉴于航行时间比较长，截取其中具有代表性的一段航程进行分析，如表1所示。

从随船记录的与卫星测姿导航数据分析比对，组合系统不论在低纬度还是在高纬度航行，系统工作稳定。试验结果表明，光纤罗经误差变化范围的最大值小于1°，组合罗经系统航向误差变化范围的最大值小于0.5°，电罗经在高位度区域误差超过6°。由此可知，在高纬度航行中精度光纤罗经/卫星测姿组合系统明显优于传统电罗经。

表1 极区航行艏向数据Tab.1 Heading data in polar navigation

4 结 论

本文采用光纤罗经和卫星测姿的组合方案，设计的基于Kalman滤波器的速度、姿态组合算法。在卫星测姿的基线长度为3 m的条件下，静态情况航向角精度为0.05°；在动态情况下，根据与高精度的PHINS导航系统对比，航向角优于0.1°；在高纬度航行实验中航向误差明显优于电罗经。实验结果表明，该算法能够明显提高卫星测姿、光纤罗经的姿态精度，达到的优势互补的目的。

（Reference）

[1] 张科, 刘海鹏, 李恒年, 钱山. SINS/GPS/CNS组合导航联邦滤波算法[J]. 中国惯性技术学报, 2013, 21(2): 226-230. Zhang Ke, Liu Hai-peng, Li Heng-nian, Qian Shan. SINS/ GPS/CNS integrated navigation federal filtering algorithm[J]. Journal of Chinese Inertial Technology, 2013, 21(2): 226-230.

[2] 屈蔷, 刘建业, 熊智. 基于加性四元数误差方程的惯性/天文姿态组合算法[J]. 中国惯性技术学报, 2011, 19(6): 316-319. Qu Qiang, Liu Jian-Ye, Xiong Zhi.Inertial/Celestial attitude integrated algorithm based on additive quaternion [J]. Journal of Chinese Inertial Technology, 2011, 19(6): 316-319.

[3] 戴东凯, 王省书, 战德军, 等. 基于单轴旋转INS/GPS组合姿态误差观测的垂线偏差测量方法[J]. 中国惯性技术学报，2015, 23(4): 172-178. Dai Dong-kai, Wang Xing-shu, Zhan De-jun, et al. Measurement of dynamic vertical deflections by observing attitude errors of single-axis rotation INS/GPS system[J]. Journal of Chinese Inertial Technology, 2015, 23(4): 172-178.

[4] Ramanandan A, Chen A, Farrell J A. Inertial navigation aiding by stationary updates[J]//IEEE Transacation on Intelligent Transportation Systems, 2012, 13(1): 235-248.

[5] Dai D K, Wang X S, Zhan D J, et al. An improved method for dynamic measurement of deflections of the vertical based on the maintenance of attitude reference[J]. Sensors, 2014, 14(9): 16322-16342.

[6] 胡佩达, 高钟毓, 张嵘, 韩丰田. 基于三轴摇摆台的高精度姿态试验系统[J]. 中国惯性技术学报, 2013, 21(4): 271-274. Hu Pei-da, Gao Zhong-yu, Zhang Rong, Han Feng-tian. High accuracy attitude testing system based on three-axis test table[J]. Journal of Chinese Inertial Technology, 2013, 21(4): 271-274.

[7] Titterton D H, Weston J L. Strapdown inertial navigation technology[M]. London: Peter Pergrinus Ltd, 1997: 384-391.

[8] Johnson G, Waid J, Primm M, et al. Ship attitude accuracy trade study for aircraft approach and landing operations[C]//IEEE Position Location and Navigation Symposium, 2012: 783-790.

[9] 袁群哲, 周红进. ADCP与光纤陀螺罗经组合导航[J].中国惯性技术学报, 2007, 15(11): 713-720. Yuan Qun-zhe, Zhou Hong-jin. FOG compass/acoustic Dopper current profile integrated navigation[J]. Journal of Chinese Inertial Technology, 2007, 15(11): 713-720.

[10] Dini D H, Mandic D P, Julier S J. A widely linear complex unscented Kalman filter[J]//IEEE Signal Processing Letters, 2011, 18(11): 623-626.

[11] Grejner-Brzezinska D A, Yi, Toth Y C K. Precision GPS/ INS navigation in urban canyons: Dealing with GPS losses of lock[C]//Proc IAG 2001 Scientific Assembly. Budapest, Hungary, 2001.

[12] Yang Yun-chun, Farrell J A. Two antennas GPS -aided INS for attitude determination[J]//IEEE Transactions on Control Systems Technology, 2003(11): 905-917.

Integrated navigation algorithm of FOG-compass and satellite-attitude

YANG Ye, LIU Nai-dao, MENG Fan-bin, ZHANG Wen-jie, Dong Jin-fa
(Tianjin Navigation Instrument Research Institute, Tianjin 300131, China)

In order to improve the precision and stability of an integrated FOG compass/satellite-attitude navigation, an integrated navigation algorithm is proposed based on FOG-compass/satellite-attitude. Firstly, the error models of an integrated FOG-compass/satellite-attitude system are mathematically established, and the state equation and the observation equation of the integrated system are derived by taking the attitude as the system’s observation. Then, an improved Kalman filter is proposed and used to ensure the stability and reliability of the filter, and improve the accuracy of the integrated navigation. The static test and dynamic in-car test both show that the heading error of the integrated system is less than 0.1°. The test for ship sailing at high latitude shows that this heading error is less than 0.5°.

FOG compass; satellite-attitude; integrated navigation; Kalman filter

U666.1

：A

2015-09-10；

：2015-11-25

天津市科技兴海项目（KJXH2013-09，KJXH2014-10）；天津市海洋经济创新发展区域示范项目（CXSF2014-3）

杨晔（1968—），男，研究员，硕士生导师，从事导航、制导与控制方面研究工作。E-mail: liuyuyangye@eyou.com

1005-6734(2015)06-0746-05

10.13695/j.cnki.12-1222/o3.2015.06.009