CN113525631B - 一种基于光视觉引导的水下终端对接***及方法 - Google Patents

Abstract

本发明公开了一种基于光视觉引导的水下终端自主对接***及方法。该***包括水下对接坞站和用于对接的自主水下机器人；水下对接坞站采用框架式构形，并在其特定位置安装六个标识光源构建三维引导灯阵；水下机器人除了具备自主航行能力外，还配备有水下摄像头和图像处理单元，用以采集水下图像并做处理。水下终端自主对接方法包括水下图像处理方法和分阶段对接控制方法。图像处理方法根据摄像头采集到的图像信息解算出机器人与对接坞站的相对位姿量，并以此推算机器人的位置跟踪偏差。基于不同距离下摄像头拍摄引导灯阵得到的不同结果，设计了远距离、近距离分阶段对接控制方法，采用不同的对接控制策略，实现对机器人的入坞控制。

Description

一种基于光视觉引导的水下终端对接***及方法

技术领域

本发明涉及海洋工程与技术领域，涉及到自主水下机器人的自主对接回收领域，更具体的，涉及一种基于光视觉引导的水下终端对接***及方法。

背景技术

自主水下机器人(Autonomous Underwater Vehicle：AUV)具有自动化程度高、灵活性好、作业时间长等特点，但是在使用过程中也面临很多实际问题。这些高度自动化的设备通常携带的能源有限，在执行完任务甚至在执行任务的过程中就有能源补充的需求，此外还需要适时进行数据交互、设备检修等，因此需要在合适的时间对AUV进行回收。其中水下自主对接回收技术是当前研究的一个热点。

水下机器人的自主对接回收一般分为两个阶段：1)AUV从较远处返航到接近回收坞站的特定区域为远距离导引阶段，该阶段通常采用声学和组合导航的方式来进行引导；2)当AUV到达对接地点附近，从对接初始位置区域到完成对接为近距离导引阶段(或称终端对接阶段)，该阶段需要获取AUV与对接坞站之间较高精度的相对位姿关系，主要依赖于声学引导、电磁引导和视觉引导。随着近年来图像传感器和计算机视觉技术的不断发展，基于机器视觉的目标定位算法在水下终端自主对接回收中得到越来越多的研究和应用。

目前采用视觉引导的水下对接方案主要有纯视觉引导、单目单灯引导、单目多灯引导、双目引导等。纯视觉引导对水质要求较高，引导距离相对较短，环境适应性较差；同时由于坞站没有布置主动引导光源，水下机器人不易对坞站目标进行捕获。单目单灯引导由于无法获取距离信息，只能通过水平偏角和垂直偏角引导，不能保证机器人入坞时的朝向和坞站的开口朝向一致，碰撞风险较大，对接成功率相对较低。此外，现有单目多灯的引导方案中，引导光源基本都布置在坞站开口的同一平面上，由于摄像头视场有限的原因，在距离很近(约1-2米范围内)时摄像头将丢失引导光源，从而使得最后阶段无法纠偏，不利于对接。而双目***本身相对复杂，加之水下成像质量较差导致视差图求解精度不高，也具有一定的应用局限性。

发明内容

基于上述背景和现有方法面临的一些问题，本发明提供了一种基于光视觉引导的水下终端自主对接回收***及方法。本发明考虑了仅仅依靠二维航向引导导致的对接成功率低、近距离下摄像机视场受限以及由于水下机器人的三维运动带来的水下图像中坞站旋转等实际工程问题，为自主水下机器人的自动化回收提供了一套符合海洋工程实践、自动化程度较高的水下终端自主对接回收方案，旨在基于水下对接坞站和自主水下机器人***，通过图像处理方法和远距离、近距离分阶段引导控制方法，实现基于光视觉引导的水下机器人终端自主对接。

为了实现上述目的，按照本发明的一个方面，提供了一套基于光视觉引导的水下终端对接回收***，包括：专门设计的配置有三维引导灯阵的水下对接坞站、自主水下机器人。

