基于IMM 算法的多AUV 协同定位水声传播延迟处理方法

陈世杰 ,刘锡祥 *,黄永江 ,章彩霞 ,陶育杰 ,童金武

(1.东南大学 仪器科学与工程学院,江苏 无锡,214026;2.微惯性仪表与先进导航技术教育部重点实验室,江苏 南京,210096;3.南京工程学院 工业中心创新创业学院,江苏 南京,211167)

0 引言

协同定位概念在20 世纪90 年代由Kurazume等[1]首次提出,之后各国学者将其运用到自主水下航行器(autonomous undersea vehicle,AUV)系统中,并开展了一系列有关海洋资源探索与水下目标搜寻领域的研究[2-5]。近年来,AUV凭借作业范围广、自主性强、安全性高等优势被广泛应用。在执行既定行动任务时,协同定位成为AUV 协同系统能否成功完成任务的关键技术。

对于水下工作的AUV,常用的陆上导航方式如卫星导航、地形辅助等受环境影响不能充分发挥作用甚至失效,而具有自主导航能力的惯性导航以及航位推算等方法因其不依靠外界信息的特点,被广泛应用于水下导航系统。但航位推算方法的导航误差会随着时间的推移不断累积,无法满足长航时的定位需求。多AUV 协同系统即是通过相互观测与通信实现各AUV 间信息共享,运用基于几何学或概率学的方法进行数据融合,提高AUV 系统整体定位精度[6-8]。

多AUV 系统协同定位的精度依赖于AUV 间通信与探测两部分信息的传递,探测信号获得AUV彼此间的观测距离,通信信号实现AUV 间位置信息的共享。复杂的水下环境使得电磁波等通信手段无法有效工作,水声通信是当前最常用的通信方法,但是数据的处理以及信号的传输都需要一定时间,信息存在滞后性,直接影响其利用效果[9-11]。

基于此,众多学者提出了一系列改进方案。文献[12]对基于飞行时间(time of flight,TOF)技术的主从式协同导航定位进行了整周期滤波的算法研究,揭示出协同定位精度与AUV 间的相对位置密切相关;文献[13]考虑了水声延迟问题,分析了AUV 间相对定位信息延迟对滤波状态估计的影响,提出了一种基于状态估计均方差最小的误差修正方法;文献[14]采用了一种非等间隔实时更新(incoordinate interval real-time update,IIRU)的协同定位算法,对主AUV 的航迹预测与时间更新过程进行转停判断,兼顾了主从AUV 间状态相关性并有效降低了水声延迟引起的系统定位误差,但当主AUV 运动状态改变时,定位结果会受到影响。交互多模型(interacting multiple model,IMM)算法具有自适应的特点,尤其在对机动目标进行追踪时,能够实现目标各运动状态模型之间的转换。在跟踪机动目标时不可能只靠单一模型来描述运动状态,在整个运动阶段时间内,采用多个模型才能更好地描述目标的运动。文献[15]分别比较了IMM 算法对3 种运动模型计算的误差性能,与单一滤波器比较,观察组合后算法在目标机动方面的性能有所提高。文献[16]提出一种混合交互多模型(Hybrid-IMM,HIMM)算法,自适应地选择合适的模型来描述变化的环境,提高复杂水下环境导航系统的性能。

文中针对主从式多AUV 协同系统中存在的水声通信延迟问题进行研究,在从AUV 扩展卡尔曼滤波(extended Kalman filter,EKF)器中接收到延迟水声信号时,回溯到发送时间节点进行重新滤波,并采用IMM 方法,实现从AUV 滤波器对主AUV不同运动状态的跟踪,降低其状态估计误差。设计主AUV 运动状态变换的航行轨迹,对该方法进行了分析讨论与仿真验证,证明使用IMM 方法精确跟踪主AUV 状态时,协同系统整体定位精度得到提高。

1 AUV 协同系统水声延迟

AUV 协同定位方式主要有主从式和并行式2 种。其中,主从式AUV 协同定位方式兼顾精度和成本,是当前研究的主要内容,该定位方式将多AUV 分为主、从AUV,主AUV 配备高精度惯导设备,由水声设备向从AUV 发送声学信号;从AUV装备低精度航位推算和水声通信设备等,并通过水声通信获取主AUV 的位姿信息和主从AUV 间的相对距离。主从式AUV 协同定位方式如图1所示。由于单领航主从式多AUV 系统的可观测性较弱,文中选用双领航式以增强系统的可观测性[17-18]。

