（1）远距离引导过程。

当跟随者AUV从远距离（数十米）的距离向跟随者接近的过程中进行搜索—聚集的动作。对于每个跟随者AUV来说，在任务开始前已经确定其在协同运动队列中的位置。换言之，其预期位置已经预先设定。首先，跟随者AUV先下潜0．2 m，等到深度到达所设定的深度并稳定后，向右转向360°。如果在下潜过程中或环视过程中，图像处理程序成功识别到光信标阵列的四个光源，则进行接近操作。反之，如果环绕360°仍未能找到光信标阵列，则将搜索深度增加0．2 m后重复搜索，直到触发安保措施使任务失败，跟随者AUV浮出水面。

如果能够找到光信标阵列，那么根据提取出的4个点，利用P4P算法求解计算摄像机坐标系下领航者AUV的6自由度信息，根据这些信息，结合跟随者自身搭载的传感器，将领航者AUV在跟随者AUV摄像机坐标系下的位姿信息转换为惯性坐标系下的位姿信息，由此获得领航者AUV的状态信息。跟随者AUV可以通过自身传感器得到自身的状态信息，从而获得协同运动的编队误差。

由于领航者AUV时刻处于运动的状态，因此目标有可能移出视野。对于这种情况，一旦领航者AUV从跟随者AUV的视野中丢失后，为了尽快使目标回归视野，跟随者AUV会根据目标丢失前所处的位置估计丢失方向，在水平方向上转向。如果超过一定时间仍未找到，跟随者AUV将会在此位置环绕360°进行寻找，如果成功找回，则继续进行接近。否则，增加搜索深度再次寻找，直到触发安保条件，使任务失败，跟随者AUV浮出水面。

在此过程中，跟随者AUV将不断尝试识别领航者AUV所搭载的Ar Marke。由于距离较远时，并非每一帧都能够识别到AR Marker，为了后续的控制稳定起见，只有当连续若干帧画面都能够识别到AR Marker后才进行近距离跟随。

（2）近距离追踪过程。(www.daowen.com)

当跟随者AUV接近到能够稳定识别AR Marker的距离时，开始近距离跟随模式。由于在远距离接近过程结束时已经调整到能够识别AR Marker的位置和姿态，因此，跟随者AUV在近距离跟随开始时就已经能够从AR Marker中获取领航者AUV的6自由度信息，无需进行寻找。根据6自由度信息和自身传感器回传的数值，跟随者AUV实时计算协同运动编队的控制输出，作为运动控制的目标进行精确控制。

如果由于障碍物干扰等因素导致视野中丢失目标，则保持最后一次看到目标时所得到的姿态角目标和侧移指令，同时保持当前深度，主推进器停转，跟随者AUV相当于以惯性继续追踪领航者AUV，在此期间，跟随者AUV不断对视觉传感器采集到的图像识别AR Marker和光信标阵列。如果在一定时间内找回AR Marker，则继续进行近距离的实时追踪，如果找到了光信标，则重返远距离接近过程，重新进行接近。如果一段时间过后仍然没能找到，那么跟随者AUV将在此位置旋转360°，尝试寻找AR Marker或光信标阵列。如果找到前者，则继续进行近距离跟随；如果找到后者，则重新进行远距离接近。反之，如果转过一周后仍然未能找到任何目标，则在当前的深度基础上增加0．2 m，搜索AR Marker或光信标阵列，直到触发安保条件，任务失败，跟随者AUV在自身浮力的作用下浮出水面。

（3）安保措施。

由于跟随者AUV的协同运动过程中没有人工干预，为了防止意外情况造成航行器无谓的损失，需要通过安保程序进行保证任务执行的安全性。

我们设计了超时安保和过深安保，通过指定任务的最大执行时间和最大下潜深度来保证协同任务的安全运行。当航行器在搜索过程中始终无法找到目标时，由于有最大潜深的存在，航行器不会无限制地下潜，造成水密承压结构损坏。此外，还能够避免领航者AUV的故障导致过深下潜的情况出现，从而保护跟随者AUV。超时安保能够保证任务在任何状态下都是可结束的，是保证任务安全的最后一道屏障。