基于因子图的AUV集群速度估计和协同定位

李鑫滨 ,袁蕊霞 ,闫 磊 ,韩 松,3

(1.燕山大学河北省工业计算机控制工程重点实验室,河北秦皇岛 066004;2.东北大学秦皇岛分校计算机与通信工程学院,河北秦皇岛 066004;3.河北省智能康复及神经调控重点实验室,河北秦皇岛 066004)

1 引言

随着装备制造技术发展,海洋勘探技术成为许多国家的战略重点[1-2].多AUV(autonomous underwater vehicle)协作系统由于可以有效的执行复杂的海上任务被得到广泛关注[3-4].准确的定位是实现多AUV协同操作的技术保证.由于陀螺仪、加速度计等惯性传感器的测量漂移[5],使得定位误差累积增长.为了实现AUV集群中更好的定位,基于声学距离测量及信息共享的协同定位方法被提出,用于实现定位误差的协同修正[6-9].

然而,在复杂的水下环境中,传统的协同定位方法仍然存在一些挑战.在使用距离测量信息时,时间信息是一个重要的信息,轻微时钟偏差也会导致显著的性能损失.在水下环境中,AUV仅依靠自身的本地时钟信息,但由于本地器件和环境差异,本地时钟存在不同的时间倾斜和漂移问题.对于具有时间漂移的协同定位系统,文献[10]重新推导滤波过程,得到了最优状态估计,文献[11]推导出随机两步时延滤波器.文献[12]将时延转化为测量偏置,重构了测量方程,但其并未解决时钟倾斜问题.文献[13]虽然考虑了时钟倾斜,但其需要有时钟同步的锚节点作为辅助,这在没有锚节点的AUV协作系统中难以实现.因此,仍需要考虑AUV间存在的异步时钟问题.

在传统定位中,由于陀螺仪、加速度计等惯性传感器的漂移,速度信息往往存在较大偏差.多普勒计程仪可以通过测量相对于水底的相对速度推得自身速度,缺点是其过于昂贵,并且在深海中可能无效.文献[14]提出从宽带声通信信号中提取传播时间(time of arrival,TOA)和多普勒频移,将其与惯性测量融合进行定位,然而,其使用的方法需要发射机的位置固定.文献[15]提出了一种速度估计和位置预测方法,该方法使用邻居AUV辅助自身进行速度估计.然而,参考AUV的速度信息可能不准确,直接利用参考AUV的错误速度信息会导致较大的估计误差.因此,如何在不能提供准确参考AUV速度时,充分利用测量信息进行状态估计需要深入研究.

其中扩展卡尔曼滤波(extended Kalman filter,EKF)以其计算简便的优势常用于协同定位中[12,16-17].然而,EKF对非线性方程进行泰勒展开,由于忽略高阶项,导致了建模误差.因子图提供了一个全局函数分解成局部函数的乘积的自然图形描述[18-19],将大规模的全局运算划分为简单的局部运算,解决了多源融合应用中实现边缘后验概率分布的问题.一些文献将基于因子图的高斯消息传递算法用于协同定位[20-21].高斯消息传递算法具有自然分布和良好的可伸缩性,这使得高斯消息传递算法非常适合于多AUV问题.然而,由于距离测量与位置关系呈非线性,线性化会带来阶段性的误差.因此,对于AUV协同定位系统而言,如何降低系统在线性化过程导致的定位误差,仍然是需要解决的关键问题.

综上所述,在AUV集群系统中协同定位中的时钟异步、不准确的速度以及协同定位中线性化过程带来的影响仍然是定位的难点.针对上述问题,首先,本文建立了时延与位置的关系,提出了基于泰勒迭代方法来实现AUV时钟参数估计,解决了AUV间各不相同的时间倾斜和偏移对距离测量带来的误差影响.然后,针对由惯性导航系统测量的速度具有误差累积的问题,将每个AUV的位置和速度作为变量节点,建立一个整体的协同定位与速度估计因子图模型,通过因子图的消息传递计算因子变量的边缘分布,得到了位置和速度变量的估计,有效降低了速度和位置测量的误差.最后,针对测量函数线性化过程具有较大误差的问题,提出了根据变量的协方差矩阵构造一个自适应因子的方法,在因子图的消息传递过程中计算边缘分布时,用自适应因子调整了变量的置信度,降低了线性化过程带来的误差影响.