图1 主从式AUV 协同定位方式示意图Fig.1 Diagram of master-slave AUV cooperative positioning method

1.1 协同定位无延迟模型

AUV 于水下三维空间航行,其深度信息可由深度传感器等设备直接获取,实际航行过程中俯仰角不发生较大的变化,在一般算法研究中,将运动模型简化,深度信息不计入状态,运动学模型由三维空间简化到二维平面下[19]

式中:x(k),y(k),θ(k)分别表示AUV 的经度、纬度和航向角,状态值X(k)=[x(k),y(k),θ(k)]T;V(k),ω(k)分别为AUV 速度和角速度,输入量为u(k)=[V(k),ω(k)]T。二者受互不相关的高斯白噪声干扰,噪声误差协方差阵如下

AUV 航位推算运动方程为

远程方程包含非线性项,由于EKF 针对非线性系统有较好的表现,并且计算量小,易于工程实现,适宜处理水下AUV 定位,经EKF 线性化可得

式中,Φ(k+1,k)为系统状态转移矩阵,G(k)为系统噪声驱动矩阵,二者均为依据EKF 泰勒展开的雅可比矩阵。

从AUV 航位推算位姿信息状态可表示为Xs(k+1,k)=[xs(k+1),ys(k+1),θs(k+1)]T,主AUV的位姿信息为Xm(k+1,k)=[xm(k+1),ym(k+1)]T,计算主从AUV 间相对位置距离信息

量测方程为

式中:h(·)为关于状态量的非线性函数;ωZ为不相关的零均值高斯白噪声。

方差矩阵

量测方程的雅可比矩阵[20]

1.2 水声延迟问题描述

水声信号分为声学探测信号和声学通信信号2 种,使用不同设备发出,为避免相互干扰,二者在不同时刻发送,分别形成距离探测信号和状态传递信号。距离探测生成原理为[21]

声学信号在不同AUV 之间传播,受水下传播速度c影响发出与到达时间存在一定时间间隔即为延迟时间(tk-tl),该延迟时间对定位精度的影响通常不能忽略。水声信号的时延情况如图2 所示,tl时刻主AUV 发送的声学信号,经过一段时间后在tk时刻到达从AUV,此时间段内各AUV 仍保持航行状态,当前主AUV 位置信息已经发生改变,探测距离Zm,s不再是式(5)所示同一时刻主从AUV 间的距离信息,传递的真实测距信息意义为

图2 水声传递信号时间示意图Fig.2 Diagram of underwater acoustic transmission signal time

另一时刻tcom,主AUV 发送自身位置信息,经过一段传播时间后,被从AUV 接收并使用。

根据式(11)的延迟时间函数进行建模,依据图2 中的声学信号传递过程选择降低2 种信号的发送频率,预先确定信号每次发送的时间节点,一定程度上避免了信号出序到达以及从AUV 同时接收到多个水声信号时处理优先级的问题。

1.3 水声信号延迟处理

由相对距离信息引起的非线性滤波问题可以使用EKF 进行解决,在此基础上建立水声信号的延迟处理方法如下。

上述协同环境中低频的声学信号可能减少一段时间内滤波可用的量测信息,针对不同时间节点发送的声学探测与通信信号,将二者均作为滤波更新中的量测值,实现量测信息的增加。从AUV在接收到距离探测信号时,可获取主从AUV 间距离,但EKF 量测更新步骤中新息的建立需要同时获得量测距离与估计距离,而此时主AUV 位置信息尚未到达,估计距离无法计算。因此,在协同过程中可在从AUV 滤波器中建立主AUV 的航迹预测模型,使用对主AUV 的状态预测值计算估计距离,完成距离量测更新。从AUV 接收到主AUV状态位置信息时,只需对主AUV 的航迹预测模型进行修正即可。

1)无水声信号到达

在无水声信号到达时,从AUV 的EKF 滤波器只进行航位推算时间更新,同时对主AUV 航迹进行预测,联立主从AUV 状态进行状态扩增,系统预先规定2 种信号的发送时间节点,当同步时间戳运行到每个确定的时间节点,保存对主AUV 的状态预测值、输入信息及状态误差协方差阵。

