一种视觉激光融合的托盘位姿估计方法、系统及装置

技术领域

本发明涉及位姿识别领域，尤其涉及一种视觉激光融合的托盘位姿估计方法、系统及装置。

背景技术

基于单一信息源实现的位姿识别方法都可以独立的完成托盘位姿估计，但是由于传感器特性与算法特点，无法通过单一方法适应仓库环境，由于单一信息源，会因传感器探测范围、探测角度等的局限性，影响识别方法的适用范围。目前普遍使用的基于RGB-D传感器的识别方法，由于RGB-D深度传感器探测角度较小，横向探测范围和探测距离较小，导致该识别方法同样存在探测角度较小，横向探测范围和探测距离较小的问题。

发明内容

为了解决上述技术问题，本发明的目的是提供一种视觉激光融合的托盘位姿估计方法、系统及装置，能够提高托盘识别与拾取系统探测角度和探测范围，从而提高对复杂仓库环境的适应能力。

本发明所采用的第一技术方案是：一种视觉激光融合的托盘位姿估计方法，包括以下步骤：

S1、获取当前图像数据和激光数据并融合，得到托盘距离初值；

S2、根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置；

S3、根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据；

S4、将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息；

S5、根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合；

S6、基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合；

S7、基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果。

进一步，所述获取当前图像数据和激光数据并融合，得到托盘距离初值这一步骤，其具体包括：

S11、获取当前图像数据和激光数据；

S12、将当前图像数据中的每个像素进行分类，得到类别矩阵；

S13、根据类别矩阵将不是托盘类别的激光数据剔除，得到托盘类别激光点数据集合；

S14、根据托盘类别激光点数据集合提取托盘距离，得到托盘距离初值。

进一步，所述根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置这一步骤，其具体包括：

S21、根据托盘距离初值确定类别模板尺寸；

S22、对类别矩阵和预构建的类别模板进行压缩，得到压缩后的类别矩阵和压缩后的类别模板；

S23、通过滑动窗口方法确定托盘的准确位置，得到托盘在图像中的位置。

进一步，所述根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据这一步骤：

S31、基于雷达与相机的空间转换矩阵将激光数据投影到图像并根据托盘在图像中的位置确认投影在托盘分类的激光点；

S32、去除投影在托盘分类以外的激光点，得到筛选后的激光数据。

进一步，所述将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息这一步骤，其具体还包括：

S41、将距离初值和预设阈值进行比较；

S42、判断到距离初值大于预设阈值，选择改进的增量式提取算法对筛选后的激光数据进行处理，得到直线段信息；

S43、判断到距离初值不大于预设阈值，选择霍夫直线提取算法对筛选后的激光数据进行处理，得到直线段信息。

进一步，所述判断到距离初值大于预设阈值，选择改进的增量式提取算法对筛选后的激光数据进行处理，得到直线段信息这一步骤，其具体包括：

S421、判断到距离初值大于预设阈值，在筛选后的激光数据中选择前4个点，建立窗口并构造一条直线；

S422、计算窗口内拟合的直线角度，记录直线与坐标轴夹角，记为初始角度；

S423、保证窗口为4个点，由左至右，向窗口加入下一个点，放出窗口内第一个点，并重新计算直线与坐标轴夹角，作为新直线参数；

S424、判断到新的直线角度变化小于阈值，且与初始角度参数差值小于阈值，将该点记作直线上一点，返回S423，否则得到线段并进行步骤S425；

S425、判断到线段包含的点数小于点数阈值或线段长度大于长度阈值，舍弃并进入S427，否则提取直线并进入S426；

S426、记录提取得到的直线参数并进入S427；

S427、取筛选后的激光数据中余下未处理的前四个点，构造一条直线并返回步骤S423，循环直至处理完筛选后的激光数据中所有点；

S428、对所有记录提取得到的直线参数进行整理，得到直线段信息。

进一步，所述根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合这一步骤，其具体包括：

S51、根据直线段信息得到多条直线以及对应的数据点；

S52、选取每一条直线两侧方向上到该条直线距离小于阈值的数据点，得到对应数量的托盘候选点集合。

进一步，所述基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合这一步骤，其具体包括：

S61、对托盘候选点集合进行去噪预处理，得到去噪后的托盘候选点集合；

S62、将托盘候选点集合转为二值图，得到候选点集合二值图；

S63、基于托盘候选点集合对应的直线计算斜率并创建模板；

S64、将候选点集合二值图与对应的模板匹配，得到托盘位置；

S65、一组候选点集合得到一组托盘位姿向量；

S66、整合所有托盘位姿向量，得到托盘位姿候选集合。

进一步，所述基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果这一步骤，其具体包括：

S71、将托盘位姿候选集合分为3个子集；

