复杂水下环境下高精度SINS/USBL误差标定技术

张 涛，王 健，张佳宇

(1. 东南大学仪器科学与工程学院，南京 210096；2. 北京控制与电子技术研究所，北京 100038)

0 引言

在捷联惯性导航系统(Strapdown Inertial Navigation System, SINS)/超短基线(Ultra-short Baseline, USBL)一体化产品的研制方面，水下定位的迫切需求促使水声定位技术进一步发展，USBL定位系统及装备朝着惯性/水声一体化的方向发展。SINS/USBL一体化系统中，载体坐标系和声学坐标系在出厂前不可避免地存在安装误差角和杆臂误差，并且均为固定的常值。因此，在导航与定位前需要提前进行标定并补偿，精确标定后无需重复标定，这也是一体化系统的突出优势。影响高精度USBL定位系统定位精度的主要因素有系统自身误差、海洋环境参数测量误差以及SINS/USBL之间的安装误差。其中，声学基阵安装误差是定位系统的主要误差源, 从数值角度粗略分析:1°的航向安装误差角将会引入约1.7%斜距的水平位置误差，因此，在实际使用前需要进行安装误差的标定[1]。

国内外众多研究所和公司在标定算法方面进行了研究，但是国外公开报道的文献和技术资料并不多。2003年，D.Philip等对影响声学基阵标定精度的因素进行了分析，包括采集数据的质量、USBL基阵的几何结构、高效的数值计算算法和观测值的权重等[2]。R.Mcewen等在2005年通过对分立式USBL系统的研究发现，声学传感器与姿态传感器之间的平均安装误差在2.5°左右，标准差在0.5°左右[3]。国内对于USBL标定技术的研究以哈尔滨工程大学、中国海洋大学和东南大学等科研机构为主[4]。将全球定位系统(Global Positioning System， GPS)与水下声学定位系统相结合, 中国海洋大学的唐秋华利用空间测距交会的方法确定水下应答器的位置, 并推导了声学基阵坐标系和载体坐标系之间的转换关系, 通过最小二乘法求取出6个校准参数，实现了对安装误差角的标定[5]。哈尔滨工程大学的郑翠娥等提出了一种基于最小二乘的USBL定位系统安装误差校准方法，并推导了完整的误差标定模型，长江和海试结果证明了其有效性[6-8]。基于对安装误差角所导致的定位误差的分析，台湾国立中央大学的陈信宏等利用测量船绕着海底的应答器行驶，先确定翻滚误差角，然后确定航向误差角，最后是纵摇误差角，并通过仿真验证和实际实验证明了算法的有效性，但航向安装误差角和纵向安装角的估计会出现振荡[9-10]。在此基础上，东南大学的童金武基于布放的2个应答器提出了一种基于增量迭代的安装误差角快速标定算法，但是该算法对应答器的布放有要求，限制了其应用[11-12]。同时，在实际标定的环境中，由于海洋环境并不安静，受波浪、船只和波浪所形成的空气气泡及温度、盐度等复杂水文环境的影响，水声数据的量测噪声的特性是未知的，且存在不规律的野值，这些都会影响标定的精度[13-14]。

为了提高标定算法的适用性和实用性，本文对复杂水下环境下高精度SINS/USBL一体化系统误差标定技术展开了研究，旨在实现对一体化系统安装误差角和杆臂误差的精确估计。

1 标定误差的定义

(1)

(2)

其中，Δx表示投影在载体坐标系x坐标方向的偏移量；Δy表示在y坐标方向的偏移量；Δz表示在z坐标方向的偏移量。

2 基于量测信息滤波估计的误差标定技术

进行安装误差标定时，将应答器投锚在水底，其位置可以由长基线的方式精确标定。SINS/USBL一体化装置通过刚性连接的压力杆投放于水中，压力杆和测量船之间同样刚性连接在一起，同时，搭载高精度的RTK-GPS。安装USBL系统的测量船围绕着应答器进行运动，在每个航迹位置，声头与应答器相互应答通信，记录航迹上各个测量点的GPS提供的位置信息、SINS敏感的方位/姿态信息以及USBL系统测量的水声信息等。声学定位系统向应答器发出询问信号，应答器检测到该询问信号后回复定位信号，标定方案的设计如图1所示。

图1的标定方案可以简化为图2的标定模型。

图1 标定方案设计示意图

图2 标定模型示意图

(3)

(4)

(5)

结合式(3)，考虑标定误差后，应答器在u系下的位置可表示为

(6)

