单轴旋转航位推算系统姿态误差补偿方法*

廉 璞，牟 东，青 泽，张延顺

（1.中国工程物理研究院电子工程研究所，四川 绵阳 621999；2.北京航空航天大学仪器科学与光电工程学院，北京 100191）

0 引言

航位推算（Dead Reckoning DR）导航系统利用已知的航向、速度、时间等信息推算出新的航位，是一种自主导航定位系统［1-2］。与捷联惯导系统相比，DR 导航系统误差随时间累积变化较小［3］，常与里程仪或多普勒测速仪（Doppler velocity log，DVL）组合用于地面车辆或水下运载体的导航定位中［4-7］。水下某运载体采用DVL/DR 组合导航方式，DVL 利用多普勒效应实现载体速度测量，DR 导航系统通过惯导解算的姿态角与测量速度进行航位推算，实现导航定位。姿态精度是影响定位精度的主要因素。而载体持续旋转会使姿态误差增大，进而引起导航系统定位误差［8］。因此，需要对载体航行过程中的姿态误差进行补偿，提高旋转航位推算导航系统的姿态角测量及解算精度。

针对旋转运载体的姿态角解算与误差补偿，文献［9］研究了旋转轴不正交误差对于水平方向上的姿态角影响，提出一种基于Kalman 滤波的水平姿态角误差修正方法。文献［10］提出通过BP 神经网络对误差建模，修正Kalman 滤波增益矩阵系数，提高旋转弹姿态测量的方法。学者们针对旋转载体提出基于磁强计测量的姿态角解算方法［11-12］。文献［11］充分利用传感器数据，实现了姿态更新与磁强计输出同步。文献［12］以地磁方位角为观测量，通过弹体绕质心动力学方程估计弹体俯仰角和偏航角。文献［13］把红外与地磁复合的姿态测量技术用于旋转弹的姿态获取，并对弹上红外与地磁测量误差补偿方法开展了研究工作。但是，地磁传感器在使用过程中易受到干扰，并且在使用前需要进行磁校准。文献［14］通过设计多加速度计的非质心配置方案，实现中空结构的MEMS 测量单元对高速旋转体的滚转角测量。

基于以上技术背景，本文在不改变传感器配置的情况下，利用旋转过程加速度计输出信号周期性出现峰值的现象，提出了将加速度计输出信息提取到的角度变化量与惯导更新角度进行融合的姿态角补偿算法，并进行仿真研究。

1 航位推算导航系统定位误差分析

1.1 航位推算导航定位方法

航位推算（DR）定位方法是根据捷联惯导提供的载体实时姿态，对速度矢量进行投影和位置更新，从而得到载体位置信息。

选择东-北-天地理坐标系为导航坐标系，记为n 系；将惯导与运载体固连，选择右-前-上坐标系为载体坐标系，记为b 系。水下运载体沿载体轴向运动，载体速度可以表示为

将载体系下的速度vb投影到导航坐标系，得到

由式（2）和式（3）可见，速度误差和姿态误差是影响DR 导航系统误差的两个重要因素。本文主要研究姿态误差影响及补偿方法。

1.2 姿态四元数误差分析

在姿态更新中，为避免欧拉角法存在的解算奇异问题，以及方向余弦法计算量大的缺点，本文采用四元数法完成姿态解算及分析［15］。

定义四元数q：

式中，q0表示四元数中的标量，后三项为四元数的矢量部分。

姿态更新四元数微分方程可以表示为

对式（6）两边求导，得

根据式（5）可得四元数估计值的微分方程为

式中，ω0为带有测量误差的角速度。

将式（5）和式（8）代入式（7）可得

将式（6）代入式（9）并消除同类项，可得

陀螺仪测量模型可以表示为

式中，εb为陀螺仪的漂移误差，ωg为陀螺仪的均值白噪声。

将式（11）代入式（10）可得

误差四元数中第一项为1，对其求导为0，对姿态误差分析无影响，因此，式（12）可以化简为

由式（15）可以看出，姿态误差主要是由陀螺漂移和陀螺测量噪声引起。并且随着时间的累积，陀螺漂移引起的姿态误差将持续增大，因此，需要对该误差项进行补偿。

2 姿态误差补偿模型

2.1 滤波融合姿态补偿方案

对于持续旋转的水下运载体，当加速度计敏感轴垂直向下或向上时，其输出信号会周期性地出现峰值，即相邻两峰值信号出现，对应着该段时间内滚动角转过了180°。此信息可以作为量测量与陀螺仪求解的姿态融合，以补偿陀螺漂移引起的姿态误差。

该方法首先用捷联惯导姿态更新算法求解载体的姿态角，然后利用加速度计随载体运动周期性变化的特点，将加速度计输出信号提取的角度增量与已解算得到的角度滤波融合，抑制姿态误差的累积。采用滤波融合姿态补偿方案如图1 所示。

图1 姿态补偿方案原理框图

2.2 基于加速度计输出的姿态补偿方法

加速度计ax、ay、az分别沿载体坐标系的3 个坐标轴安装，如图2 所示，其中加速度计ax、az与载体的滚转轴垂直安装。