2 系统建立与问题描述

2.1 系统建立

在协作系统中包含n个主AUV,m个从AUV,主AUV配备了高精度的惯性导航系统(inertial measurement unit,IMU),从AUV配备了低精度的惯性导航系统,主AUV相对于从AUV具有较精确的位置信息.正常情况下,AUV的航向和速度信息用惯性导航系统进行测量,通过压力传感器可以准确地获得深度信息.每个AUV配备了接收机和发射机,用于测量外部时延信息和多普勒频移信息.AUV在没有锚节点辅助的环境运动,依靠自身航向、速度和深度信息粗略地获得航行器的水下位置,然后利用外部测量的时延信息和多普勒频移信息及其他AUV的位置信息对自身AUV进行位置和速度纠正.系统环境图如图1所示.

图1 AUV集群协同定位场景Fig.1 Cooperative localization scenario of AUV swarm

主AUV 的集合为ΩM={1,2,···,n},从AUV的集合为ΩS={n+1,n+2,···,m+n},所有AUV的集合为ΩA={1,2,···,m+n}.运动过程表示为离散测量,记为tk,k=0,1,2,···,K.第i个AUV在tk时刻的位置和速度分别定义为xk,i=[xk,i yk,i zk,i]T和vk,i=[vxk,i vyk,i vzk,i]T.假设AUV运动符合随机游走模型,AUVi在tk时刻的状态xk,i可以从tk-1时刻的状态xk-1,i转移过来,更新过程表示如下:

其中:ts为采样时间间隔;vk-1,i表示时间tk处的速度;表示时间tk-1处的加速度,由IMU的加速度计测量,即

2)AUVi测量的与AUVj,j ∈ΩA{i}距离为

3)多普勒效应造成的发射和接收信号的频率之差称为多普勒频移,表示为Δf,用fc表示信号的中心频率,AUVi测量的与AUVj,j ∈ΩA{i}多普勒尺度因子为:D=Δf/(Δf+fc),多普勒尺度因子与位置和速度变量的对应关系为

AUV间通过接收机与发射机进行通信,周期性通过信息交互获取信息,k时刻为完成一个交互周期,需要接收的信息及具体交互过程为:AUV1首先广播信号给其他AUV,此信号携带信息有自身的先验位置和速度信息ut,1、时间信息t及发出者信息1,信号包发出的信息,AUV2,···,AUVn接收信号,AUV2接收到AUV1的信号并进行测量后,立即向除AUV1以外的AUV广播信号,并且增加接收到的其他AUV的信息包.AUV3向除AUV2以外的AUV广播信号,其余AUV与AUV3同理,AUVj广播信号:AUV(j+1),···,AUVn接收信号.当AUVj接收到AUVi广播的信号,解析信号得到测距信息和多普勒因子测量AUV1在k时得到的全部信息,包含所有AUV的先验位置信息、测距信息及多普勒频移信息,这些信息用于算法中计算,得到各个AUV的位置估计,最后把位置估计广播给各AUV.以上过程为一次交互过程,在一个定位周期内有一次通信交互.完成交互过程后,等待下一个定位周期到达后再次进行交互.

2.2 问题陈述

3 基于因子图的AUV集群速度估计和协同定位

