文 / 胡常青 1,2,3，文龙贻彬 2，张亚婷 2，杨义勇 1 （1. 中国地质大学（北京）工程技术学院，北京 100083；2. 北京航天控制仪器研究所，北京 100039；3. 青岛海洋科学与技术试点国家实验室，青岛 266237）
摘 要：针对无人水下航行器（UUV）与无人水面艇（USV）协同作业时水下导航性能受惯导设备（INS）影响较大的问题，提出了一种基于超短基线水声定位（USBL）的 USV/UUV 协同导航方法。首先，以USV 上的高精度 INS 和 GNSS 组合后的导航结果作为基准，利用 USBL 测量得到的 USV 和 UUV 相对位置和姿态，再结合 UUV 的 INS 误差方程，建立了 INS/GNSS/USBL/INS 滤波的状态方程和观测方程，实现了多源导航信息的融合，有效提升了 UUV 水下导航精度。其次，针对 USV 和 UUV 航行过程中 USBL因存在信号盲区带来的两者无法通信、定位导致协同导航不稳定的问题，采用基于视线法的 PID 控制方法实时改变 USV 的航向和航速，保证了 USV、UUV 航行过程中两者始终处于水声信号有效的通信、定位距离和角度范围内。仿真和海上实验结果表明，UUV 在与 USV 协同导航后的位置误差小于 10m；USV和 UUV 航行过程中，相对距离和角度保持在设定的 USBL 有效通信定位距离和角度范围内，距离误差小于 15m，角度变化小于 1°，提出的方法可以使 USV 和 UUV 协同导航更稳定、连续。
关键字：无人水面艇；无人水下航行器；超短基线；协同导航
基于超短基线水声定位的 USV/UUV 协同导航方法
无人水面艇（Unmanned Surface Vehicle, USV）和无人水下航行器（Unmanned Underwater Vehicle, UUV）因其自身具有的优势，在军民领域逐步得到应用。但单一 USV 或 UUV 通常针对指定任务在同一空间区域进行作业，存在作业范围和效果受限、信息获取类型少、作业效率低等不足。针对该问题，跨域无人装备协同作业已成为国内外无人海洋系统的研究热点，USV 和UUV 协同作业可以在海域立体监测、海洋事件应急处理等领域中获得更好的效果、更高的效率。
USV、UUV 协同作业的关键技术之一是协同导航。USV 可以较为容易获得高精度惯导信息或 GNSS信息从而得到精确的导航信息，但 UUV 受体积、重量、能源的限制，自身所装备的导航设备精度一般不高，也无法利用 GNSS 信息，难以满足长时间高精度导航作业的要求。而声波在水下传播相比无线电、微波等信号衰减小，传播距离更远，因此广泛用来水下通信和定位。超短基线（Ultra Short Base Line, USBL）具有尺寸小、安装方便等特点，适合用于水下运动载体间的通信和定位[1]。
近些年来，国外也报道了很多利用水声通信和定位技术实现无人系统间协同导航的研究，但目前大多数研究工作都集中在多艘具有感测和操纵能力的UUV利用水声通信定位技术进行协同导航[2-4]。在基于几何解算的协同定位技术研究中，Vaganay[5]首次提出了移动基线概念，利用时间同步信号和水声传播延迟进行相对位置测量。Curcio[6]等人利用一艘 UUV 和两艘 USV，实现了基于该方法的协同导航定位。WeiChen[7]等针对移动基线定位系统提出了实时滚动时域估计（Moving Horizon Estimation, MHE）方法，仿真结果与卡尔曼滤波方法进行比较，表明了该方法的有效性。国内目前USV 和 UUV 协同导航的研究还比较少，严浙平[8]等人分析了 USBL 水声定位数据和 GPS 数据，证明两者可进行组合导航，但并没有开展协同导航设计；徐博[9]等人基于倒置的 USBL，研究了 USBL 和潜航器 INS之间的松、紧组合滤波算法，提出基于 USBL 原始输出信息的 INS/USBL 紧耦合组合导航方法。综上，目前国内外 USV 和 UUV 协同导航的研究还主要集中在滤波算法的研究上，但在协同作业中 UUV 若要持续稳定获得高精度的导航定位信息，还需要考虑 USV 和 UUV在实际航行中的相对运动关系，否则两者距离和角度超出 USBL 有效范围会导致协同导航不稳定、不连续。
为此，本文提出了一种基于 USBL 的 USV/UUV协同导航方法。首先，结合 UUV 的 INS 误差方程，建立了 INS/GNSS /USBL/INS 松组合模型和滤波方程；其次针对USBL因存在信号盲区导致 USV和 UUV协同导航不稳定、不连续问题，提出了一种基于视线法的线性 PID 控制方法，通过实时改变 USV 的航向和航速以适应 USBL 有效距离和角度；最后对该方法进行了仿真和试验验证。
基于 USBL 的协同导航无人系统如图 1 所示。USV 能够综合利用自身高精度 INS、GNSS 与水声定位信息，辅助 UUV 导航定位。USBL 包括安装在 USV端的问答器和安装在 UUV 端的应答器，用于实现USV与UUV之间的通讯和相对定位。UUV通过USBL接收 USV 上发出的控制指令和定位信息，并根据接收到的信息完成与自身 INS 导航信息的融合。
1.2 协同导航原理
USBL的问答器通过伸缩杆安装在距USV 船底2m深的位置，应答器直接安装在 UUV 本体上。这样整个系统可以建立三个坐标系。取东北天坐标系为导航坐标系（n 系）。USV 和 UUV 建立各自的载体坐标系（b 系），其中，x 轴沿载体横轴向右，y 轴沿纵轴向前，z 轴与 x、 y 轴构成右手坐标系。USBL 基元阵建立的坐标系与载体坐标系重合，在此假定安装误差角为零，见图 2。
其中，pAC为固定在 UUV 上的应答器相对于固定在USV 上的问答器的位置。
通过 GNSS 或其组合导航后获得的 USV 的位置pGUSV，UUV 在当地坐标中的位置可以表示为
其中，pUSV是 USV 在当地坐标系中的位置，可由 pGUSV计算得到；pACUSV是固定在 USV 上的问答器相对于 USV在当地坐标系中的位置；pACUSV是固定在 UUV 上的应答器相对于问答器的位置。在本文中，由于 UUV 与应答器是固连安装，因此将 UUV 和应答器视为一个整体。
因此，根据 USBL 和 GNSS 的观测结果，可以得到观测方程为
1.3 GNSS/INS/USBL 与 UUV 惯导的松组合导航
使用USV 上高精度INS 与GNSS 导航信息与USBL 定位信息来校正 UUV 的导航误差，进而提高 UUV 导航精度。选择 UUV 惯导设备的经纬度和高度位置误差δpn、三个方向的速度误差δVn、姿态误差δn、陀螺漂移 n和加速度计偏置误差n构构建 15 维系统状态变量：
可以得到 UUV 状态方程：
其中，F为系统状态转移矩阵，
然后选择UUV惯导解算出来的经纬度位置信息与USV的INS/GNSS/USBL解算出来的对应信息的差值作为量测量，建立量测方程。
根据上述系统状态和观测方程，进行卡尔曼滤波，可以得到校正后 UUV 的位置信息。
1.4 USBL 有效距离及角度约束下协同导航保持方法
由于 USBL 的有效通信定位距离和波束角有限（本文USBL 波束角范围是周向 2π，轴向π 4），当 USV 和 UVV处在特定角度和位置时，会进入信号盲区，如图 3 所示。此时 USV 和 UUV 之间无法有效进行通信、定位，协同导航不稳定、不连续。
为了使协同导航保持稳定，需要保证 USV、UUV 航行过程中两者始终处于有效的水下通信范围内，即满足如式(8)的约束条件，从而实现 USV/UUV 稳定、可靠的协同导航。
式中：e 为 USV 端问答器至 UUV 端应答器的相对距离，为 USV 与 UUV 间的距离矢量和 USV 端问答器水平面所构成的线面角，emax为 USBL 的最大有效通信距离。应答器与问答器的相对距离和角度都是实时可测，并假定 USBL 的问答器距水面深度为hb 。从式(8)可以得到，USV 与 UUV 的有效空间关系：
从式(8)可以得到，USV 与 UUV 的有效空间关系：
满足式(9)就可避免 USV 与 UUV 进入信号盲区，并可以计算出 UUV 距水面深度 h 和 USV 与 UUV 的最佳保持距离 d。
其中，eb为 USBL 的最佳通信距离，可从厂家提供的设备资料中获得。
UUV 和 USV 之间的相对距离可以由由pAC获得，为了能够保持稳定的相对距离 d，因 USV 机动性更好，采用调整 USV 运动航向和航速的方法，本文采用基于视线法的线性 PID 方法，如图 4 所示。
首先在航路段上根据上述导航解算获得的 USV 与UUV 的相对距离结果以及预定的有效保持距离计算出一个虚拟的追踪目标，然后引导 USV 沿着到追踪目标的视线方向航行，并根据与 UUV 相对速度实时调整航速。虚拟追踪目标确定方法为超前置位法。超前置位法中的虚拟目标与 USV 在航路段上的垂直投影点的距离是一个固定不变的常数。随着 USV 不断接近计划航线，航向偏差也随之不断减小，能够达到同时减小位置偏差和航向偏差的效果，从而确保 USV 与 UUV 航行过程中保持稳定的相对距离，避免进入信号盲区。
平面坐标系上，虚拟点的位置 D(xd , yd)满足：
其中，d 表示期望的 USV 和 UUV 相对距离，l 表示视线长度。航向角计算公式如式(14)所示。
式中， ab为矢量AB与 y 轴的夹角，表示该航路段的计划航向。船舶的位置偏差是 P 到直线 AB 的垂直距离，在计划航线的左侧为正，右侧为负，计算公式如式(15)所示。
其中，ab表示矢量AP与 y 轴的夹角，计算公式如式(16)所示。
USV 的航速可以根据目前与 UUV 的相对距离、USV 当前舵角等信息计算获得。
2.1 实验条件
UUV 惯导设备：陀螺零偏稳定性优于 8 (°)/h，加速度计零偏稳定性优于 1mg。
USV 高精度惯导设备：陀螺零偏稳定性优于0.008 (°)/h，加速度计零偏稳定性优于 0.02mg；GNSS精度——水平速度精度优于 0.2 m/s，位置精度优于5m；组合导航精度——水平姿态精度优于 0.002°，航向精度优于 0.03°（初始对准后组合导航 500s）。
USBL 性能指标：斜距测量误差为≤0.01 m，测角误差为≤ 0.1°。
2.2 实验方法
UUV 惯导与 USV 的 GNSS/高精度惯导组合导航系统安装在同一平板上，通过平板上的水平与方位基准面，两惯导本体坐标系保持重合。通过跑车试验采集 UUV 惯导的原始数据与 GNSS/高精度惯导组合导航数据，与计算机随机生成的符合 USBL 精度要求的数据一起依据上文建立的组合滤波算法模型进行仿真。
为设定滤波解算周期为 1 s，仿真时间为 500 s，对松组合导航算法进行仿真。
实际航行轨迹如图 5 所示，由于跑车数据只有水平面轨迹，图中仅显示水平面轨迹，纵坐标代表经度，横坐标代表纬度。
2.3.2 UUV 松组合导航位置误差曲线
USV 的 GNSS/高精度惯导/USBL 与 UUV 惯导松组合导航的位置误差曲线图如图 6 所示。
通过以上松组合导航算法仿真结果可以得到：UUV惯导与 USV 的 INS/GNSS/USBL 组合导航滤波后的位置误差没有发散，经卡尔曼滤波后，经纬度和高度位置最大误差分别是 11.2332 m、9.7655 m、0.5847 m，解决了 UUV 导航误差累积问题。
在某内湖和威海国家浅海试验场开展 UUV 和USV 水域协同导航实验，验证协同导航滤波算法效果以及协同导航保持方法效果。
在协同导航滤波算法效果的内湖实验验证中，USV 保持在初始位置不动，设计的航线使得 UUV 在航行过程中始终处于与 USV 的有效水声定位通信范围，主要考核 UUV 在协同导航下到达预设目标点的能力。
协同导航保持方法效果的海上实验验证中，UUV和 USV 都处于航行状态，设定 USV 航线，考核 USV是否能够根据 USBL 有效距离和波束角的约束条件自动调整自身航向和航线，以保证与 UUV 的协同导航能持续、稳定。
3.1 协同导航滤波算法效果验证实验
实验时，USV 在 UUV 初始位置附近保持不动，通过 USBL 给 UUV 装订目标点经纬度和深度信息。UUV 航行速度约 2~3 节，保持定深航行。两次航行试验中，UUV 装订的目标点信息一致。UUV 第一次航行时，处于自主航行模式，完全依靠自身惯导信息导航；第二次航行时，处于协同导航模式，利用 USV 上 的 INS/GNSS/USBL 导航信息，结合自身惯导信息，组合导航模式下航行。
图 7 中紫色虚线为 UUV 自主导航期望航行路径，紫色实线是 UUV 自主导航下实际航迹；绿色虚线为协同导航模式下的期望航行路径，绿色实线是协同导航模式下 UUV 实际航迹。从图 7 中可以看出， 200m左右的航行中：在 UUV 自主导航模式下，无法到达指定目标点，离目标点距离偏差值为 50m，且随着航行时间还不断增加；在协同导航模式下，UUV 在航行过程中，路径跟踪误差能够得到修正，过程中与预期的路径跟踪偏差最大值为 16m（海流和控制精度影响），最终离目标点的偏差小于 10m。
试验开始前， UUV 在水下设置不同的航行目标点，并保持 20m 定深航行。考虑通信速率和水文条件，本次试验设定 USV 与 UUV 通信定位的有效距离范围为 200±10%m（需同时满足 USBL 距离和角度约束条件），USV、UUV 初始相对距离设置在 100~180 m，220~300 m 范围内，USV 根据获得的 UUV 实时航迹自动调整自身航向和航速，以满足 USBL 有效通信距离和角度，持续稳定与 UUV 保持协同状态。USV 航行时的控制参数经试验取kp 1.2 ，ki 0.2，kd 0.1。
协同保持过程中的数据如图 9 所示，红色和黑色点分别为通过协同保持算法计算出的保持 USV 与UUV 距离 200m 时目标点和经过点，紫色路线为 USV运动轨迹，蓝色路线为 UUV 运动轨迹，箭头为运动方向，共开展五次航行实验。从测试数据可以看出，USV 可以保持预设的相对距离和角度，全程可与 UUV进行有效的协同导航，相对位置误差控制在 15m 以内（第四、五次实验航行受到海流影响），相对角度 一直满足式(9)约束，变化在 1以内，如图 10~11 所示，协同保持方法具有较好的效果。
本文开展了基于 USBL 的 USV/UUV 协同导航方法研究，总结如下：
1）为了提高 UUV 的水下导航性能，以 USV 上的高精度 INS 和 GNSS 组合后的导航结果作为基准，建立了INS/GNSS/USBL/INS 卡尔曼滤波的状态方程和观测方程，实现了多源导航信息的融合，UUV 在与 USV 协同导航后的位置误差小于 10m。
2）采用基于视线法的控制方法实时改变 USV 的航向和航速，保证了 USV、UUV 航行过程中两者始终处于水声信号有效的通信、定位距离和角度范围内，距离误差小于 15m，角度变化小于 1。
仿真和实船实验表明，这种协同导航方法可以有效、稳定地提升 UUV 水下导航精度，在海洋测量、测绘、海洋立体监测等实际工程中具有较大的应用价值。
[1] 王彬, 翁海娜, 梁瑾, 等. 一种惯性/水声单应答器距离组合导航方法[J]. 中国惯性技术学报, 2017, 25(1): 86-90.
Wang B, Weng H, Liang J, et al. Inertial/acoustic range integrated navigation with single transponder[J]. Journal of Chinese Inertial Technology, 2017, 25(1): 86-90
[2] Lin Y, Hsiung J, Piersall R, et al. A multi-autonomous underwater vehicle system for autonomous tracking of marine life[J]. Journal of Field Robotics, 2017, 34(4): 757-774.
[3] Liu Z, Zhang Y, Yu X, et al. Unmanned surface vehicles: An overview of developments and challenges[J]. Annual Reviews in Control, 2016, 41: 71-93.
[4] Sonnenburg C R, Woolsey C A. Modeling, identification, and control of an unmanned surface vehicle[J]. Journal of Field Robotics, 2013, 30(3): 371-398.
[5] Vaganay J, Leonard J J, Curcio J A, et al. Experimental validation of the moving long baseline navigation concept[C]//2004 IEEE/OES Autonomous Underwater Vehicles. 2004: 59-65.
[6] Curcio J, Leonard J, Vaganay J, et al. Experiments in moving baseline navigation using autonomous surface craft[C]//Proceedings of OCEANS 2005 MTS/IEEE. 2005: 730-735.
[7] Chen W, Yan W, Cui R. Moving horizon estimation for moving long baseline based on linearpositioning model[J]. IFAC-PapersOnLine, 2016, 49(5): 115-119.
[8] 严浙平, 张龙海, 赵海彬. 小型 USV 定位与跟踪 UUV 的试验方法研究[J]. 计算机测量与控制, 2013, 21(6): 1558-1561.Yan Z, Zhang L, Zhao H. Research of small USV positioning and experimental methods of UUV tracking[J]. Computer Measurement & Control, 2013, 21(6): 1558-1561.
[9] 徐博, 郝芮, 王超, 等. 基于倒置声学基阵的INSUSBL 组合导航算法研究[J]. 海洋技术学报,2017, 36(5): 46-53.Xu B, Hao R, Wang C, et al. Research on INSUSBL integrated navigation algorithm based on inverted acoustic array[J]. Journal of Ocean Technology, 2017, 36(5): 46-53.
[10] 胡常青, 朱玮, 何远清, 等. 无人水面艇自主导航技术[J]. 导航与控制, 2019, 18(1): 19-26, 90.Hu C, Zhu W, He Y, et al. Autonomous navigation technology of unmanned surface vehicle[J].Navigation and Control, 2019,18(1): 19-26, 90.
[11] Nad D, Ribeiro M, Silva H, et al. Cooperative surface/ underwater navigation for AUV path following missions[J]. IFAC-PapersOnLine, 2016, 49(23): 355-360.
[12] Hu C, Fu L, Yang Y. Cooperative navigation and control for surface-underwater autonomous marinevehicles[C]//IEEE 2nd Information Technology, Networking, Electronic and Automation Control Conference. 2017: 589-592.
本文整理自《中国惯性技术学报》期刊 2019年06月 第27卷 第3期，转载请备注论文作者，说明文章来源，并备注由“智慧海洋公众交流平台”微信公众号整理。
声明：本公众号相关内容均来自主流媒体及公众号，非商业用途，并不意味着赞同其观点或证实其内容的真实性。版权归原作者所有，如有发现侵犯您的权益，请后台联系编辑，我们会尽快删除相关侵权内容。
