梁钟泓，谢元平，张永健，王 林，魏 国，高春峰

（国防科技大学 前沿交叉学科学院，长沙 410073）

自主水下潜航器（Autonomous Underwater Vehicle,AUV）在极地的科考、海底勘探等领域有着重要地位，而高精度的导航技术是实现水下潜航的关键[1]。惯性/多普勒测速仪（Inertial Navigation System/Doppler Velocity Log, INS/DVL）组合导航方式凭借其完全自主、高精度等优势，成为AUV导航的主要手段[2]。但是由于极区经线快速收敛，传统地理坐标系编排下的惯性导航方式在极区会出现很大的误差[3]，需要在进入极区时切换导航坐标系才能保证导航的精度。常用的极区导航坐标系有格网系[4]、横坐标系[5]等。其中，横坐标系参照地理坐标系的构建方法，将现有地理极点旋转90 °后到赤道作为新的极点构建而成，通过定义横经纬度，解决了AUV在极区的航向参考问题。

目前关于横坐标系导航的研究多集中于惯性导航算法优化方面。文献[6]提出了基于参考椭球模型的横坐标系导航算法，并验证了横坐标系适用范围，证明在中纬度地区横坐标系导航与传统惯性导航方式有着相同的精度。文献[7]从理论上重新推导基于椭球地球模型的极区导航机理，避免求解椭球半径，优化了算法的精度。文献[8]以载体所在位置的卯酉圈半径为半径，构建了虚拟圆球用于横向导航，相较于椭球模型的复杂编排，该方法在保持精度的前提下简化了运算。在极区组合导航方面，目前的研究主要集中在极区范围内组合导航滤波器设计。文献[9]比较了格网系与椭球模型横坐标系下的组合导航的精度，证明惯性/卫星组合导航在椭球模型横坐标系下具有更好的性能。文献[10]设计了横坐标系下的INS/DVL组合导航算法，可以有效抑制极区内方位失准角的增大。然而以上算法并没有考虑导航过程的连续性，忽视了载体进出极区时导航坐标系切换引起的滤波器超调与误差状态估计不连续的问题[11]。

针对这一问题，本文提出了基于协方差变换的INS/DVL全球组合导航算法，建立了横地理坐标系与地理坐标系下系统误差状态及协方差矩阵的转换关系，在载体进入极区切换导航坐标系时，同时完成导航参数、误差状态以及其协方差矩阵的转换，解决了滤波器结构变化导致的状态估计值震荡及收敛错误的问题，使得滤波器能够平滑切换，实现全球导航。

1 横坐标系的定义

定义横地球坐标系为e'系，定义横地理坐标系为t系（地理坐标系与横地理坐标系的定义均为“东-北-天”）。如图1所示，横地球坐标系是由地球坐标系旋转两次得到的[12]，旋转方式为：Oxe y e ze绕xe轴顺时针旋转90 °得到Ox′y′z′，Ox′y′z′绕z′轴顺时针旋转90 °得到Oxe'y e'ze'。根据坐标系之间的变换关系可以得到：

在横坐标系的定义下，N'与S'分别为横北极与横南极，Oxe'ye'为横赤道面，Oxe'ze'为横本初子午面。P为地球上一点，OP与横赤道面的夹角定义为P点所在位置的横纬度Lt，ON'P平面与横本初子午面的夹角定义为P点所在位置的横经度λt。南北极点在横坐标系的定义下位于横赤道上，在极区导航时切换至横坐标系就可以解决极区附近导航误差增大的问题。

地球坐标系e与导航系n之间的转换关系可以用来表示[13]：

式中，L、λ分别为P点所在位置的纬度、经度。横坐标系下的横经纬度、横地理系定义与地理坐标系类似，同样的方法可以得到横地球坐标系与横地理坐标系之间的转换关系。

横经纬度与经纬度的之间的关系[12]：

或者，

根据式(1)-(3)，地理坐标系与横地理坐标系之间的转换关系可以表示为：

