基于不变误差定义的INS/USBL 紧组合导航算法

郭 瑜，徐 博，余志强，田鑫雨

（1.哈尔滨工程大学 智能科学与工程学院，哈尔滨 150001；2.武汉第二船舶设计研究所，武汉 430064）

以惯性导航系统（Inertial Navigation System,INS）为核心的组合导航技术已被广泛用于水下导航领域[1]。由于电磁波在水中衰减严重，常见的全球导航卫星系统(Global Navigation Satellite System,GNSS)等依赖电磁波传递信息的导航方式均不可用。在水下场景多以声学设备为辅助对INS 误差进行抑制。超短基线声学定位系统（Ultra-short Baseline Acoustic Positioning System,USBL）具有体积小、易部署、定位精度高等特点，将USBL 与INS 进行组合可以克服USBL 更新频率低、导航数据不连续以及INS 误差易发散的缺陷，成为了研究热点[2,3]。

文献[4]以USBL 解算的载体位置为观测量，结合惯导系统和其他传感器构建了联邦滤波结构，提高了系统的容错性。文献[5]直接利用接收机和声学信标的相对距离为观测量，构建了基于扩展卡尔曼滤波器的INS/USBL 组合导航系统。此外针对极区导航这类特殊场景，文献[6][7]基于网格坐标系构建了USBL 辅助下的组合导航系统。在现有的INS/USBL 松组合导航系统中，直接采用位置信息或相对位置矢量构建量测。实际上USBL 的原始测量为方位角和斜距，受安装偏差角、姿态误差等因素的影响，在位置信息解算的过程中容易引入确定性误差，并且滤波参数很难根据USBL 传感器精度准确选取[8]。为了解决这类问题，文献[8]-[10]直接利用USBL 测量的方位角和斜距信息构建观测量，得到了INS/USBL 紧组合导航系统，并采用基于Student’s t分布的鲁棒滤波器处理测量野值。针对USBL 异常信息检测的问题，文献[11]采用自适应模糊神经网络诊断和修复异常信息，提高了INS/USBL 紧组合导航系统的鲁棒性。相比于松组合，INS/USBL 紧组合导航系统已经具有更高的精度。

上述提到的组合导航系统中向量误差的定义都是计算值减去真实值。实际上，惯导系统解算的参考坐标系与真实的参考坐标系间存在失准角，使得解算的速度和位置矢量与真实的速度和位置矢量不处于同一坐标系。对于不同坐标系中的矢量，直接做差来定义误差是不准确的，易产生空间不一致性。惯导解算的参考坐标系和真实参考坐标系间的失准角越大，传统误差定义越不准确。为了解决上述问题，近些年来，李群理论开始被引入组合导航系统的研究中。通过将转动和平动定义在同一个李群空间中，能够有效改善空间不一致性，提高导航精度[12]。李群空间上定义的误差可以称为不变误差，所谓的不变是指对真实的群状态和惯导解算的群状态做相同的线性变换后，误差保持不变。文献[13]在惯性坐标系下基于不变误差定义构建新的惯导误差模型，并将其应用在初始对准中实现了粗对准和精对准的统一。文献[14]基于李群非线性误差构建了INS/GNSS/磁力计组合导航系统，相比于传统误差模型展示出了更好的姿态精度。文献[15]基于左不变误差和右不变误差的定义推导了地球坐标系下INS/GNSS 和INS/里程计（Odometer,OD）组合导航系统，结果表明左不变误差定义下的INS/GNSS组合导航系统和右不变误差定义下的INS/OD 组合导航系统更适合在大初始姿态误差条件下工作。文献[16]比较了不同惯导系统机械编排和误差定义对INS/多普勒计程仪（Doppler Velocity Log,DVL）组合导航系统的影响，结果表明在小初始姿态误差条件下，左不变误差定义下的INS/DVL 组合导航系统的精度最高。文献[17]则将李群理论应用在了INS/USBL 松组合导航系统中，在右不变误差定义下推导了松组合的观测方程。

考虑到传统误差定义的缺陷和INS/USBL 松组合的局限性，本文采用了左不变/右不变误差定义下的误差微分方程[15]，根据左不变和右不变误差的定义推导紧组合量测方程，构建了不变误差定义下的INS/USBL 紧组合导航系统。对不同的INS/USBL 组合导航算法进行了比较，结果表明：所提出的基于右不变误差定义的INS/USBL 紧组合导航系统比松组合具有更高的定位精度，与传统误差定义下的INS/USBL 紧组合导航系统相比，对初始姿态精度的需求更低，能够在初始对准效果不好的条件下工作。