水下对接坞站采用框架式设计，并在其特定位置安装了六个被人为标号了的标识光源构建了三维引导灯阵，其中五个分布于坞站正面的边缘(最上边缘的中间为0号光源，左边缘中间为1号光源，右边缘中间为2号光源，左边缘下方为3号光源，右边缘下方为4号光源)；一个位于坞站后面支架的中间(此为5号光源)。这种不共面的灯阵布置方案是为了对水下机器人进行远距离、近距离分阶段的视觉引导，以提高对接成功率。水下坞站以底面平行与水面的姿态放置于水中合适位置，其坐标系以5号光源为坐标原点，Z轴垂直指向坞站开口的正前方，X轴水平向右，Y轴垂直向下，该坐标系为世界坐标系{Q_w-X_w-Y_w-Z_w}。所有标识光源由防水型高亮度LED灯构成，以尽可能增加引导距离。水下对接坞站的具体尺寸已知，各个标识光源在世界坐标系下的坐标也是已知量。

该***中的自主水下机器人为自主式过驱动(水平面)机器人，其在水平面以“X”形式对称配置了四个同型号推进器，可实现平移和转向运动，垂直方向配置一个推进器实现升沉运动。该水下机器人具备自主航行能力，同时，为了实现基于视觉引导的水下对接任务，还具备水下摄像头和图像处理子***。水下摄像头用以拍摄水下机器人前进方向上的视频画面，相机坐标系{O_c-X_c-Y_c-Z_c}规定如下：Z轴沿相机光轴指向正前方，X轴垂直于光轴指向相机右侧，Z轴垂直于光轴向下。图像处理子***负责对摄像头采集到的水下图像进行处理，以判断水下机器人是否成功捕获到对接坞站，如果捕获到坞站图像，则进一步解算出坞站和水下机器人之间的相对位姿关系，从而引导机器人实现自主对接。

为实现上述目的，按照本发明的另一个方面，提供了一种基于光视觉引导的水下终端自主对接方法。包括水下图像处理方法和远距离、近距离分阶段对接控制方法。图像处理方法负责对摄像头采集到的图像信息进行处理，提取出标识光源信息，并根据此解算出机器人与水下坞站的相对位姿量，进一步推算机器人的位置跟踪偏差。基于不同距离下摄像头拍摄引导灯阵得到的不同结果，设计了远距离、近距离分阶段对接控制方法，采用不同的对接控制策略，实现对机器人的入坞控制。

进一步地，所述图像处理方法具体步骤如下：

a)获取图像，通过图像预处理进行降噪并增强图像特征；

b)根据相机离线标定得到的相机内参数和畸变系数对图像进行去畸变处理；

c)对三维引导灯阵的各个标识光源的重心坐标进行提取，表示为(u_i，v_i)，i＝0，1...5；

d)距离较远时，摄像头能够拍摄到完整的灯阵，图像中包含所有标识光源，此时通过光源一致性匹配算法实现图像中的光源坐标(2D点)和坞站上的实际光源坐标(3D点)一一匹配。当距离较近时不能拍摄到完整灯阵，只能拍摄到5号光源，则跳过一致性匹配的处理，将此时5号光源的重心坐标记录为(u_c，v_c)。

e)当图像中检测到完整灯阵时，根据光源一致性匹配算法的匹配结果并结合EPnP算法解算出水下机器人和坞站的相对位姿，即相机坐标系相对于世界坐标系的旋转矩阵和平移向量，其中平移向量被表示为T＝[x_t y_t z_t]^T。当近距离下只能拍摄到5号光源时，根据下式解算出对接坞站相对于机器人的水平偏角ψ₁和垂直偏角ψ₂：

其中c_x、c_y为相机内参数，θ_hor为相机水平开角，θ_ver为相机垂直开角。W_img为图像宽度，H_img为图像高度。

进一步地，所述分阶段对接控制方法分为远距离引导控制和近距离引导控制两个阶段。

a)在距离较远时，摄像头能捕获到完整灯阵，此时能解算出三维相对位姿，在该阶段，将自主机器人的对接控制问题转化为水下三维路径跟踪问题，跟踪参考路径即为世界坐标系的Z_w轴，方向为Z_w轴负方向。通过这种控制方式，不仅能够进行航向纠正，同时也能使得水下机器人与坞站开口中心线的横向偏距趋近于零，使得在对接的最后阶段，水下机器人的姿态更利于对接，提高对接成功率。同时，为了简化控制器设计，将水平面控制和垂直面控制相互解耦。

垂直面控制：使用PI-D控制器进行机器人深度的闭环控制，深度误差：z_e＝y_t。

