针对多域协调的水下无人船路径规划及轨迹追踪研究

赵永鹏

（贵州大学机械工程学院，贵州 贵阳550025）

1 概述

近年来，随着人类对海洋的勘探开发日益频繁，水下定位技术在铺设海底光缆线路、获取大规模海洋空间和环境数据、勘探开发海洋资源等领域呈现多样化。人们在实际应用中对定位设备的性能、精度、成本和安全性都提出了更高的要求。水下无人船是一种携带传感器的无缆平台，能够在靠近海床的高度进行测量。将传感器承载平台靠近海底可以显著提高采集数据的分辨率。

选择合适的控制策略，充分了解运动关系，对潜艇制导导航系统的设计和研制具有重要意义。然而，与此相关的大多数流行研究都集中在一些简化的非线性运动关系的降阶。典型地，潜艇的任务设计过程包括三个主要的挑战组成部分: 方向舵尺寸的优化(在设计状态下决定);潜艇自动驾驶仪的设计，以保证在物理允许的情况下平稳操作，以减少液压油的消耗，从而减少噪音的产生，并减少在复杂情况下(例如，沿海水域、导弹发射或侵略性采矿)过度运动所造成的脆弱性。

2 模型建立

2.1 定位模型

测距定位系统一般是由导航控制单元、发射换能器、水听器和应答器四部分组成。在实际工作中，还需要无人船、GPS 系统、计算机等。发射换能器和水听器一般安装在船底或船侧，应答器固定在水下节点。单点测距定位系统的工作原理：导航控制单元产生的询问电信号。发射换能器将电信号转换为声学信号，并将其发射到水中。当应答器接收到询问信号时，它将发出带有时间戳的响应信号。

图1 四种不同的情况涉及顶部声学定位系统

与前文所述一样，USBL 系统根据声学条件包含固有偏差，没有充分考虑所有的偏差来源。通常，我们在这种情况下的操作程度是不同的。

我们另外取矩阵R2∈SO(2)表示可能时变航向下在(水平)平面上的旋转矩阵：

斜对称矩阵S(λ)∈SS(3)为向量叉积算子，旋转矩阵Rp(f(θ))对一个角度θ 关于主轴的导数等价于矩阵乘以一个斜对称矩阵，其中E 是对应于主轴的基向量。

考虑二维空间中的定位方法，基于测量水下节点接收信标节点查询信号到达时间的倾斜距离定位方法。必须至少有三个信标节点。利用TOA 计算水下节点和不同信标节点的倾斜距离信息。水下节点的实际位置在环内，测量倾斜误差为环宽。提出了一种基于测量倾斜误差的区域交叉口定位方法。无人信标船按照规划路径在水上航行，轨迹如图2 所示。为无人信标船的声学测量倾斜位置。“为水下节点的位置。”图2 中的环是一次倾斜测量后水下节点可能存在的区域通过交环逐渐减小水下节点的定位范围。利用至少三个圆的交点区域的中心作为水下节点的定位位置。

图2 无人信标船测量航迹无人信标船测量航迹

2.2 准确定位模型

本工作中制定的跟踪目标的规划范围是在相对较长的时间范围内执行的。由于这个原因，短期动态变得不那么重要了。因此，采用一个简单的欠驱动船舶运动学模型来表示物理约束。取车辆状态分别为北向位置、东向位置和航向输入要计算的是前进速度(浪涌)和偏航率。假定船舶由于水流具有可忽略的侧向滑移和横向运动。由于其深度，水下环境是一个三维空间。假设水下节点的深度信息H 为，则水下节点与信标船在三维空间的倾斜距离L 由直达声波飞行时间来测量将三维定位转化为二维定位。无人信标与水下节点的水平测量距离R 存在λ 乘以真实值为标准差的测量误差。声速误差是测量误差的主要来源，是一种随机误差。

设无人信标船的坐标为(x,y)。设水下节点的坐标为(n,e)。水下节点位于半径为的圆上R(R-U(1-λ) R,(1+λ)R))，以无人信标船为中心。水下节点坐标计算公式:

3 轨迹规划

本文提出了一个轨迹规划的最优控制问题。OCP 通过一个直接的多重射击方法被实现为一个NLP。直接法将控制问题离散化，在优化前将控制问题分解为射击区间，并分别求解每个子问题。

图3 水下100 米时的稳态标准偏差

多重射击方法将控制输入和状态都视为决策变量。这导致问题具有更大的规模和更多的自由度，但较少的非线性依赖于控制输入。这提高了非线性问题的收敛性和数值稳定性。由于状态被作为决策变量，也可以提供一个初步的猜测，以加快收敛。采用移动视界方法，在有限的时间范围内解决问题。

步骤1:模拟发送和接收声音脉冲的过程。信标船在第n 点的GPS 位置。

步骤2:用到达时间法测量距离。使用N T 反向估计节点与信标船之间的水平距离。

步骤3:画一个新的定位环，即加入极限方程。

步骤4:估计响应节点的方位，求解方程，判断定位区域是否缩小到需要的尺度。如果它没有达到要求的规模，继续下一步。

步骤5：修正航向角度。

令矩阵Pk, j 为拍摄间隔k 开始时的协方差，其中j∈{1，…，nt}为目标。考虑两种公式。第一个公式假设所有车辆在整个射击视界以相同的速度排列。在这种情况下，超光速率将根据被跟踪车辆的数量而降低。第二种模式只能在每一个射击视界内使用一辆车。引入二元决策变量，将公式转化为混合整数非线性规划。二元决策变量bk,j∈{0,1}用于选择在每个拍摄间隔范围内的车辆。

4 分析讨论

通过一系列的仿真验证了目前所提出的方法。INS 的不确定性被认为介于低端和高端系统之间。这样做是为了在规划范围内切实增加目标的不确定性。通过在直线轨迹上测量水下航行器的位置而不将测量结果发送到INS，可以获得合理的不确定性设置。NLP 参数的调整是通过首先将喘振和偏航速率约束设置为适合中型飞机的合理值并调整喘振惩罚表1 来完成的。以产生合理的轨迹。然后将其余参数调整为更倾向于平滑转弯度变化的轨迹。运动学约束应反映基本动力系统的可实现性能。所有的模拟都使用移动水平的方法进行，其中轨迹在每次迭代中向前移动一步。求解器是基于前一个迭代的解决方案启动的。第一次迭代使用的解是最小化到目标的水平距离。

5 结论

本文提出了一种自主水面航行器的轨迹规划方法。使用无人信标船作为信标节点降低了信标节点的部署和回收成本。针对声速误差引起的测量误差，提出了一种区域相交定位求解方法。通过优化信标船的航迹，快速减小了交点区域的面积。该方法通过测量信标船与水下节点之间的距离，而不是测量水下节点的方向，从而实现远距离定位。轨迹最小化了一个或多个水下航行器的水平不确定性。假定水面车辆装备有能够测量到车辆的方向和距离的超短基线收发器。将目标和约束表述为一个优化问题。解决这个问题的前提是，在规划范围内，每辆车的移动都是已知的，例如，通过知道下一个路径点。该解决方案适合实时重新规划，以考虑中断、计划中不均衡的进展或不可预见的中断。由于方法的性质，与直接配点法相关的代价与庞特里亚金最小原理相对应的代价略有不同，因为前者是一个数值过程，而后者是一个解析过程。在今后的研究中，应考虑多环的路径优化方法。