基于分层式结构的多AUV协同导航方法及仿真

冯 鹏，熊 凌

（1.武汉科技大学 机器人与智能系统研究院，武汉430081；2.武汉科技大学 信息科学与工程学院，武汉430081）

自主水下航行器AUV 是探索和研究海洋的重要工具。 随着对海洋领域的研究不断扩大和深入，单个航行器很难完成复杂的水下任务， 多AUV 协作系统被提出。 这也是近年水下航行器发展的主流趋势。 多AUV 通过信息共享来进行协同导航，有利于提高系统导航的整体精度和鲁棒性，增强了系统的协作能力。

国内外学者对多AUV 协同导航进行了诸多研究。 欧盟资助的“自主水下航行器的合作认知控制”项目[1]中，多AUV 协同控制和导航是研究重点之一，对此做了大量的仿真模拟和硬件测试。文献[2]研究了EKF 和无迹卡尔曼滤波在组合导航方面的应用并进行了海上试验，验证了算法的有效性；文献[3]将多航行器分为移动航行器和地标航行器两组，移动航行器通过对静止地标航行器观测定位，移动航行器与地标航行器是交替的，每个AUV 可在较小的漂移范围内导航定位；文献[4]提出协作SLAM 框架，用于解决多AUV 协同定位通讯受限问题；文献[5]提出一种基于Kullback-Leibler 距离估计的协同导航算法，仿真验证了其有效性；文献[6]将GM-PHD滤波算法和信息熵理论应用于系统定位，该算法对多个位置估计值加权求和以得到最终的状态估计；文献[7-8]研究了协同导航过程中通信延迟的问题，提出误差补偿算法以提高定位的精度；文献[9]总结了近年来多AUV 协同导航的研究进展，包括结构分类、协同定位算法和对未来发展的初步预测。

1 协同导航结构

协同导航结构可分为主从式结构、并行式结构和分层式结构[10]。 在此研究分层式结构。

在多AUV 系统中，每个AUV 的导航设备性能存在差异， 可以将AUV 群中导航精度相当的AUV归为一层，并按导航精度高低分层。 同层的AUV 可以共享导航信息，导航精度高的在高层，导航精度低的在低层，高精度层AUV 向底精度层AUV 传递导航信息。 分层式结构中的相邻2 层如图1 所示。AUV1和AUV2在同一层， 为高精度层；AUV3，AUV4和AUV5在同一层，为低精度层。

图1 多AUV 的分层式结构Fig.1 Hierarchical structure of multiple AUV

在此重点研究分层式结构中高低精度层之间AUV 的协同导航。 低精度层AUV 通过水声信号测量计算与高精度层AUV 之间的相对距离、 相对方位角， 同时接收高精度层AUV 广播的自身位置信息，通过扩展卡尔曼滤波EKF 融合内部和外部信息完成协同导航定位。

2 多AUV 协同导航

2.1 AUV 运动模型

AUV 是在水下三维空间中运动的载体，位置坐标为 （x，y，z）。 深度z 能由深度传感器直接测得，可以单独考虑。于是，将AUV 的三维运动投影到二维平面，建立AUV 的二维非线性离散数学模型，即

式中：x（k-1），y（k-1）和ψ（k-1）］分别为在k-1时刻的横坐标、纵坐标和偏航角，此时的运动状态为X（k）= ［x（k），y（k），ψ（k ）］T；V（k-1）和ω（k-1）为前向速度和偏航角速度，可由多普勒测速仪DVL（doppler velocimeter）和陀螺仪测得。 实际中，传感器的测量带有误差，故

式中：Vm（k-1）和ωm（k-1）分别为AUV 的线速度和偏航角速度测量值；ξV（k-1）和ξω（k-1）为测量噪声。假设测量噪声相互独立， 都为零均值的高斯白噪声，噪声协方差为

AUV 的数学模型可简写为

2.2 水声测量模型

AUV 主要通过水声脉冲信号进行相对距离和方位角的测量。 水声测距方法有水声脉冲到达时间TOA（time of arrival）测距法和水声脉冲接收强度RSS（received signal strength）测距法，通常使用TOA 测距。 TOA 方法中，只要测出水声脉冲在2 个AUV 之间的传播时间Δt，根据声信号在海水中的传播速度c，就能算出2 个AUV 的相对距离。 AUV 之间时钟同步，可以通过水声脉冲单向传播时间，测得2 个AUV 之间的斜距。

将高精度层AUV 记为AUVi（i=1，2），低精度层AUV 记为AUVj（j=3，4，5）。AUVi和AUVj的斜距为

AUVi和AUVj的坐标分别为（xi（k），yi（k），zi（k）），（xj（k），yj（k），zj（k）），其 中 的zi（k）和zj（k）分 别 为AUVi，AUVj的深度，由深度传感器测得。 可以将斜距投影到水平面上，根据

可计算得到2 个AUV 在水平方向的距离。 测量的距离与状态量的关系为

除了能测得相对距离， 还能测得相对方位角。水下航行器的声呐通常由水听器阵列组成，水声脉冲到达不同的水听器用时不同。

