CN109062229A - 基于双目视觉的水下机器人系统的领航跟随编队方法 - Google Patents

Abstract

本发明公开一种基于双目视觉的水下机器人系统的领航跟随编队方法，能够有效解决小型水下机器人在水下弱通信环境或无通讯环境中编队困难的问题。水下机器人系统包括两个以上设置有双目摄像头的水下机器人，两个以上水下机器人采用串级分层结构，即该串级分层结构的水下机器人系统中上一级的水下机器人是其下一级水下机器人的领航机器人，对应下一级水下机器人是其上一级机器人的跟随机器人；通过跟随机器人对其自身领航机器人的跟随，按照设定的编队队形完成水下机器人系统的编队。跟随机器人对其自身领航机器人的跟随过程包括对领航机器人的目标跟踪、基于双目视觉对领航机器人的点匹配以及水下跟随控制。

Description

基于双目视觉的水下机器人系统的领航跟随编队方法

技术领域

发明涉及一种编队方法，具体涉及一种水下机器人系统的水下编队方法。

背景技术

机器人技术的发展极大的方便了人们的各种活动，两栖机器人由于其水陆两栖的多功能性受到的人们的关注，小型的两栖机器人可以工作在狭小空间的环境中，逐渐成为了研究热点。但小型的两栖机器人由于带载能力小和工作效率低等劣势，为了扩展单个机器人的工作范围并且完成多样性任务的要求，需要多个机器人进行编队协调等任务。由于水下使用的特点，水可以吸收电磁波，所以陆地上一般的无线通信在水下时失效。现在水下通信的技术是利用水声通信，但水声通信不仅效率低、通信复杂而且需要专用设备，所以对于小型机器人的水下编队策略而言通信困难是一个难点。

在机器人编队中，机器人间的定位是重要的技术关键点，现在的定位手段对于陆上机器人来说有多种，但对水下机器人来说，可供选择的定位方法并不多。机器人的定位一般凭借传感器，装载在机器人上的传感器是机器人获得外部信息的重要的途径，常见的水下传感器有水声传感器、视觉传感器、激光传感器、GPS浮标、加速度计和陀螺仪等。所以目前水下定位系统有水声定位系统、基于GPS定位和视觉定位系统等。水声定位系统依赖于水下的水声通信，对于水声通信，常见的设备是声呐，水声通信是将信息转换电信号，利用换能器将电信息转换声信号，声信号通过水介质进行传递，接收时，通过接收换能器将声信息转换为电信号，再通过解码最后得到有用信息。水声定位是通过测量声波信号的传播时间或者相位差来完成定位。GPS定位是通过卫星来实现导航和定位，由于GPS无线电信号不能在水中很好的传播，如果要利用GPS信号，必须通过GPS浮标接收信号，所以基于GPS的水下定位系统由水面上的GPS浮标、GPS卫星系统、水下收发器、陆上或船上数据处理中心等组成。视觉定位系统利用获得的图像信息通过算法处理来获得目标的位置信息，根据相机的种类可分为可见光摄像头和ToF摄像头，对于可见光摄像头，又可分为单目摄像头、双目摄像头和多目摄像头。ToF的测距原理是利用近红外光的发射和反射的时间差或相位差来实现测距的。双目测距的原理是根据目标在左右相机成像的几何关系来解算的。目前水下机器人的视觉定位中应用比较多的是双目摄像头。

机器人的编队系统可以分为编队体系和编队的控制方案。多机器人的编队体系可分为分散式与集中式。分散式编队结构可分为分布式结构和分层式结构。分层式结构中的机器人具有层级关系，上层机器人对下层机器人具有部分主控能力，下层机器人还有部分自主性，具有集中式和分布式结构的优点。分布式编队结构中的机器人之间相互平等协作，各个机器人通过其他机器人信息及环境信息进行自己任务的决策，每个机器人均具有自主性。这种结构编队的系统灵活。对于集中式编队结构，是编队中的管理机器人根据环境信息和各个机器人反馈的信息对编队中的其他机器人进行管理，管理机器人决定了各个机器人的运动策略，从而完成多机器人系统的编队。集中式编队结构需要各个机器人与管理机器人之间需要信息交互，整个机器人编队依赖与管理机器人。

