36、海洋机器人导航：同时轨迹规划与位置估计（上）

海洋机器人导航：同时轨迹规划与位置估计（上）

1. 研究背景与目标

在海洋机器人领域，减少操作努力和成本是一个重要目标，其中降低参考对象（RO）的数量尤为关键。由此产生了一个问题：是否可以仅使用一个RO来完成水下物体的外部导航？这个RO需要以一种确保记录的数据能够用于目标位置估计的方式移动。

2. 任务场景分析

2.1 场景描述

在一个任务场景中，有一个水下目标，其位置在笛卡尔坐标系中表示为 $\mathbf{p}_0(t) = [x_0(t), y_0(t), -z_0(t)]^T$，目标沿着未知轨迹移动。一个水面机器人RO，其位置为 $\mathbf{p}_1(t) = [x_1(t), y_1(t), 0]^T$，负责根据有噪声的声学测距测量值，持续估计目标的导航参数，主要是位置。

RO以恒定时间间隔发送参考ping信号。目标接收到ping信号后，会在已知的恒定等待时间 $t_w$ 后回复目标ping信号，并发送其当前深度测量值。RO接收到目标ping信号后，使用先进的测量模型测量到目标的距离，并获取目标深度。如果目标深度变化频繁，可以应用第二个线性卡尔曼滤波器来更精确地估计深度。

2.2 RO的任务

RO有两个主要任务：
- 仅使用测距测量值持续估计目标位置。
- 规划自己的轨迹，以最大化从下一次测距测量中获取的信息量。通常情况下，基于测距测量进行目标位置估计至少需要三个RO，而单个RO需要选择合适的轨迹，以实现作为单一参考对象的位置估计。

3. 估计方法与引导控制器

3.1 目标位置估计方法

解决该任务的总体思路是结合两种方法：基于三个RO的测距测量估计目标位置的方法，以及在已知目标轨迹的情况下为单个RO计算最优轨迹的方法。

对于目标位置估计，使用之前验证过的方法，该方法即使在少于三个测距测量值的情况下也能更新目标位置估计。在当前场景中，由于始终只有一个测量值，因此可以重用之前讨论的大部分模型和方法。

测量模型采用先进模型，并进行了一些简化，不考虑与距离相关的测量噪声。假设RO在时间 $s_r$ 发送参考ping信号，目标在时间 $s_t$ 接收到，距离为 $r_r$。目标在 $s_t + t_w$ 发送目标ping信号，RO在 $s_1$ 接收到，距离为 $r_t$。真实测量值 $r’(s_1)$ 为：
$r’(s_1) = r_r(s_t) + r_t(s_1) + 2w$
其中 $r_r(s_t) = |\mathbf{p}_0(s_t) - \mathbf{p}_1(s_r)|$，$r_t(s_1) = |\mathbf{p}_0(s_t + t_w) - \mathbf{p}_1(s_1)|$，$w$ 是表示测量误差的平稳、零均值高斯噪声。

可以根据已知信息计算参考ping信号到达时间的估计值 $\hat{s}_t$，进而计算测量值的估计值 $\hat{r}(s_1)$：
$\hat{r}(s_1) = \hat{r}_r(\hat{s}_t) + \hat{r}_t(s_1)$
$= \sqrt{(\hat{x}_0(\hat{s}_t) - x_1(s_r))^2 + (\hat{y}_0(\hat{s}_t) - y_1(s_r))^2 + \hat{z}_0^2(\hat{s}_t)}$
$+ \sqrt{(\hat{x}_0(\hat{s}_t + t_w) - x_1(s_1))^2 + (\hat{y}_0(\hat{s}_t + t_w) - y_1(s_1))^2 + \hat{z}_0^2(\hat{s}_t + t_w)}$
对于 $\hat{z}_0$，可以使用最后接收到的测量值或线性卡尔曼滤波器的当前估计值。

3.2 轨迹规划方法

在当前场景中，虽然不知道目标的精确轨迹，但可以根据当前的目标位置估计值 $\hat{\mathbf{p}}_0$ 来规划RO的轨迹。所有其他原则与之前讨论的已知目标轨迹时的方法相同。

设置区间长度 $T$ 为1 s，步长 $t_{step}$ 为0.1 s，假设每个时间步可以进行一次测距测量。需要注意的是，这个频率在实际海洋场景中并不现实，仅用于轨迹规划中的经验格拉姆矩阵计算。对于位置估计，模拟测量或发送参考ping信号的频率为0.5 Hz，这是一个较为现实的频率。

