基于主从式水下自主航行器移动组网的合作目标定位方法

赵婧旭 赵 晨 周 锋*

①(哈尔滨工程大学水声技术重点实验室 哈尔滨 150001)

②(海洋信息获取与安全工信部重点实验室(哈尔滨工程大学)工业和信息化部 哈尔滨 150001)

③(哈尔滨工程大学水声工程学院 哈尔滨 150001)

④(北京遥感设备研究所 北京 100854)

1 引言

海洋有着丰富的矿产资源，长期以来一直是世界关注的焦点。认识海洋以及海洋资源的研究离不开水下高技术手段。人力无法进行对深海大规模、复杂的开发和调查工作[1-4]，这就需要强而有效的水下装备以及水下目标定位系统的支持。近年来，根据海洋研究和开发的需求，水下目标定位系统需要具备长时间、大范围和远距离的特点。传统水下目标定位系统包括长基线系统、短基线系统、超短基线系统及综合定位系统，是以“基线”为基础对目标与系统之间的声信号传播特征进行采集、处理的方法。传统水下目标定位系统需要通过布置于海底的应答器、目标与船体之间的通信完成定位过程，实现较为复杂，且海底应答器布放易受到环境影响，不适用于未知区域中的目标定位。

水声通信技术的发展使得水下自主航行器(Autonomous Underwater Vehicle, AUV)集群作业在海洋环境监测、海洋资源开发利用以及海洋国防安全等领域呈现出重要的应用价值。所以在水下探索、勘探等作业场景中，能够实现高精度位置协同的AUV编队对水下目标定位有重大意义。水下AUV协同定位分为并行式和主从式两种方法。并行式AUV协同定位系统中，AUV编队由相同结构和功能的AUV组成，通过相邻AUV间的信息交流获取位置观测值并进行定位校准。主从式AUV协同定位系统中，AUV编队由多个装配有高精度和低精度惯性导航系统的AUV组成，通过领航AUV向跟随AUV传递相对距离信息的方式进行定位。相比于并行式AUV协同定位系统，主从式AUV协同定位系统能够兼顾导航精度与设备成本，广泛应用于实际场景中[5-9]。

多AUV编队系统的精度与对目标的定位精度直接相关。一些提高AUV集群协同定位精度的方法也逐渐被提出，文献[10]提出一种基于迭代扩展卡尔曼滤波的建议分布和线性优化重采样的快速同步定位与构图(Simultaneous Localization And Mapping, SLAM)方法，提高了AUV及路标的位置估计精度。文献[11]应用了一种能自动学习卡尔曼滤波噪声协方差参数的方法，通过仿真分析，进一步提高了单领航者AUV协同导航系统的导航精度。文献[6]提出了一种基于增广扩展卡尔曼滤波的定位算法，以解决由于测量时间延迟而导致的定位失效问题。文献[12]将AUV按照导航精度高低分层，高精度层的AUV向低精度层的AUV传递导航信息，建立AUV的数学模型并采用扩展卡尔曼滤波(Extended Kalman Filter, EKF)融合内外部导航信息，对低精度层AUV的位置状态估计更新。

针对现有的主从式多AUV编队目标定位系统存在的领航AUV无法对多个跟随AUV定位、跟随AUV位置计算传递信息过多的问题，本文提出一种基于主从式AUV移动组网协同的定位方案。AUV集群由一个高精度AUV与若干低精度AUV组成。高精度AUV对部分低精度AUV进行基于EKF的位置修正并将其转化为参考AUV，其余低精度AUV为待测AUV。参考AUV负责修正待测AUV的位置信息。在一个定位周期中，通过在不同时刻选取不同观测值的方法来对低精度AUV进行位置修正，一周期结束后即可开启目标定位。本方法可以实时绘制水下多AUV编队的位置网络，同时不需要AUV集群保持一定的相对运动，增加了AUV集群的灵活性，可以进行水下目标的定位。

2 主从式多AUV编队目标定位系统

主从式多AUV编队目标定位系统(以下简称AUV定位系统)是指利用水下主从式AUV集群对水中目标进行定位的系统。水下AUV集群首先修正自身位置信息，再运用基于信号传播时延测量(Time-Of-Arrival, TOA)的多边交汇法对给定范围的海域中目标进行扫描定位。图1为AUV定位系统示意图。

图1 AUV水下定位系统示意图

由于水下作业过程中，AUV可以通过压力传感器得到高精度的深度数据。在AUV集群运动深度保持一致的情况下，可以将3维运动模型简化为2维运动模型。图2为AUV的2维运动模型。

图2 AUV移动示意图

3 基于EKF的多AUV编队融合观测协同目标定位方法

针对AUV定位系统中，AUV编队自身位置修正中，领航AUV与跟随AUV传递信息量过大、无法对多个跟随AUV进行定位问题，本文提出一种基于扩展卡尔曼滤波的融合观测协同定位法，可以实现多AUV之间的协同定位以及对目标定位。本方法在原有AUV定位系统进行改进，以定位周期为单位，对AUV编队的运动进行观测以及修正，并在一个周期结束后利用球面多边交汇定位方法对水下目标进行定位。AUV编队构成情况为：由1个装备有高精度惯性导航系统(Inertial Navigation System, INS)的领航AUV以及多个装配有低精度INS的跟随AUV。采用两种基于不同观测方法的扩展卡尔曼滤波器对低精度AUV进行位置修正，分别为速度观测法和速度距离双观测法。在1个定位周期中，根据上一定位周期结束时各低精度AUV与高精度AUV距离选定参考AUV，其余为待测AUV。AUV优先级为高精度AUV、参考AUV、待测AUV，即参考AUV接收来自高精度AUV的位置信息，待测AUV接收来自参考AUV的位置信息。全部低精度AUV在没有接收来自上一级AUV的测距信号时，均采用速度观测法进行定位。到达测距时间以及接受到来自上一级AUV的测距信号后，开启基于速度距离双观测法进行定位。图4为AUV集群一周期内观测流程示意图。

