水下机器人的检测识别方法、装置和计算机存储介质与流程

本发明涉及计算机技术领域，尤其涉及一种水下机器人的检测识别方法、装置和计算机存储介质。

背景技术：

现阶段水下移动机器人(rov、auv等)主要搭载水下声纳系统(包括多种形式的声纳例如成像声纳、多波束声纳、侧扫声纳、前视声纳等)对水下作业周围环境或目标进行感知。

然而声纳系统的存在诸多不足之处，其中最大的问题是成像分辨率低，因而很难有效精确识别较小尺寸的目标，此外声纳系统成本高且功耗高，因而在成本受约束及携带能量十分有限的rov作业任务中不能适用。

因此，现有水下机器人搭载的声纳系统成本功耗高且成像分辨率低的问题。

技术实现要素：

本发明主要目的在于提供一种水下机器人的检测识别方法、装置和计算机存储介质，旨在解决现有水下机器人搭载的声纳系统成本功耗高且成像分辨率低的问题。

为实现上述目的，本发明提供一种水下机器人的检测识别方法，所述水下机器人的检测识别方法包括以下步骤：

接收视觉传感器采集的图像信息；

基于所述图像信息检测目标水下机器人的二维坐标；

将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标；

将所述三维坐标经过平滑滤波处理得到所述目标水下机器人的定位信息。

在一实施例中，所述基于所述图像信息检测目标水下机器人的二维坐标的步骤包括：

确定所述图像信息是否包括目标水下机器人，预设的参考图像包括所述目标水下机器人；

当所述图像信息包括目标水下机器人时，基于所述图像信息确定所述目标机器人的包围矩形盒；

确定所述包围矩形盒在预设的二维图像坐标系下的二维坐标。

在一实施例中，所述确定所述图像信息是否包括目标水下机器人的步骤包括：

将所述图像信息输入目标模型，根据所述目标模型的输出结果确定所述图像信息是否包括目标水下机器人；

所述目标模型根据水下机器人的图像数据和所述水下机器人在水下工作环境的图像数据训练得到，所述水下机器人的图像数据为训练正样本，所述水下工作环境的图像数据为训练负样本。

在一实施例中，所述将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标的步骤包括：

采用预设的参数矩阵将所述包围矩形盒投影至预设的三维空间形成三维立体锥体；

按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标；

在所述三维立体锥体内部搜索所述目标水下机器人的点云团块；

采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块；

利用点云特征提取算法提取所述各个独立的点云团块的三维特征描述子；

利用匹配算法将各个三维特征描述子与目标点云三维特征描述子进行匹配，以得到匹配得分；

将匹配得分最高的点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵；

基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在一实施例中，所述将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标的步骤包括：

采用预设的参数矩阵将所述目标包围矩形盒投影至预设的三维空间形成三维立体锥体；

按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标；

在所述三维立体锥体内部搜索所述目标水下机器人的点云团块；

采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块；

利用点云注册算法将所述各个独立的点云团块与初始标定的目标点云团块进行匹配，得到与所述初始标定的目标云团块匹配的点云团块；

将所述点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵；

基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在一实施例中，所述采用预设的参数矩阵将所述目标包围矩形盒投影至预设的三维空间形成三维立体锥体的步骤之后，还包括：

基于当前时刻所述目标水下机器人的定位结果为中心，沿着所述目标水下机器人的深度方向拓展预设距离，形成梯形锥体；其中，所述梯形锥体深度中心为当前目标的深度坐标值。

在一实施例中，所述确定所述图像信息是否包括目标水下机器人的步骤之后，所述水下机器人的检测识别方法还包括：

在确定所述图像信息不包括目标水下机器人时，以最近一次成功检测的目标水下机器人位置为中心、目标水下机器人的矩形包围盒为基准，以预设尺寸递增的矩形包围盒作为本次检测结果输出；

累积所述目标水下机器人检测丢失帧数；

当所述目标水下机器人检测丢失帧数大于设定阈值时，将所述图像信息作为所述目标水下机器人的包围盒区域；在所述目标包围盒区域中搜索所述目标水下机器人。

在一实施例中，还包括：

当检测到所述目标水下机器人的当前位置与前一帧位置之间的聚类超过设定阈值时，采用最近一次成功检测到的所述目标水下机器人的位置作为本次检测的目标位置；

统计连续发生跳变的帧数；

当所述连续发生跳变的帧数大于设定数目且连续跳变点保持在同一位置附近，将所述连续跳变点作为所述水下机器人位置。

在一实施例中，还包括：

获取当前获取的所述定位信息与初始位置信息之间的距离，其中，所述初始位置信息为首次检测到的所述定位信息；

在所述距离大于预设距离时，根据所述距离大于预设距离的次数；其中，所述距离小于预设阈值范围；