使用之前介绍的模型，控制器可以将运动向量的梯度 $r$ 设置为0或 $\pm\Delta r$，该值在整个区间内保持不变。这会产生三种可能的轨迹。通过计算RO在区间内每个时间步在所有三种可能轨迹上的位置，以及在相同时间步的目标位置先验估计值，可以确定经验格拉姆矩阵及其特征值，最终选择最小特征值最大的轨迹。

4. 方法命名

我们提出的这种方法是一种适用于海洋机器人的替代导航方法。类似于同时定位与地图构建（SLAM），我们将这种通过合适轨迹实现良好估计的方法命名为同时轨迹规划与位置估计（STAP）。

5. 仿真验证

5.1 仿真参数设置

使用MATLAB进行仿真，目标在一个椭圆轨迹上移动，椭圆长轴为25 m，短轴为20 m，速度为0.3 m/s，深度恒定为20 m。RO的速度为2 m/s，最大航向变化率 $\Delta r$ 设置为0.5 rad/s。RO每2 s发送一次参考ping信号，等待时间 $t_w$ 设置为0.5 s。每次声学通信有10%的概率无法被接收，测量噪声方差设置为0.1 $m^2$，仿真采样时间为0.1 s。

每次仿真运行总时间为480 s，目标在椭圆轨迹上完成略多于一圈的运动。

5.2 仿真结果分析

仿真编号	先验估计均值（m）	先验估计方差（$m^2$）	后验估计均值（m）	后验估计方差（$m^2$）
1	0.92	0.37	0.83	0.31
2	0.88	0.18	0.80	0.15
3	0.93	0.20	0.87	0.18
4	0.90	0.20	0.84	0.19
5	0.95	0.28	0.87	0.21
6	0.95	0.24	0.84	0.19
均值	0.92	0.25	0.84	0.21

从仿真结果可以看出，估计值在短初始化阶段后收敛。估计误差会影响规划轨迹的宽度，当估计目标位置在真实位置左侧时，轨迹会更靠近目标轨迹；当估计位置在右侧时，轨迹会变宽，旋转半径增大。

总体而言，仅使用一个RO基于测距测量实现了合作导航，最优传感器放置方法（经验可观测性格拉姆矩阵）的应用取得了成功，证明了其重要性。

下面是RO轨迹规划的简单流程图：

graph TD
    A[开始] --> B[设置初始参数]
    B --> C[发送参考ping信号]
    C --> D[接收目标ping信号并测量距离]
    D --> E[估计目标位置]
    E --> F[基于估计位置规划三种可能轨迹]
    F --> G[计算经验格拉姆矩阵及特征值]
    G --> H[选择最小特征值最大的轨迹]
    H --> I[RO按选定轨迹移动]
    I --> J{是否到达区间末尾}
    J -- 否 --> C
    J -- 是 --> K[结束当前区间，开始下一轮规划]
    K --> C

海洋机器人导航：同时轨迹规划与位置估计（下）

6. 海洋机器人导航现状与挑战

海洋机器人在工程科学领域具有巨大的潜力和重要性，许多工业和科学应用都需要高度自主且可靠的解决方案。然而，目前海洋机器人领域仍面临诸多挑战。

6.1 导航难题

导航是海洋机器人面临的主要障碍之一。与陆地或空中机器人不同，海洋环境中无法使用基于卫星的导航系统，视觉方法也因能见度低和缺乏明显特征物体而效果不佳。当前的导航解决方案通常依赖多种方法的结合，传感器能提供高频、高质量的测量数据，但转换为当前位置时误差会随时间累积；基于声学的测距和/或测向测量可实现全局导航，但测量误差大、频率低，且声学通信不可靠，消息易丢失。

6.2 通信问题

缺乏高带宽的可靠通信也是一个重要问题。由于海洋机器人设备成本高，需要与中央站建立定期通信，以便接收机器人的状态消息并进行交互，如取消任务和将机器人带回安全位置进行回收。

7. 合作机器人团队的机遇与挑战

近年来，对合作机器人团队的兴趣显著增长。使用多个海洋机器人可能会产生涌现效应，使团队具备单个机器人所没有的新能力。然而，这也对导航、引导和控制任务提出了更高要求，需要额外的努力和大型测试设施。

目前，严格意义上很难将机器人团队称为“自主”，因为往往需要20 - 30人的中央站团队来监督和控制任务执行。未来的研究需要提高车辆的自主水平，减少所需的人力。