1 USBL 工作原理

USBL 主要由声学信标和声学基阵构成。声学信标通常放置在海底，准确位置已知，声学基阵则安装在航行器上。USBL 利用声学信号到达声学基阵阵元的相位差解算声学信标与基阵中心的相对方位角，利用声学信号从声学信标到基阵中心的往返时延解算斜距，USBL 工作原理如图1 所示。

图1 USBL 工作原理Fig.1 USBL working principle

假设USBL 测量的方位角信息为α和β，斜距信息为r，则声学信标和基阵之间的相对位置矢量可以表示为：

2 基于李群误差的INS/USBL 紧组合导航算法

本文提出的算法主要包括纯惯性解算、组合导航模型构建、误差估计与反馈校正。首先，INS 实时解算导航参数，并构建导航误差状态方程。当USBL 数据更新时，利用USBL 输出的方位角和斜距信息构建量测方程并采用卡尔曼滤波器对误差状态进行估计，得到误差估计结果后对INS 进行反馈校正。组合导航系统结构图如图2 所示。

图2 INS/USBL 组合导航系统结构图Fig.2 Structure of INS/USBL integrated navigation system

2.1 INS 机械编排

与之前INS/USBL 松组合的研究工作类似[17]，本文同样采用地球系下的INS 机械编排方式。

INS 根据初始的姿态阵、速度矢量和位置矢量，结合式(2)就可以实时解算导航参数。式(2)所示的地球坐标系下INS 机械编排，相比于文献[8]中东-北-天坐标系下的机械编排，计算量更小，且在南北极点处没有计算奇异点，具备全球导航能力[18]。

更为重要的是，式(2)所示的INS 机械编排满足李群中“群仿射”性质，即对于式(3)(4)所示的群状态χ和动力学模型

其中，χA和χB分别表示不同的群状态。

则将群状态误差对数投影到李代数向量空间上后，李代数向量空间上误差模型独立于导航参数[19]。“群仿射”性质使得这类误差模型适用于较大的初始姿态误差条件。

2.2 INS/USBL 紧组合导航系统误差状态方程

在传统误差模型中将姿态误差定义在特殊正交群空间，将速度和位置误差定义在欧几里得空间，而李群理论将姿态矩阵、速度矢量和位置矢量构建在同一李群空间(如式(3)所示)，定义导航误差时不存在空间不一致问题。根据李群中左不变误差和右不变误差的定义分别推导两种误差定义下的惯导/超短基线紧组合导航算法。首先给出不同误差定义下的状态方程[15]。

2.2.1 左不变误差状态方程

式(3)所示群状态的左不变误差形式为[15]：

其中，ηl表示左不变误差群状态；表示由惯导解算的导航参数构成的群状态；χ表示由真实的导航参数构成的群状态分别为惯导解算的地球系到载体系的姿态变换矩阵、速度矢量和位置矢量。

所谓的左不变是指，对和χ左乘一个相同的矩阵后，ηl保持不变。利用李群和李代数间的对数映射关系得到左不变误差定义下的误差向量为[15]：

其中，φl表示左不变误差定义下的姿态失准角，φl=ϕla，a是单位旋转矢量；dvl和dpl分别表示左不变误差定义下的速度误差矢量和位置误差矢量。

由式(7)可以看出，最终得到的速度误差向量和位置误差向量中均考虑了姿态失准角φl的影响。因此李群理论的引入使得导航误差的定义更为准确。

从式(14)中可以看出，状态转移矩阵中只包含惯性器件的输出。对于中高精度的INS，陀螺漂移和加速度计零偏所引起的误差与载体角运动和线运动信息相差多个数量级。整个状态转移矩阵很准确，即使初始姿态误差很大，不准确的姿态阵也不会对状态转移矩阵的准确性产生影响，滤波性能容易被保证。

2.2.2 右不变误差状态方程

式(4)所示群状态的右不变误差形式为[15]：

其中，ηr为右不变误差群状态。所谓的右不变是指对和χ右乘一个相同的矩阵后，ηr保持不变。类似于左不变误差微分方程，利用李群和李代数间的对数映射关系可以得到右不变误差定义下的误差向量和其微分方程[15]：

其中，φr表示右不变误差定义下的姿态失准角；dvr和dpr分别表示右不变误差定义下的速度误差矢量和位置误差矢量。

选取误差向量φr、dvr、dpr、和 ∇b为状态量，得到右不变误差下的组合导航误差状态方程。

