基于高斯和粒子滤波的AUV水下地形辅助导航方法

韩月，陈鹏云，沈 鹏

（1. 中北大学机电工程学院，太原030051；2. 太原旅游职业学院现代教育信息中心，太原030032；3. 国家深海基地管理中心，青岛266237）

1 引 言

构建精确、可靠、自主的导航系统一直是制约自主式水下无人航行器（Autonomous Underwater Vehicle，AUV）远距离深潜与自主作业的一个瓶颈问题［1］。根据导航所采用传感器的不同，AUV 水下导航技术大致可分为三类：水声学导航、惯性基导航、地球物理导航［2］。水声学导航需要预先布置声学基阵，不利于AUV 独立作业，且导航范围受限。惯性基导航不需要外部支持，导航推算所需的数据全部由自身携带的传感器获得，但其定位误差随时间累积，需要定期上浮接收卫星信号修正，不适合AUV 长期、独立、全天候、高精度的水下导航要求。基于地磁场、重力场、水下地形等地球物理信息导航由于具有全天候、无源性且不易受外界干扰的特点，成为各国AUV水下导航方法研究的热点。地磁场、重力场导航由于受到测量方法和测量精度等因素的限制，在现阶段很难应用于实践。随着多波束测深系统的发展，高精度的海底地形测量成为可能，因此采用水下地形辅助导航（Underwater Terrain-Aided Navigation，UTAN）可以满足AUV 水下精确导航定位的需求。UTAN不需要布放外部传感器，不需要上浮修正，不存在累积误差，可以长时间水下导航定位［3］。作为一种对惯性导航系统提供精确位置修正的有效手段，UTAN 技术在近十年得到了快速的发展，国外多个研究机构或组织正致力于相关技术研究，并进行了水下地形匹配的海中试验［4-6］。

现有的地形辅助导航算法大多为地形高度匹配算法，基于地形高度变化信息，可以通过UTAN算法估算载体位置。水下地形具有很强的非线性特征，UTAN 是一个非线性估计问题。因此，系统的后验概率密度函数不能直接获得，只能使用模型近似。一般来说，近似模型可以分为两类：一类方法是使用一阶泰勒展开将非线性模型转换为线性模型，再使用卡尔曼滤波处理线性模型。由于地形的非线性，实际地形不能依靠一阶泰勒展开来近似，滤波容易发散［7］。另一类为基于直接概率准则的非线性估计方法。对于该类方法，Bergman N 等［8］在贝叶斯框架下提出了飞机的地形辅助导航模型，并指出问题实质是非线性贝叶斯后验概率估计。再引入了粒子滤波（Particle Filtering，PF）求解地形辅助导航的非线性贝叶斯估计［9］，取得了很好的效果。此后，PF 就成为地形辅助导航算法的研究热点。

PF的主要缺陷是重采样导致的粒子贫化问题，这将导致PF性能急剧下降。针对这一缺陷，近年来科研人员提出许多用于海底地形辅助导航的改进PF算法［10-12］。这些方法改善并优化了重采样过程，以增加粒子多样性。本质上，这些方法只能推迟颗粒耗尽，而不能完全消除。由于水下地形特征小于陆地地形，因此粒子贫化的影响更为严重。Kotecha J H 等［13］提出一种高斯和粒子滤波器（Gaussian Sum Particle Filtering，GSPF），使用高斯和分布函数作为PF 的重要密度函数，没有重采样过程，从原理上消除了颗粒消耗，并且无重采样步骤也减少了滤波的计算量。

本文在建立基于贝叶斯估计的水下地形辅助模型基础上，采用高斯和粒子滤波方法近似贝叶斯估计问题，并进行回放式仿真试验。

图1 基于多波束测深的AUV水下地形辅助导航示意图Fig.1 Schematic diagram of terrain-aided navigation using multi-beam sounding

2 地形辅助导航的贝叶斯估计模型

由于深度计无累积误差，因此在地形辅助导航模型中不需要考虑AUV垂向坐标的变化，可建立水下地形辅助导航系统模型如下：

其中，式（1）为系统的状态方程，式（2）为系统的量测方程。xt是AUV在t时刻的平面坐标；ut是提供参考导航信息的惯性导航系统所指示的位移；vt是惯性导航系统的误差；yt是实时测深数据；h（xt）是先验数字地图中xt位置的水深值；et是多波束测深误差。