2 横地理坐标系下INS/DVL组合导航滤波器设计

2.1 系统状态方程

综合惯导系统的误差项与多普勒测速仪的误差项，横坐标系表示下的INS/DVL组合导航的系统误差状态选为惯导系统的姿态误差φt、速度误差δvt、位置误差δ pt、陀螺常值零漂εb、加速度计常值零偏∇b，同时增广多普勒测速仪的比例因子误差δk、俯仰安装误差角δθ、航向安装误差角δψ共18维，即：

相关的状态方程为：

且有：

式中：

其中，R为地球半径。

2.2 观测方程

其中，vd为DVL测速仪理论输出值。将姿态矩阵表示为：

式中ςb=[δθ0δψ]T。将式(17)(18)带入式(16)得到：

式中忽略了二阶小量，υ为观测噪声。

3基于协方差变换的INS/DVL全球组合导航算法

AUV驶入和驶出极区时，导航坐标系要在地理坐标系与横地理坐标系之间切换，切换时既需要将导航参数完成转换，又要保证滤波器的系统状态估计值不会因为滤波器结构的变化而震荡，这是实现全球组合导航的关键。

3.1 导航参数的切换

在中低纬度地区，INS/DVL组合导航滤波器在地理坐标系构建[14]，输出的导航参数为地理坐标系下解算得到的姿态矩阵、速度vn、位置pn；AUV航行至极区时，需要切换到横地理坐标系下导航，导航参数为横地理坐标系下表示的姿态矩阵、速度vt、位置pt。切换时根据坐标系间的转换关系，确定导航参数的转换关系为：

同理，在AUV驶出极区时导航参数需要从横地理坐标系转换至地理坐标系。

位置转换由式(5)确定。

3.2 INS/DVL组合导航滤波器

导航坐标系切换后，组合导航滤波器的结构也会随之改变，Kalman滤波器中的系统误差状态及其协方差矩阵、状态转移矩阵、观测方程均需要完成转换。由于在两种导航坐标系下的系统状态方程、观测方程是事先建立的，所以在切换滤波器时只需要完成系统误差状态与协方差矩阵的转换。

中低纬度下，INS/DVL组合导航滤波器的系统误差状态在地理坐标系下表示为：

其系统状态方程、观测方程由式(8)-(11)、式(16)推得在此，不再赘述。对比式(7)，系统误差状态中姿态误差、速度误差和位置误差与导航系定义相关，陀螺零漂和加速度零偏、多普勒测速仪的比例因子误差、安装角误差与导航系定义无关，滤波器切换时无需转换，所以系统误差状态的转换只需要完成姿态误差、速度误差、位置误差的转换。

姿态误差的转换关系：

对式(2)两侧做扰动，得到：

式中：

另一方面，对式(4)两侧微分，得到：

将式(26)带入τt的表达式可以得到：

对式(6)两侧做扰动，得到：

根据定义可知，

将式(24)(27)(28)带入式(29)，以及根据δCbn的定义可以得到：

整理式(30)，得到姿态误差φt与φn之间的转换关系：

速度误差的转换关系：

与姿态误差转换关系类似，将式(28)带入δvt中即可得到速度误差的转换关系：

位置误差的转换关系：

位置误差的转换关系可以通过式(26)确定，高度误差在两个坐标系下是一致的，无需转换。

系统误差状态xn（t）与xt（t）之间的转换关系可以由式(31)-(33)确定：

其中，

根据协方差的定义，确定不同导航坐标系下的滤波器系统误差状态的协方差矩阵转换关系为：

同理，AUV驶出极区时，滤波器误差状态及其协方差矩阵的转换关系为：

整个算法的流程如图2所示。

图2 算法流程图Fig.2 Algorithm flow chart

4 实验验证

为验证算法的有效性，分别通过船载实验、半实物仿真实验进行验证。船载实验在中低纬度地区进行，半实物仿真实验按照文献[6]中方法，保持载体相对于地面的姿态和速度不变，将实验所在位置的纬度转换至高纬度地区，基于船载实验数据仿真产生高纬度地区的测试数据。