从式(21)(22)中可以看出，如果忽略惯性器件误差，误差状态的传播只与有关，不会受到含有误差的导航参数的影响。短时间内大初始导航误差对导航系统的影响比惯性器件误差的影响大得多。如果算法能在短时间内快速准确地收敛，那么不准确的惯性器件误差系数项对系统的影响就很小，几乎可以忽略。因此，这类右不变误差模型同样适合初始姿态误差较大的场合。

2.3 INS/USBL 松组合导航系统量测噪声分析

松组合导航系统将USBL 相对于声学信标的位置矢量与INS 相对于声学信标的位置矢量做差作为量测向量[17]。假设USBL 测量的方位角信息和斜距信息含有零均值高斯噪声，则USBL 测量的方位角信息和斜距信息表示为：

其中，α、β和r表示真实值；wα、wβ和wr表示测量噪声。由式(1)可知的计算涉及到非线性变换，这使得含有随机噪声的位置矢量相对于真实位置矢量不是一个无偏量[20]。例如：

即使USBL 测量的方位角和斜距信息的噪声是零均值高斯噪声，松组合的量测噪声也不再符合零均值假设，进而影响了松组合的滤波精度。因此，本文在不变误差定义下推导新的紧组合导航模型，直接使用方位角和斜距信息，以获得比文献[17]松组合模型更高的导航精度。

2.4 INS/USBL 紧组合导航系统量测方程

首先推导左不变误差下的量测方程，在左不变误差定义下，式(25)可以重写为：

对式(31)进行整理，得到左不变误差定义下的紧组合量测方程：

接下来推导右不变误差定义下的量测方程，与式(29)的推导过程类似，在右不变误差定义下，式(25)可以重写为：

同理可得到右不变误差量测向量为：

量测方程经过整理得：

从左不变误差定义下的量测方程中可以看出，量测矩阵Hl包含有姿态阵，当初始姿态误差很小时，含有误差的姿态阵并不会对量测矩阵Hl的准确性产生明显影响，结合完全独立于导航参数的左不变误差状态方程，使得左不变误差定义下的紧组合导航系统在较小的初始姿态误差条件下有着良好的性能。当姿态解算误差很大时，不准确的使整个量测矩阵的准确度受到负面影响，因此，在初始姿态误差较大时，左不变误差下的误差模型会面临收敛速度和收敛精度下降的问题。

构建紧组合导航系统的误差模型后可以采用线性卡尔曼滤波器对导航误差进行估计，根据误差估计结果对INS 进行反馈校正，抑制系统误差发散。

3 算法验证

3.1 数值仿真验证

在数值仿真中，将本文所提出的左不变/右不变误差定义下的惯导/超短基线紧组合导航算法（Left Invariant Error-based Tight couple Kalman Filter/ Right Invariant Error-based Tight couple Kalman Filter,LIETKF/ RIETKF）与右不变误差定义下的惯导/超短基线松组合导航算法（Right Invariant Error-based loose couple Kalman Filter,RIELKF）[17]以及传统误差定义下的惯导/超短基线紧组合导航算法（Tight couple Kalman Filter,TKF）进行比较。

基于式(2)中地球系机械编排的TKF 模型为：

仿真参数如表1 所示，所有噪声方差阵均按照表1中参数设置。仿真轨迹如图3 所示，初始姿态为[0 °,0 °,45 ° ]，初始速度为2m s，初始位置为[45.7796 °,126.6705 °,0 m]。采用三角函数模拟海浪、海风等外界扰动导致的载体角运动和线运动，俯仰、横摇和航向的摇摆幅值分别设置为[2 °,4 °,2 ° ]，摇摆周期分别为12 s、10 s 和8 s；横荡、纵荡和垂荡加速度幅值均设为0.2m s2，周期均设置为20 s。设置多个信标有助于提高系统可观测性，两个信标经纬度分别为[45.79 °,126.677 ° ]和[45.786 °,126.695 °]，深度100 m。

表1 仿真参数Tab.1 Simulation parameters

图3 仿真轨迹Fig.3 Simulation trajectory

首先对比小初始姿态误差条件下 LIETKF、RIETKF 以及传统误差定义下的TKF 的性能。初始水平姿态误差角从[0 °,0.2 °] 区间内随机选取，初始航向误差角从[0 °,1 °] 区间内随机选取。进行50 次蒙特卡洛仿真，采用均方根误差（Root Mean Square Error,RMSE）衡量算法的精度，蒙特卡洛仿真中RMSE 统计方式为：

其中，k表示时刻，j表示第j次蒙特卡洛仿真表示k时刻第j次蒙特卡洛仿真的导航误差；M为总的仿真次数，本文M=50。