时间更新部分如下

构建一步预测协方差阵的更新如下

初始化时,考虑主从AUV 间的相关性,其中

2)水声距离探测信号到达

tk+1时刻,从AUV 接收到主AUV 于tl时刻发送的距离探测信号,解析数据获取主从AUV 间距离信息,Zm,s(l,k+1)表示Xm(l)与Xs(k+1)之间的距离,此时X(k+1,k)=[Xm(k+1,k),Xs(k+1,k)]T为滤波器估计状态,使用在时间更新阶段预先保存的发送时间节点的主AUV 估计状态值调整滤波器扩增状态Xl(k+1,k)=[Xm(l),Xs(k+1,k)]T以及协方差矩阵Pl(k+1,k)=diag[Pm(l),Ps(k+1,k)]进行量测更新,即

式中:Z(k+1)表示计算距离与观测距离建立的新息;R(k+1)表示距离量测值Zm,s(l,k+1)的方差矩阵;Xl(k+1,k)=[Xm(l),Xs(k+1,k)]T表示2 个时间节点的主从状态,按照式(12)时间更新到X(k+1,k)=[Xm(k+1,k),Xs(k+1,k)]T。

3)水声通信信号到达

从AUV 于tk+1时刻接收到主AUV 于tl时刻发送的水声状态通信信号,该信号包含主AUV 在tl时刻的位置信息Xm,r(l)、状态误差协方差阵Pm,r(l)和速度角速度输入信息u(k)=[V(k),ω(k)]T,将状态位置信息Xm,r(l)作为量测值,在从AUV 滤波器中对主AUV 的航迹预测状态值进行量测更新,使用时间更新过程中发送节点预先保存的主AUV 估计状态值Xm(l,l-1)与Pm(l,l-1)完成量测更新,即

状态量测更新后,完成u(l)=[V(l),ω(l)]T输入信息的替换更新,并利用式(12)将时间更新到当前时间节点,X(k+1)=[Xm(k+1),Xs(k+1)]T。

延迟扩展卡尔曼滤波(delay EKF,DEKF)算法分别利用主AUV 发送的探测距离信息与传递的状态信息进行量测更新与输入量的更新,对于时间延迟的影响,进行回溯形式的对应时间节点状态信息更新,完成重新滤波。考虑低频水声信号,主AUV 作机动性航行时,从AUV 滤波器中对主AUV 状态的预测无法实现及时更新,例如主AUV作加速运动时,输入量为速度等信息,在无信号到达的时间段,速度保持在一定值,对主AUV 的运动状态估计会出现较大偏差。IMM 算法对于运动状态的追踪具有一定的优势,在DEKF 算法中,位置与输入等状态信号到达时,进行IMM 算法更新运动模型概率,有望在准确跟踪主AUV 运动状态的前提下,提升协同系统整体定位精度。

2 IMM 模型

2.1 IMM 算法

IMM 算法通过马尔可夫转移概率及模型概率的更新实现多模型间的切换。在上述主从式多AUV 协同系统中,从AUV 滤波器需要对主AUV进行航迹预测,选择IMM 算法能够实现良好的跟踪效果。一次IMM 跟踪算法主要包含输入模型交互、滤波器滤波、概率模型的更新和新的数据融合4 个步骤[16]。IMM 算法流程如图3 所示。

图3 IMM 算法流程图Fig.3 Flow chart of IMM algorithm

首先,在IMM 算法中预先设定多个运动目标,状态方程为

式中:X为系统状态向量;Aj为目标状态转移矩阵;Wj为均值为零的高斯白噪声。

任意模型j的观测方程

式中:Z为量测向量;Hj为模型j的观测矩阵;Vj为均值为零的高斯白噪声。

模型转换矩阵

式中,pij表示从模型i转移到模型j的概率。

1)输入交互

根据前一时刻系统状态估计和协方差估计来推测当前时刻的量测值Z(k),然后对模型进行重新初始化计算,其中新的初始值通过不同模型之间的马尔科夫运算矩阵获得,对模型k-1时刻的状态最优估计值和估计协方差矩阵分别设定为:Xj(k-1|k-1)和Pj(k-1|k-1)。进一步推导交互后模型的初始化条件

其中

2)滤波器滤波

IMM 算法的EKF 算法如下。