本节为集群中的AUV设计速度估计与位置估计的协同定位算法,在使用外部测量进行位置估计时,需要先进行时钟参数估计,整个算法结构框图如图2所示.其中: 黑色虚线框外上方的部分为测量信息,由惯性测量、距离测量和多普勒因子测量组成,为整个结构的输入;左侧黑色虚线框为状态的先验信息,由惯性信息通过航位推算所得,所得结果作为算法结构部分的初值输入;右侧黑色虚线框为算法结构部分,输入为结构图左侧所得变量的均值与协方差,输出为此部分所重新计算的变量均值与协方差;黑色虚线框外下方的的部分表示算法结构部分的输出替换掉之前的先验信息,作为算法结构部分新的输入,进行迭代计算.

图2 系统结构框图Fig.2 System structure diagram

图2中,算法结构主要分为两部分,图中左上红色虚线框代表第1部分,另一个红色虚线框代表第2部分.第1部分内容为基于泰勒算法的异步时钟参数估计,文章将在第3.1节进行介绍.第2部分内容为基于因子图的协同定位和速度估计,文章将在第3.2节进行介绍.

3.1 基于泰勒算法的异步时钟参数估计

在本小节中,考虑时钟异步环境,将AUV的时钟等效为具有时钟倾斜和时钟偏移的模型.AUVi的本地时钟表示为

其中:t0表示基准时间;bi表示AUVi时间与基准时间的偏差;ai表示AUVi收到信号时的时钟倾斜.

AUVj传输信号到AUVi的到达时间表示为

则传播时间可表示为

其中Δτi,j表示测量噪声.

未知参数有[ai bi]T,i∈ΩA和[xk,i yk,i]T,i∈ΩS,式(11)左侧的测量时延构成测量向量,右侧进行泰勒展开,可通过泰勒迭代法进行估计.由τi,j组成的测量向量为T={τi,j},i,j ∈ΩA,待估参数有[ai bi]T,i ∈ΩA.用f(φ)表示测量和变量的函数关系,测量模型可表示成如下向量行式:

其 中:φ=[a1···am b1···bm xk,1···xk,m yk,1···yk,m]T;e=[Δτ1,2··· Δτm,m+n-1]T表示测量噪声向量;T=[τ1,2τ2,1···τ1,m+n τm+n,1···τi,m+n τm+n,i···τm+n-1,m+n τm+n,m+n-1]T.

将式(12)在φ-Δφ处进行泰勒展开,化成参数φ的线性函数.将f(φ)在φ-Δφ处展开,可得

可构造如下方程:

由已知信息(时间测量和主AUV的位置信息)及未知参数(从AUV的位置及时钟参数)根据AUV间位置与传播时间的关系构成式(14)的方程,通过泰勒迭代算法,可得到集群中每个AUV的时钟倾斜和时钟偏移参数的初始估计

3.2 基于因子图的协同定位和速度估计

得到时钟参数后,可以利用测距信息计算AUV的位置和速度变量的边缘概率分布.如第2.2节所述,由于直接计算边缘概率分布较复杂,本文采用计算简便的基于因子图的消息传递算法.AUV可以通过消息传递利用外部测量信息来协同推断其位置和速度.当分解后的因子图与网络拓扑相匹配时,可以使用有效的分布式推理消息传递[19],通过消息传递算法应用于因子图,得到了变量的近似边缘分布(“belief”),即b(x)≃p(x|z).在消息传递算法中,信念(b(x))和消息(µg→x(x))是重要的传输信息.可以将任何变量和函数的关系用因子图表示,在本文中,将每个AUV的位置和速度作为变量节点,将距离测量及多普勒因子测量及前一时刻和后一时刻变量转移关系作为函数节点.以此建立协同速度和位置估计的因子图模型,利用因子图模型描述以惯性测量、距离测量、多普勒因子为观测量的AUV协同定位与速度估计过程,以因子图的信息传递用于AUV的位置和速度估计.将式(8)进行中因子分解,分解后的因子图如图3所示,图3中函数节点分别为

图3 因子图模型Fig.3 Factor graph model

1)在贝叶斯框架下的位置和速度信念.