对于多机器人系统编队的控制方案，现在形成多种控制方案，常用的有领航跟随法、基于行为法、基于图论法、人工势场法和虚拟结构法等。领航跟随法是通过获得领航机器人的位置信息来控制跟随机器人的运动状态。基于行为法是通过对各种行为的融合实现机器人的编队。基于图论法是通过图论对编队系统建立模型，随后对编队的机器人系统进行稳定性分析。基于人工势场法是将目标、障碍物与机器人之间看作引力和斥力，通过力的矢量合成来计算机器人的运动方向，进而完成机器人的编队。虚拟结构法将编队系统中的各个机器人视作虚拟的刚体，每个机器人对应各自的位置，来保持机器人的编队。

现在水下机器人的编队系统，一部分大都停留在理论和仿真方面，虽然算法成熟但难于实现；另一部分大都通过缆线实现编队中各个机器人间的高速通信，从而实现机器人间的编队。现在小型机器人的水下编队是一个难点。虽然有不少学者尝试通过其他方法来解决小型机器人水下编队的问题，但现在还没有发展出成熟的方法。

现有水下小型自主航行器/机器人编队存在以下问题：

(1)现有水下小型机器人的编队还没有形成系统的成熟的解决方案，部分采用小型化的水声设备进行编队，但是小型化的水声设备通信不仅价格昂贵、速度慢，而且延迟通信效率低下，不能形成很好的编队效果。

(2)现有的水下机器人编队所利用的定位方法中水声定位和GPS水下定位不能适用于小型的水下机器人设备。

(3)现在部分水下小型机器人的编队，虽然利用视觉，但单目视觉不能直接获得目标的三维信息，单目机器人的图像拼接类算法不仅算法复杂、计算量大而且精度不高，不方便做大规模推广。ToF相机虽然某些特殊的型号可以在水下使用，但是可获得的深度值太小。

(4)现有的双目机器人匹配算法，大多用人采用对目标进行立体匹配的方法，不仅计算量大，而且有许多信息冗余，效率低下，对于一般的嵌入式处理器很难达到实时，所以一般不能应用在机器人的编队中。

(5)现有多机器人编队的体系和控制策略不仅很难在真实的机器人的应用，而且方法复杂不利于实现。

发明内容

针对上述问题，本发明提出一种基于双目视觉的水下机器人系统的领航跟随编队方法，能够有效解决小型水下机器人在水下弱通信环境或无通讯环境中编队困难的问题。

基于双目视觉的水下机器人系统的领航跟随编队方法，所述水下机器人系统包括两个以上设置有双目摄像头的水下机器人，两个以上水下机器人采用串级分层结构，即该串级分层结构的水下机器人系统中上一级的水下机器人是其下一级水下机器人的领航机器人，对应下一级水下机器人是其上一级机器人的跟随机器人；通过跟随机器人对其自身领航机器人的跟随，按照设定的编队队形完成水下机器人系统的编队；

所述跟随机器人对其自身领航机器人的跟随过程为：

步骤一：对领航机器人的目标跟踪

跟随机器人利用双目摄像头对其自身领航机器人进行跟踪拍摄，并将其自身领航机器人在跟踪框的中心点作为目标点，分别得到目标点在双目摄像头的两个摄像机以像素为单位的图像坐标系中的坐标；

步骤二：基于双目视觉对领航机器人的点匹配

通过对领航机器人的目标点进行点匹配，解算目标点在设定的参考坐标系O_wX_wY_wZ_w下相对于跟随机器人的三维坐标(X_w′,Y_w′,Z_w′)；

步骤三：水下跟随控制

由步骤二得到的目标点相对于跟随机器人的三维坐标(X_w′,Y_w′,Z_w′)，得到跟随将机器人与领航机器人之间的距离以及领航机器人与跟随机器人之间在设定的参考坐标系下X_w、Y_w、Z_w三个方向上的夹角α，β，θ，其中：