时间更新

滤波增益

量测更新

3)模型概率更新

采用极大似然函数法实现模型的更新,通过计算当前模型和当前运动目标状态的相似度来给出当前最适合跟踪模型所占权重。

在k时刻与j模型最匹配的极大似然函数为

模型j的更新概率为

式中,c为归一化常数,即

4)数据融合

根据每个模型单独计算的跟踪结果和模型匹配的权重给出k时刻交互数据的最终输出结果,即,

总体估计值

总体协方差

以上为IMM 卡尔曼滤波跟踪算法的一次递推过程,每一次将上一次的交互输出作为下一次的交互输入值,完成整个滤波跟踪过程的循环。

2.2 运动模型

鉴于主从式多AUV 协同系统中AUV 运动机动性并不高,选择几种常用的运动模型作为IMM算法中的子模型,包括恒定速度(constant velocity,CV)模型、恒定加速度(constant acceleration,CA)模型和恒定转率和速度(constant turn rate and velocity,CTRV)模型,各子模型状态转移方程如下。

1)CV 模型

状态向量

状态转移方程

式中,µa和µω分别为AUV 加速度和角速度的零均值高斯白噪声。

2)CA 模型

状态向量

状态转移方程

3)CTRV 模型

状态向量

状态转移方程

当偏航角为0 时

在IMM 算法中使用上述3 个子运动模型进行EKF 滤波更新并完成IMM 循环流程,实现滤波器对主AUV 运动状态的跟踪。

3 仿真结果与分析

3.1 仿真环境设置

为验证算法的有效性,进行如下仿真研究。采用双领航式主从AUV 协同系统,主从AUV 状态更新周期皆为Δt=1 s,探测信号和状态通信信号传播速度均为Vp=1 500 m/s,发送时间间隔均设置为20 s。从AUV 作速度为3 m/s的定深直线航行;主AUV 分别在时间[0 s,1 000 s]、[1 500 s,2 000 s]和[2 500 s,3 000 s]内作匀速直线运动,[1 000 s,1 500 s]内作加速度为0.1 m/s2的匀加速度运动,[2 000 s,2 500 s]内以-0.1 m/s2的加速度作变速运动,初始速度设置为3 m/s,主AUV 高精度导航设备速度和角速度传感器的测量噪声分别为σv(k)=0.3 m/s和σω(k)=0.05(°)/s,从AUV 低精度导航设备速度和角速度传感器的测量噪声分别为σv(k)=0.5 m/s,σω(k)=0.1(°)/s。IMM-DEKF 算法仅使用3 个运动模型,因此模型转换矩阵取值为p11=p22=p33=0.98,p12=p21,p13=p31,p23=p32,p12=p13=p23=0.01。

上述环境条件下,验证文中协同定位算法,并将以下算法结果进行仿真对比。其中:S1 为理想环境下无延迟,水声信号频率与航位推算频率一致的情况;S2 为忽略通信延迟的EKF 协同定位算法;S3 为考虑通信延迟的DEKF 协同定位算法;S4为考虑通信延迟的IMM-DEKF 协同定位算法。

3.2 仿真结果

绘制航迹图与定位误差均方根误差(root mean square error,RMSE)曲线,对定位误差进行分析比对,误差曲线是运行100 次蒙特卡洛仿真所得定位误差的统计平均值。