在所述次数大于预设次数时，将所述次数对应的定位信息的平均值作为所述目标机器人的初始位置。

在一实施例中，还包括：

在所述次数小于或等于所述预设次数时，采用所述所述距离对应的定位信息更新所述初始位置信息。

为实现上述目的，本发明还提供一种水下机器人的检测识别装置，所述装置包括存储器、处理器以及存储在所述存储器并可在所述处理器上运行的水下机器人的检测识别程序，所述水下机器人的检测识别程序被所述处理器执行时实现如上所述的水下机器人的检测识别方法的各个步骤。

为实现上述目的，本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有水下机器人的检测识别程序，所述水下机器人的检测识别程序被处理器执行时实现如上所述的水下机器人的检测识别方法的各个步骤。

本发明提供的水下机器人的检测识别方法、装置和计算机存储介质，水下移动机器人将目标机器人三维坐标经过滤波器的平滑滤波处理得到目标水下机器人的定位信息，根据定位信息对目标机器人进行跟踪锁定等操作。具体为，水下移动机器人包括目标跟踪器，目标跟踪器的任务是实时跟踪目标水下机器人的当前时刻位置并预测下一时刻位置，同时还起到对原始检测目标水下机器人位置结果进行滤波平滑作用，最后将目标水下机器人的定位信息发送至rov运动控制器，rov运动控制器根据目标位置变化量来调整自身位姿以实现对目标的实时锁定跟踪。

附图说明

图1是本发明实施例方案涉及的装置的硬件架构示意图；

图2为本发明水下机器人的检测识别方法的第一实施例的流程示意图；

图3为本发明涉及的水下智能一体机示意图；

图4为将水下智能一体机搭载在rov本体上的示意图；

图5为配置卡尔曼滤波器对目标轨迹跟踪及滤波效果示意图；

图6为本发明水下机器人的检测识别方法的第二实施例的流程示意图；

图7为一种常用的小型水下移动机器人rov示意图；

图8为2d图像目标检测结果示意图；

图9为本发明构建的坐标系系统示意图；

图10为本发明第二实施例中步骤s30的具体流程示意图；

图11为本发明第二实施例中步骤s30的具体流程示意图；

图12为点云匹配/注册示意图；

图13为调整固定锥体深度值的示意图；

图14为本发明水下机器人的检测识别方法的第三实施例的流程示意图；

图15为检测丢失处理示意图；

图16为本发明水下机器人的检测识别方法第四实施例的流程示意图；

图17为检测跳变处理示意图；

图18为本发明水下机器人的检测识别方法第五实施例的流程示意图；

图19为目标水下机器人初始位置初始化标定过程示意图；

图20为目标点云模型自动化标定示意图；

图21为完整的目标检测与跟踪系统及算法的示意框图。

具体实施方式

应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。

本发明实施例的主要解决方案是：水下移动机器人将目标机器人三维坐标经过滤波器的平滑滤波处理得到目标水下机器人的定位信息，根据定位信息对目标机器人进行跟踪锁定等操作。具体为，水下移动机器人包括目标跟踪器，目标跟踪器的任务是实时跟踪目标水下机器人的当前时刻位置并预测下一时刻位置，同时还起到对原始检测目标水下机器人位置结果进行滤波平滑作用，最后将目标水下机器人的定位信息发送至rov运动控制器，rov运动控制器根据目标位置变化量来调整自身位姿以实现对目标的实时锁定跟踪。

作为一种实现方式，可以如图1所示，图1是本发明实施例方案涉及的装置的硬件架构示意图。

本发明实施例方案涉及的是音视频文件的处理装置，装置包括：处理器101，例如cpu，存储器102，通信总线103。其中，通信总线103用于实现这些组件之间的连接通信。

存储器102可以是高速ram存储器，也可以是稳定的存储器(non-volatilememory)，例如磁盘存储器。如图1所示，作为一种计算机存储介质的存储器102中可以包括水下机器人的检测识别程序；而处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

接收视觉传感器采集的图像信息；

基于所述图像信息检测目标水下机器人的二维坐标；

将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标；

将所述三维坐标经过平滑滤波处理得到所述目标水下机器人的定位信息。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

确定所述图像信息是否包括目标水下机器人，预设的参考图像包括所述目标水下机器人；

当所述图像信息包括目标水下机器人时，基于所述图像信息确定所述目标机器人的包围矩形盒；

确定所述包围矩形盒在预设的二维图像坐标系下的二维坐标。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

将所述图像信息输入目标模型，根据所述目标模型的输出结果确定所述图像信息是否包括目标水下机器人；

所述目标模型根据水下机器人的图像数据和所述水下机器人在水下工作环境的图像数据训练得到，所述水下机器人的图像数据为训练正样本，所述水下工作环境的图像数据为训练负样本。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

采用预设的参数矩阵将所述包围矩形盒投影至预设的三维空间形成三维立体锥体；