水平面控制：解耦后的水平面控制问题为二维路径跟踪问题，引入视线制导率(Line of Sight：LOS)进行水下机器人的航向引导。横向偏距y_e＝x_t，制导角为：

其中Δ为前视距离，是一个可调参数。根据水下机器人导航***给出的航向信息ψ(机器人航向以顺时针为正)和图像处理阶段得到的相对位姿关系，可计算得到-Z_w轴(期望路径)的航向，定义为ψ_r，则期望航向：ψ_d＝ψ_r+ψ_los，航向偏差：

ψ_e＝ψ_d-ψ＝ψ_r+ψ_los-ψ

采用PI-D控制器进行航向闭环控制。

b)距离较近时，通过单个光源(5号光源)进行引导，此时无法准确计算出相机和坞站的三维位姿，只能获取坞站中心相对于机器人摄像头的两个位置偏角，即ψ₁和ψ₂。

水平面控制：采用和远距离控制时一样的控制器和参数，此时航向偏差：ψ_e＝ψ₁。

垂直面控制：此时实际的深度误差无法准确度量，可根据垂直偏角ψ₂对深度进行微调，采用比例控制器。

进一步地，图像处理方法步骤(d)中光源一致性匹配算法的具体实现如下：1)求解对接坞站在图像中相对于像素坐标系的旋转角度，根据这个角度对提取的所有标识光源重心坐标进行反向旋转，得到新的重心坐标，以此补偿机器人横滚运动造成的坞站在图像中的旋转问题。2)对旋转后得到的重心坐标点按照v值的大小进行升序排列；3)v值最小的点对应0号光源；4)找出v值最大的两个点，然后比较其u的大小，u值小的对应3号光源，大的对应4号光源；5)对于剩下的三个重心点，比较u的大小，u值最小的点对应1号光源，最大的点对应2号光源，最后剩下的一个点对应5号光源。通过该标识光源一致性匹配算法可以将图像中的光斑重心点和坞站上实际安装的标识光源一一对应起来。

总体而言，本发明所构思的水下终端自主对接***和方法与现有技术相比，能够取得如下有益效果：

1.本发明设计的水下对接坞站，其三维引导灯阵中将标识光源布置为左右对称而上下不对称的模式，这有利于通过图像处理方法解算坞站在画面中的旋转角度并对其进行补偿，从而提高光源一致性匹配算法的准确性和鲁棒性。另外，将标识光源分布在前后两个平面，可以使得在近距离情况下机器人搭载的摄像头仍然可以捕获5号光源，从而获取最后阶段的引导信息。

2.与坞站设计思想一致，在较远距离时，摄像头能够捕获完整灯阵，此时通过EPnP算法计算水下机器人和坞站的三维相对位姿；而在距离很近的情况下，摄像头只能捕获5号光源，此时通过图像处理方法来计算坞站相对于水下机器人的水平偏角和垂直偏角。通过坞站的特殊设计，并结合不同的图像处理策略，使得水下机器人在由远及近的整个对接过程都能获得运动引导信息。这相比于很多现有的在最末端无法引导的技术方案有明显优势。

3.本发明所提出的分阶段对接控制方法，远距离时在基于视觉的三维相对位姿解算结果下，将水下对接问题转化为水下机器人的三维路径跟踪问题，并将其解耦为水平面和垂直面控制，降低了对接控制的难度。其次，在水平面控制中，结合视线制导率，将水下机器人逐步引导至坞站开口的中心线上，即水下机器人的航向尽量正对坞站开口朝向，这相比与单目单灯引导中纠正航向而不是纠正横向偏距的控制方案能更好地提高对接成功率，并降低水下机器人和坞站的碰撞程度。

附图说明

图1为对接***及相对位姿关系示意图；

图2为图像处理方法流程图；

图3为单个光源引导下的偏角示意图(由单个光源引导时水平偏角和垂直偏角示意图)；

图4为分阶段对接控制方法流程图；

图5为标识光源一致性匹配算法流程图；

图6为标识光源提取与一致性匹配后的结果图。

具体实施方式

为了使本发明的目的、技术方案及优点更加明晰，以下结合附图及实例，对本发明进行进一步详细解释说明。应当理解，此处所描述的具体实施例仅用以解释本发明，并不用于限定发明。

为了实现上述目的，按照本发明的一个方面，提供了一套基于光视觉引导的水下终端对接回收***，包括：专门设计的配置有三维引导灯阵的水下对接坞站、自主水下机器人。