跟随机器人依据解算出的距离和三个夹角控制其自身的三维运动实现对领航机器人的跟随。

进一步的，所述步骤一中：双目摄像头在对领航机器人进行跟踪时，利用核化相关滤波目标跟踪算法对领航机器人进行跟踪。

进一步的，在对领航机器人跟踪过程中，当领航机器人被遮挡或领航机器人丢失时通过预先训练好的基于可变形部件模型的目标检测算法重新检测领航机器人，发现领航机器人后再通过核化相关滤波目标跟踪算法进行跟踪。

进一步的，所述步骤二中，令O_wX_wY_wZ_w为参考坐标系，O_c1X_c1Y_c1Z_c1为双目摄像头中的左摄像机的相机坐标系，O_c2X_c2Y_c2Z_c2为双目摄像头中的右摄像机的相机坐标系，本方案中令双目摄像头中的左摄像机的相机坐标系作为参考坐标系；

则目标点在每个摄像机中的图像坐标系中的坐标与其在参考坐标系中的坐标的关系为：

上式中：u₁和v₁为目标点在左摄像机以像素为单位的图像坐标系中的坐标；u₂和v₂为目标点在右摄像机以像素为单位的图像坐标系中的坐标；R₁和t₁为左摄像机的外参数，其中R₁为旋转参数，为3×3的矩阵，t₁为平移参数，是三维的平移向量；α_x1、α_y1、u₀₁和v₀₁为左摄像机的内参数；R₂和t₂为右摄像机的外参数，其中R₂为旋转参数，为3×3的正交单位矩阵，t₂为平移参数，是三维的平移向量(矢量)；α_x2、α_y2、u₀₂和v₀₂为右摄像机的内参数；Z_c1′为点M在左摄像机的相机坐标系下Z_c1轴的坐标值；Z_c2′为点M在右摄像机的相机坐标系下Z_c2轴的坐标值；(X_w′,Y_w′,Z_w′)为目标点在参考坐标系中的坐标；

其中两个摄像机的内参数和外参数通过在水下环境对双目摄像机进行标定得到，目标点在两个摄像机图像坐标系中的坐标值由步骤一得到，将上述两式进行联立，可以解出目标点在参考坐标系中的坐标(X_w′,Y_w′,Z_w′)。

有益效果：

(1)本发明根据水下一般的水声效率低，小型机器人无法携带大的水声通信装备，以及水下小型机器人编队的需求等特点，对水下机器人组采用串联分级的编队结构，通过双目摄像机完成对领航机器人的相对定位，根据得到的领航机器人的相对位置坐标，采用领航跟随编队策略完成对小型水下机器人的编队。

(2)针对双目视觉立体匹配存在效率低效问题，本发明采用点匹配的策略，结合检测、识别和跟踪算法，实时得到领航机器人的位置坐标，随后控制策略结合来实现编队。基于双目跟踪、双目点匹配的相对定位和领航跟随控制方案，集成视觉系统和控制系统，跟踪和双目点匹配的方法，不仅速度快，而且精度高，不需要机器人间进行大规模的数据通信即可完成编队。该方法具有结构清晰、分工明确、思路清晰、确实可行的特点。

(3)本发明利用可见光双目摄像机对目标完成点匹配，解决领航机器人水下相对定位问题，采用本发明的方法即能够完成对领航机器人的三维信息的获取，又可以达到实时，与领航跟随算法相结合，从而完成水下小型机器人的编队任务。

(4)本发明的方法可操作性强，基本适用于多种情况下的机器人编队，具有一定的普遍适用性，对于不能进行大规模数据通信和除视觉外没有其他的定位方法的情况下的机器人编队系统具有一定的优势。

附图说明

图1为本发明的水下机器人组的水下领航跟随编队方法的框架图；

图2为双目摄像头的点匹配的示意图(图中o₁xy为以毫米为单位的相机坐标系)；

图3为领航机器人相对于跟随机器人的位置关系示意图；

图4为水下直线编队的运行效果的示意图。