按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标；

在所述三维立体锥体内部搜索所述目标水下机器人的点云团块；

采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块；

利用点云特征提取算法提取所述各个独立的点云团块的三维特征描述子；

利用匹配算法将各个三维特征描述子与目标点云三维特征描述子进行匹配，以得到匹配得分；

将匹配得分最高的点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵；

基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

采用预设的参数矩阵将所述目标包围矩形盒投影至预设的三维空间形成三维立体锥体；

按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标；

在所述三维立体锥体内部搜索所述目标水下机器人的点云团块；

采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块；

利用点云注册算法将所述各个独立的点云团块与初始标定的目标点云团块进行匹配，得到与所述初始标定的目标云团块匹配的点云团块；

将所述点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵；

基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

基于当前时刻所述目标水下机器人的定位结果为中心，沿着所述目标水下机器人的深度方向拓展预设距离，形成梯形锥体；其中，所述梯形锥体深度中心为当前目标的深度坐标值。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

在确定所述图像信息不包括目标水下机器人时，以最近一次成功检测的目标水下机器人位置为中心、目标水下机器人的矩形包围盒为基准，以预设尺寸递增的矩形包围盒作为本次检测结果输出；

累积所述目标水下机器人检测丢失帧数；

当所述目标水下机器人检测丢失帧数大于设定阈值时，将所述图像信息作为所述目标水下机器人的包围盒区域；在所述目标包围盒区域中搜索所述目标水下机器人。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

当检测到所述目标水下机器人的当前位置与前一帧位置之间的聚类超过设定阈值时，采用最近一次成功检测到的所述目标水下机器人的位置作为本次检测的目标位置；

统计连续发生跳变的帧数；

当所述连续发生跳变的帧数大于设定数目且连续跳变点保持在同一位置附近，将所述连续跳变点作为所述水下机器人位置。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

获取当前获取的所述定位信息与初始位置信息之间的距离，其中，所述初始位置信息为首次检测到的所述定位信息；

在所述距离大于预设距离时，根据所述距离大于预设距离的次数；其中，所述距离小于预设阈值范围；

在所述次数大于预设次数时，将所述次数对应的定位信息的平均值作为所述目标机器人的初始位置。

在一实施例中，处理器101可以用于调用存储器102中存储的水下机器人的检测识别程序，并执行以下操作：

在所述次数小于或等于所述预设次数时，采用所述所述距离对应的定位信息更新所述初始位置信息。

基于上述装置的硬件构架，提出本发明水下机器人的检测识别方法的实施例。

参照图2，图2为本发明水下机器人的检测识别方法的第一实施例，所述水下机器人的检测识别方法包括以下步骤：

步骤s10，接收视觉传感器采集的图像信息。

在本实施例中，在作业场所为水下环境尤其是指近海海域水下环境，作业主体为水下移动机器人(remotelyoperationalrobot,以下简称rov)，检测与跟踪的目标包括静止目标或机动目标，rov本体上搭载视觉传感器。

因此，针对rov自身特点设计一款水下智能一体机。设计水下智能一体机的目的是为了让视觉传感器感知信息能够被实时被一体机获取。参照图3，图3为本发明涉及的水下智能一体机示意图。水下一体机主要由一套视觉传感器、一台嵌入式主控计算机、一个锂电池组成，三者被密封在一个水下专用密封舱中，密封舱接口保留了充电接口(便于给锂电池充电)、网络通信接口(与其他设备进行组网数据交互)，。水下智能一体机既能够当作一个独立的设备独立工作运行，也能够方便的集成到rov本体，通过网络接口与rov本体设备组件局域网进行数据交互。

参照图4，图4为将水下智能一体机搭载在rov本体上的示意图；201为rov本体，202为水下智能一体机。一体机通过通信线缆连接到rov主控密封舱，一体机和rov本体组建局域网。

水下移动机器人接收由视觉传感器采集的图像信息，图像信息包括水下移动机器人工作场景的环境信息和目标水下机器人的信息。视觉传感器课优选为双目立体相机，双目立体相机能够输出2d图像和三维点云数据。

步骤s20，基于所述图像信息检测目标水下机器人的二维坐标。

在本实施例中，水下移动机器人的2d目标检测器基于图像信息检测目标水下机器人并定位出目标水下机器人的二维坐标。

步骤s30，将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标。

在本实施例中，水下移动机器人将二维坐标投影至预设的锥体三维空间以得到目标水下机器人对应的三维坐标。

步骤s40，将所述三维坐标经过平滑滤波处理得到所述目标水下机器人的定位信息。