图2 加速度计安装示意图

当垂直于载体行进方向的z 轴加速度计垂直向上时，输出信号最大，此时加速度计ax的输出在0附近。载体绕y 轴旋转90°，x 轴方向的加速度计ax垂直向下，输出为极小值，az≈0。当载体继续y 轴旋转90°，z 轴加速度计az垂直向下，输出信号最小，ax≈0。载体继续旋转，ax出现峰值，az≈0。随着载体的持续滚转，ax、az两轴加速度计的输出信号周期性出现峰值。

利用这一现象，可以建立加速度计输出信息与横滚角之间的关系，表示为

将加速度计信号提取得到的姿态角四元数与惯导陀螺解算的姿态角四元数进行滤波融合，首先建立系统的状态方程和量测方程，再设计自适应滤波器实现滤波估计。

2.2.1 状态方程

基于1.2 节推导得到的姿态四元数误差方程，建立姿态滤波融合的状态方程。

选择状态变量

2.2.2 量测方程

对加速度计输出信号的分析可知，加速度计信号提取得到的姿态角不随陀螺仪的漂移而发散，因此，可以将加速度计输出量出现极值时对应的姿态四元数作为真值q，惯导陀螺仪解算可以得到姿态四元数估计值，将两者的差值作为量测量建立系统的量测方程

2.2.3 Sage-Husa 自适应滤波算法

Sage-Husa 自适应滤波算法能利用量测量修正系统过程噪声和量测噪声，从而抑制滤波发散，提高滤波精度，该算法的原理简单、实时性较好［16］。持续滚转的运载体会引起陀螺仪和加速度计的测量噪声，通过初始先验值难以准确描述实际噪声，因此，本文采用Sage-Husa 自适应滤波算法估计噪声特性，实现姿态误差的准确估计。

首先对连续的系统状态方程和量测方程做离散化处理，得到离散的系统方程为

Sage-Husa 自适应滤波算法可以表示为［16］

3 仿真验证

3.1 仿真条件

为检验姿态误差补偿算法的正确性和有效性，利用轨迹发生器产生仿真数据。

仿真初始条件：初始位置（纬度34°、经度116°、高度0），初始角度误差（航向角1°、俯仰角0.5°、横滚角0.5°）。

仿真轨迹：载体速度25 m/s，并且存在±1.5 m/s的随机误差，为验证算法有效性，参考载体的实际运动状态，分别设计了两条路径进行验证，运动时间为400 s，具体的三维轨迹曲线和平面轨迹如图3、图4 所示。

图3 路径1 的轨迹曲线

图4 路径2 的轨迹曲线

惯导系统的仿真参数见表1。

表1 惯导仿真参数

利用本文提出的算法对轨迹发生器生成的数据进行导航解算及姿态补偿，并以设定的导航真实值为基准［17-18］。

3.2 仿真结果

根据以上的仿真条件，分别采用未经姿态补偿的旋转DR 导航系统和基于姿态补偿算法的旋转DR 导航系统，对不同路径进行仿真。

路径1 的姿态误差曲线和导航位置误差如图5、图6 所示。图中蓝色虚线表示未采用姿态补偿算法进行DR 导航的结果，红色实线表示采用姿态角补偿方法后DR 导航的结果。DR 导航误差在运动400 s 时的统计结果如表2 所示。

图5 路径1 导航姿态误差曲线

图6 路径1 导航位置误差曲线

表2 路径1 误差统计表

从图中可以看出，位置误差曲线出现了3 次峰值，这与仿真路径中存在的转弯及环形运动有关，随着时间的增加，采用姿态补偿算法的导航效果越好。在姿态角误差曲线中，俯仰角和横滚角误差经补偿后减小显著。

从误差统计结果中可以看出，在运动400 s 时，俯仰角、横滚角、航向角误差分别由1.68°、-0.55°、-0.35°减小到0.74°、-0.06°-0.20°。位置误差由补偿前的53.4 m 减小到41.1 m。

路径2 的姿态误差曲线和导航位置误差如图7、下页图8 所示。导航的误差统计结果如表3所示。

表3 路径2 误差统计表

图7 路径2 导航姿态误差曲线

图8 路径2 导航位置误差曲线

对于姿态角误差，采用补偿算法后，有效降低了陀螺仪漂移引起的姿态角误差累积。俯仰角误差和横滚角误差从补偿前的1.46°和-1.1°降低到0.74°和-0.64°，分别减少了49.3 %和41.8 %。在400 s 时，位置误差由134.1 m 减小到67.8 m，误差减小了49.4%。

4 结论

本文针对旋转过程中引起的航位推算导航系统定位误差问题，在误差机理分析的基础上，提出了将加速度计输出信息提取的姿态角作为量测的姿态误差补偿方法，以减小由陀螺漂移引起的姿态误差和定位误差。通过不同路径下的仿真研究，验证了本文提出的算法能降低陀螺漂移引起的姿态角误差，提高了单轴旋转DR 导航系统的姿态测量精度，以及导航定位精度。