具体实施方式

下面结合附图并举实施例，对本发明进行详细描述。

本实施例以小型的仿生两栖球形机器人(仿生两栖球形机器人在水下通过四个喷水电机的喷水对机器人进行驱动，在陆地通过四条腿进行仿生物龟的爬行运动)为平台，提供一种基于双目视觉的小型水下机器人编队系统的领航跟随编队方法，该方法通过双目视觉解决水下机器人的相对定位问题，以点匹配解决双目立体匹配计算量大、复杂、帧速慢等问题，利用改进的领航跟随编队的控制方案，解决在机器人实际应用中，编队困难的问题。

小型水下机器人编队系统由多个小型的仿生两栖球形机器人构成，且该编队系统采用串级结构，即该编队系统中上一级的机器人是其下一级机器人的领航机器人，则对应下一级机器人是其上一级机器人的跟随机器人，通过不断的串级实现整个系统的编队。如编队系统中串联三个小型的仿生两栖球形机器人，按串联顺序依次为机器人A、机器人B和机器人C，在机器人A是机器人B的领航机器人(对应机器人B是机器人A的跟随机器人)，机器人B是机器人C的领航机器人(对应机器人C是机器人B的跟随机器人)。

该编队方法的总体思路为：在编队系统中，跟随机器人通过视觉系统实现对其自身领航机器人的实时位置的获取，即通过双目视觉对其自身领航机器人进行目标跟踪，跟随机器人依据跟踪的位置信息通过双目摄像机的点匹配得到领航机器人的三维相对坐标，完成对领航机器人的相对定位，跟随机器人通过得到的三维相对坐标，计算得到领航机器人与其在各个方向的偏角和距离，然后跟随机器人的控制单元依据得到的偏角和距离对其自身进行三维运动控制，从而实现对领航机器人的编队跟随。即本方法是以领航跟随的编队控制策略实现跟随机器人对领航机器人的水下编队为导向来进行工作。

基于此，本发明的步骤包括：对领航机器人的目标跟踪、基于双目视觉水下多机器人的相对定位和水下编队控制，该方法的框架如图1所示。

步骤一：对领航机器人的目标跟踪：

跟随机器人利用安装在其壳体上的双目摄像头对其自身领航机器人进行跟踪拍摄，并将其自身领航机器人在跟踪框的中心点作为目标点，分别得到目标点在两个摄像机以像素为单位的图像坐标系中的坐标。

双目摄像头在对领航机器人进行跟踪时，利用核化相关滤波目标跟踪算法对目标(领航机器人)进行跟踪，核化相关滤波目标跟踪算法是通过在线训练学习进行参数的更新。

但在跟踪过程中可能遇到目标被遮挡，目标突然快速移动造成目标跟丢，使训练样本被污染，因此在对目标进行跟踪时将核化相关滤波目标跟踪算法与基于可变形部件模型的目标检测算法相结合，当目标被遮挡或目标丢失时通过预先训练好的基于可变形部件模型的目标检测算法重新检测目标，发现目标后再进行跟踪，由此既避免检测算法的花费时间较长，又可以防止跟随机器人的双目视觉系统跟丢领航机器人。

步骤二：基于双目视觉对领航机器人的点匹配

双目摄像头是根据目标在两个摄像机的位置进行几何的运算来实现对目标的三维定位。由于在机器人编队系统中，对跟随机器人来说只需知道其对应的领航机器人的相对其的三维坐标即可以实现在编队中的运动控制，所以在本步骤中通过对领航机器人的目标点进行点匹配，来解算出领航机器人的目标点相对于跟随机器人的三维坐标。

如图2所示，其中O_wX_wY_wZ_w为参考坐标系，O_c1X_c1Y_c1Z_c1为双目摄像头中的左摄像机的相机坐标系，O_c2X_c2Y_c2Z_c2为双目摄像头中的右摄像机的相机坐标系，本方案中令双目摄像头中的左摄像机的相机坐标系作为参考坐标系，即坐标系O_wX_wY_wZ_w和坐标系O_c1X_c1Y_c1Z_c1重合。