在本实施例中，水下移动机器人将目标机器人三维坐标经过滤波器的平滑滤波处理得到目标水下机器人的定位信息，根据定位信息对目标机器人进行跟踪锁定等操作。具体为，水下移动机器人包括目标跟踪器，目标跟踪器的任务是实时跟踪目标水下机器人的当前时刻位置并预测下一时刻位置，同时还起到对原始检测目标水下机器人位置结果进行滤波平滑作用，最后将目标水下机器人的定位信息发送至rov运动控制器，rov运动控制器根据目标位置变化量来调整自身位姿以实现对目标的实时锁定跟踪。

本发明专利优选采用卡尔曼滤波器进行平滑滤波处理，实现目标跟踪功能。卡尔曼滤波器包含预测和更新两个步骤，这两个步骤循环反复执行实现对目标水下机器人位置的持续跟踪。

首先，对卡尔曼滤波器进行建模，即合理的构建卡尔曼滤波器若干方程及相关参数。一、目标水下机器人的状态变量：目标水下机器人在三维空间中自由运动，因此包含6个自由度状态即位置x,y和z、姿态roll,pitch,yaw，定义为x＝(x,y,z,roll,pitch,yaw)，为1维向量，滤波器就是对目标的这6个状态量进行预测及更新；二、目标的运动模型：要求定义目标的运动模式，对于目标水下机器人这类低机动目标，通常假设其做匀速运动或匀加速运动，运动模型的形式将影响误差矩阵参数配置；三、传感器的观测模型：观测模型由可以被传感器直接测量输出的状态量构成，由于上述定义的目标状态空间的6个状态量都是由3d点云目标检测器直接计算输出的(即变换矩阵)，因此观测模型包含了状态空间中所有状态量。四、误差矩阵：包括状态转移误差q和观测误差r，q代表了目标运动模型的不确定性，由目标运动模型的具体形式推算得到或自己标定获得，r代表了传感器本身的测量误差，通常由传感器厂商提供或本领域技术人员标定所得；参照图5，图5为配置卡尔曼滤波器对目标轨迹跟踪及滤波效果示意图，其中901为原始定位结果，存在一个明显的定位跳变点904，903为目标真实轨迹点，902为滤波后目标位置，很明显地，卡尔曼滤波结果更接近真实目标轨迹，且具有一定的跳变点抑制能力，滤波后轨迹更加平滑，并且具有预测下一时刻目标位置的能力，如图905所示为根据目标运动模型预估的目标下一时刻位置点；906为真实目标轨迹的下一时刻位置点。

在本实施例提供的技术方案中，水下机器人接收视觉传感器采集的图像信息；基于图像信息检测目标并定位出目标水下机器人的二维坐标；将二维坐标投影至预设的锥体三维空间以得到目标水下机器人对应的三维坐标；结合了2d图像成像快和3d点云成像高精度高分辨率的优点，将目标机器人的三维坐标经过滤波器的平滑滤波处理得到目标机器人的定位信息，水下机器人根据定位对目标水下机器人进行实时跟踪锁定等操作。从而解决了现有水下机器人搭载的声纳系统成本功耗高且成像分辨率低的问题。

在针对有些作业场景例如水下三维重建、水下设备巡检等要求具有高精度高分辨率三维点云成像，目前通常会装备水下专用蓝/绿激光雷达扫描仪以机械扫描方式获取短距离范围内环境物体的高精度高分辨率三维点云数据，但此种机械扫描式激光雷达成像速度慢无法对高机动目标进行实时感知成像。利用本发明第一实施例的技术方案能够解决此问题。

参照图6，图6为本水下机器人的检测识别方法的第二实施例，基于第一实施例提出本发明第二实施例，在本实施例中步骤s20包括：

步骤s21，确定所述图像信息是否包括目标水下机器人，预设的参考图像包括所述目标水下机器人。

在本实施例中，针对水下移动机器人的形态特点，参照图7，图7为一种常用的小型水下移动机器人rov示意图；具有以下共有特征：2个浮力仓(303)，1个主控仓(304)，具有水平分布4个推进器(306)及垂直分布4个推进器(307)从而可以在水下进行灵活的3轴6自由度运动，以及挂载的本发明专利提出的水下智能一体机(305)。从正视图来看，水下机器人具有左右对称结构，这将有利于特征提取构建机器学习模型。本发明专利提出采用2层嵌套式模型方法，将水下机器人分割成2部分：整体部分(301)和局部部分(302)，局部模型包含了所有水下机器人都具备的共有特征即2个浮力仓和1个主控仓，这几个共有特征具有明显的几何特征即矩形和圆形因而机器学习算法容易识别。当执行识别流程时，2层模型以嵌套方式运行，即先执行整体部分模型识别，只有当整体模型识别成功后再执行局部模型识别，当且仅当2层模型都被成功识别出来，算法才判定成功检测到目标。这种2层嵌套式模型带来的好处是极大的增加了基于2d图像的目标检测识别准确率。

步骤s21包括：

将所述图像信息输入目标模型，根据所述目标模型的输出结果确定所述图像信息是否包括目标水下机器人。