为了便于计算，假设vt、et均为高斯白噪声，因此，t 时刻的系统的先验概率密度函数（Probability Density Function，PDF）可用式（3）表示：

若用实时测深数据yt计算地形辅助导航系统误差，基于贝叶斯原理，需得到系统的后验PDF。假设条件独立，贝叶斯估计的状态更新方程为：

式（4）为后验PDF，其分母是一个归一化常量，且与xt无关。根据系统的状态方程可知，由p(xt-1|yt-1)通过位置更新可得到状态预测PDF，假设位置变量相互独立，系统状态预测方程如下：

由式（1）、（2）、（4）、（5）可得地形辅助导航的递推Bayesian滤波估计模型为：

贝叶斯滤波最大困难是后验PDF 的计算，对于一个非线性随机系统，很难得到系统后验PDF解析解，计算难度较大［14］。对此，本文用高斯和滤波近似贝叶斯滤波，并利用粒子滤波对地形辅助导航模型进行数值求解，提出一种基于高斯和粒子滤波的地形辅助导航方法。

3 水下地形辅助导航的GSPF算法

3.1 高斯和滤波器

通过多个高斯函数（分量）加强混合，高斯和滤波器（Gaussian Sum Filtering，GSF）可用来近似贝叶斯滤波。从原理上讲，如果选取的高斯分量足够多，GSF可无限逼近贝叶斯滤波的效果。由于式（1）和式（2）所描述的水下地形辅助导航模型中的噪声均为高斯白噪声，所以其在t 时刻的先验PDF 可用高斯和函数p(·)近似。水下地形辅助导航的GSF模型可描述为：

（1）量测更新

在t 时刻，系统状态的预测分布可用如下高斯和函数所示［15］：

其中，G 为并行GSF 的数目，μti，Σti，ωti可利用式（9）计算得到：

在获取t 时刻的多波束测深数据后，后验PDF如下：

（2）时间更新

由于p(xt|Yt)可用高斯和函数近似，因此p(xt+1|Yt)也可用高斯和函数近似，即：

3.2 地形辅助导航算法

基于有限高斯分量组成的高斯和函数，GSF 可近似系统状态的后验PDF。一般来说，其采用一组并行的扩展Kalman滤波器（Extended Kalman Filter，EKF）实现系统的状态估计。而基于一阶泰勒展开的EKF舍弃了高阶项，对于强非线性系统滤波器存在发散的可能性。因此，本文用PF代替GSF中的并行EKF，提出一种基于GSPF 的水下地形辅助导航算法，其计算过程如下：

（1）初始化：t=0，且t0时刻系统状态的先验分布可定义为其中，ω0j=p(x0)中采样获得初始粒子集且粒子的权值

（2）采集多波束实时测深数据yt，并从先验数字地图中提取局部地形数据h（xt）；

（3）量测更新

（a）更新粒子的重要性权值：

（b）计算每个高斯分量的均值和协方差：

（c）计算高斯分量j，j=1，2，…G的权值：

（d）归一化高斯分量权值：

（4）输出地形辅助导航滤波的状态估计和方差估计：

状态估计：

方差估计：

（5）预测更新

（a）对于每一个高斯分量j，j=1，2，…G，在重要性密度函数中抽样得到样本集

（d）更新预测的均值和方差；

（6）基于式（1）预测t+1 时刻的参考导航位置xt+1；

（7）t=t+1，转到步骤（2）。

本文提出的水下地形辅助导航算法流程如图2所示。

图2 水下地形辅助导航算法流程Fig.2 The Flow chart of UTAN based on GSPF

利用PF代替并行EKF，可避免EKF近似GSF所引入的误差。与基于重采样技术的PF 相比，GSPF利用预测更新得到粒子集，状态的后验PDF可用粒子和粒子权重表示。因此GSPF 无粒子退化现象，从原理上避免重采样所带来“粒子贫化”现象。同时没有重采样过程，GSPF 计算量与标准PF 相比较小，具有更好的实时性。

4 仿真结果及分析

图3 所示为AUV 搭载多波束测深系统的某次水下地形测量所获得的先验数字地图，其范围的大小为1000m × 900m，最小网格分辨率为1m × 1m。该测量区域的水深在5～40m 之间，包含了沟壑、坑洼地带、平坦地形等大部分近海海域所具有的水下地形特征。

图3 先验地形数据和独立多波束测线Fig.3 The underwater digital terrain map and independent measuring line