仿真统计了三种算法的姿态角和位置的RMSE，如图4-5 所示。

图4 小初始姿态误差下三种紧组合算法姿态RMSEFig.4 Attitude RMSE of three tightly integrated navigation algorithms with small initial attitude error

图5 小初始姿态误差下三种紧组合算法位置RMSEFig.5 Position RMSE of three tightly integrated navigation algorithms with small initial attitude error

从图4-5 可以看出，即使初始姿态误差较小，三种算法仍有较明显区别。对于姿态中最为重要的航向角，TKF 的误差最大，并且TKF 的收敛速度明显比LIETKF 和RIETKF 慢，这是由于传统误差模型中速度和位置误差定义未考虑计算参考坐标系与真实参考坐标系间的失准角，误差模型的准确性不如不变误差定义，影响了TKF 的滤波收敛速度和收敛精度。

对于两种不变误差定义下的紧组合导航算法，从图4 可以看出，与LIETKF 相比，RIETKF 的航向精度略高、收敛速度更快。为了更细致地体现不同算法的精度，表2 统计了运行400 s 后三种算法的平均RMSE（Average RMSE,ARMSE），可以看出RIETKF和LIETKF 的定位精度和姿态精度非常接近，但RIETKF 略好，这是因为RIETKF 量测矩阵的精度完全不会受到导航误差的影响，而LIETKF 量测矩阵中会引入姿态误差和位置误差。因此，相比于TKF 和LIETKF，在小初始姿态误差条件下RIETKF 的导航精度略高。

表2 400 s 后三种紧组合算法的ARMSETab.2 ARMSE of three tightly integrated navigation algorithms after 400 s

在较大的初始姿态误差条件下，比较TKF、LIETKF 和RIETKF 三种算法的性能。初始水平姿态误差角从[0 °,10 °] 区间内随机选取，初始航向误差角从[0 °,60 ° ]区间内随机选取，进行50 次蒙特卡洛仿真，得到LITEKF、RIETKF 和TKF 的姿态误差图，如图6 所示。

图6 三种紧组合算法姿态误差蒙特卡洛仿真结果Fig.6 Monte Carlo simulation results of attitude errors for three tightly integrated navigation algorithms

从图6 可以看出，在这种较大初始姿态误差条件下，传统的TKF 容易发散，这是因为传统的线性误差模型并不能描述大姿态误差下非线性的误差传播。从式(42)可以看出，TKF 的状态转移矩阵中包含有加速度计输出在地球系下的投影，在进行滤波估计时，所采用的坐标系变换矩阵是INS 解算的在初始姿态误差较大的条件下，不准确的使得TKF的状态转移矩阵存在较大误差，严重影响了滤波估计精度，甚至无法收敛。

LIETKF 的收敛情况比TKF 好得多，但在部分条件下，收敛精度较差，存在较大的姿态误差，而RIETKF 始终能够实现很高的收敛精度。导致LIETKF和RIETKF 如此大区别的主要原因在于LIETKF 的量测矩阵中存在姿态阵(见式(33))，而RIETKF 的量测矩阵可以转化为只与声学信标位置有关。与不准确的对TKF 的影响类似，大初始姿态误差使得滤波初始时刻LIETKF 的量测矩阵极不准确，严重影响了滤波精度。综合考虑小初始姿态误差和大初始姿态误差下的仿真结果，右不变误差定义更适合惯导/超短基线紧组合导航系统，RIETKF 使得紧组合导航系统对于初始对准精度的需求降低。

为了证明紧组合导航系统相比于松组合导航系统的精度优势，比较右不变误差定义RIELKF 和RIETKF的导航性能。同样进行50 次蒙特卡洛仿真，初始水平姿态误差角从[0 °,5 °] 区间内随机选取，初始航向误差角从[0 °,30 °] 区间内随机选取，得到 RIELKF 和RIETKF 的姿态和位置RMSE 图，如图7-8 所示。从图7 可以看出，随着时间的增长，松组合和紧组合姿态误差的区别逐渐减小。图8 统计了RIETKF 和RIELKF 三个位置方向的RMSE，可以看出两种算法在东向和天向上的位置误差区别最为明显。

图8 RIELKF 和RIETKF 的位置RMSEFig.8 Position RMSE of RIELKF and RIETKF

图9 航行轨迹Fig.9 Test trajectory

图10 参考速度Fig.10 Reference velocity

