法向量位置模型下的水下长航时惯性导航阻尼算法

刘 潺，吴文启，冯国虎，王茂松

（国防科技大学 智能科学学院，长沙 410073）

长时间水下航行是潜航器执行任务重要的应用需求。惯性导航具有连续性好、自主性高、隐蔽性强的特点，是水下导航最基本的有效手段[1]。纯惯性导航系统的垂直通道具有不稳定性，水平通道具有舒勒振荡特性。由于水下环境的限制,潜航器能够获取的外部信息主要有来自计深仪的参考高度信息和来自计程仪的参考速度信息，因此垂直通道和水平通道的阻尼技术就成为了水下航行器提高长航时导航精度的主要手段[2]。传统的惯性导航阻尼技术是在传统当地水平地理坐标系下编排的，并不适用于极区[3]。另外当潜航器出入极区时，需要将传统导航算法与极区算法进行切换，切换过程会影响阻尼内部过程的连续性与一致性，因此在全球统一的力学编排下实现阻尼算法十分必要[4]。

立足现代控制理论，应用卡尔曼滤波技术实现阻尼[5-8]的研究较多。然而卡尔曼滤波技术通常用于观测噪声为零均值的情况，而计程仪外参考速度信息可能存在未知常值误差，水下的“惯性/速度”组合模式按传统方法处理会使导航精度成为计程仪速度量级。而采用卡尔曼滤波增益得到常数项变阻尼参数的方法设计阻尼网络，其位置稳态误差通常不是无静差的。

本文针对传统算法的问题，基于法向量位置模型分别在垂直通道和水平通道中设计实现了阻尼网络，水平通道阻尼后的导航参数为三阶无静差。基于北极实际导航数据的仿真实验结果表明，本文提出的阻尼算法能够有效抑制舒勒周期振荡误差，提高了导航性能。

1 基于法向量位置模型的力学编排与误差微分方程

文献[9]对法向量位置模型的建立和法向量的位置表示方法进行了详细的推导。如图1所示，由载体对应参考椭球位置的卯酉圈构造一个虚拟圆球，其球心为对应卯酉圈的圆心，圆球半径为卯酉圈半径ER与大地高度h之和。

图1 虚拟圆球法向量示意图Fig.1 Sketch of virtual sphere N-vector

基于法向量位置模型的力学编排方案采用地球坐标系进行导航解算。载体垂直方向上的微分方程为：

结合水平通道和垂直通道，位置微分方程为：

式中KR为尺度系数矩阵，kR为尺度变换系数，RE为卯酉圈半径，RN为子午圈半径，即：

地球坐标系下的速度微分方程为：

地球坐标系下的姿态微分方程为：

因此，静基座下垂直通道的速度、高度误差微分方程为[9]：

其中，δvh为垂直通道速度误差，δh为垂直通道高度误差，ωs为舒勒周期角频率，δha为外部参考高度误差，δfb为比力误差矢量。

水平通道的姿态、速度以及位置误差微分方程式为[9]：

其中，φe为姿态角误差矢量为水平速度误差矢量，δη为法向量误差矢量,为角速度误差矢量。

2 垂直通道阻尼算法

当系统无外部参考高度输入时，则式(5)的解析解在δha=δh的条件下包含随时间增长的指数项因此，纯惯性导航系统的垂直通道是不稳定的，需要在垂直通道中引入参考高度进行反馈补偿，使系统闭环稳定从而抑制高度发散。由式(5)可以得到经过阻尼后垂直通道高度和垂直速度的误差微分方程为：

式中uD1与uD2是引入的反馈，即：

引入反馈补偿后，传统垂直通道阻尼的原理图如图2所示。

图2 垂直通道阻尼框图Fig.2 Block diagram of vertical damping

将式(7)变换为矩阵形式：

则阻尼系统的特征方程为：

只需要配置合理的阻尼系数k1D与k2D，就能够使系统的两个极点都位于左半平面，也就是通过阻尼使得垂直通道变成一个渐近稳定的回路。

只考虑外参考高度误差，由图2可以得到传递函数为：

图3 垂直通道阻尼系统的波特图Fig.3 Bode diagram of vertical damping

计深仪指示高度信息受高频的水面波浪影响较大。由图3可得，垂直通道阻尼系统具有低通滤波器的特性，使计深仪指示高度信息的高频误差得到了衰减。

设外参考高度误差为常值误差，即：

考虑到ω0远大于ωs，由拉式变换终值定理可得垂直通道速度的稳态误差为：

其中δvhss为垂直速度的稳态误差，由于式(13)中的系数可得外参考高度常值误差对垂直速度的稳态误差的影响非常小。由于水下潜航器的垂直通道解算在很多应用场景下都是不能忽略的，因此本文的阻尼方法具有较强的现实意义。

3 水平通道阻尼算法

在法向量位置模型下水平通道误差微分方程的基础上，通过引入计程仪的外参考速度进行反馈补偿，将原本临界稳定的系统转换为渐近稳定的系统，建立阻尼模型的系统方程为：

其中

令尺度变换系数kR近似为1，则特征方程为：

当三个频率函数都为0时：

观察特征方程根的情况，除三个零根外，系统的其他6个特征根全部为纯虚根，分别对应地球周期、舒勒周期和傅科周期的角频率，此时特征方程退化为纯惯导的形式。