所述目标模型根据水下机器人的图像数据和所述水下机器人在水下工作环境的图像数据训练得到，所述水下机器人的图像数据为训练正样本，所述水下工作环境的图像数据为训练负样本。

在本实施例中，针对2层模型对应的水下机器人整体部分和局部部分，分别采集水下机器人图像数据，含有水下机器人的图像将作为训练正样本，正样本图像包括陆地及水下环境中的机器人照片，场景要尽量多样性(不同光照条件、不同浑浊度水体、不同环境背景等)，这样做将有助于提高基于模型识别应对环境变化能力。另外，还收集制作了负样本(不包含水下机器人)，负样本为水下工作环境，负样本要尽量从水下机器人经常执行任务的环境中采集。当正样本及负样本都制作完成以后，就按照机器学习常规训练模型流程来训练出目标模型文件。对于不同的水下机器人来说，数据集各不相同，取决于水下机器人的形状，在本实施例中，数据集中的预设的参考图像包括目标水下机器人。

水下机器人将图像信息输入目标模型，根据模型输出结果确定图像信息中是否包括目标水下机器人。

步骤s22，当所述图像信息包括目标水下机器人时，基于所述图像信息确定所述目标机器人的包围矩形盒。

在本实施例中，当图像信息包括目标水下机器人即整体模型和局部模型都识别成功时，基于图像信息确定目标水下机器人的最小外接矩形包围盒。

步骤s23，确定所述包围矩形盒在预设的二维图像坐标系下的二维坐标。

在本实施例中，水下机器人确定包围矩形盒在预设的二维图像坐标系即2d图像坐标系下的二维坐标(包围矩形盒的几何中心)。参照图8，图8为2d图像目标检测结果示意图；501为2d图像坐标系，502为目标水下机器人，503为最小矩形包围盒(代表目标水下机器人的范围)以及二维坐标(504)

在本实施例提供的技术方案中，水下机器人根据本发明提供的2层嵌套式模型确定图像信息中是否包括目标试下机器人，具体为，将图像信息输入目标模型，目标模型为经过训练正样本和训练负样本训练得到的模型，根据目标模型输出结果确定图像信息是否包括目标水下机器人。通过2层嵌套式模型进行确定图像信息中是否包括目标水下机器人，极大的增加了基于2d图像的目标检测识别准确率。

参照图9，图9为本发明构建的坐标系系统，包括相机坐标系o_camera、世界坐标系o_world和锁定坐标系o_lock。各坐标系的作用描述如下：相机坐标系的原点在视觉传感器成像中心，为三维点云所在坐标系，也是立体锥体搜索区域所在的坐标系，3d点云目标检测器将在相机坐标系下检测目标。锁定坐标系的原点由自主或人工位置标定结果决定，通常位于距离视觉传感器一定距离的视场角中心位置，3d点云目标检测器输出的目标三维位置坐标就是在锁定坐标系下的。世界坐标系为一个全局绝对坐标系系统，用来指示水下机器人(401)以及被检测到的目标(402)在全局坐标系下的绝对位置。以上三者坐标系系统可以通过变换矩阵进行相互转换。

参照图10，图10为本发明第二实施例中步骤s30，将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标的具体步骤，具体包括：

步骤s31，采用预设的参数矩阵将所述包围矩形盒投影至预设的三维空间形成三维立体锥体。

在本实施例中，预设的参数矩阵为视觉传感器内部参数矩阵。在已知视觉传感器内部参数(供应商提供或自己标定)的情况下，2d目标检测器利用内部参数矩阵将目标包围矩形盒投影到三维空间形成一个三维立体锥体。

步骤s32，按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标。

在本实施例中，具体的投影公式如下式所示：pcam为相机坐标系下的位置，pimg为图像坐标系下的像素位置，k为内参矩阵，fx和fy为焦距，cx和cy为光心。

pcam=k^-1．d．pimg

水下机器人按照投影公式将二维坐标转换成在相机坐标系下的三维坐标。

步骤s33，在所述三维立体锥体内部搜索所述目标水下机器人的点云团块。

在本实施例中，3d云目标检测器获取由2d目标检测器生成的三维立体锥体区域，在三维立体锥体内部搜索所述目标水下机器人的点云团块。

步骤s34，采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块。

在本实施例中，3d云目标检测器采用聚类算法对锥体内点云团块进行聚类，形成各个独立的点云团块。聚类算法包括但不限于：基于欧几里得距离的聚类算法和基于正态分布的聚类算法等。

步骤s35，利用点云特征提取算法提取所述各个独立的点云团块的三维特征描述子。

在本实施例中，3d云目标检测器利用点云特征提取算法提取所述各个独立的点云团块的三维特征描述子。可以被提取的三维点云特征(三维特征描述子)可以是例如特征直方图(pfh)描述子、快速点特征直方图(fpfh)描述子等。