令点M为目标点，则该目标点在每个摄像机中的图像坐标系中的坐标与其在参考坐标系中的坐标的关系为：

上式中：u₁和v₁为点M在左摄像机以像素为单位的图像坐标系中的坐标；u₂和v₂为点M在右摄像机以像素为单位的图像坐标系中的坐标；R₁和t₁为左摄像机的外参数，其中R₁为旋转参数，为3×3的矩阵，t₁为平移参数，是三维的平移向量；α_x1、α_y1、u₀₁和v₀₁为左摄像机的内参数；R₂和t₂为右摄像机的外参数，其中R₂为旋转参数，为3×3的正交单位矩阵，t₂为平移参数，是三维的平移向量(矢量)；α_x2、α_y2、u₀₂和v₀₂为右摄像机的内参数；Z_c1′为点M在左摄像机的相机坐标系下Z_c1轴的坐标值；Z_c2′为点M在右摄像机的相机坐标系下Z_c2轴的坐标值；(X_w′,Y_w′,Z_w′)为目标点M在参考坐标系中的坐标值，即在左摄像机的相机坐标系中的坐标值。

其中两个摄像机的内参数和外参数通过在水下环境对双目摄像机进行标定得到，目标点在两个摄像机图像坐标系中的坐标值由步骤一得到，将上述两式进行联立，可以唯一的解出目标点在左摄像机的相机坐标系中的坐标值(X_w′,Y_w′,Z_w′)，该坐标值即为领航机器人相对跟随机器人的相对三维坐标。

通过双目视觉的点匹配运算相对传统双目立体匹配运算来说降低了运算的复杂度，从而能够提高运算的速度。且本方法中所有的点匹配是根据双目摄像机中的两个摄像头分别利用核跟踪算法跟踪到的位置，相比于双目立体匹配后在寻找目标的位置的方法精度更高。

步骤三：水下跟随控制

对于水下机器人编队的控制策略问题，由上述步骤二跟随机器人得到领航机器人的相对三维坐标(X_w′,Y_w′,Z_w′)，可由相对三维坐标求出领航机器人相对于跟随机器人的位置关系，如图3所示，跟随将机器人与领航机器人之间的距离为领航机器人与跟随机器人之间在三个方向X_c1、Y_c1、Z_c1上的夹角分别是α，β，θ，其中，

跟随机器人依据解算出的距离和三个夹角控制其自身的三维运动，本方案采用PID控制来实现跟随机器人的三维运动控制，具体为：通过夹角α来进行跟随机器人航向的控制，通过夹角β进行跟随机器人深度方向的控制，通过夹角θ进行跟随机器人前进方向的控制，通过d_l进行跟随机器人航行速度的控制，进而控制跟随机器人与领航机器人之间的距离。由此跟随机器人通过控制自身的三维运动来实现对领航机器人的跟随编队，随后该跟随机器人作为下一级机器人的领航机器人对下一级机器人进行领航，这样不断的进行，从而完成整个机器人系统的编队。本方案中将双目点匹配与跟随控制结合后可以达到15fps左右，可以达到实时，满足编队系统中机器人的实时运动。

机器人的编队是通过控制编队系统中各个机器人的运动，让整个机器人编队系统在运动中保持一定的形状，通过多机器人的编队协作可以提高机器人的工作范围和工作效率，可以进行功能上的互补等，常见的机器人编队有直线型，三角形等。如图4所示是以小型两栖机器人为平台进行水下直线编队的运行效果的示意图。

在编队跟随控制中通过串级的分层的结构体系完成整个系统的架构，即上一级的跟随机器人是下一级的领航机器人，实际最终只有一个领航者，即串联结构最顶层的机器人，通过后续层级的机器人不断的领航跟随来实现整个系统的编队。

综上所述，以上仅为本发明的较佳实施例而已，并非用于限定本发明的保护范围。凡在本发明的精神和原则之内，所作的任何修改、等同替换、改进等，均应包含在本发明的保护范围之内。

Claims (4)