如图1所示，水下对接坞站采用框架式设计，并在其特定位置安装了六个被人为标号了的标识光源构建了三维引导灯阵，其中五个光源分布于坞站正面的边缘(最上边缘的中间为0号光源，左边缘中间为1号光源，右边缘中间为2号光源，左边缘下方为3号光源，右边缘下方为4号光源)；一个位于坞站后面支架的中间处(此为5号光源)。这种不共面的灯阵布置方案是为了对水下机器人进行远距离、近距离分阶段的视觉引导，以提高对接成功率。水下坞站以底面平行与水面的姿态放置于水中合适位置，其坐标系以5号光源为坐标原点，Z轴垂直指向坞站开口的正前方，X轴水平向右，Y轴垂直向下，该坐标系为世界坐标系{O_w-X_w-Y_w-Z_w}。坞站上的所有标识光源由防水型高亮度LED灯构成，结合水体对不同波长光线散射和吸收的特性，选用蓝绿色光源以尽可能增加在水下的穿透距离。水下对接坞站的具体尺寸已知，各个标识光源在世界坐标系下的坐标也是已知量。

该***中的自主水下机器人为自主式过驱动(水平面)机器人，其在水平面以“X”形式对称配置了四个同型号推进器(#1～#4)，可实现平移和转向运动，垂直方向配置一个推进器(#5)实现升沉运动。开发了自动控制***使该水下机器人具备自主航行能力，同时，为了实现基于光视觉导引的水下对接任务，还配备了水下摄像头和图像处理子***。水下摄像头用以拍摄水下机器人前进方向上的视频画面，为了获得较大的视野，采用广角摄像头，相机坐标系{O_c-X_c-Y_c-Z_c}规定如下：Z轴沿相机光轴指向机器人的正前方，X轴垂直于光轴指向相机右侧，Z轴垂直于光轴向下。图像处理子***通过相关算法对摄像头采集到的水下图像进行处理，以判断水下机器人是否成功捕获到坞站上的引导光源，如果捕获到引导光源图像，则进一步解算出坞站和水下机器人之间的相对位姿关系，从而引导机器人实现自主对接。

为实现上述目的，按照本发明的另一个方面，提供了一种基于光视觉引导的水下终端自主对接方法。包括水下图像处理方法和远距离、近距离分阶段对接控制方法。图像处理方法负责对摄像头采集到的图像进行处理，提取出标识光源信息，并根据此解算出机器人与水下坞站的相对位姿量，进一步推算机器人的位置跟踪偏差。

如图2所示，所述图像处理方法具体步骤如下：

a)水下机器人中的图像处理子***通过艇内局域网获取网络摄像头拍摄的视频流，然后从视频流中提取图像帧，由于水下图像噪点较多，故通过高斯滤波进行图像降噪，并通过拉普拉斯算子运算增强图像特征；

b)(***工作前需要对机器人的摄像头进行水下标定)根据相机离线标定得到的相机内参数和畸变系数对图像帧进行去畸变处理，在这一步中保留原图中的所有像素，并且无需对矫正后的图像进行裁剪缩放，以尽可能保证图像中像素点的真实位置关系；

c)对三维引导灯阵的各个标识光源的重心坐标进行提取，表示为

在这一步中，首先对上述处理后的图像进行固定阈值分割，得到标识光源对应的光斑区域，然后通过形态学开运算去除可能存在的杂质区域，接着采用Canny边缘检测算子提取图像中各光斑的边缘信息，最后通过灰度质心法计算各个光斑的重心坐标，此即光源在图像中的重心坐标。灰度质心法计算公式如下：

其中用I(u，v)表示(u，v)处像素点的强度值，m，n分别表示u，v方向像素的数量，

为所求质心坐标；

d)距离较远时，摄像头能拍摄到完整的引导灯阵，此时需要通过光源一致性匹配算法实现图像中的光源重心坐标(2D点)和坞站上的实际光源坐标(3D点)一一匹配，即找出坞站上1～5号标识光源分别对应图像中的哪个光斑。当距离很近时(约距坞站1～2米)由于摄像头视角的限制，只能拍摄到5号光源，此时无需一致性匹配，将捕获到的5号光源的重心坐标记录为(u_c，v_c)。