图3中黑色曲线所示的路线为海试中一次多波束测量所沿路线，用于模拟仿真试验中实时多波束测深数据。该测线由试验海区内的一次独立多波束测深试验获得，其数据不参与先验地图的制作，可保证实时测深数据和先验数字地图数据的相互独立。

仿真试验采用回放式仿真的形式，即实时测深数据通过不断读取独立测线中的原始多波束数据进行模拟。由于实时测深数据和先验数字地图的数据均来源于海中试验，因此回放式仿真可以模拟真实海洋环境条件下的水下地形辅助导航过程，从而验证算法的可行性。分别基于高斯分量数目、粒子数、滤波周期以及初始定位误差的不同，研究水下地形辅助导航算法精度的影响因素。

（1）初始定位误差50m，滤波周期2s，粒子数2000，选取不同的高斯分量数目，得到的地形辅助导航滤波结果如图4所示。

图4 基于不同高斯分量数目的导航结果Fig.4 The navigation results with different Gaussian component number

作为对比，IPF 地形辅助导航算法［16］也被同时执行。从图4 可以看出，高斯分量数目的变化对滤波收敛速度无影响，但是高斯分量数目较少时，GSPF导航精度比IPF导航精度低；此时若增加高斯分量数目，GSPF 导航精度可达到IPF 精度；但若进一步增加高斯分量数目（＞10），GSPF 导航精度并未增加。如果粒子数相同，高斯分量的数目的增加将导致计算量的增加，因此在保证地形辅助导航精度的前提下，高斯分量数目的取值越小越好，本文中高斯分量数目的取值为10。

（2）初始定位误差50m，滤波周期2s，高斯分量数目10，选取不同的粒子数，得到的地形辅助导航滤波结果如图5所示。

从图5 可以看出，不同粒子数的对导航滤波性能并不产生明显影响，增加粒子数不能有效提高滤波收敛速度和改善滤波收敛后误差区间的稳态，而粒子数过多会影响地形辅助导航的实时性，因此粒子数的数目不能过多。

（3）初始定位误差50m，高斯分量数目10，粒子数1000，选取不同的滤波周期，得到的地形辅助导航滤波结果如图6所示。

从图6 可以看出，滤波周期和滤波收敛所需的航行距离成反比：当滤波周期为1s时，在经过100m的航行后滤波已收敛；当滤波周期为32s时，穿越整个匹配区域滤波仍发散。

图5 基于不同粒子数的导航结果Fig.5 The navigation results with different particle number

图6 基于不同滤波周期的导航结果Fig.6 The navigation results with different filtering period

（4）高斯分量数目10，粒子数1000，滤波周期2s，选取不同的初始定位误差，得到的地形辅助导航滤波结果如图7所示。

由图7 可以看出，滤波收敛所需时间受初始定位误差的影响较大。若初始定位误差较小，在航行很短距离后滤波就可收敛；反之，若初始定位误差较大，需要较长时间航行滤波才能收敛。

基于上述仿真试验可知，在高斯分量数目、粒子数、滤波周期以及初始定位误差等影响因素中，滤波周期和初始定位误差对GSPF算法性能的影响较大。滤波周期和初始定位误差越小，滤波收敛时间越短，地形辅助导航实时性越好。这是因为滤波周期短，迭代后粒子的权值与真实情况更为接近，初始定位误差小，粒子的分布范围更接近贝叶斯滤波的PDF分布。

图7 基于不同初始误差的导航结果Fig.7 The navigation results with different initial positioning error

5 结 论

本文提出了一种基于贝叶斯滤波模型的水下辅助匹配导航模型，并针对贝叶斯滤波后验概率求解困难的问题，用高斯混合函数近似贝叶斯滤波的后验概PDF，提出一种基于高斯和粒子滤波的水下地形辅助导航方法，并通过仿真试验对地形辅助导航中的影响因素进行了研究，得到如下结论：

（1）与基于重采样技术的粒子滤波算法相比，本文提出的方法通过滤波更新和预测获得粒子，计算量较低。

（2）高斯分量数目较少时，随着数目的增多，GSPF结果也更加接近重采样粒子滤波结果，但当高斯分量达到一定数量时，进一步增加其数目并不能有效改善滤波性能。

（3）基于GSPF 的地形辅助导航方法对初始定位误差较为敏感，因此用其进行地形辅助导航时，应尽可能减小导航系统的初始定位误差。