图4 基于EKF的融合观测协同定位法流程图

3.1 恒定航向角速度和速度幅度模型

3.2 基于EKF的多AUV编队融合观测协同目标定位算法实现

基于EKF的多AUV编队融合观测协同目标定位算法(以下简称AUV编队融合定位算法)对AUV的观测有速度观测法和速度距离双观测法。如图4，采取两种观测方法以时间为节点交替进行。测速观测法是AUV通过加速度计与陀螺仪测得自身速度、航向角速度，得到k时刻观测矩阵为

利用式(13)计算估计误差协方差矩阵，以供下一时刻使用。待AUV编队如图3所示的定位周期结束后，利用式(15)中球面多边交汇定位方法开启对水中目标的定位。

图3 目标向AUV所在平面投影图

4 仿真分析

根据本文提出算法，分别对AUV定位系统和AUV编队融合定位系统做出仿真分析。仿真参数设置：静止目标所在平面深度为30 m，声速c=1500 m/s，AUV的有效通信距离为10 km，深度为20 m。高精度AUV起始点水平坐标为 (-1270,1270)，航速2 m/s，航向角45°，航行时间3600 s。低精度AUV1, AUV2, AUV3的起始点水平坐标分别为(1000,-1000), (0,-2000), (0,4000)，航速1.5 m/s，航向角分别为60°, 60°, -60°，航向角速度分别为-2·10-4rad/s, 2·10-4rad/s, 1·10-4rad/s。航速量测误差服从均值为0，标准差为0.1 m/s的高斯分布，航向角角速度量测误差均值为0，标准差为0.01°的高斯分布，测时误差均值为0，标准差为20 ms的高斯分布。AUV集群的理论航迹如图5所示。

图5 AUV编队理论路径

4.1 仿真实验1 AUV协同定位精度与目标定位精度分析

仿真结果如图6、图7所示。图6为两种定位系统的AUV编队自身位置误差对比图，图6(a)的AUV位置的均方误差最大值为36 m，图6(b)的AUV位置的均方误差最大值为14 m。从图7仿真数据结果得到，图7(a)的定位均方根误差最小值为15 m，最大均方误差值为124 m，图7(b)的定位均方根误差最小值为14 m，最大均方误差值为98 m。可以看出，AUV编队融合定位系统的目标定位精度，高于AUV定位系统。AUV编队融合定位系统的误差分布图中蓝色区域面积大于AUV定位系统。

图6 两种定位系统协同定位误差

图7 两种定位方法定位误差分布图

进一步研究本文方法相对于AUV定位系统的优越性，增加AUV定位系统的领航AUV数量，将图6、图7仿真参数中的低精度AUV1作领航AUV进行仿真，得到图8。从仿真数据结果得到，双领航的AUV定位系统的定位均方根误差最小值为13 m，最大均方误差值为97 m，蓝色区域面积较大于图7(b)。可以看出，本文算法仅需一个高精度AUV，对目标定位精度与双领航的AUV定位系统相似。

图8 双领航主从式多AUV编队目标定位系统定位误差分布图

4.2 仿真实验2 不同量测误差与定位误差关系实验

在实际应用中，AUV在水下运动过程中，受到海洋环境的影响，会使得航速与航向角速度的测量误差增大。为了比较航速、航向角速度量测误差对两种方法的影响，在图7的基础上，分别添加不同的航速、航向角速度量测误差，计算两种方法对应的最大均方误差变化如图9。可以看出，相对于AUV编队融合定位系统，AUV定位系统的定位精度受量测误差的影响较大，对恶劣环境的适应能力较差。故AUV编队融合定位系统更适用于未知海域中的目标定位。

图9 定位误差与量测误差关系图

为进一步比较量测误差对两种方法的影响，随机选取20个静止目标位置，在航速量测误差服从均值为0，标准差为0.3 m/s的高斯分布，航向角角速度量测误差均值为0，标准差为0.15°的高斯分布的环境下，分别采用AUV定位系统和AUV编队融合定位系统实现定位并记录随机静止目标的真实位置与测量结果并进行比较，结果如图10。可以看出在较大量测误差的情况下，两种方法均可得到较为相近的定位结果，本文方法相比于多AUV编队目标定位系统的定位误差较小。

图10 随机位置静止目标定位结果图

5 结束语

本文提出一种基于EKF的融合观测协同定位方法。本方法基于扩展卡尔曼滤波，通过设置定位周期和观测时间间隔的方式，将低精度AUV分为参考AUV与待测AUV，从而设置3种观测模式：高精度AUV观测参考AUV、低精度AUV自身观测以及参考AUV对待测AUV进行观测，对不同时刻的不同AUV采用不同的状态观测方式。本方法相比于主从式多AUV编队目标定位系统，获取得到的AUV位置信息更为准确且受环境影响小。多AUV集群仅含一个高精度AUV，实现了自身定位与水中目标定位，精度与双领航主从式AUV目标定位系统相近，可以实现低功耗、远距离的水中目标定位。