选择USBL原始输出的斜距与方位角信息作为观测量，则

(7)

(8)

(10)

根据式(10)，系统的状态方程可表示为

(11)

其中，Fusbl(t)=06×6。

3 基于Huber M估计的非线性鲁棒自适应标定技术

在实际的水下复杂工作环境中，水下声场是一种时变、多变和不稳定的声学信道环境。受水下复杂声学环境的影响，水声数据的量测噪声表现为未知或时变的统计特性。同时，由于水下多径的影响和基于宽带扩频的相干信号的相互干扰，估计的相关峰会产生错乱，导致水声定位系统的测量信息夹杂着没有规律性的野值，将会影响标定的精度，甚至导致无法完成标定工作。因此，本文提出了面向水下SINS/USBL一体化标定的新型鲁棒自适应标定算法。在高斯近似滤波框架下，Kalman滤波包括时间更新和量测更新两个阶段。在量测更新阶段，基于变分贝叶斯的自适应滤波算法对未知的噪声特性进行估计，采用M估计算法对声学野值进行鲁棒处理，并通过高斯牛顿迭代将M估计嵌入到变分贝叶斯框架中，使得滤波算法兼具鲁棒性和自适应性。

3.1 基于Huber M估计的非线性鲁棒滤波

为了解决标定过程中由于存在野值而导致滤波估计性能下降的问题，本文采用基于M估计的Kalman滤波算法。通过重构经典的Kalman系统和量测方程，使其具有归一化的噪声特性，引入Huber的代价核函数ρ，得到具有鲁棒特性的M估计的Kalman滤波算法。本节的创新之处在于将M估计直接应用于非线性的观测模型中，避免了对其进行线性化近似，得到了一种真正意义上的非线性鲁棒自适应滤波，具体的研究思路如下[15]：

构建非线性递归量测模型为

(12)

(13)

(14)

(15)

(16)

则非线性递归量测模型可以整合为

(17)

M估计的主要问题可转化为对上述非线性递归模型的求解，而上述非线性递归方程又可以转化为对如下Huber的代价函数的求解

(18)

其中，n为状态维数，d为量测维数，这种代价函数结合了l1和l2范数的性质，具有较好的鲁棒性。定义残差向量ζk=zk-g(xk)，ζi,k是向量ζk的第i个元素，p(·)是正定对称函数且p(0)=0，可表示为

(19)

其中，γ是调节因子，一般取值为γ=1.345。纯高斯分布条件下，该代价函数的估计效率是基于l2范数估计的95%。

定义φ(ζi,k)=p′(ζi,k)，令式(18)的代价函数取最小值时，则

(20)

定义ψ(ζi,k)=φ(ζi,k)/ζi,k，则有

(21)

一种解决方案是构造伪观测噪声协方差，定义权重矩阵为Ψ=diag[ψ(ζi,k)]，用来修正量测噪声协方差，即

(22)

=Λk|k(d+1:d+n,d+1:d+n)

=Pk|k-1

(23)

(24)

另外一种解决方案是构造伪观测值，当|ζi,k|≥γ时，采用γ/|ζi,k|替代ζi,k，定义修正后的残差为

(25)

(26)

(27)

=(Λk|k)1/2(1:d,1:d)[g(xk)(1:d)]+

Ψ(1:d,1:d)ζk(1:d)

(28)

上述两种处理方法的效果是相同的，可以采用上述任何一种方法重新构造信息,新方法无需对观测方程进行线性化近似，保证了非线性滤波的精度。

3.2 基于Huber M估计的鲁棒自适应滤波

针对SINS/USBL标定的误差模型和滤波方程，本文采用非线性滤波估计的方法，旨在实现对安装误差角和杆臂误差的准确估计。考虑到外部量测信息噪声特性未知且夹杂着大量野值的情况，本文提出了基于Huber M估计的非线性变分贝叶斯滤波算法，并将其应用于SINS/USBL的误差标定中。在变分贝叶斯的自适应滤波的基础上，高斯牛顿迭代将M估计嵌入到变分贝叶斯的滤波框架中。同时，M估计可直接应用于非线性的观测模型，避免了对其进行线性化近似，得到了一种真正意义上的非线性鲁棒自适应滤波，提高了滤波的精度、鲁棒性和自适应性。

4 仿真验证