为了方便观察阻尼状态下的特征方程，令三个方向上水平速度阻尼的频率函数相等，即频率函数H=H1=H2=H3，则原特征方程可以简化为：

令频率函数Q满足：

则可以得到：

因此只要选择适当的阻尼频率函数Q，就能使特征方程的根具有负实部，即实现了水平阻尼。水平速度阻尼选择频率函数的过程中，应当同时考虑惯性仪表的输出高频噪声，外参考速度的高频测量噪声和低频的海流速度影响。若则外参考速度的常数误差就不会影响阻尼速度。若则舒勒阻尼回路具有二阶以上的高频衰减特性，能有效过滤外参考速度和惯性仪表输出的高频干扰信号。

如果H为常数项频率函数H=2ξωs[10]，即：

只考虑外速度参考误差，则法向量位置稳态误差为：

由式(21)可得，常系数反馈校正网络的法向量位置稳态误差是有差的，其大小与外参考速度误差成正比。文献[6][7][8]均应用卡尔曼滤波技术设计阻尼网络，本质上都是变阻尼系数为常数项的比例反馈，虽然具有良好的高频特性，变阻尼系数也能够有效控制超调，但是在外参考速度误差较大的情况下并不适用。

如果选择H为相位超前串联校正网络[10],即：

可以得到由外速度参考误差引起的法向量位置稳态误差为：

是对时间的一阶导数，即相位超前串联校正网络的法向量位置稳态误差为一阶无静差。

如果选择H为相位滞后-超前串联校正网络[10],即：

则可以得到由外速度参考误差引起的法向量位置稳态误差为：

是对时间的三阶导数，即相位滞后-超前串联校正网络的法向量位置稳态误差为三阶无静差。因此，从位置稳态误差的角度考虑，本文选取相位滞后-超前串联校正网设计阻尼。

经过垂直阻尼后，得到阻尼后的高度记为hDamp，垂直速度记为vhDamp。经过水平阻尼后，得到阻尼后的法向量记为ηDamp，水平速度记为。

综上，法向量位置模型下的阻尼系统框图如图4所示。

图4 法向量位置模型下的阻尼系统框图Fig.4 Bode diagram of damping based on n-vector

4 仿真与实际航行数据半实物仿真试验

4.1 垂直通道阻尼仿真

为了验证该阻尼算法的适用性和有效性，采用文献[11]中的数据转换方法将1 h静态数据（200 Hz）转换至北纬85 °，航行器起始高度设为水下30 m，用于阻尼的参考高度设为10 m的常值偏差+均方差为2 m的随机噪声误差，然后进行法向量位置模型下的纯惯导（高度通道不置0）和垂直通道阻尼的仿真。

图5为基于法向量位置模型的纯惯导和高度通道阻尼的导航结果（垂直高度、垂直速度、横向位置）的对比，图中蓝色虚线为纯惯导的结果，红色实线为高度通道阻尼方法的导航结果。由图5可以看出该阻尼算法适用于极区，并且有效抑制了高度通道的发散。

图5 阻尼前后导航结果对比Fig.5 Comparison on navigation results between damped and undamped

4.2 水平通道与垂直通道实际航行数据阻尼试验

采用北极科学考察的一段船载实际惯性导航数据和计程仪数据进行法向量位置模型下的半实物仿真试验。本段数据采用GNSS卫星导航数据作为参考真值，航行器从起始位置（56.99° N ,174.1°E）开始，经过72 h穿越180 °经线运动至（71.37° N ,169.5°W）。垂直通道解算以文献[12]中的“惯性/卫星”组合导航方法为基准，进行阻尼时消除了初始高度的常值偏差；水平通道解算的位置误差做了归一化处理。经纬度对比、横经度横纬度对比、垂直通道和水平通道的导航解算结果对比如图6-8。

图6 经纬度对比和横经度横纬度对比Fig.6 Comparison on latitudes and longitudes and comparison on transversal latitudes and longitudes

经纬度对比和横经度横纬度对比如图6所示，从图中可以看到该阻尼算法适用于穿越极区、穿越对向子午线的场景，具有全球适用性。

观察垂直通道导航参数对比曲线图7可得，垂直通道阻尼算法能够抑制垂直通道的发散误差，阻尼后的高度和垂直速度与参考的惯性/卫星组合导航方法结果基本一致，高度误差不超过1.5 m，证明了算法的有效性。

图7 垂直通道导航参数对比Fig.7 Comparison on navigation results of vertical channel

观察水平速度对比曲线图8(a)(b)可得，阻尼后的水平速度相比于纯惯导解算的水平速度，周期性振荡误差得到了明显抑制。观察位置误差对比曲线图8(c)(d)(e)可得，相比纯惯导系统，阻尼算法有效抑制了位置误差的周期振荡，定位的归一化误差最大值大约减小了47%，提高了导航性能。

图8 水平通道导航参数对比Fig.8 Comparison on navigation results of horizontal channel

5 结 论

本文以水下长航时惯性导航为背景，基于全球统一编排的法向量位置模型，分别在垂直通道和水平通道中设计实现了阻尼算法。试验结果表明，该阻尼算法不需要出入极区的算法切换，避免了切换带来的阻尼过程不连续的问题，具有全球适用性，有效抑制了垂直通道的发散误差和水平通道的舒勒周期振荡误差，提升了导航性能。下一步将针对恶劣海况、航行器大机动等条件对阻尼切换、变阻尼算法等内容进行深入研究。