为求得AUV的位置和速度信念,可通过图4中因子图模型及贝叶斯框架得,第i个AUV,i ∈ΩA的位置和速度信念表示为

图4 时钟参数误差图Fig.4 Clock parameter error

2)对测量函数进行泰勒展开.

得到变量的信念在贝叶斯框架下的表示后,本文致力于变量的信念计算.通过消息传递,变量间利用函数关系可以进行信念的更新,达到计算边缘分布的目的.在计算过程中,测量和变量间转换关系是一个关键点.常规做法是将其进行线性化处理,鉴于循环消息传递的迭代性质,在时刻k和第L次迭代中,根据估计位置对测量函数进行一阶泰勒展开,即

与距离测量相似,根据之前的估计位置对多普勒因子测量函数同样进行一阶泰勒展开,即

3)自适应因子.

符号N(x,mx,Vx)表示x服从均值mx,方差为Vx的正态分布,符号mx表示变量x的均值,符号Vx表示变量x的方差.由于因子图中的所有信息和信念都是高斯形式,它们可以通过相应的均值和协方差进行参数化.由于一阶泰勒展开进行线性化会导致近似结果与实际有一定偏差,从而在参数化的计算过程中引入了误差.为了抑制线性化的影响,提出了根据变量的协方差矩阵构造自适应因子β.该自适应因子构造为

其中tr(·)表示矩阵的迹.

在信念的均值和协方差计算中引入的自适应因子β,β随当前计算的第i个AUV信念和观测的第j个AUV信念的协方差变化,当第j个AUV信念的协方差增大时,β也增大,先验和观测融合后协方差变化减小,均值变化也减小;在消息传递迭代过程中,随着信念接近于实际值时,第j个AUV信念的协方差减小,β也减小,先验和观测融合后协方差变化增大,均值变化也增大.因此,在观测可信度低时,利用自适应因子β降低均值变化量,在观测可信度高时,利用自适应因子β提高均值变化量,实现调整线性化过程的影响.

4)位置信念的均值和协方差计算.

根据文献[19]的消息传递算法,通过参数化计算,可以得到第i个AUV,i ∈ΩA位置信念的均值和协方差为

5)速度信念的均值和协方差计算.

与位置信念参数化相似,速度信念可做参数化.第i个AUV,i ∈ΩA速度信念的均值和协方差为

基于迭代消息传递,确定了AUV在时刻k的位置和速度信念,然后通过MMSE得到位置和速度估计.综合上述步骤,文章将基于因子图的AUV速度估计和协同定位算法具体步骤总结为表1所示.

表1 基于因子图的AUV集群速度估计和协同定位Table 1 Velocity estimation and cooperative localization of AUV swarm based on factor graph

4 仿真与分析

在本文中,使用MATLAB进行了数值模拟.在本实验中,主AUV个数设置为2个,从AUV个数设置为4个,初始位置在600×600×50 m3范围内随机产生.设每个AUV的x轴、y轴、z轴初始速度分别为1 m/s,0.4 m/s,0.25 m/s,初始加速度为-0.001 m/s2,-0.001 m/s2,-0.00015 m/s2,总共行驶600 s,每10 s进行一次外部信息测量.

为了模拟主AUV和从AUV不同精度的惯性导航设备,每一步(指进行外部测量的10 s)主AUV加速度噪声设置为N(0,0.0001),每一步从AUV加速度噪声设置为N(0,0.01).在本文中,设深度信息由深度计提供,深度计误差服从均值为深度的5%、方差为1的正态分布,AUV深度运动范围在[0 m,150 m]之间.根据文献[12]与文献[23]所述的测量设备,在600×600×50 m3范围内将距离测量噪声设置为N(0,1)较为合理.根据文献[14]所述多普勒因子误差,将多普勒因子测量噪声设为N(0,1×10-3).设2个主AUV,4个从AUV编号依次为1～6,则时钟参数设置为:a=[1.05 1.05 1.05 1.05 1.05 1.05],b=[0 0.12-0.1 0.5 0.1-0.02],消息传递迭代次数为5.最后,利用上述信息对AUV的位置和速度进行估计,仿真通过50次蒙特卡罗随机实验来计算目标位置和速度估计值的误差.