S72、分别对3个子集进行迭代权重计算并选择权重最大的聚类中心作为托盘中心；

S73、根据选择的聚类中心选择对应的托盘候选点集合并进行斜率计算，得到托盘位姿。

本发明所采用的第二技术方案是：一种视觉激光融合的托盘位姿估计系统，包括：

距离初值计算模块，用于获取当前图像数据和激光数据并融合，得到托盘距离初值；

位置识别模块，用于根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置；

第一筛选模块，用于根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据；

直线提取模块，用于将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息；

第二筛选模块，用于根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合；

匹配模块，用于基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合；

托盘位姿估计模块，用于基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果。

本发明所采用的第三技术方案是：一种视觉激光融合的托盘位姿估计装置，包括：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如上所述一种视觉激光融合的托盘位姿估计方法。

本发明方法、系统及装置的有益效果是：本发明将来自于RGB-D传感器与激光雷达的数据进行融合处理，提高托盘识别与拾取系统探测角度和探测范围，得到更为准确、可靠的托盘位姿估计，从而提高智能叉车在移动过程中，对复杂仓库环境的适应能力。

附图说明

图1是本发明一种视觉激光融合的托盘位姿估计方法的步骤流程图；

图2是本发明一种视觉激光融合的托盘位姿估计系统的结构框图。

具体实施方式

下面结合附图和具体实施例对本发明做进一步的详细说明。对于以下实施例中的步骤编号，其仅为了便于阐述说明而设置，对步骤之间的顺序不做任何限定，实施例中的各步骤的执行顺序均可根据本领域技术人员的理解来进行适应性调整。

如图1所示，本发明提供了一种视觉激光融合的托盘位姿估计方法，该方法包括以下步骤：

S1、获取当前图像数据和激光数据并融合，得到托盘距离初值；

S2、根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置；

S3、根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据；

S4、将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息；

S5、根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合；

S6、基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合；

S7、基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果。

进一步作为本方法的优选实施例，所述获取当前图像数据和激光数据并进行数据融合，得到托盘距离初值这一步骤，其具体包括：

S11、获取当前图像数据和激光数据；

具体地，基于RGB-D传感器获取图像数据和基于激光雷达获取激光数据。

S12、将当前图像数据中的每个像素进行分类，得到类别矩阵；

具体地，基于分类器对图像中的每个像素进行分类，得到每个像素的类别信息，然后根据每个像素的类别信息建立类别矩阵。

S13、根据类别矩阵将不是托盘类别的激光数据剔除，得到托盘类别激光点数据集合；

S14、根据托盘类别激光点数据集合提取托盘距离，得到托盘距离初值。

具体地，当激光投影到图像中的像素分类与类别矩阵对应时，表示该激光点探测到托盘。

进一步作为本方法的优选实施例，所述根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置这一步骤，其具体包括：

S21、根据托盘距离初值确定类别模板尺寸；

S22、对类别矩阵和预构建的类别模板进行压缩，得到压缩后的类别矩阵和压缩后的类别模板；

S23、通过滑动窗口方法确定托盘的准确位置，得到托盘在图像中的位置。

具体地，所述托盘支架坐标的表达式如下：

上式中，

进一步作为本方法优选实施例，所述根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据这一步骤，其具体包括：

S31、基于雷达与相机的空间转换矩阵将激光数据投影到图像并根据托盘在图像中的位置确认投影在托盘分类的激光点；

具体地，空间转换矩阵获取步骤如下：标定采用异形标定板，标定板中间镂空一个三角形，当激光雷达扫描到标定板时，获得直线。当标定板的尺寸已知，通过标定板上，激光点长度比例，推算激光扫描平面与标定板边缘交点，获得交点A、B、C、D，在标定板坐标系{E}和激光雷达坐标系{L}的坐标。基于已知点，通过SVD算法计算转换矩阵。

S32、去除投影在托盘分类以外的激光点，得到筛选后的激光数据。

进一步作为本方法优选实施例，所述将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息这一步骤，其具体还包括：

S41、将距离初值和预设阈值进行比较；

S42、判断到距离初值大于预设阈值，选择改进的增量式提取算法对筛选后的激光数据进行处理，得到直线段信息；

S43、判断到距离初值不大于预设阈值，选择霍夫直线提取算法对筛选后的激光数据进行处理，得到直线段信息。

具体地，直线提取算法是托盘识别的关键环节。在激光雷达数据集合中，托盘支架表现为由离散点组成的短小线段。而线段所在直线包含托盘位置及姿态特征。因此，直线提取算法应对短小线段敏感。但当探测距离H和探测角度a

进一步作为本方法优选实施例，所述判断到距离初值大于预设阈值，选择改进的增量式提取算法对筛选后的激光数据进行处理，得到直线段信息这一步骤，其具体包括：

