2. 中国人民解放军69016部队, 新疆 乌鲁木齐 830001
近年来，多自主水下航行器(multiple autonomous underwater vehicles,MAUVs)协作系统实现了系统中各AUVs间的协调与信息交互，可有效地减少任务时间，提高工作效率，在海底勘探、水下侦查、目标搜索等方面有着广阔的应用前景。
MAUVs协同定位是在协作系统的基础上发展而来的一种新型定位方法，其利用水声通信技术，交换共享各自实时相对距离与方位信息进行融合，从而获得更高的整体定位精度[1]。当前在AUVs间水声通信带宽限制下，协同定位系统以领航-跟随式(leader-follower)结构为主，各AUVs间相对距离信息主要通过测量水声信号传播到达时间(TOA)，信号接收强度(RSS)等手段来获得，在实际复杂环境中，水声信号均会受到非视距(non line of sight,NLOS)传播的影响[2, 3]，造成AUVs间较大的相对距离测量误差，进而影响MAUVs协同定位精度。
在未获得NLOS距离量测先验信息的基础上，文献[4, 5, 6]将NLOS量测误差作为残差未知量，重新构造定位方程，并分别利用二次规划、最小二乘及半定规划等最优估计算法进行求解。文献[7]通过在线均值与变量估计法设计了NLOS识别方法，有效减小了NLOS测量有偏误差，并将所得距离量测信息应用于移动目标跟踪算法中。在文献[8]中，假设NLOS测量未被识别的前提下，建立了距离量测变化模型，并将Kalman滤波算法应用于NLOS量测平滑过程中，获得了理想的距离量测信息。
目前，在已有的大多数MAUVs协同定位问题研究中，均理想化地假设AUVs间的距离量测为视距(line of sight,LOS)量测。针对此问题，本文基于双领航者的MAUVs协同定位系统，在无法实时识别LOS/NLOS量测的情况下，以4状态Markov链描述了定位过程中的LOS/NLOS量测模型转换过程，利用交互多模(interacting multiple models,IMM)理论设计了一种距离量测平滑算法，从而希望实现LOS/NLOS量测模型的自适应匹配，有效提高AUVs间的相对距离量测估计精度。
1 问题描述在实际海洋环境中，特别是浅海海域，由海面、海底及目标障碍物所引起的信号多径传输效应会对水声换能器之间的距离测量造成影响。研究发现[2]，在水下多障碍物环境中，由水下目标障碍物(如礁石、船壳等)所引起的信号非视距传播，相较于海面、海底及温跃层对信号的反射与散射，会造成更大且难以忽略的水声测距误差。当MAUVs协同编队在复杂环境下航行时，如图 1所示。其中，领航AUV1与跟随AUV间为NLOS传播，由于障碍物的遮挡，信号从A点出发在B点经过折射才能传至C点(图中实线)，无法直线传播(图中虚线)，继而造成两AUVs间的量测距离大于实际距离；领航AUV2与跟随AUV间则可认为是LOS传播。
 |