根据设计的标定方案，利用仿真数据验证算法的有效性。在仿真实验中，首先设置搭载船航行的轨迹和状态，生成惯性设备实时的姿态信息和搭载船的位置信息，而应答器位置已知，根据船和应答器的位置可以推算出水声设备输出的斜距和方位角的真值，根据表2所示的传感器参数模拟复杂水下声场环境下的水声数据。仿真实验中涉及的主要传感器有高精度全球卫星定位系统(RTK GPS)、USBL定位系统及其IMU，各传感器误差设置如表3所示。

标定的轨迹如图3所示，搭载船的起始位置设置为(118.7718°E, 32.1459°N, 32m)，应答器相对船起始点的相对坐标为[-100,200,-100]m。不失一般性，SINS/USBL一体化系统的标定误差设置如表4所示。

表1 基于Huber M估计的非线性鲁棒自适应伪代码

表2 USBL系统仿真参数

表3 惯性设备仿真参数

图3 标定轨迹

表4 一体化系统的标定误差设置

在实际复杂的水文环境下，USBL输出水声信息的噪声特性未知且存在野值，为了模拟这种情况，根据如下方式产生野值干扰的量测噪声

(29)

(30)

其中，vr和vdir分别表示斜距和方位角噪声;w.p.表示以概率，也就是说方差为原来100倍的量测噪声以10%概率出现在原来的高斯分布中，用以模拟野值的干扰。为了分析USBL有无野值的区别，图 4给出了其有无野值时的概率密度对比曲线。

图4 概率密度对比

由图4可知，由于量测野值的干扰，量测噪声均具有厚尾分布，从而使高斯分布不再匹配系统的量测噪声分布，表现为“矮胖”，拖着长长的“尾巴”，这是因为野值也以一定的概率充当了“尾巴”。由于野值的存在，基于递归量测更新的高斯近似滤波算法对非线性标定模型的精度提升有限。因此，在本节中高斯近似滤波算法采用容积Kalman滤波(Cubature Kalman Filter, CKF)即可。将推导的基于Huber M估计的非线性鲁棒标定技术简称为C-HCKF，将推导的基于Huber M估计的鲁棒自适应标定技术简称为C-HVBCKF。SINS/USBL一体化系统安装误差角的标定结果如图5～图7所示，安装误差角各标定方法的统计结果(最后100s)如图8所示。

针对USBL输出的水声数据出现的噪声特性未知且夹杂着野值的情况，由图5～图7的标定结果可以看出，本文提出的基于Huber M估计的非线性变分贝叶斯滤波方法C-HVBCKF实现了对安装误差角的有效估计，这是因为Huber M估计结合了l1/l2两种范数构建代价函数，在变分贝叶斯的滤波框架内，通过加权矩阵对观测野值进行限制，而变分贝叶斯又可以实现对时变量测噪声的估计。结合图8定量的统计结果可知，提出的C-HVBCKF算法均方根误差(Root Mean Squared Error, RMSE)的估计精度可稳定在0.01°数量级。安装杆臂的估计结果如图9～图11所示，安装杆臂各标定方法的统计结果(最后100s)如图12所示。

图5 纵向安装误差角

图6 横向安装误差角

图7 航向安装误差角

图8 安装误差角标定方法对比

图9 横向杆臂估计

图10 纵向杆臂估计

图11 天向杆臂估计

由图9～图11可知，本文提出的基于Huber M估计的非线性变分贝叶斯滤波方法C-HVBCKF同样实现了对安装杆臂的有效估计，分析其原因同安装误差角估计类似。由安装杆臂估计的对比曲线也可以看出，基于一般CKF标定算法无法完成对安装杆臂的估计，因为其并未对未知的量测噪声进行估计，也未对声学野值进行处理，导致滤波估计精度下降甚至发散。结合图12定量的统计结果可知，本文提出算法的RMSE估计精度相对其他算法有大幅提升，具有更好的滤波性能，表现为高精度的状态估计和快速的收敛速度，安装杆臂误差(RMSE)在0.1m以内。

5 结论

针对SINS/USBL一体化组合样机系统需要提前进行精确标定的问题，提出了基于量测信息滤波估计的误差标定方法，精确校正后无需重复标定。

与其他的标定方法相比，该算法表现出更好的滤波性能，具体表现为更快的收敛速度、更高的估计精度和更平滑的滤波曲线，证明了提出算法的有效性和优越性。虽然基于变分贝叶斯的自适应标定算法C-VBCKF和基于M估计的鲁棒标定算法C-HCKF也表现出不错的滤波性能，但是，由于两个算法均无法同时兼具对时变噪声的估计和野值的抑制，因此，在一定程度上影响了滤波中状态估计的精度，而普通的CKF标定算法几乎无法完成标定任务。