S421、判断到距离初值大于预设阈值，在筛选后的激光数据中选择前4个点，建立窗口并构造一条直线；

S422、计算窗口内拟合的直线角度，记录直线与坐标轴夹角，记为初始角度；

S423、保证窗口为4个点，由左至右，向窗口加入下一个点，放出窗口内第一个点，并重新计算直线与坐标轴夹角，作为新直线参数；

S424、判断到新的直线角度变化小于阈值，且与初始角度参数差值小于阈值，将该点记作直线上一点，返回S423，否则得到线段并进行步骤S425；

S425、判断到线段包含的点数小于点数阈值或线段长度大于长度阈值，舍弃并进入S427，否则提取直线并进入S426；

S426、记录提取得到的直线参数并进入S427；

S427、取筛选后的激光数据中余下未处理的前四个点，构造一条直线并返回步骤S423，循环直至处理完筛选后的激光数据中所有点；

S428、对所有记录提取得到的直线参数进行整理，得到直线段信息。

具体地，这部分采用改进后的增量式直线提取算法，基于空间序列顺序对数据点集合进行处理，以加入新数据点后拟合直线角度变化值，作为该点是否在直线上的判断条件。同时，增加线段初始角度、线段点数最小阈值和线段长度阈值，筛选出符合托盘支架几何约束的线段，改进原增量式算法中对曲率较小的曲线不敏感问题。

进一步作为本方法优选实施例，所述根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合这一步骤，其具体包括：

S51、根据直线段信息得到多条直线以及对应的数据点；

S52、选取每一条直线两侧方向上到该条直线距离小于阈值的数据点，得到对应数量的托盘候选点集合。

具体地，距离公式如下：

其中，其中Dis候选数据点距离直线段距离，(x,y)为候选数据点坐标，k、b为直线斜率与截距。

进一步作为本方法优选实施例，所述基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合这一步骤，其具体包括：

S61、对托盘候选点集合进行去噪预处理，得到去噪后的托盘候选点集合；

S62、将托盘候选点集合转为二值图，得到候选点集合二值图；

S63、基于托盘候选点集合对应的直线计算斜率并创建模板；

S64、将候选点集合二值图与对应的模板匹配，得到托盘位置；

S65、一组候选点集合得到一组托盘位姿向量；

S66、整合所有托盘位姿向量，得到托盘位姿候选集合。

进一步作为本方法优选实施例，所述基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果这一步骤，其具体包括：

S71、将托盘位姿候选集合分为3个子集；

S72、分别对3个子集进行迭代权重计算并选择权重最大的聚类中心作为托盘中心；

S73、根据选择的聚类中心选择对应的托盘候选点集合并进行斜率计算，得到托盘位姿。

具体地，从托盘位姿候选集合中，筛选出托盘最优位姿，转化为无监督聚类分析问题，采用K-means算法，选择成员最多的分类，融合获得托盘最优位姿。

目标函数δ和聚类核心c

其中P

如图2所示，一种视觉激光融合的托盘位姿估计系统，包括：

距离初值计算模块，用于获取当前图像数据和激光数据并融合，得到托盘距离初值；

位置识别模块，用于根据托盘距离初值和当前图像数据进行托盘识别，得到托盘在图像中的位置；

第一筛选模块，用于根据托盘在图像中的位置筛选激光数据，得到筛选后的激光数据；

直线提取模块，用于将距离初值和预设阈值进行比较并根据比较结果选择对应的直线提取方法对筛选后的激光数据进行处理，得到直线段信息；

第二筛选模块，用于根据直线段信息和预设规则对筛选后的激光数据再次筛选，得到托盘候选点集合；

匹配模块，用于基于模板匹配方法对托盘候选点集合进行匹配，得到托盘位姿候选集合；

托盘位姿估计模块，用于基于K-means算法对托盘位姿候选集合进行筛选，得到托盘位姿估计结果。

一种视觉激光融合的托盘位姿估计装置：

至少一个处理器；

至少一个存储器，用于存储至少一个程序；

当所述至少一个程序被所述至少一个处理器执行，使得所述至少一个处理器实现如上所述一种视觉激光融合的托盘位姿估计方法。

上述方法实施例中的内容均适用于本装置实施例中，本装置实施例所具体实现的功能与上述方法实施例相同，并且达到的有益效果与上述方法实施例所达到的有益效果也相同。

以上是对本发明的较佳实施进行了具体说明，但本发明创造并不限于所述实施例，熟悉本领域的技术人员在不违背本发明精神的前提下还可做作出种种的等同变形或替换，这些等同的变形或替换均包含在本申请权利要求所限定的范围内。