e)当图像中检测到完整灯阵时，根据光源一致性匹配算法的匹配结果(即上述2D-3D匹配点对)并结合n点透射问题的任何一种有效解法(如EPnP算法)，解算出水下机器人和坞站的三维相对位姿，即相机坐标系{O_c-X_c-Y_c-Z_c}相对于世界坐标系{O_w-X_w-Y_w-Z_w}的旋转矩阵和平移向量，其中平移向量表示为T＝[x_t y_t z_t]^T。当近距离下只能拍摄到5号光源时，如图3所示，灰色矩形区域为摄像头拍摄的画面，其中白色斑点表示5号标识光源的成像区域，坐标为(u_c，v_c)，o′为光轴与成像平面的交点，坐标为(c_x，c_y)，通过相机标定可得。∠o′OB为水平偏角ψ₁，∠o′OA为垂直偏角ψ₂，根据几何比例关系和摄像头的开角信息，得到这两个偏角的计算公式：

θ_hor为相机水平开角，θ_ver为相机垂直开角。W_img为图像宽度，H_img为图像高度。

进一步地，所述分阶段对接控制方法分为远距离引导控制和近距离引导控制两个阶段。根据图像处理阶段得到的不同结果，自动采用不同的对接控制策略，实现对机器人的入坞控制。

1.在距离较远时，摄像头能捕获到完整的引导灯阵，此时通过图像处理能解算出三维相对位姿。如图1所示，在该阶段，将自主机器人的对接控制问题转化为水下三维路径跟踪问题，跟踪参考路径即为世界坐标系的Z_w轴，方向为Z_w轴负方向。通过这种控制方式，不仅能够进行单纯的航向纠正，同时也能使得水下机器人与坞站开口中心线的横向偏距趋近于零，使得在对接的最后阶段，水下机器人的朝向与坞站开口方向一致，有利于机器人入坞。同时，为了简化控制器设计，将水平面控制和垂直面控制相互解耦。

垂直面控制：使用PI-D控制器进行机器人深度的闭环控制。摄像头的安装位置基本位于机器人垂直面的中间处，所以摄像头与坞站在垂直面的位置关系可以直接反映水下机器人与坞站在垂直面的位置关系。此时输入控制器的深度误差为：z_e＝y_t。控制器输出为垂直面推进器的转速(n₅)。

水平面控制：解耦后的水平面控制问题为二维路径跟踪问题，在此引入视线制导率(Line of Sight：LOS)进行制导控制。如图1所示，灰色矩形为坞站中心线所在的水平面，将机器人重心投影到该平面(M点)，

为机器人航向，MB为机器人与坞站中心线的横向偏距y_e＝x_t，制导角为：

其中Δ为前视距离(即AB)。根据水下机器人导航***给出的航向信息ψ(机器人航向以顺时针为正)和图像处理阶段得到的相对位姿关系，可计算得到Z_w轴负方向(期望路径)的航向，定义为ψ_r，则期望航向：ψ_d＝ψ_r+ψ_los，航向偏差：

ψ_e＝ψ_d-ψ＝ψ_r+ψ_los-ψ

采用PI-D控制器进行航向闭环控制。控制器输出转向扭矩，进一步通过推力分配算法和推进器推力曲线计算得到水平面四个推进器的转速(n₁～n₄)，最后将所有推进器的期望转速下发给转速闭环控制单元，实现对机器人的对接控制。

2.距离很近时，摄像头只能拍摄到单个光源(5号光源)，此时为单目单灯引导，无法计算出相机和坞站的三维位姿，只能获取坞站中心相对于机器人摄像头的水平偏角ψ₁和垂直偏角ψ₂。另外，通过前期的三维引导控制，水下机器人的位姿已经经过了较长时间的调整，在进入近距离引导阶段时，二者的相对位姿已经处于相对较好的状态，单灯引导即可纠正最后阶段的小偏差。在这种前提下，水平面控制：采用和远距离控制时一样的控制器和控制参数，只是此时航向偏差：ψ_e＝ψ₁。垂直面控制：此时实际的深度误差无法准确度量，可根据垂直偏角ψ₂对深度进行微调，采用比例控制器。