步骤s36，利用匹配算法将各个三维特征描述子与目标点云三维特征描述子进行匹配，以得到匹配得分。

在本实施例中，3d云目标检测器利用匹配算法将各个三维特征描述子与目标点云三维特征描述子进行匹配，以得到匹配得分。点云特征匹配算法包括但不限于为基于改进的三维霍夫变换的implicitshapemodel算法。

步骤s37，将匹配得分最高的点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵。

在本实施例中，3d云目标检测器将匹配得分最高的点云团块作为述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵t(r,t)(包含旋转r和平移t)。

步骤s38，基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在本实施例中，水下机器人基于三维坐标和变换矩阵获取目标水下机器人在锁定坐标系下的三维坐标。

参照图11，图11为本发明第二实施例中步骤s30，将所述二维坐标投影至预设的锥体三维空间中以得到所述目标水下机器人对应的三维坐标的具体步骤，具体包括：

步骤s31，采用预设的参数矩阵将所述包围矩形盒投影至预设的三维空间形成三维立体锥体。

步骤s32，按照投影公式将所述二维坐标转换成在相机坐标系下的三维坐标。

步骤s33，在所述三维立体锥体内部搜索所述目标水下机器人的点云团块。

步骤s34，采用聚类算法对所述点云团块进行聚类，形成各个独立的点云团块。

步骤s39，利用点云注册算法将所述各个独立的点云团块与初始标定的目标点云团块进行匹配，得到与所述初始标定的目标云团块匹配的点云团块。

在本实施例中，3d云目标检测器利用点云注册算法将各个独立的点云团块与初始标定的目标点云团块进行匹配，得到与初始标定的目标云团块匹配的点云团块。点云注册算法：可以为iterativeclosestpoint(icp)算法(迭代最近点算法)、generalizediterativeclosestpoint(通用迭代算法)(gicp)算法、normaldistributionstransform(正态分布变换算法)(ndt)算法等。

步骤s310，将所述点云团块作为所述目标水下机器人，输出所述点云团块到初始标定的目标点云团块之间的变换矩阵。

在本实施例中，3d云目标检测器将点云团块作为所述目标水下机器人，输出点云团块到初始标定的目标点云团块之间的变换矩阵t(r,t)(包含旋转r和平移t)。

步骤s311，基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

在本实施例中，水下目标机器人基于所述三维坐标和所述变换矩阵获取所述目标水下机器人在锁定坐标系下的三维坐标。

上述为两种不同的方案获取当前目标点云团块和初始标定的目标点云团块之间的变换矩阵t(r,t)(包含旋转r和平移t)。参照图12，图12为点云匹配/注册示意图。包括三维特征描述子(图12804)、当前目标点云团块(图12802)、初始标定的目标点云团块(图12801)、变换矩阵(图12803)。

上述实施例中步骤s31的步骤之后，还包括：

步骤s31a，基于当前时刻所述目标水下机器人的定位结果为中心，沿着所述目标水下机器人的深度方向拓展预设距离，形成梯形锥体；其中，所述梯形锥体深度中心为当前目标的深度坐标值。

在本实施例中，参照图13，图13为调整固定锥体深度值的示意图。本发明提出了一种自适应锥体深度值动态调整方法，在由2d目标矩形区域投影生成的3d锥体区域，默认情况下，这个锥体的深度从相机光心位置开始直到无穷远处，形成一个三角锥体(图13左图602)，导致锥体区域太深，因而点云聚类搜索范围也随之增大，无疑会增加计算量。通常情况下，水下机器人在视觉传感器(601)前后若干帧范围内的运动空间有限，并且基于目标运动模型也是可以对目标下一时刻位置进行较为准确的预估的，基于上述前提，本发明专利提出了一种自适应动态调整锥体深度值的方法，具体实现方法描述如下：根据当前时刻目标定位结果(604)，605为中心线，以当前目标位置为中心，沿着深度方向前后各拓展一段距离d，最终形成了一个梯形锥体(图13右图603)，其中锥体深度中心即为当前目标深度坐标值，锥体深度值(高度值)即为2*d，距离d的设定要确保锥体可以完全覆盖住目标且留有一定余量。经过如此设定的深度值能够让锥体区域刚好能够覆盖现有目标并跟随目标定位结果进行实时动态调整，这样就能够去掉大部分没用锥体区域，从而大大降低点云聚类搜索区域范围的同时剔除了许多干扰点云，进而加速算法实时性的同时提高了算法识别准确率。

实际工程应用中，在目标水下机器人进行检测跟踪时，针对异常情况的处理策略对系统的稳定运行至关重要。目标检测跟踪系统最常见异常就是目标检测丢失与目标检测跳变。本发明分别针对检测丢失与检测跳变两种检测异常情况提出了针对性的处理策略方法。