1)时钟参数估计结果.

本文评估了时钟参数估计的均方误差(mean square error,MSE).首先,本文给出了所估计时钟参数与真实值的误差值.然后,分析了时钟异步对位置估计的影响.对于时钟倾斜参数,以估计参数和实际参数差值的绝对值作为评价指标;对于时钟漂移参数,以AUV1 为基准时钟,计算其余AUV与AUV1的时钟漂移,并以估计参数和实际参数差值的绝对值作为评价指标,即

结果如图4,时钟倾斜的误差均在1.2×10-3以内,时钟漂移的误差均在1.0×10-3以内.2)定位和速度估计结果.

本文分析了存在时钟异步和不存在时钟异步时,所提算法的位置和速度估计误差.其中,位置估计结果与文献[20]的(posterior linearization belief propagation,PLBP)算法、EKF、无迹卡尔曼滤波(unscented Kalman filter,UKF)、航位推算(dead reckoning,DR)做对比,其中PLBP算法中L参数为5,M参数为5,在与对比算法深度测量信息一致的条件下,通过仿真对比了x和y方向的位置和速度估计结果.以实际位置(速度)与估计位置(速度)之间的均方根误差(root mean square error,RMSE)作为评价指标.

首先,文章为了验证第3.2节的算法,对于不存在时钟异步情况下,本文所提算法和DR,EKF,UKF,PLBP进行了对比.AUVs运动轨迹如图5所示,AUV在区域内运动,圆标记表示从AUV轨迹的起点,星标记表示主AUV轨迹的起点,图中蓝色曲线代表真实轨迹,红色曲线代表本文所估计轨迹.

图5 不存在异步时钟时的AUV轨迹Fig.5 Trajectories of AUVs when there is no asynchronous clock

图6为位置误差结果,在600 s内,DR方法、PLBP,EKF,UKF,本文所提算法平均误差如表2的情况1所示,将平均误差以及所提算法较其他算法提升比例进行了总结.因为随着AUV的运动,误差会逐渐累积,导致DR方法的结果为没有修正时AUV的误差,而EKF,UKF和PLBP有一定的修正作用,本文所提算法引入的速度估计使位置估计时有更准确的先验位置信息,因此位置误差更低.

表2 位置平均误差Table 2 The average error of the position

图6 不存在异步时钟时位置误差Fig.6 Position error when there is no asynchronous clock

为了验证速度估计部分的效果,对本文方法、最小二乘法(least sqaure,LS)、EKF及DR的速度估计进行了仿真,仿真结果如图7所示.在600 s内,DR,LS算法,EKF算法.本文所提算法所求速度的平均误差如表3中情况1所示.本文算法相对于DR,LS算法和EKF算法精度分别提升了96%,89%,53%,上述数据在表3中情况1进行了总结.这是由于DR方法对速度误差没有降低作用,而LS算法依赖于较准确的主AUV速度,使得LS算法对误差有一定抑制效果;EKF算法不像LS算法需要尽量多的测量方程,并且根据测量及前一时刻状态进行滤波,其比LS算法误差小,而本文的算法不仅将主AUV的速度用于从AUV进行校准,并且通过迭代进行相互校准,因此比EKF算法效果好.由结果可知,本文方法EKF,LS算法均对速度误差有抑制作用,本文方法效果最明显.

表3 速度平均误差Table 3 The average error of the velocity

图7 不存在异步时钟时速度误差Fig.7 Speed error when there is no asynchronous clock