图4 为主从式多AUV 系统各AUV 理想轨迹与实际航行轨迹图,局部放大图为[2 000 s,2 500 s]时间段内,2 个主AUV 均作减速运动过程中,从AUV 根据主AUV 通信传递的信息完成滤波的轨迹细节图,可以看出在此时间段,S4 算法下从AUV实际航行轨迹更靠近理想轨迹。图5 和图6 表明,随着主AUV 运动状态的改变,从AUV 对其的航迹预测会产生较大误差。由图7 可知,S2 算法表明延迟时间的存在使得从AUV 协同定位结果产生了极大的误差;而在整个运动过程中,S3 算法都能够在一定程度上降低由水声通信延迟引起的定位误差,但S3 算法在[1 000 s,1 500 s]和[2 000 s,2 500 s]两段变速运动时间内,相较于其自身在其他时间段内产生了明显的误差增大;S4 算法对此有明显的改善。下文仅针对S3 与S4 算法即DEKF和IMM-DEKF 这2 种算法进行比较,并给出IMM方法全过程模型概率变化图。图8 单独比较DEKF 与IMM-DEKF 算法定位结果误差。表1 列出了主AUV 在[1 000 s,1 500 s]时段作变速运动,滤波器对主从AUV 状态估计的具体误差数据;表2 列出了主AUV 在[1 500 s,2 000 s]时段作匀速运动,滤波器对主从AUV 的估计状态误差,此时间段内,IMM-DEKF 与DEKF算法对主从状态估计定位误差相当;最后,表3 列出了在[2 000 s,2 500 s]内主AUV 又作变速运动期间,滤波器对主从AUV 状态估计的具体误差数据。由表4 统计的误差降低百分比可知,主AUV作变速运动时,IMM-DEKF 对主AUV 的状态预测误差相较于DEKF 算法降低了40%以上,从AUV定位误差降低了30%左右。由图9 的全过程概率分布图可知,整个协同定位过程中,IMM-DEKF 算法可有效跟踪主AUV 的运动状态。图10 和图11表示从AUV 滤波器对双领航主AUV 状态预测的误差,在变速运动期间,IMM-DEKF 算法对于主AUV 的航迹预测误差相较于使用DEKF 有明显降低,实现了对主AUV 状态更优的跟踪结果。

图4 多场景各AUV 轨迹图Fig.4 Track diagram under multi-scene of each AUV

图5 滤波器中主AUV1 RMSE 曲线Fig.5 RMSE curves of master AUV1 in the filter

图7 多场景下从AUV 定位距离RMSE 曲线Fig.7 RMSE curves of slaver AUV positioning distance in multi-scene

图8 DEKF 与IMM-DEKF 算法下的从AUV 距离RMSE曲线对比Fig.8 Comparison of RMSE curves from slaver AUV between DEKF and IMM-DEKF algorithms

表1 [1 000 s,1 500 s]时段滤波状态误差平均值Table 1 Average values of state errors in [1 000 s,1 500 s]

表2 [1 500 s,2 000 s]时段滤波状态误差平均值Table 2 Average values of state errors in [1 500 s,2 000 s]

表3 [2 000 s,2 500 s]时段滤波状态误差平均值Table 3 Average values of state errors in [2 000 s,2 500 s]

表4 IMM-DEKF 算法下各AUV 距离误差下降百分比Table 4 Percentage decrease in distance error of each AUV

图9 IMM-DEKF 算法下AUV 模型概率更新过程Fig.9 Model probability update processes of AUV in IMM-DEKF algorithm

图10 DEKF 与IMM-DEKF 算法下主AUV1 航迹预测距离RMSE 曲线Fig.10 RMSE curves of track prediction distance from master AUV1 under DEKF and IMM-DEKF algorithms

图11 DEKF 与IMM-DEKF 算法下主AUV2 航迹预测距离RMSE 曲线Fig.11 RMSE curves of track prediction distance of master AUV2 under DEKF and IMM-DEKF algorithms

4 结束语

协同定位技术是多AUV 系统的关键技术之一。文中针对主从式多AUV 系统水下运行过程中不可避免的水声通信延迟进行定位算法优化。水声通信传播时间延迟依赖于水声速度与主从AUV 间距,延迟时间随距离的增加而增加。文中在建立AUV 非线性运动模型的基础上,分析了基于EKF 的延迟时间处理算法,将探测信号与状态通信信号分时分地发送,避免信号出序到达,同时增加固定时间段内可用量测信息量,在从AUV 滤波器中对主AUV 进行航迹预测状态估计,为有效实现对主AUV 运动状态的跟踪,使用3 种常见运动模型作为IMM 算法子滤波器模型,通过马尔可夫转移概率及模型概率的更新实现多模型间的切换。仿真结果表明,基于IMM 的协同定位水声传播延迟处理方法能够有效跟踪主AUV 运动状态,影响从AUV 定位精度,减小由水声通信延迟引起的定位误差。

下一步将针对以下几点开展研究:1)解决短时间间隔通信条件下信号出序到达问题,以及考虑可能存在的通信信号丢失等情况;2)优化IMMDEKF 算法中多模型交互的模型概率更新部分,丰富模型以及加速运动模型匹配,以应对更加复杂的水下环境,并缓解IMM 方法的滞后特点。