基于修正观测值的水下移动节点定位算法

冯国君，单志龙,2，项 婉

（1.华南师范大学计算机学院，广州 510631；2.华南师范大学网络教育学院，广州 510631）

海洋开发是国家重要发展战略，水下定位技术是海洋开发的关键一环，广泛应用于环境监测、灾害预报、辅助导航、资源勘探、国防军事等领域[1]。水下定位系统受声速变化及定位目标移动等因素影响，实现水下移动目标实时高精度定位相对困难[2]。

水下定位系统一般用声呐通讯，但是声速在水下环境中并非恒定，而是随海水的温度、盐度、深度的变化而变化，声波沿弯曲路径传播。由于自身机动或者受洋流等因素影响，水下节点大多时间都处于运动状态，导致其不能在同一时刻及位置接收不同信标节点定位信号，造成的位置估计偏差较大[3]。图1为移动节点定位示意图。

针对上述问题，学者提出了多种解决方案。文献[4]提出基于修正时延的粒子滤波算法，根据上一时刻目标位置估计出当前时刻接收不同定位信号的时间间隔，利用这一时间间隔对目标状态进行粒子滤波或者卡尔曼滤波，得出当前时刻的目标位置估计。文献[5]提出修正扩展卡尔曼滤波方法（MEKF），在扩展卡尔曼滤波算法基础上，预测阶段采用多点预测的方式，使预测时间点对应信号到达目标节点时刻。以上方法的定位精度虽然比原始滤波方法的有所提高，但是没有考虑声速变化及声线弯曲的影响，造成定位误差较大。文献[6]同样使用扩展卡尔曼滤波方式对目标进行跟踪，预测更新时间点加上信号传播时间，同时引入平滑算法处理采样周期不固定问题。文献[7]则采用卡尔曼滤波“对齐”不同信标节点到达时间戳，同时应用声线跟踪算法修正弯曲声线，凸优化方法解算目标位置，在深海试验中取得较高的定位精度。以上文献都是采取一定的方法修正不同信号到达时间，虽然说水声通讯延迟相对较大，在水下环境中不可忽视，但是相对滤波方法的观测周期来说，不同信号到达目标时间戳的差值还是相对较小，修正精度较高的时间戳相对比较困难。文献[8]提出迭代逼近的方式修正弯曲声线。但是由于移动节点测量不是在同一时刻获得的，迭代逼近的方式只能初步修正弯曲声线的影响，不能消除节点移动给观测距离带来的测量误差，因此需要进一步修正观测值。

图1 移动节点定位示意图Fig.1 Mobile node positioning diagram

本文提出基于修正观测的粒子滤波算法（Modified Observations Particle filter,MOPF），从观测距离上减少声线弯曲及节点移动给定位带来的影响，实现对移动节点的高精度定位。

1 标准粒子滤波算法

在已知运动参数的情况下，可以对节点运动建模，采用序贯滤波的方式得出节点位置最佳估计。粒子滤波就是一种基于蒙特卡罗方法的随机采样序贯滤波方法[9]，其核心思想是通过在状态空间传播的随机样本近似后验概率密度，以样本均值代替积分运算，从而获得状态最小方差的过程。它摆脱了解决非线性滤波问题时随机量必须满足高斯分布的制约，对变量参数的非线性非高斯有更强的建模能力。粒子滤波算法先通过先验概率密度产生一组随机粒子，通过测量向量不断更新粒子权重，然后用带有权重的粒子更新先验概率密度函数，得出状态值的估量。

粒子滤波的算法流程为：

(1)粒子集初始化。

k=0 时刻，对于粒子i=1,2...N,由先验p(0x) 生成采样粒子

(2)对于k=1,2...,循环执行以下步骤：

① 重要性采样：对于i=1,2...N,从重要性概率密度中生成采样粒子计算粒子权值，并进行归一化，得到粒子集

③ 输出：计算k时刻的状态估计值：

2 MOPF 算法

粒子滤波对非线性非高斯随机系统有很好的跟踪效果。根据移动节点监测自身运动参数，对移动目标运动建模，运用粒子滤波算法产生随机粒子，根据水声定位系统测量值确定粒子权重，组合粒子权重即可得出目标方位。