AUVi与AUVj的方位角记为φij。 如图2 所示，以2 个水听器为例， 简要说明相对方位角的测量。AUVj上的2 个水听器A 和B 接收到AUVi上点P发出水声脉冲， 根据接收到的时间可以计算PA，PB。 将PA，PB 投影到水平面内得到CA，CB；r 为2个水听器的距离，r<

测量的方位角与状态量的关系为

图2 相对方位角的测量Fig.2 Relative azimuth measurement

距离和方位角的测量都带有噪声，将测量方程简写为

2.3 算法实现

多AUV 协同导航系统为非线性系统。 EKF 是解决非线性系统常用的方法， 其主要问题是线性化。 在状态估计时，对系统方程在前一状态估计值处做实时的线性泰勒近似；在预测步骤中，对测量方程在相应的预测位置也进行线性泰勒近似。

如图1 所示，高精度层AUV 发出水声脉冲，随后发送位置信息。 低精度层AUV 接受到水声脉冲后计算出相对距离或相对方位角。 低精度层AUV利用测量值和高精度层AUV 的位置信息， 通过EKF 滤波估计自身位置状态。 高精度层AUVi状态记为Xi（k）（i=1，2）；低精度层AUVj状态记为Xj（k）（j=3，4，5）。

AUV 的运动模型为式（4）所示。 AUVj状态的一步预测（k）为

预测协方差P-（k）为

式中：F（k-1）为f（k-1）关于X（k-1）的雅克比矩阵；L（k-1）为f（k-1）关于u（k-1）的雅克比矩阵。将（k-1），Vm（k-1）代入式（13）、式（14）计算，得

如1.2 节所述的相对距离和相对方位角测量模型， 若低精度层AUVj测量与高精度层AUV1，AUV2的距离，则测量方程（10）中的g（k）为

若低精度层AUVj测量相对AUV1，AUV2的方位角，则g（k）为

对于不同的测量值可以构造不同的观测方程，测量噪声的协方差为R（k）。 计算g（k）的雅克比行列式H（k），将（k）代入，得

由式（12）、式（17），计算卡尔曼增益K（k），得

AUVj状态估计（k）的测量更新为

协方差更新为

式（19）为利用测量更新值得到的位置信息估计。 AUV3，AUV4和AUV5可以根据上述算法估计自身位置信息。 EKF 滤波位置估计误差为

3 仿真试验及其结果分析

利用MatLab 进行仿真分析。 多AUV 的真实运动轨迹如图3 所示， 假设航行时各AUV 深度不发生变化。

图3 多AUV 运动轨迹Fig.3 Multiple AUV trajectory

AUV1和AUV2在同一深度航行， 均以3 m/s 沿直线匀速航行， 航向角均为70°； 初始位置分别为（300，0，-30），（900，0，-30）。

AUV3，AUV4和AUV5在同一深度航行，均以3 m/s沿直线匀速航行，航向角均为60°；初始位置分别为（0，0，-60），（350，0，-60），（700，0，-60）。 这3 个AUV 的DVL 速度测量噪声方差为（0.3 m/s）2，陀螺仪测量噪声方差为（0.2°）2，常值误差为0.1°，初始位置误差为（-5，-5），仿真周期为1 s。

仿真1 将低精度层3 个AUV 与高精度层2个AUV 的相对距离作为测量值用于EKF。 距离测量噪声方差均为（5 m）2。 基于距离的定位误差如图4 所示。

图4 基于相对距离的定位误差比较Fig.4 Comparison of position errors based on relative distance

图4 为低精度层3 个AUV 推算误差和基于距离的协同定位误差比较。 低精度层AUV 若通过自身导航信息进行推算时，定位误差会不断增大。 而通过协同导航定位，可以有抑制误差的增长。

仿真2 将低精度层3 个AUV 与高精度层2个AUV 的相对方位角作为测量值。 方位角测量噪声方差均为（0.5°）2。 AUV3，AUV4，AUV5基于方位角的定位误差如图5 所示。

图5 基于相对方位角的定位误差比较Fig.5 Comparison of position errors based on relative azimuth

图5 为低精度层AUV 的推算误差和基于方位角的协同定位误差比较。计算AUV3，AUV4，AUV5协同定位误差稳定后的平均误差见表1。 在仿真条件下，基于距离和基于方位角的协同定位能减小低精度层AUV 的定位误差，平均误差在2 m 以内。

表1 协同导航定位平均误差Tab.1 Cooperative navigation average error

4 结语

研究了分层式结构的多AUV 协同导航， 将多AUV 群进行分层， 充分利用高精度层AUV 的导航信息。AUV 之间导航信息的共享和观测值的测量需要通过水声信号进行。 在仿真试验中选择相对距离和相对方位角分别作为观测量进行EKF。 仿真结果表明，该方法能够提高多AUV 中低精度层AUV 的导航定位精度，有利于多AUV 水下任务的执行。

声 明

最近社会上有一些机构和个人，冒充《自动化与仪表》编辑部收取文章发表版面费，本刊特在此作如下声明：《自动化与仪表》杂志的网站为www.zdhyyb.com，投稿邮箱为zdhyyb@vip.sina.com，其他的网站和邮箱地址，均与本编辑部无关，请作者提高警惕，谨防上当！

《自动化与仪表》编辑部