| 图 1 MAUVs系统中的NLOS测距 |
利用水声信号传播延迟时间计算AUVs间距离，则领航AUV κ(κ=1,2)与跟随AUV在k 时刻的距离量测为
式中，c为水中声速，一般可认为c≈1 500 m/s，ΔTκ为AUVs间水声信号发送与接收时间差,dκ(k)为相应AUVs间的真实距离；mκ(k)为NLOS条件下的距离偏差，且mκ(k)>0，而在LOS条件下mκ(k)=0; wκ(k)为协方差为σi2(k) 的零均值高斯白噪声，且有
为了说明AUVs间距离量测在LOS/NLOS条件下的转换，引入变量 M(k)={r1(k),r2(k)}表示在k时刻量测模型所处的条件，且有M(k)∈{Mj}rj=1，其中r=4为量测模型可能的个数，则量测模型所有可能情况为M1={LOS,LOS},M2={LOS,NLOS},M3={NLOS,LOS},M4={NLOS,NLOS}。为了描述这4种量测模型间未知的相互转换过程，引入4 状态1阶Markov链进行表示，其相应转换概率为
由于在实际MAUVs协同定位过程中，变量M(k)表示的量测模型所处条件很难被识别，因此需要设计相应的算法在LOS/NLOS混合条件下对AUVs间距离量测进行估计。 2 NLOS量测平滑算法 2.1 量测模型
在基于领航者的MAUVs协同定位系统中，可定义AUVs间距离量测估计的状态向量为
式中, v1(k)和v2(k)分别表示领航AUVs与跟随AUV间距离量测d1(k)和d2(k) 的相应变化速度，从而其状态方程可表示为 式中，δt为采样周期，υ(k) 为基于距离量测变化加速度的驱动噪声，并假设为零均值高斯白噪声。继而可简化(4)式为
根据距离量测(1)式，可定义LOS条件下的量测与噪声分别为 rκ,LOS(k)、wκ,LOS(k)，NLOS条件下的量测与噪声分别为rκ,NLOS(k)、wκ,NLOS，假设NLOS修正偏差δm 通过前期实验先验已知，且LOS条件下的距离偏差为0，从而可有
当AUVs间的距离量测所处条件为M1时,建立距离量测方程如下
可简化为 当距离量测所处条件为 M2、M3及M4 时,分别可得 且同样有 2.2 基于IMM的量测平滑算法
交互多模(IMM)算法在给定系统模型集的前提下,使各个模型并行地进行独立滤波,并根据贝叶斯规则对各自估计结果作概率加权求和处理,从而求得系统状态估计。在NLOS量测平滑算法设计中,如图 2所示,利用4个并行独立Kalman滤波器结合各自对应量测模型对状态向量θ(k)同时进行估计,分别根据估计所得误差通过似然函数同步获得各量测模型概率,继而联合各独立滤波器估计结果求得状态向量θ(k)的估计更新。整个量测平滑算法结构包括输入交互、模型匹配滤波及输出交互等3部分,其具体算法如下。
 |
| 图 2 距离量测平滑算法结构示意图 |
1) 输入交互: ∀i,j∈｛1,2,3,4｝
假设量测模型间Markov转换概率πij已知,则模型i到模型j的混合概率为
式中，μi(k-1)为k-1时刻量测模型i的概率,
为归一化常数,且有
从而,k-1时刻第j个模型滤波器的混合先验状态估计
0j(k-1|k-1)和相应协方差矩阵P0j(k-1|k-1)分别为
2) 模型匹配滤波: ∀j∈｛1为LOS／LOS滤波器,2为LOS／NLOS滤波器,3为NLOS／LOS滤波器,4为NLOS／NLOS滤波器｝
利用经典Kalman滤波设计4种量测模型下的滤波器,其状态一步预测及预测均方误差分别为
其中驱动噪声协方差阵Q为常量,且有Q(k-1)=E[υ(k-1)υT(k-1)]=συ2。而其滤波增益、状态估计及相应均方误差则分别为 式中,在不同量测模型下,各量测噪声协方差矩阵 Vj各不相同,且有Vj(k)=E[Wj(k)Wj T(k)],而根据量测方程式(7)~(10) 可知各量测矩阵相同,均为
3) 输出交互: ∀j∈｛1,2,3,4｝
在多个模型匹配滤波器交互过程中,量测模型概率体现了各滤波器间的权重大小,k时刻量测模型j的概率可更新为
其中c为归一化常数,且
继而,利用各量测模型概率更新联合其相应模型匹配滤波器估计结果,可计算得状态向量估计及误差协方差矩阵分别为
最后,根据(3)式定义,可将估计所得状态向量中的距离量测值表示为
3 仿真结果
为了验证文中所设计NLOS距离平滑算法的有效性,利用Matlab进行仿真分析研究。
在仿真实验中,如图 3所示,所有AUVs在同一深度航行,经过2次变向改变编队队形,航行速度均为4 m/s。跟随AUV转速计测量误差为 σV2=(0.1m/s)2的零均值高斯白噪声,磁罗盘的测量误差为0.5°的常值误差加上σΦ2=(2°)2的零均值高斯白噪声。领航、跟随AUV间的NLOS量测误差为13～15 m的随机常值加上σNLOS2=(4m)sup>2的零均值高斯白噪声,LOS量测误差为σNLOS2=(2m)2 的零均值高斯白噪声。两领航AUVs以不同频率发送水声信息,跟随AUV状态更新频率为0.1 Hz。
 |
| 图 3 MAUVs仿真运动轨迹 |
在MAUVs协同定位过程中,给定量测模型间转换概率分别为:(1)当i=j时,πij=0.7;(2)当i≠j时,πij==0.1 ,其中NLOS量测占所有量测量的30％。如图 4和图 5所示。
 |