4.1 船载实验

实验船只搭载INS/DVL组合导航系统在长江某段航行实验，系统中陀螺零漂稳定性优于0.02 °/h，加速度计零偏稳定性优于50 μg，DVL的精度为0.5%V±5 mm/s。船载实验的起始位置为北纬30.8581 °，东经110.9877 °。实验时前900 s进行对准，对准后进入导航。在导航过程中，惯导系统的误差状态均进行反馈校正，多普勒测速仪的误差只进行估计，未进行反馈，前1 h使用地理坐标系进行导航，然后切换至横地理坐标系导航模拟驶入极区，航行1 h后再切换至地理坐标系模拟驶出极区，实验总时长为3 h。

中低纬度地区，地理坐标系下构建的INS/DVL组合导航滤波器不存在跟纬度相关的误差放大效应，将其组合导航结果作为对照，比较有协方差变换与无协方差变换的情况下，由于导航坐标系切换带来的系统误差状态估计值的变化。由于采用速度观测，所以导航坐标系切换时速度震荡并不明显，除速度误差外其他参数的相对估计误差如图3-7所示。

图3 相对姿态误差Fig.3 Relative attitude error

图3为相对姿态误差曲线，无协方差变换情况下俯仰角的误差震荡峰值为0.3 ′，横滚角误差峰值0.23 ′。由于航向角的可观测性较差，受导航坐标系切换的影响较大，误差峰值为2.54 ′。协方差变化算法的相对姿态误差不明显，能够保证滤波状态估计值在导航坐标系切换前后的一致性。

图4为相对位置误差曲线，从图中可以看出，有协方差变换时的东向相对误差峰值为10.38 m，北向相对误差峰值为27.72 m。而无协方差变换时，东向相对误差峰值为30.11 m，北向相对误差峰值为60.57 m。虽然高度参数理论上不受导航坐标系切换的影响，但是由于其他误差的耦合效应也发生了震荡，震荡峰值为2.06 m。

图4 相对位置误差Fig.4 Relative position error

图5 陀螺零漂相对估计误差Fig.5 Relative estimation error of the gyro drift

图6 加速度计零偏相对估计误差Fig.6 Relative estimation error of the accelerometer bias

图5-6为陀螺零漂、加速度计零偏的相对估计误差，有协方差变换时几乎没有发生震荡，无协方差变换时，三轴陀螺零漂的估计震荡峰值分别为0.0045 °/h、0.0058 °/h、0.049 °/h，三轴加速度计零偏的估计震荡峰值分别为45.21 μg、97.13 μg、3.11 μg。陀螺零漂、加速度计零偏在导航坐标系切换前后出现了持续震荡，甚至出现了收敛错误。由于方位陀螺(z轴)零漂的可观性弱于水平陀螺零漂，水平加速度计（x、y轴）零偏的可观性弱于垂向加速度计零偏，相应的状态估计震荡更为剧烈。

图7为DVL比例因子及安装参数的相对估计误差曲线。无协方差变换时，AUV进出极区过程中导航坐标系切换会使得其误差参数估计值产生震荡，并且出现了估计值收敛错误。

图7 DVL误差参数相对估计误差Fig.7 Relative estimation error of DVL error parameters

从图3-7可以看出，当导航坐标系切换时，协方差变换算法可以实现组合导航滤波器的平滑过渡，状态估计值震荡不明显。但如果忽略滤波器协方差矩阵的转换，状态估计值会出现不同程度的震荡，部分参数甚至收敛至错误值。

为进一步验证算法有效性，进行了六组实验，模拟AUV进出极区时导航坐标系的切换过程。无协方差变换时状态估计相对误差震荡峰值见表1，有协方差变换时状态估计相对误差震荡峰值见表2。

表1 无协方差变换的状态估计相对误差Tab.1 Relative error of state estimation with non-covariance transformation