根据上述分阶段对接控制策略，图4给出了完整的对接控制流程图。根据拍摄到的引导光源的不同结果，自动切换控制阶段，同时选择对应的控制方法。由于控制的超调或外界扰动等原因，可能导致摄像头航向波动过大，丢失引导目标，此时引入目标丢失计数器T，如果引导光源在视野中连续丢失次数超过规定阈值T_max，则认为当前对接任务失败，退出对接控制循环。此外，每次捕获到引导光源时，都会将丢失计数器清零，下次丢失目标时重新开始累计。在控制量的解算时，考虑到可能出现的误检测和数据波动，引入限幅滤波和滑动滤波器，以获得更加稳定的相对位姿量。

进一步地，图像处理方法步骤(d)中光源一致性匹配算法的具体实现流程和判断逻辑如图5所示：1)首先判断是否提取到所有标识光源，如果是，则进行一致性匹配处理，否则跳过；2)通过最小矩形法求解对接坞站在图像中相对于像素坐标系的旋转角度，根据这个角度对提取的所有标识光源重心坐标进行反向旋转，得到新的重心坐标，以此补偿机器人横滚运动造成坞站在图像中的旋转问题；3)对旋转后得到的重心坐标点按照v值的大小进行升序排列；4)v值最小的点对应0号光源；5)找出v值最大的两个点，然后比较其u的大小，u值小的对应3号光源，大的对应4号光源；6)对于剩下的三个重心点，比较u的大小，u值最小的点对应1号光源，最大的点对应2号光源，最后剩下的一个点对应5号光源；7)在判断出光源标号后，映射回旋转前的坐标，从而完成匹配过程。通过该标识光源一致性匹配算法可以将图像中的光斑重心点和坞站上实际安装的标识光源一一对应起来。如图6所示为光源一致性匹配的结果示例，可以看出，通过本算法的处理，图像中的光源得到正确的匹配。

至此，基于光视觉引导的水下终端对接***和方法的实施例结束。

注：该发明中对接控制方法中的图像处理是实时进行的，且满足对接控制算法对控制周期的要求。

以上描述了本发明的基本原理、主要特征和本发明的优点。本行业的技术人员应该了解，本发明不受上述实施例的限制，上述实施例和说明书中描述的只是说明本发明的原理，在不脱离本发明精神和范围的前提下，本发明还会有各种类似的变化和改进，这些变化和改进都落入要求保护的本发明范围内，本发明要求保护范围由所附的权利要求书及其等效物界定。

Claims (4)

1.一种基于光视觉引导的水下终端自主对接***，用以实现水下机器人的自主对接回收，其特征在于，包括：水下对接坞站、自主水下机器人；

a)水下对接坞站采用框架式设计，并在其特定位置安装了六个被人为标号了的标识光源构建了三维引导灯阵，其中五个分布于坞站正面的边缘，上边缘的中间为0号光源，左边缘中间为1号光源，右边缘中间为2号光源，左边缘下方为3号光源，右边缘下方为4号光源，一个位于坞站后面支架的中间为5号光源；水下对接坞站以底面平行于水面的姿态放置于水中，其坐标系以5号光源为坐标原点，Z轴垂直指向坞站开口的正前方，X轴水平向右，Y轴垂直向下，该坐标系为世界坐标系{O_w-X_w-Y_w-Z_w}；所有标识光源由防水型高亮度LED灯构成，以尽可能增加引导距离；水下对接坞站的具体尺寸已知，各个标识光源在世界坐标系下的坐标也是已知量；

b)该***中的自主水下机器人为自主式过驱动机器人，其在水平面以“X”形式对称配置了四个同型号推进器，可实现平移和转向运动，垂直方向配置一个推进器实现升沉运动；该自主水下机器人具备自主航行能力，还具备水下摄像头和图像处理子***；水下摄像头用以拍摄自主水下机器人前进方向上的视频画面，相机坐标系{O_c-X_c-Y_c-Z_c}规定如下：Z轴沿相机光轴指向正前方，X轴垂直于光轴指向相机右侧，Y轴垂直于光轴向下；图像处理子***负责对水下摄像头采集到的水下图像进行处理，以判断自主水下机器人是否成功捕获到水下对接坞站，如果捕获到水下对接坞站图像，则进一步解算出水下对接坞站和自主水下机器人之间的相对位姿关系，从而引导机器人实现自主对接。

2.一种基于权利要求1的基于光视觉引导的水下终端自主对接***的自主对接方法，其特征在于，包括水下图像处理方法和远距离、近距离分阶段对接控制方法，所述图像处理方法具体步骤如下：