另一方面，多个海洋机器人的存在也为单个机器人的导航带来了新机遇。例如，至少可以使用一个具有GPS访问权限的水面舰艇作为导航锚点，为队友提供导航支持。

8. 研究方法与应用实例

为了解决海洋机器人导航问题，研究人员采用了多种方法，并通过多个实例进行了验证。

8.1 合作导航实例

从文献中的一个例子开始，研究人员扩展了设置，以解决第一个基准场景，即一组三个水面舰艇基于仅测距测量来监督潜水员。该解决方案在仿真和实际海上试验中都得到了验证。

之后，研究人员探讨了是否可以用更少的水面舰艇实现类似结果，提出了两种策略：一是进行额外测量，二是让剩余的水面舰艇以最大化从仅测距测量中获取信息量的方式进行机动。

第一种策略在合作导航章节中进行了讨论，通过一个由三个车辆组成的团队（其中两个为水下车辆）成功实现了闭环编队机动。其中一个水下车辆使用USBL系统获取额外的测向测量，为其他两个车辆提供相对导航数据，另一个水下车辆通过通信也能进行良好的相对导航。该结果在硬件在环（HIL）仿真中得到了验证。

8.2 最优传感器放置

第二种策略更为复杂，需要研究传感器位置对估计质量的影响，从而引出了最优传感器放置的原理。

研究人员首先介绍了一般概念，并通过文献中的一个例子进行了演示，然后使用最大似然法计算了3D示例中传感器与目标之间的最优距离，在2D场景中则发现距离无关紧要。该方法通过数值蒙特卡罗仿真进行了验证，结果表明理论预测的位置确实能获得最佳结果。

接着，研究人员将讨论扩展到动态场景，采用经验可观测性格拉姆矩阵的方法，为不同目标行为寻找单个水面舰艇的最优轨迹。通过计算经验格拉姆矩阵的奇异值分解或特征值，找到了水面舰艇的最优轨迹，并与文献中的其他结果进行了比较。

9. 合作导航与最优传感器放置的结合

最后，研究人员将合作导航和最优传感器放置的结果结合起来，实现了两个场景。第一个场景是该领域的一个入门示例，水面舰艇的轨迹规划主要基于文献中的结果。

在第二个场景中，成功实现了最后一个基准场景。核心思想是根据扩展卡尔曼滤波器不断获得的位置估计来规划水面舰艇的最优轨迹。仿真结果表明，这种方法总体上是可行的，成功将海洋机器人的合作导航与最优传感器放置相结合，并提出了同时轨迹规划与位置估计（STAP）的新方法。

10. 未来研究方向

未来的研究将集中在以下几个方面：

10.1 实际验证

在实际海上试验中验证所提出的概念，进一步验证其在真实环境中的有效性。

10.2 方法优化

研究使用两个水面舰艇是否能提高估计结果的质量，以及使用配备USBL系统的水面舰艇和规划最优轨迹的概念是否能改善整体结果。

10.3 特定场景研究

随着未来任务场景对特定机器人团队的需求增加，需要研究最优导航解决方案，例如测距参考车辆也在水下且无法访问GPS或进行可靠通信的场景。

10.4 系统改进

提高系统的鲁棒性，减少所需的操作人员数量，使系统更加自主。改进操作员的图形界面，使其更易于非专业人员操作。

下面是海洋机器人导航研究的总体流程图：

graph TD
    A[提出问题：减少RO数量实现导航] --> B[分析场景与方法]
    B --> C[研究合作导航与最优传感器放置]
    C --> D[结合两种方法实现STAP]
    D --> E[仿真验证STAP方法]
    E --> F[总结成果与挑战]
    F --> G[规划未来研究方向]

总之，海洋机器人导航领域充满了挑战和机遇。通过不断的研究和创新，有望提高海洋机器人的自主水平和导航能力，为海洋科学和工业应用带来更多的可能性。

以下是海洋机器人导航研究的关键要点总结表格：
| 研究方面 | 具体内容 |
| ---- | ---- |
| 背景与挑战 | 海洋机器人导航面临导航困难和通信问题，合作机器人团队带来新机遇但也有挑战 |
| 研究方法 | 合作导航、最优传感器放置、结合两种方法实现STAP |
| 应用实例 | 监督潜水员、水下车辆编队、寻找最优轨迹 |
| 未来方向 | 实际验证、方法优化、特定场景研究、系统改进 |