1.基于双目视觉的水下机器人系统的领航跟随编队方法，其特征在于：所述水下机器人系统包括两个以上设置有双目摄像头的水下机器人，两个以上水下机器人采用串级分层结构，即该串级分层结构的水下机器人系统中上一级的水下机器人是其下一级水下机器人的领航机器人，对应下一级水下机器人是其上一级机器人的跟随机器人；通过跟随机器人对其自身领航机器人的跟随，按照设定的编队队形完成水下机器人系统的编队；

所述跟随机器人对其自身领航机器人的跟随过程为：

步骤一：对领航机器人的目标跟踪

跟随机器人利用双目摄像头对其自身领航机器人进行跟踪拍摄，并将其自身领航机器人在跟踪框的中心点作为目标点，分别得到目标点在双目摄像头的两个摄像机以像素为单位的图像坐标系中的坐标；

步骤二：基于双目视觉对领航机器人的点匹配

通过对领航机器人的目标点进行点匹配，解算目标点在设定的参考坐标系O_wX_wY_wZ_w下相对于跟随机器人的三维坐标(X_w′,Y_w′,Z_w′)；

步骤三：水下跟随控制

由步骤二得到的目标点相对于跟随机器人的三维坐标(X_w′,Y_w′,Z_w′)，得到跟随将机器人与领航机器人之间的距离以及领航机器人与跟随机器人之间在设定的参考坐标系下X_w、Y_w、Z_w三个方向上的夹角α，β，θ，其中：

跟随机器人依据解算出的距离和三个夹角控制其自身的三维运动实现对领航机器人的跟随。

2.如权利要求1所述的基于双目视觉的水下机器人系统的领航跟随编队方法，其特征在于，所述步骤一中：双目摄像头在对领航机器人进行跟踪时，利用核化相关滤波目标跟踪算法对领航机器人进行跟踪。

3.如权利要求2所述的基于双目视觉的水下机器人系统的领航跟随编队方法，其特征在于，在对领航机器人跟踪过程中，当领航机器人被遮挡或领航机器人丢失时通过预先训练好的基于可变形部件模型的目标检测算法重新检测领航机器人，发现领航机器人后再通过核化相关滤波目标跟踪算法进行跟踪。

4.如权利要求1所述的基于双目视觉的水下机器人编队系统的领航跟随编队方法，其特征在于：所述步骤二中，令O_wX_wY_wZ_w为参考坐标系，O_c1X_c1Y_c1Z_c1为双目摄像头中的左摄像机的相机坐标系，O_c2X_c2Y_c2Z_c2为双目摄像头中的右摄像机的相机坐标系，本方案中令双目摄像头中的左摄像机的相机坐标系作为参考坐标系；

则目标点在每个摄像机中的图像坐标系中的坐标与其在参考坐标系中的坐标的关系为：

上式中：u₁和v₁为目标点在左摄像机以像素为单位的图像坐标系中的坐标；u₂和v₂为目标点在右摄像机以像素为单位的图像坐标系中的坐标；R₁和t₁为左摄像机的外参数，其中R₁为旋转参数，为3×3的矩阵，t₁为平移参数，是三维的平移向量；α_x1、α_y1、u₀₁和v₀₁为左摄像机的内参数；R₂和t₂为右摄像机的外参数，其中R₂为旋转参数，为3×3的正交单位矩阵，t₂为平移参数，是三维的平移向量(矢量)；α_x2、α_y2、u₀₂和v₀₂为右摄像机的内参数；Z_c1′为点M在左摄像机的相机坐标系下Z_c1轴的坐标值；Z_c2′为点M在右摄像机的相机坐标系下Z_c2轴的坐标值；(X_w′,Y_w′,Z_w′)为目标点在参考坐标系中的坐标；

其中两个摄像机的内参数和外参数通过在水下环境对双目摄像机进行标定得到，目标点在两个摄像机图像坐标系中的坐标值由步骤一得到，将上述两式进行联立，可以解出目标点在参考坐标系中的坐标(X_w′,Y_w′,Z_w′)。