| 图 4 修正偏差δm=14 m时领航与跟随AUV间距离 |
 |
| 图 5 修正偏差δm=9 m时领航与跟随AUV间距离 |
在NLOS量测的影响下,领航、跟随AUVs间的测量距离较之真实距离具有较大的正偏差;而在LOS量测随机噪声的影响下,领航、跟随AUVs间的测量距离同样会偏离真实量测,只是偏离幅度较小。在经过IMM算法进行平滑之后,领航、跟随AUVs间的测量距离误差大幅减小。继而,通过图 4和图 5对比可以发现,在不同NLOS修正偏差假设下,NLOS修正偏差越接近真实偏差,AUVs间的距离量测估计精度越高,越能贴近真实量测距离,反之亦然。
为了比较不同距离量测对MAUVs协同定位精度的影响,将图 4和图 5中相应的测量距离与IMM平滑距离应用于文中所设计协同定位算法中,并分别进行50次Monte-Carlo仿真求解对应均方根误差(RMSE)。其中,在k时刻的跟随AUV位置估计RMSE可表示为
如图 6和图 7所示，受NLOS量测误差的影响，使用原始测量距离的协同定位误差具有较大幅度的波动。若使用经过IMM算法平滑后的估计距离，则可以有效的减小协同定位误差。比较图 6和图 7可以发现，与距离平滑估计结果相类似，MAUVs协同定位精度与NLOS修正偏差同样有关，NLOS修正偏差越接近真实偏差，测量距离估计越精准，随之得到的协同定位精度越高。
 |
| 图 6 修正偏差δm=14 m时跟随AUV协同定位误差 |
 |
| 图 7 修正偏差δm=9 m时跟随AUV协同定位误差 |
本文针对基于双领航者的MAUVs协同定位系统，在无法识别LOS/NLOS量测的情况下，以4状态Markov链描述了定位过程中的4种量测模型转换过程，并设计了一种基于交互多模(IMM)理论的距离量测平滑算法，有效地减轻了NLOS误差对AUVs间相对距离量测的影响，提高了跟随AUV定位性能。同时通过2组仿真实验发现，NLOS修正偏差越精准，则距离量测估计精度和协同定位精度越高。由于在算法设计中量测模型转换概率及NLOS修正偏差为初始给定恒值，因而，在进一步地研究过程中，提高算法的自适应性将成为新的研究方向。
| [1] | Willcox S, Goldberg D, Vaganay J, Curcio J. Multi-Vehicle Cooperative Navigation and Autonomy with Bluefin CADRE System[C]//International Federation of Automatic Control Conference(IFAC), 2006:1-6 |
| Click to display the text | |
| [2] | Diamant R, Tan H, Lampe L. LOS and NLOS Classification for Underwater Acoustic Localization[J]. IEEE Trans on Mobile Computing, 2014, 13(2):311-323 |
| Click to display the text | |
| [3] | Emokpae L, DiBenedetto S, Potteiger B, Younis M. UREAL:Underwater Reflection-Enabled Acoustic-Based Localization[J]. IEEE Sensors Journal, 2014, 14(11):3915-3925 |
| Click to display the text | |
| [4] | Yu K, Guo Y. Improved Positioning Algorithms for Nonline-of-Sight Environment[J]. IEEE Trans on Vehicular Technology, 2008, 57(4):2342-2353 |
| Click to display the text | |
| [5] | Nawaz S, Trigoni N. Convex Programming Based Robust Localization in NLOS Prone Cluttered Enviroments[C]//10th International Conference on Information Processing in Sensor Networks(IPSN), 2011:318-329 |
| Click to display the text | |
| [6] | Vaghefi R, Buehree R. Cooperative Sensor Localization with NLOS Mitigation Using Semidefinite Programming[C]//9th Workshop on Positioning Navigation and Communication (WPNC), 2012:13-18 |
| Click to display the text | |
| [7] | Yu K, Dutkiewicz E. NLOS Identification and Mitigation for Mobile Tracking[J]. IEEE Trans on Aerospace and Electronic Systems, 2013, 49(3):1438-1452 |
| Click to display the text | |
| [8] | Liao J, Chen B. Robust Mobile Location Estimator with NLOS Mitigation Using Interacting Multiple Model Algorithm[J]. IEEE Trans on Wireless Communications, 2006, 5(11):3002-3006 |
| Click to display the text |
2. PLA Unit 69016, Urumqi 830001, China