同时,本文比较了存在时钟异步时,文章的算法和DR,EKF,UKF,PLBP的位置误差.如图8所示,在600 s内,DR方法,PLBP,EKF,UKF,本文所提算法平均误差如表2的情况2所示.5种方法中,本文所提方法误差最小,比PLBP提升了86%,比EKF提升了74%,比UKF提升了75%,上述数据在表2的情况2中进行了总结.由于DR不使用测距信息,时钟异步影响的测距误差对DR无影响,DR与无时钟异步时基本一致,EKF,UKF由于没有考虑时钟异步,在不准确的测距信息影响下,导致效果变差,而文章考虑了时钟异步后,效果优于其他算法.由于解决了异步时钟的影响,图8中本文算法误差与图7中不存在异步时钟时误差相比基本一致,表明解决时钟异步问题对定位精度有所提升.对本文方法LS,EKF及DR的速度估计进行了仿真.在600 s内,DR,LS算法、EKF算法,本文所提算法所求速度的平均误差如表3的情况2所示.由于时钟异步不影响多普勒频移信息,表3中情况2结果与不存在时钟异步时表3中情况1的结果基本一致.

图8 存在异步时钟时位置误差Fig.8 Position error when asynchronous clock exists

3)不同参数下估计结果.

本节将进行数值模拟,以评估如何计算这个迭代次数的影响,此外,还考虑了各种环境参数来验证算法的性能.本文分析了不同噪声对算法影响,其中,测距噪声为零均值,方差为0.5～2.5的高斯噪声,多普勒因子噪声为零均值,方差为0.5×10-3～2.5×10-3的高斯噪声,本实验对不同噪声下的结果进行分析,以所有时刻实际位置与估计位置之间的均方根误差之和作为评价指标,即

图9为不同测距噪声下对位置和速度估计影响的实验,图9(a)为位置估计误差,如表6所示,本文所提方法比DR,EKF,UKF,PLBP的位置误差小.图9(b)为速度估计误差,DR,LS,EKF,本文算法平均误差如表7所示,在不同测距噪声下速度估计误差仍然是本文所提方法最小.如表6-7相关数据所示,本文所提方法适应不同测距噪声,在不同测距噪声时仍能有良好效果.

表6 位置平均误差比较Table 6 The average error of the position m

表7 速度平均误差比较Table 7 The average error of the velocity m/s

图9 不同测距噪声影响Fig.9 Effects of different ranging noise

图10为不同多普勒因子噪声下对位置和速度估计影响的实验,与图9相似,在不同多普勒因子噪声下,DR,PLBP,EKF,UKF,本文算法的平均位置估计误差和平均速度估计误差在表6-7中进行了总结.位置估计误差和速度估计误差中本文所提方法最小.因此,仿真结果证明随着噪声变化,本文所提算法的估计误差呈平稳趋势,所提算法在不同噪声下有最低的位置和速度误差.

图10 不同多普勒因子噪声影响Fig.10 Effects of different doppler factor noise

5 结论

本文提出了一种基于因子图的速度估计和协同定位算法,该方法在AUV协作系统中利用距离测量和多普勒频移测量进行误差修正.首先,文章建立了时钟倾斜和时延与位置的关系,通过当前的后验信息利用泰勒算法近似计算时钟参数.针对由惯性导航系统测量的速度具有误差累积问题,本文建立了利用距离测量和多普勒频移测量的因子图模型,利用因子图模型描述以距离测量和多普勒因子为观测量的AUV协同速度和位置估计过程,提出基于因子图的速度估计和协同定位算法,通过因子图的消息传递,计算因子变量的边缘分布,有效降低了速度和位置测量的误差.最后,在消息传递过程中设计一自适应因子,采用自适应因子来调整观测的置信度,降低线性化过程带来的误差影响.仿真结果表明,该算法估计的时钟参数误差在0.001 以内,在不存在时钟异步时,该算法比DR,EKF,UKF,PLBP有更低的位置误差,比DR,LS,EKF有更低的速度误差.在存在时钟异步时,该算法通过准确估计时钟参数,其位置误差和速度误差与不存在时钟异步时基本一致,该算法实现了准确、高效的定位.