表2 协方差变换算法的状态估计相对误差Tab.2 Relative error of state estimation with covariance transformation

对比表1-2的数据可以发现，有协方差变换时姿态误差、陀螺零漂、加速度计零偏的相对估计误差相较无协方差变换减少一个数量级。由于横地理坐标系下的INS/DVL组合导航滤波器是基于地球圆球模型设计的，存在模型误差[6]。使得在导航坐标系切换过程中，无论有无协方差变换均出现了一定程度的位置误差震荡，但采用协方差变换算法后位置误差仍减小50%以上，证明了该算法的有效性。

4.2 半实物仿真实验

为检验算法在高纬度地区的适用性，采用半实物仿真方法，将原数据转换至高纬度地区，转换后的起始位置为北纬79.95 °，AUV在5500 s时航行至北纬80 °进入极区，此时导航坐标系从地理系切换至横地理坐标系，图8为高纬度下AUV的行进轨迹。

在高纬度地区，横地理坐标系下重新定义了航向参考与横经纬度，在此坐标系下设计的INS/DVL组合导航滤波器不存在误差方法效应。因此，可以将横地理坐标系下的导航结果作为参考，比较协方差变换算法与无协方差变换的导航结果，如图8-12所示。

图8 高纬度下AUV的轨迹Fig.8 AUV trajectory at high latitude

图9给出了相对姿态误差的对比结果，其中无协方差变换时，导航坐标系切换带来的航向角误差峰值为26.05 ′，俯仰角与横滚角误差也达到了0.8 ′。

图9 高纬度下相对姿态误差Fig.9 Relative attitude error at high latitude

从图10可以看出，导航坐标系切换后协方差变换算法可以保证位置误差估计的一致性，与参考结果一致，但无协方差变换情况下位置误差出现了震荡、发散，东向误差峰值为113 m，北向误差峰值为84 m。图11-12显示，切换导航坐标系后，无协方差变换时方位陀螺(z轴)零漂、水平轴(x、y轴)加速度计零偏在高纬度地区的相对估计误差明显增大并且收敛至错误值。x、y轴陀螺零漂、z轴加速度计零偏震荡幅度相对较小，但陀螺零漂收敛较差。

图10 高纬度下相对位置误差Fig.10 Relative position error at high latitude

图11 高纬度下陀螺零漂相对估计误差Fig.11 Relative estimation error of the gyro bias at high latitude

图12 高纬度下加速度计零偏相对估计误差Fig.12 Relative estimation error of the accelerometer bias at high latitude

图13为DVL误差参数的相对估计误差，无协方差变换时导航坐标系切换引起的跳变幅值增大，震荡后发散。

图13 高纬度下DVL误差参数相对估计误差Fig.13 Relative estimation error of DVL error parameters at high latitude

从仿真结果可以得到如下结论，随着纬度升高，地球自转角速度在水平面的投影分量减少，并且载船的机动性较弱，相关误差状态的可观性减弱，导航坐标系切换对于组合导航滤波器状态估计值一致性的影响变大，无协方差变换时误差状态估计值的震荡变大，而协方差变换算法能够保证滤波器的平滑过渡，状态估计精度得以保证。

5 结 论

为解决AUV进出极区导航坐标系切换引起的系统误差状态估计不一致导致的滤波超调问题，实现全纬度导航，本文提出了一种基于协方差变换的INS/DVL组合导航滤波算法，该算法以横地理坐标系与地理坐标系为例，建立了系统误差状态及其协方差矩阵在两种坐标系之间的转换关系，解决了坐标系切换带来的误差状态估计不连续的问题。本文分别通过船载实验与半实物仿真实验验证了该算法的有效性，由此证明协方差变换算法全纬度适用。相较于无协方差变换，该算法能有效减少导航坐标系切换带来的导航参数误差，保持导航的连续性。此外，该算法只需在现有实现方案的基础上进行少量的调整，有利于惯导系统的升级。未来的研究工作将探索协方差变换算法在卡尔曼滤波阻尼切换等场景下的应用。