36、海洋机器人导航：同时轨迹规划与位置估计（STAP）的创新探索

海洋机器人导航：同时轨迹规划与位置估计（STAP）的创新探索

1. 研究背景与动机

在海洋机器人领域，减少参考对象（RO）的数量以降低操作成本和工作量是一个重要的研究方向。通常情况下，基于仅测距的测量方式对水下目标进行定位估计至少需要三个 RO。那么，能否仅使用一个 RO 来完成水下目标的外部导航呢？这便是本文要探讨的核心问题。

2. 任务场景描述

在我们讨论的任务场景中，有一个水下目标，其在笛卡尔坐标系中的位置表示为 $\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 的任务有两个：一是基于有噪声的声学测距测量，持续估计目标的位置；二是规划自身的轨迹，以最大化从下一次测距测量中获取的信息量。

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

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

为了解决上述任务，我们结合了两种之前介绍过的方法：
- 目标位置估计方法 ：参考之前基于三个 RO 的测距测量来估计目标位置的方法。该方法在模拟和实际试验中都被证明是有效的，并且即使在测量数据少于三个时，也能进行目标位置估计的更新。在当前场景中，由于只有一