a)获取图像，通过图像预处理进行降噪并增强图像特征；

b)根据相机离线标定得到的相机内参数和畸变系数对图像进行去畸变处理；

c)对三维引导灯阵的各个标识光源的重心坐标进行提取，表示为(u_i，v_i)，i＝0，1...5；

d)距离较远时，摄像头能够拍摄到完整的灯阵，图像中包含所有标识光源，此时通过光源一致性匹配算法实现图像中的光源坐标和水下对接坞站上的实际光源坐标一一匹配；当距离很近时不能拍摄到完整灯阵，只能拍摄到5号光源，则跳过一致性匹配的处理，将此时5号光源的重心坐标记录为(u_c，v_c)；

e)当图像中检测到完整灯阵时，根据光源一致性匹配算法的匹配结果并结合EPnP算法解算出水下机器人和坞站的相对位姿，即相机坐标系相对于世界坐标系的旋转矩阵和平移向量，其中平移向量被表示为T＝[x_t y_t z_t]^T；当近距离下只能拍摄到5号光源时，根据下式解算出水下对接坞站相对于水下机器人的水平偏角ψ₁和垂直偏角ψ₂：

其中c_x、c_y为相机内参数，θ_hor为相机水平开角，θ_ver为相机垂直开角，W_img为图像宽度，H_img为图像高度；

基于不同距离下摄像头拍摄引导灯阵得到的不同结果，设计了远距离、近距离分阶段对接控制方法，远距离时基于空间相对位姿关系，将对接控制问题转化为水下三维路径跟踪问题进行解决；近距离时，基于水平方向和垂直方向的偏角信息，对末端对接进行位姿微调控制，通过两个阶段的协同配合最终实现对机器人的入坞控制。

3.如权利要求2所述的自主对接方法，其特征在于，所述分阶段对接控制方法分为远距离引导控制和近距离引导控制两个阶段；

a)在距离较远时，摄像头能捕获到完整灯阵，此时能解算出三维相对位姿，在该阶段，将自主机器人的对接控制问题转化为水下三维路径跟踪问题，跟踪参考路径即为世界坐标系的Z_w轴，方向为Z_w轴负方向，同时，为了简化控制器设计，将水平面控制和垂直面控制相互解耦；

垂直面控制：使用PI-D控制器进行机器人深度的闭环控制，深度误差：z_e＝y_t；

水平面控制：解耦后的水平面控制问题为二维路径跟踪问题，引入视线制导率进行水下机器人的航向引导，横向偏距y_e＝x_t，制导角为：

其中Δ为前视距离，是一个可调参数；根据水下机器人导航***给出的航向信息ψ和图像处理阶段得到的相对位姿关系，可计算得到-Z_w轴的航向，定义为ψ_r，则期望航向：ψ_d＝ψ_r+ψ_los，航向偏差：ψ_e＝ψ_d-ψ＝ψ_r+ψ_los-ψ，同样采用PI-D控制器进行航向闭环控制；

b)距离较近时，通过5号光源进行引导，此时无法准确计算出相机和坞站的三维位姿，只能获取坞站中心相对于机器人摄像头的两个位置偏角，即ψ₁和ψ₂；

水平面控制：采用和远距离控制时一样的控制器和参数，此时航向偏差：ψ_e＝ψ₁；

垂直面控制：此时实际的深度误差无法准确度量，可根据垂直偏角ψ₂对深度进行微调，采用比例控制器。

4.如权利要求3所述的自主对接方法，其特征在于，光源一致性匹配算法的具体实现如下：1)求解对接坞站在图像中相对于像素坐标系的旋转角度，根据这个角度对提取的所有标识光源重心坐标进行反向旋转，得到新的重心坐标，以此补偿机器人横滚运动造成的坞站在图像中的旋转问题；2)对旋转后得到的重心坐标点按照v值的大小进行升序排列；3)v值最小的点对应0号光源；4)找出v值最大的两个点，然后比较其u的大小，u值小的对应3号光源，大的对应4号光源；5)对于剩下的三个重心点，比较u的大小，u值最小的点对应1号光源，最大的点对应2号光源，最后剩下的一个点对应5号光源；通过该标识光源一致性匹配算法可以将图像中的光斑重心点和坞站上实际安装的标识光源一一对应起来。

Images