表3 统计了400 s 后两种算法的姿态和位置ARMSE。对于姿态中最重要的航向指标，两者的ARMSE 几乎完全相同，差别仅有0.0001°，因此，单从姿态精度方面难以表明紧组合导航系统和松组合导航系统孰优孰劣。然而，RIELKF 的位置ARMSE 比RIETKF 大，这是因为松组合以USBL 解算的相对于声学信标的位置矢量为观测，在解算相对位置矢量过程中方位角和斜距经过了一系列非线性变换，使得松组合量测噪声难以保证零均值高斯特性，噪声方差难以准确设置，影响了导航定位精度。相比于松组合导航系统，紧组合导航系统在东向、北向和天向上位置精度分别提升了15.79%、2.61%和22.96%。综上所述，对于右不变误差定义下的惯导/超短基线组合导航系统，从定位精度方面考虑应该首选紧组合工作方式。

表3 400 s后右不变误差定义下紧组合和松组合算法的ARMSETab.3 ARMSE of RIELKF and RIETKF after 400 s

3.2 半实物仿真验证

由于缺乏USBL 测量的方位角和斜距数据，因此采用半实物仿真对本文算法进行验证。惯导系统数据来源于一次海试实验中的自研光纤捷联惯导系统，航行轨迹和速度分别如图 9-10 所示。初始位置为[18.23927 °,109.39400 °]，采用法国PHINS 惯导系统和GPS 组合提供参考信息。所设置的信标位置分别为[18.244 °,109.41 °] 和[18.260 °,109.407 °]，深度100 m。根据设置的信标位置和实验船参考位置模拟解算相对方位角和斜距。所采用的设备参数如表4 所示。

表4 设备性能Tab.4 Equipment performance

模拟的超短基线斜距刻度系数误差0.2%，斜距噪声标准差2 m，方位角噪声标准差0.2°，更新周期3 s。将左不变/右不变误差定义下的惯导/超短基线紧组合导航算法（LIETKF/RIETKF）和右不变误差定义下的惯导/超短基线松组合导航算法（RIETKF）的性能进行比较，为了保证算法的可重复性，同样进行了50次蒙特卡洛仿真。初始水平姿态误差角从[0 °,10 °] 区间内随机选取，初始航向误差角从[0 °,60 °] 区间内随机选取。图11-12 展示了RIELKF、LIETKF 和RIETKF三种算法的姿态和位置RMSE。

图11 三种算法姿态RMSEFig.11 Attitude RMSE of three algorithms

从图11 可以看出，三种算法最终的收敛精度几乎相同，但是收敛速度有着较大的区别。对于姿态中最重要的航向角，RIETKF 的航向误差收敛速度最快，RIELKF 和LIETKF 的收敛速度则慢得多。在RIETKF收敛很长时间后，RIELKF 和LIETKF 的航向误差仍比RIETKF 大。从图12 可以看出，对于东向位置和北向位置，紧组合模式RIETKF 和LIETKF 的定位误差要比松组合模式RIELKF 的定位误差小；对于天向位置误差，紧组合算法与松组合算法的区别更加明显。

图12 三种算法位置RMSEFig.12 Position RMSE of three algorithms

表5 统计了2000 s 后姿态和位置的ARMSE。对于重要的航向角精度，RIETKF 相比于LIETKF 和RIELKF 分别提高了8.46%和16.55%。对于定位精度，RIETKF 和LIETKF 较为接近，在东向和天向位置上，RIETKF 与LIETKF 相比，精度分别提高了2.72%和10.91%，仅仅在北向定位精度上比LIETKF 低0.52%，0.52%的差别几乎可以忽略。此外RIETKF 的定位性能更是全面优于RIELKF，在东向、北向和天向上，相比于 RIELKF 分别提高了 14.65%、30.26%和73.91%。从算法的收敛速度和定位精度考虑，RIETKF的性能最好，这与数值仿真得到的结论相同。

表5 2000 s 后三种算法的ARMSETab.5 ARMSE of three algorithms after 2000 s

4 结论

本文针对现有INS/USBL 紧组合导航算法误差定义不准确，影响导航精度的问题，引入李群误差理论，提出基于不变误差定义的INS/USBL 紧组合导航算法。仿真结果表明：1）传统误差定义下的INS/USBL紧组合导航算法收敛速度和收敛精度均不如不变误差定义下的紧组合导航算法，且在较大的初始姿态误差条件下更容易发散；2）不变误差定义下，紧组合算法的定位精度高于松组合算法；3）右不变误差定义更适合INS/USBL 紧组合导航系统，相比于左不变误差定义能够应对更大的初始姿态误差。