对于随机系统，观测值的测量误差往往决定了系统跟踪的精度。要实现水下高精度定位，必然要考虑声线弯曲对测距的影响。对于长基线等声学定位系统，由于声线入射角不可获得，修正弯曲声线相对困难。基于此，本文提出二次修正测量值的MOPF 算法。算法原理及主要改进流程如下：

(1)用迭代逼近的方式修正声线弯曲的影响，得到信标节点Aj到未知节点的观测距离dj。假设此时在粒子滤波算法中，产生的随机粒子所处方位与声信标Aj的距离为，则粒子权重表示为：

其中，d0为未知节点到锚节点的真实距离，φ测量噪声，dm为由于节点运动带来的测量误差。dm是关于移动节点所处方位、运动速度的函数，当节点的方位和运动的速度确定时，dm是个确定值，相当于移动节点在信标节点方向上的运动距离，可以通过移动节点在信标方向的速度分量与信号到达时间差的乘积近似获得。

如图2所示，假设A、B、C、D 为信标节点，运动目标分别在t1、t2、t3、t4时刻接收到不同锚节点定位信号，对应传播距离为r1、r2、r3、r4。当运动目标接收到最晚到达信号时计算自身位置，此时目标与锚节点距离应该为r1'、r2'、r3'、r4。由图2可知，节点移动导致定位精度随位置变化的主要原因在于，用测得的信号时间计算的距离近似代替节点在接收到最后到达信号时所处的位置与声信标对应的距离，即用rj近似rj的值。而rj与的差值可以看作节点在信'标节点方向运动的距离。用移动节点在信标节点方向上的分速度，乘以信号到达时间差，可以得到的近似值。

图2 第二次修正观测值原理图Fig.2 Second modified observation value diagram

其中，vx、vy为移动节点在当前方位上x轴和y轴平均速度。由于节点运动趋势会发生改变，仿真过程使用前几个历史位置的平均值以及节点自身监测的运动参数近似获得。α为目标方位与信标节点连线在垂直方向上的夹角。实验过程通过上一历史位置与信标节点的方位近似计算获得。tj为第j个信标节点定位信号到达时间，为最后定位信号到达时间。

在求得距离修正值后，粒子权重更新为:

MOPF 算法的完整流程如图3所示。

图3 修正观测值粒子滤波算法流程图Fig.3 Flow chart of particle filter algorithm for correction of observation value

3 仿真实验

如图4所示，在水面布设4 个浮标A，B，C，D呈正方形布局，间隔1600 m，仿真中浮标位置在x,y,z坐标轴上设定10 cm 定位误差。设置移动节点在水下475 m 平面上，从信标组成几何中心出发，绕半径为500 m 圆形轨迹，以确定的速度逆时针做匀速圆周运动。假设移动节点配置高精度深度传感器，各个信标节点与移动节点经过同步时钟校准。信标节点每间隔5 s 同时向移动节点发送定位信号。仿真过程中使用南海某海域实测声速剖面精确计算声线弯曲路径及传播时延[10]，测时误差设定为50 μs。以上仿真环境参照文献[11]。粒子滤波粒子数为1000。

图4 移动节点运动轨迹Fig.4 Mobile node trajectory

节点移动性对定位误差造成的影响与目标运动的速度、目标相对于信标点的方位有关。如果忽略声速多变及声线弯曲的影响，用固定声速计算观测距离，平均定位误差达数十米，与精确修正声线的算法不具可比性。本文实验均在修正弯曲声线的条件下进行。

定义定位误差为：

其中(x,y)代表某次定位中，接收到最后到达的定位信号时移动节点所处的位置。(x',y')代表算法解算目标位置。

（1）定位精度对比

为了验证MOPF算法的优势，本节对MOPF算法、最小二乘算法、MEKF算法[5]的定位精度进行对比。最小二乘法是水下定位惯用的解算方法。MEKF算法考虑节点移动性的影响，将扩展卡尔曼滤波采用多点预测的方式进行位置估算[5]。