参照图14，图14为本发明水下机器人的检测识别方法的第三实施例，基于第二实施例提出本发明第三实施例，在本实施例中步骤s21的步骤之后，还包括：

步骤s24，在确定所述图像信息不包括目标水下机器人时，以最近一次成功检测的目标水下机器人位置为中心、目标水下机器人的矩形包围盒为基准，以预设尺寸递增的矩形包围盒作为本次检测结果输出。

在本实施例中，为检测丢失处理策略，主要针对2d目标检测器，当目标检测丢失发生时即确定所述图像信息不包括目标水下机器人时，参照图15，图15为检测丢失处理示意图。将分别以最近一次成功检测的目标水下机器人位置(图151002)为中心、包围盒尺寸为基准，用以预设尺寸递增(图151004)的矩形包围盒作为本次检测结果输出(“假定目标”，图151003)。

步骤s25，累积所述目标水下机器人检测丢失帧数。

在本实施例中，2d目标检测器累积目标水下机器人检测丢失帧数。

步骤s26，当所述目标水下机器人检测丢失帧数大于设定阈值时，将所述图像信息作为所述目标水下机器人的包围盒区域；在所述目标包围盒区域中搜索所述目标水下机器人。

在本实施例中，当所述目标水下机器人检测丢失帧数大于设定阈值时，认为目标发生了较长时间丢失，算法将图像信息即整个图像画面(图151001)作为目标水下机器人包围盒区域，即将在整个画面中搜索目标。由于目标在视觉传感器前后两帧图像中位置不会发生巨大变化，因此本发明专利提出的处理策略能够有效应对短时间内和长时间的目标丢失情况，保障整个检测跟踪系统持续稳定运行下去。

参照图16，图16为本发明水下机器人的检测识别方法的第四实施例，基于第二实施例提出本发明第四实施例，包括：

步骤s27，当检测到所述目标水下机器人的当前位置与前一帧位置之间的聚类超过设定阈值时，采用最近一次成功检测到的所述目标水下机器人的位置作为本次检测的目标位置。

在本实施例中，为检测跳变处理策略：针对2d目标检测及3d点云目标检测器发生目标检测跳变(位置跳变)情况，参照图17，图17为检测跳变处理示意图。具体描述为：当2d/3d检测器检测到目标水下机器人当前位置(图171103)与前一帧位置(图171104)距离超过设定阈值(图171102)即判定发生了跳变，采用最近一次成功检测到的目标位置作为本次检测的目标位置。

步骤s28，统计连续发生跳变的帧数。

步骤s29，当所述连续发生跳变的帧数大于设定数目且连续跳变点保持在同一位置附近，将所述连续跳变点作为所述水下机器人位置。

在本实施例中，当所述连续发生跳变的帧数大于设定数目且连续跳变点保持在同一位置附近，将所述跳变点作为所述水下机器人位置。本发明专利提出的该跳变处理策略能够有效应对目标水下机器人短时间跳变及长时间跳变的情况。

参照图18，图18为本发明水下机器人的检测识别方法的第五实施例，基于第一实施例提出本发明第五实施例，包括：

步骤s50，获取当前获取的所述定位信息与初始位置信息之间的距离，其中，所述初始位置信息为首次检测到的所述定位信息。

步骤s60，在所述距离大于预设距离时，根据所述距离大于预设距离的次数；其中，所述距离小于预设阈值范围。

步骤s70，在所述次数大于预设次数时，将所述次数对应的定位信息的平均值作为所述目标机器人的初始位置。

在本实施例中，在水下环境中针对某些特殊应用场景(例如搭载水下光通信机进行无线光通信)往往要求两台水下移动机器人首先要能够精确对准然后才能进行正常的光通信数据交互。针对类似场景的这种需求，本发明专利提出了一种目标初始化标定方法，其目的之一为确定对准要求的目标初始位置，注意此目标位置在视觉传感器坐标系(相机坐标系)下，并构建以目标初始位置为原点的位置锁定坐标系，此后检测的目标水下机器人位置都是在此位置锁定坐标系下的；目的之二是建立目标的三维点云模型数据库，用于后续3d点云检测器进行基于目标模型的滤波及匹配。

参照图19，图19为目标水下机器人初始位置初始化标定过程示意图。

具体的初始化方法流程如下：

针对目标水下机器人初始位置的初始化，将初次检测到的目标位置作为第一个位置点(图191202空心圆圈)，对接下来连续n次检测到的目标位置点(图191203黑色实心圆圈)与第一个位置点进行距离比较，如果两者之间距离超过设定的阈值(图191201虚线圆形区域)，那么就将这n个目标点位置的平均值(图191204黑色五角星)作为最终确定的目标初始位置，并建立了锁定坐标系，即完成了目标水下机器人初始位置自动标定；

