使用鱼小雷AUV作为领航者，红小雷和白小雷AUV作为两个跟随者，分别称为跟随者AUV1和跟随者AUV2，在水池中进行多AUV协同运动试验［3］。在试验中，领航者的速度为0．5 m／s。选定以较慢的速度进行试验，一方面是受到视觉试验装置造成的水阻影响，领航者无法以较快速度前进；另一方面是为了保证试验的成功率。跟随者的编队参数列于表10-10中。水下试验场景如图10-31所示。

表10-10　多AUV协同运动试验跟随者编队参数

图10-31　多AUV协同运动水下试验场景

在协同运动任务开始前，跟随者AUV与领航者AUV之间的位置关系是散乱的，如图10-32所示。开始任务后，为了避免航行灯的灯光对彼此造成影响，两个跟随者均关闭航行灯。首先，两个跟随者AUV首先下潜到0．2 m，等待深度到达设定深度正负0．02 m后，开始逆时针旋转搜索领航者AUV的光信标阵列或AR Marker，如图10-33所示。

图10-32　任务开始前各AUV的位置关系

图10-33　跟随者AUV正在搜索领航者

随着跟随者AUV的航向逐渐指向领航者AUV的方向，两个跟随者AUV最终能看到领航者AUV的光信标阵列或AR Marker。在试验中，由于距离领航者AUV较远，两个跟随者AUV首先识别到的是领航者AUV搭载的引导光信标阵列。跟随者AUV对光信标阵列在图像中的四个点解析后，输出领航者相对于该跟随者的相对位姿，此时的主控软件界面如图10-34所示。值得注意的是，由于之前AUV在寻找过程中产生了运动，因此造成了对领航者AUV的光信标阵列识别不稳的情况。可以看到在日志记录区记录了多个“光信标惯性引导”状态，即将最后一次看到目标的偏航角与自身IMU输出的偏航角相加，作为对目标航向的预测并作为航向控制的目标。同时，保持最后一次发送的侧移指令，尝试找回目标。可以说，惯性引导的加入大大增强了任务的实现性和试验的鲁棒性。此时的试验场景如图10-35所示。

图10-34　光信标引导阶段主控软件截图

图10-35　跟随者AUV正在由光信标引导

跟随者AUV观测到光信标阵列后，根据光信标阵列提供的6自由度信息计算编队误差，并在协同运动控制器的作用下输出控制量，跟踪目标深度和航向，向着领航者AUV前进。随着跟随者AUV与领航者AUV距离的缩短，AR Marker逐渐进入视野中并越来越清晰。应当指出的是，在最初能检测到AR Marker的阶段，识别尚不稳定，因此AR Marker追踪只有当能够稳定识别同一个AR Marker 1 s以上方可进行。跟随者识别到AR Marker的试验场景见图10-36，最初识别到AR Marker时主控软件界面如图10-37所示。

图10-36　跟随者AUV识别到AR Marker(www.daowen.com)

图10-37　最初捕获到AR Marker时的主控软件截图

识别到AR Marker的同时，跟随者AUV根据自身配置文件中对领航者携带的AR Marker位置的定义和跟踪ID的设置，判断当前Marker是否是自己所应该跟踪的AR Marker。如果不是，还需要确定自己下一步的位移方向。在此过程中，跟随者利用所有能够识别到的AR Marker求解姿态，并求出均值，作为当前的控制依据和编队误差的依据。不断控制直到编队误差收敛为0，此时的主控软件界面如图10-38所示，最终达到的协同运动状态见图10-39。

图10-38　AR Marker追踪阶段的主控软件截图

图10-39　跟随者AUV利用AR Marker进行协同运动

考虑到领航者AUV处于运动状态，有可能出现领航者AUV的机动过程导致跟随者AUV在视野中丢失目标的情况。此外，跟随者AUV机动幅度过大也会使图像出现动态模糊导致无法正常识别的现象，如图10-40所示。此时，可采用与光信标阵列阶段相似的策略，如果未能成功识别，则取最后一次识别到目标的位姿信息作为预测，将AUV向该位置调整，同时保持侧移力不变。如果未能在7 s内成功找回，则重新在当前深度转向寻找。如果当前深度未能找到，则增加寻找深度后重新尝试，直到找回目标或触发安保程序。

图10-40　由于动态模糊导致未能成功识别的AR Marker

协同运动过程中，2台跟随者AUV的深度与领航者的深度之间的误差和航向角的误差，如图10-41所示。

从图10-41a中可以直观地看出，光信标引导过程的深度误差较大，而在AR Marker追踪阶段，深度误差逐渐收敛至接近于0。这是由于在光信标引导阶段，AUV的前进速度较快，较大的主推推力影响了俯仰角，导致深度控制的效果下降；与此同时，非零的俯仰角会使AUV在本体系上的前进速度在大地系的竖直方向上有分量，二者叠加作用造成深度暂时失稳。随着抵近编队位置，跟随者AUV的速度逐渐降低，同时在以AR Marker提供的位姿信息为基础的控制下，深度逐渐收敛到与领航者相同。从图10-41b中能够看到在识别到光信标阵列后，根据提取的相对姿态，跟随者与领航者之间的偏航角误差迅速收敛为0。随着二者距离的接近，AR Marker提供了稳定的偏航角误差控制，使得协同运动偏航角误差逐渐收敛为0。

图10-41　跟随者与领航者的深度误差与偏航角误差

从识别到光信标阵列开始，跟随者视觉算法提取的三轴位移和偏航角信息如图10-42所示。

图10-42　视觉算法提取的三轴位移和偏航角信息

从图中可以看到，在控制器的作用下，X、Y、Z三轴距离不断收敛，并最终达到设定的编队参数附近。偏航角也随着控制过程收敛到0。此外，能够看到，光信标阵列与AR Marker的识别过程的一致性较好，识别过程基本可以做到无缝切换，从而保证了控制的连续性。