如图5所示，最小二乘法、MEKF算法和MOPF算法的平均定位误差分别为2.0 m、1.43m 和0.72m。MOPF 算法的定位误差明显比最小二乘法和MEKF 算法低，并且在不同方位时定位精度都保持稳定。定位精度相比传统方法提高一倍。这是因为MOPF算法首先采取声线跟踪的方式修正弯曲声线，然后再根据节点的运动特定与信标节点的关系进一步修正观测距离，并且充分利用节点运动模型以及历史位置等信息得出最优的位置估计，因而能够最大限度消除声线弯曲和节点移动性带来的影响，实现了高精度定位。

图5 定位精度对比Fig.5 Positioning accuracy comparison

（2）节点所处方位对定位精度的影响

当节点处在不同方位时，定位信号到达移动节点的时间间隔差异较大，造成定位精度随目标方位变化而变化。

图6为各个信标节点定位信号到达深度为475 m平面坐标点的最大时间差等时图。由图可以看出，信号到达最大的时间差由几何中心点向四周方向逐渐扩大，在信标节点正下方最大时间差达已经达到1.2s。相对于信标节点位置，在相对对称的方位时间差相对较小，等时线呈菱形分布。该图表明，即使是目标静止不动，在不同方位接收到各锚节点的信号时刻差别也比较大，在偏离中心点位尤为明显，目标节点偏离信标节点几何中心是导致信号不能同时到达的主要因素。

图6 定位信号到达475m深度平面最大时间差等时图Fig.6 Isochronousmapof the maximum timedifference when positioning signal reaches 475m depthplane

如图7所示，当目标节点以5m/s的速度做匀速圆周运动时，第一次修正观测值后，MOPF算法平均定位误差为2.1m，定位精度及变化趋势与最小二乘法大体相当，误差随着目标位置变化而变化。这是因为第一次修正观测值只是修正了弯曲声线带来的影响，并没有修正目标的移动性带来的误差。二次修正观测值后，MOPF算法平均定位误差为0.72 m，定位误差明显下降，目标运动造成的误差得到补偿，体现了改进算法的良好性能。如对最小二乘法也使用两次修正的方法，则其平均误差为1.2m，定位精度也会有所提高。

图7 节点所处方位对定位精度的影响Fig.7 Influence of theorientation of thenodeon the positioning accuracy

（3）节点运动速度对定位精度的影响

节点运动速度决定定位信号相继到达的时间间隔内节点移动的距离。假设目标节点以同样的轨迹做匀速圆周运动。如图8所示，以2m/s速度运动的节点平均定位误差为0.58m，与5m/s速度运动节点的平均定位误差0.72 m 相比，定位误差减小，但变化幅度不大。这是因为速度变小，在接收到信标节点全部定位信号时移动节点位移相对较小，因而定位误差相对减小。但对于低速运动的水下节点，在相对较短的时间内，节点的位移不会太大，况且MOPF算法会根据节点的运动特性修正观测距离，因此速度变化并不会对定位精度产生较大的影响。对于低速移动的运动节点，造成定位精度不稳定的主要原因是节点位置偏离信标节点几何中心，定位信号不是同一时间到达，到达的前后的时间差是造成误差偏大的主要因素。

图8 运动速度对定位误差影响Fig.8 Influenceof velocity on positioning error

4 结论

针对水下定位系统中节点运动及声线弯曲造成测距误差大的问题，本文提出了MOPF算法。首先根据声线跟踪原理，采用迭代逼近的方式初步修正观测距离，补偿声线弯曲造成的测量误差；然后利用目标运动速度在信标节点方向上的投影与信号到达时间差的乘积，进一步修正观测距离。最后修正粒子滤波算法的粒子权重，通过不同权重的粒子计算目标方位。仿真实验表明，移动节点以5 m/s的速度移动时，MOPF算法的平均定位误差为0.72m。定位精度较扩展卡尔曼滤波提升了50%。MOPF算法能够有效地克服声线弯曲和目标运动对测量距离造成的影响，在提高定位精度的同时确保了定位系统稳定性。