步骤s80，在所述次数小于或等于所述预设次数时，采用所述所述距离对应的定位信息更新所述初始位置信息。

在本实施例中，在未达到n次连续检测前出现某个位置点与第一个位置点的距离超过阈值范围(图191205)，那么就以这个新的位置点作为新的第一个位置点(图191206)，后续检测的位置点与此新的第一个位置点进行距离比较，直到连续n个位置点都在设定阈值范围内，取n个位置点平均值作为目标初始位置，并建立锁定坐标系，完成目标水下机器人初始位置的自动标定。

参照图20，图20为目标点云模型自动化标定示意图。

针对目标点云模型的自动标定：首先，由用户事先定义一个目标点云模型，该点云模型的数学参数化表示为：定义模型[长，宽，高，点云数目](图201301)，要求用户点云的参数模型与目标实际模型尽量接近；然后，进行自主标定采用3d点云检测器提取出来的目标点云模型包含实时的模型参数：实时模型[长，宽，高，点云数目](图201303)，对比实时模型与定义模型，当且仅当实时模型的所有参数位于定义模型的比例范围之内(即长宽高几何尺寸以及点云数目都成比例关系)才认为此次提取到的目标点云团块有效，连续记录n次有效提取的实时目标点云模型并计算其平均值作为最终的目标点云模型，从而完成了目标点云模型的自主标定。初始化标定得到的目标点云模型将用于后续的3d目标检测器算法框架中进行对初步提取到的候选目标点云模块与目标点云模型匹配以滤除可能为错误的假目标并获取候选目标与初始目标点云模型之间的变换矩阵(相对位姿变换关系)。

参照图21，图21为完整的目标检测与跟踪系统及算法的示意框图：

本发明还提供一种水下机器人的检测识别装置，所述装置包括存储器、处理器以及存储在所述存储器并可在所述处理器上运行的水下机器人的检测识别程序，所述水下机器人的检测识别程序被所述处理器执行时实现如上所述的水下机器人的检测识别方法的各个步骤。

本发明还提供一种计算机可读存储介质，所述计算机可读存储介质存储有水下机器人的检测识别程序，所述水下机器人的检测识别程序被处理器执行时实现如上所述的水下机器人的检测识别方法的各个步骤。

上述本发明实施例序号仅仅为了描述，不代表实施例的优劣。

本领域内的技术人员应明白，本发明的实施例可提供为方法、系统、或计算机程序产品。因此，本发明可采用完全硬件实施例、完全软件实施例、或结合软件和硬件方面的实施例的形式。而且，本发明可采用在一个或多个其中包含有计算机可用程序代码的计算机可用存储介质(包括但不限于磁盘存储器、cd-rom、光学存储器等)上实施的计算机程序产品的形式。

本发明是参照根据本发明实施例的方法、设备(系统)、和计算机程序产品的流程图和/或方框图来描述的。应理解可由计算机程序指令实现流程图和/或方框图中的每一流程和/或方框、以及流程图和/或方框图中的流程和/或方框的结合。可提供这些计算机程序指令到通用计算机、专用计算机、嵌入式处理机或其他可编程数据处理设备的处理器以产生一个机器，使得通过计算机或其他可编程数据处理设备的处理器执行的指令产生用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的装置。

这些计算机程序指令也可存储在能引导计算机或其他可编程数据处理设备以特定方式工作的计算机可读存储器中，使得存储在该计算机可读存储器中的指令产生包括指令装置的制造品，该指令装置实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能。

这些计算机程序指令也可装载到计算机或其他可编程数据处理设备上，使得在计算机或其他可编程设备上执行一系列操作步骤以产生计算机实现的处理，从而在计算机或其他可编程设备上执行的指令提供用于实现在流程图一个流程或多个流程和/或方框图一个方框或多个方框中指定的功能的步骤。

应当注意的是，在权利要求中，不应将位于括号之间的任何参考符号构造成对权利要求的限制。单词“包含”不排除存在未列在权利要求中的部件或步骤。位于部件之前的单词“一”或“一个”不排除存在多个这样的部件。本发明可以借助于包括有若干不同部件的硬件以及借助于适当编程的计算机来实现。在列举了若干装置的单元权利要求中，这些装置中的若干个可以是通过同一个硬件项来具体体现。单词第一、第二、以及第三等的使用不表示任何顺序。可将这些单词解释为名称。

尽管已描述了本发明的优选实施例，但本领域内的技术人员一旦得知了基本创造性概念，则可对这些实施例作出另外的变更和修改。所以，所附权利要求意欲解释为包括优选实施例以及落入本发明范围的所有变更和修改。

显然，本领域的技术人员可以对本发明进行各种改动和变型而不脱离本发明的精神和范围。这样，倘若本发明的这些修改和变型属于本发明权利要求及其等同技术的范围之内，则本发明也意图包含这些改动和变型在内。