CN115790604A - 基于多传感器融合机器人拆垛方法及系统 - Google Patents

Abstract

本发明公开一种基于多传感器融合机器人拆垛方法及系统，所述方法包括如下步骤：S1、对相机采集到的图像和激光雷达扫描到的点云进行融合，获取垛堆的位置坐标；S2、基于蚁群算法规划机器人获取机器人当前位置到堆垛的抓取路径；S3、移动机器人基于抓取路径来抓取堆垛。针对激光雷达和视觉系统在单独进行辅助机械手拆垛时易受环境因素影响的现象，提出将视觉传感器和激光雷达两者进行融合感知，解决了单一传感器因光照原因导致的物体定位不准确等问题。

Description

基于多传感器融合机器人拆垛方法及系统

技术领域

本发明属于人工智能技术领域，更具体地，本发明涉及一种基于多传感器融合机器人拆垛系统及方法。

背景技术

工业现场通常会在工厂内安装工业机器人去完成搬运、点焊、装配等任务，这些工业机器人有效的减轻了工人的工作负荷，但是传统的搬运机器人只是单纯的进行轨迹程序运行，其轨迹程序由机器人工程师调试过后运行，当目标位置移动导致位置发生偏差时，机械臂仍是按照原先的轨迹运行，这会导致抓取错误，严重的还会与目标夹具干涉导致碰撞等情况。面对当前复杂的拆垛环境，机器人仅通过激光雷达或相机单一的传感器进行感知，激光雷达传感器抗干扰性强、不易受到光线的影响，适于全天候的使用且测距误差小可靠性强，但数据分辨率低；而视觉传感器能够获取丰富的环境特征和高分辨率的数据信息，但易受到恶劣天气的影响，且视野角度有限。基于此，单一传感器感知无法很好的满足实际生产的需求。

发明内容

本发明提供一种基于多传感器融合机器人拆垛方法，旨在改善上述问题。

本发明是这样实现的，一种基于多传感器融合机器人拆垛方法，其特征在于，所述方法包括如下步骤：

S1、对相机采集到的图像和激光雷达扫描到的点云进行融合，获取垛堆的位置坐标；

S2、基于蚁群算法规划机器人获取机器人当前位置到堆垛的抓取路径；

S3、移动机器人基于抓取路径来抓取堆垛。

进一步的，所述步骤S1具体包括如下步骤：

S11、标定相机的相机坐标系与激光雷达的雷达坐标系间的旋转矩阵和平移矩阵；

S12、标定激光雷达及相机的采样频率，控制激光雷达及相机同步进行采样；

S13、获取相机采集图像中的垛堆图像及检测框，激光点云中的垛堆点云及其检测框；

S14、将垛堆图像与垛堆点云进行匹配，将成功匹配垛堆进行融合，输出融合后堆垛的位置坐标及置信度。

进一步的，采样频率的标定方法具体如下：

以采样频率较低的激光雷达为基准，激光雷达每采一帧点云，则触发相机采集一次图像。

进一步的，基于YOLOV3算法来检测相机采集图像中的堆垛，包括堆垛的2D目标框及置信度，将2D目标框放入目标集X_C中，X_C＝{X₁,X₂…X_n}；

基于Complex-Yolo算法检测激光点云中的堆垛点云，包括堆垛点云的3D目标框及置信度，将3D目标框放入目标集Y_L中，Y_L＝{Y₁,Y₂…Y_m}。

进一步的，基于KM算法完成激光雷达检测出的堆垛点云与相机检测出的堆垛图像之间的匹配。

进一步的，KM算法中匹配边的权值计算公式具体如下:

λ_ij＝f_dis(X_i,Y_j)+f_iou(X_i,Y_j)

其中，f_dis是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的距离差，f_iou是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的目标框重叠度；λ_ij是目标集X_C中第i个堆垛图像和目标集Y_L中第j个堆垛点云Y_j间匹配边的权值。

进一步的，目标框的重叠度用交并比公式进行计算，计算公式具体如下：

KX_i是目标集X_C中第i个堆垛图像X_i的目标框，KY_j是目标集Y_L中第j个堆垛点云Y_j的目标框，IoU_ij表示堆垛图像X_i与堆垛点云Y_j的交并比。

进一步的，融合后堆垛的置信度计算公式具体如下：

式中，f_C、w_C分别为目标集X_C中目标物体类别的置信度及权重，f_L、w_L分别为目标集Y_L中目标点云类别置信度及权重，f_cls为融合后视觉目标类别置信度。

本发明是这样实现，一种基于多传感器融合机器人拆垛系统，所述系统包括：

移动机器人，设于机器人上的相机及激光雷达，与相机及激光雷达通讯连接的处理器，处理器基于上述基于多传感器融合机器人拆垛方法来控制移动机器人抓取堆垛。

针对激光雷达和视觉系统在单独进行辅助机械手拆垛时易受环境因素影响的现象，提出将视觉传感器和激光雷达两者进行融合感知，解决了单一传感器因光照原因导致的物体定位不准确等问题。同时利用蚁群算法对机械臂的路径进行处理，优化了目标垛堆的拆垛策略。

附图说明

图1为本发明实施例提供的基于多传感器融合机器人拆垛系统的机构示意图；

图2为本发明实施例提供的基于多传感器融合机器人拆垛方法流程图。

具体实施方式

下面对照附图，通过对实施例的描述，对本发明的具体实施方式作进一步详细的说明，以帮助本领域的技术人员对本发明的发明构思、技术方案有更完整、准确和深入的理解。

图1为本发明实施例提供的基于多传感器融合机器人拆垛系统的机构示意图，为了便于说明，仅示出与本发明实施例相关的部分。该系统包括：

移动机器人，设于机器人上的相机及激光雷达，与相机及激光雷达通讯连接的处理器，处理器控制移动机器人抓取目标堆垛。

图2为本发明实施例提供的基于多传感器融合机器人拆垛方法流程图，该方法具体包括如下步骤：

S1、对相机采集到的图像和激光雷达扫描到的点云进行融合，图像中含有堆垛，点云中含有堆垛点云，获取垛堆的位置坐标；

在本发明实施例中，基于相机和激光雷达的垛堆位置获取方法具体如下：

S11、标定相机的相机坐标系与激光雷达的雷达坐标系间的旋转矩阵和平移矩阵；

基于上述旋转矩阵和平移矩阵可将相机采集到图像在相机坐标下的坐标转换至雷达坐标系下，或者是将激光雷达点云在激光雷达坐标系下的坐标转换至相机坐标系，

对于激光雷达与相机坐标的联合标定，因为相机坐标系在空间上是可以通过旋转和平移得到激光雷达的坐标系，所以联合标定的实质问题即求解旋转矩阵和平移矩阵。相机和激光雷达传感器在进行定位时所获取的数据表现形式不同。将一个点P当成障碍物质点，其在相机坐标系下的坐标为(x_M,y_M,z_M)，在雷达坐标系下的坐标为(x_N,y_N,z_N)，P点在图像平面坐标系中的投影坐标为p′(u,v)。

相机采集到的数据为投影坐标为p′(u,v)和对应的深度信息Z_K，激光雷达采集到的数据是距离γ和角度α。

根据相机坐标系和雷达坐标系的几何关系，得到质点P在不同坐标系下的转化关系为：

式中，R为旋转矩阵，T为平移矩阵，根据经典相机模型，相机采集的p′(u,v)坐标数据和深度信息Z_K，其中，p′是质点P点在图像平面上的投影。相机坐标像素坐标(u,v)及深度信息z_M的关系为：

式中，f_x为X轴上以像素为单位的等效焦距，f_y为Y轴上以像素为单位的等效焦距，c_x为X轴上的基准点，c_y为Y轴上的基准点。

将采集的障碍物信息转化为激光雷达坐标系下的坐标位置，转化关系为：

激光雷达扫描的障碍物信息采用的是极坐标(r,α)表示，其中r表示障碍物与激光原点O_N的距离，α表示障碍物与O_NZ_N轴的夹角。激光雷达采集的障碍物数据(r,α)和激光雷达坐标系的关系为：

z_N＝rcosα·x_N＝rsinα

保持相机原点和激光雷达坐标原点在同一个Y轴上，设定垂直高度差为h，则相机数据和雷达数据的转化关系为：

在确定f_x、f_y、c_x、c_y相机内参数后，把多对相机数据u、v、z_M和激光雷达数据(r,α)代入式中，通过求解线性方程获得R矩阵、T矩阵，进而确相机坐标系与雷达坐标系的旋转平移关系，从而完成联合标定。

S12、标定激光雷达及相机的采样频率，控制激光雷达及相机同步进行采样；

不同传感器的工作频率各不相同，激光雷达频率在10Hz左右，相机的频率在30Hz以上。由于工作频率的不同，当采集的数据之间存在时间差时信息会由较大误差。因此需对二者传感器进行时间标定，保证融合检测结果的准确性。为了保证数据的可靠性，以采样频率较低的激光雷达为基准，激光雷达每采一帧点云，触发相机采集一次图像，即完成共同采样一帧雷达与视觉融合的数据，从而保证了激光雷达与相机采集数据在时间上同步。

S13、获取相机采集图像中的垛堆图像及检测框，激光点云中的垛堆点云及其检测框；

基于YOLOV3算法来检测相机采集图像中的堆垛图像，包括图像中堆垛的2D目标框及置信度，将2D目标框放入目标集X_C中，X_C＝{X₁,X₂…X_n}；

基于Complex-Yolo算法检测激光点云中的堆垛点云，包括堆垛点云的3D目标框及置信度，将3D目标框放入目标集Y_L中，Y_L＝{Y₁,Y₂…Y_m}。

S14、将垛堆图像与垛堆点云进行匹配，对成功匹配垛堆进行融合，输出融合后的堆垛的置信度及位置坐标。

在本发明实施例中，目标物体与目标点云的匹配过程可以看作二分图寻找最佳匹配问题，使用KM算法以目标间的距离信息及检测框交并比信息作为度量指标，完成激光雷达与相机之间的目标匹配。

KM算法解决的是带权值的二分图的最优匹配问题，对于每个点与另一边的点集连接情况加以权值作为区分匹配线路好坏标准，寻找到累计权值最大的匹配即是我们需要寻找的最优匹配结果。在发明的激光雷达与视觉目标匹配中我们使用KM匹配算法，匹配边的权值函数定义为:

λ_ij＝f_dis(X_i,Y_j)+f_iou(X_i,Y_j)

其中，f_dis是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的距离差，f_iou是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的目标框重叠度；λ_ij是目标集X_C中第i个堆垛图像物X_i和目标集Y_L中第j个堆垛点云Y_j间匹配边的权值。

在本发明实施例中，两目标框的重叠度可以用交并比公式计算，其公式定义为：

KX_i是目标集X_C中第i个堆垛图像X_i的目标框，KY_j是目标集Y_L中第j个堆垛点云Y_j的目标框，IoU_ij表示堆垛图像X_i与堆垛点云Y_j的交并比。

在激光雷达与视觉匹配之后会获得融合目标集合以及未被匹配上的单一传感器目标集合，将这些目标组成一个新集合

其中未被匹配到的视觉目标记为h^C，点云标记为h^L，剩余没有上角标的融合目标。将融合后的目标和那些未成功融合的目标重新组成一个新的集合，对于那些未融合的目标按照其原来的形式进行输出，这样就不会因为这部分信息未融合而产生丢失信息的影响。

降低步骤S13通过降低视觉目标置信度阈值，使得低置信度目标框增加，使用上述KM算法将目标集X_C与目标集Y_L进行目标匹配。使用加权平均的方式修改视觉目标置信度。加权平均的公式如下所示：

式中，f_C、w_C分别为目标集X_C中目标物体类别的置信度及权重，f_L、w_L分别为目标集Y_L中目标点云类别置信度及权重，f_cls为融合后视觉目标类别置信度。

对于原先置信度较高的视觉目标其可信度较高，可保留或增加视觉目标权重以减少点云分类算法可能带来的误检，而对于后续补入的低置信度视觉目标可通过点云算法数据提升目标置信度，因此对于低置信度目标点云目标权重可适当增加。通过多次人工整定参数后融合加权公式如下所示：

经过上式的融合加权平均后取每个目标类别的最大置信度作为最终融合结果，最后使用统一阈值进行筛选若高于设定阈值则保留目标，低于阈值则删除以避免误检现象。

S2、基于蚁群算法规划机器人获取机器人当前位置到目标堆垛的抓取路径；

步骤a.初始化起始位置和目标位置以及蚂蚁数量m、最大迭代次数N_max等算法参数，采用栅格法抽象表达环境模型。其中初始化位置即为机械臂原始Home位置，目标位置即上述融合算法得出的垛堆位置信息。

步骤b.复位蚂蚁的禁忌表、寻径长度等信息，蚂蚁从起始点出发开始搜索路径，按照移动概率公式搜索下一可到的路径中的节点，蚂蚁每走一步同时记录蚂蚁走过的途径点以及路线长度，一直循环到蚂蚁选取的节点为目标点或者没有可选节点停止搜索。移动概率公式如下：

其中，

表示t时刻蚂蚁k从当前位置i到位置j的转移概率；η_ij(t)表示蚂蚁从位置i到位置j的期望程度；其值为两者距离的倒数；τ_ij(t)为t时刻位置i和位置j连接路径上的信息素浓度；α为信息素重要程度因子，其值越大表示信息素的浓度在转移中所起的作用越大；β为η_ij(t)的重要程度因子，其值越大表示该函数在转移中的作用越大，即蚂蚁会以较大的概率转移到距离短的地点。allowed_k为蚂蚁k所要访问地点的集合。

步骤c.辨别蚁群迭代的总数是不是已抵达最大迭代次数，倘若还未抵达最大迭代次数，继续进行未完的迭代寻径，倘若抵达最大迭代次数，终止寻

步骤d.输出最优路径信息，算法结束。

S3、移动机器人基于抓取路径来抓取目标堆垛。

本发明进行了示例性描述，显然本发明具体实现并不受上述方式的限制，只要采用了本发明的方法构思和技术方案进行的各种非实质性的改进，或未经改进将本发明的构思和技术方案直接应用于其它场合的，均在本发明的保护范围之内。

Claims (9)

1.一种基于多传感器融合机器人拆垛方法，其特征在于，所述方法包括如下步骤：

S1、对相机采集到的图像和激光雷达扫描到的点云进行融合，获取垛堆的位置坐标；

S2、基于蚁群算法规划机器人获取机器人当前位置到堆垛的抓取路径；

S3、移动机器人基于抓取路径来抓取堆垛。

2.如权利要求1所述基于多传感器融合机器人拆垛方法，其特征在于，所述步骤S1具体包括如下步骤：

S11、标定相机的相机坐标系与激光雷达的雷达坐标系间的旋转矩阵和平移矩阵；

S12、标定激光雷达及相机的采样频率，控制激光雷达及相机同步进行采样；

S13、获取相机采集图像中的垛堆图像及检测框，激光点云中的垛堆点云及其检测框；

S14、将垛堆图像与垛堆点云进行匹配，将成功匹配垛堆进行融合，输出融合后堆垛的位置坐标及置信度。

3.如权利要求2所述基于多传感器融合机器人拆垛方法，其特征在于，采样频率的标定方法具体如下：

以采样频率较低的激光雷达为基准，激光雷达每采一帧点云，则触发相机采集一次图像。

4.如权利要求2所述基于多传感器融合机器人拆垛方法，其特征在于，基于YOLOV3算法来检测相机采集图像中的堆垛，包括堆垛的2D目标框及置信度，将2D目标框放入目标集X_C中，X_C＝{X₁,X₂…X_n}；

基于Complex-Yolo算法检测激光点云中的堆垛点云，包括堆垛点云的3D目标框及置信度，将3D目标框放入目标集Y_L中，Y_L＝{Y₁,Y₂…Y_m}。

5.如权利要求3所述基于多传感器融合机器人拆垛方法，其特征在于，基于KM算法完成激光雷达检测出的堆垛点云与相机检测出的堆垛图像之间的匹配。

6.如权利要求5所述基于多传感器融合机器人拆垛方法，其特征在于，KM算法中匹配边的权值计算公式具体如下:

λ_ij＝f_dis(X_i,Y_j)+f_iou(X_i,Y_j)

其中，f_dis是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的距离差，f_iou是目标集X_C中第i个堆垛图像X_i和目标集Y_L中第j个堆垛点云Y_j之间的目标框重叠度；λ_ij是目标集X_C中第i个堆垛图像和目标集Y_L中第j个堆垛点云Y_j间匹配边的权值。

7.如权利要求6所述基于多传感器融合机器人拆垛方法，其特征在于，目标框的重叠度用交并比公式进行计算，计算公式具体如下：

KX_i是目标集X_C中第i个堆垛图像X_i的目标框，KY_j是目标集Y_L中第j个堆垛点云Y_j的目标框，IoU_ij表示堆垛图像X_i与堆垛点云Y_j的交并比。

8.如权利要求2所述基于多传感器融合机器人拆垛方法，其特征在于，融合后堆垛的置信度计算公式具体如下：

式中，f_C、w_C分别为目标集X_C中目标物体类别的置信度及权重，f_L、w_L分别为目标集Y_L中目标点云类别置信度及权重，f_cls为融合后视觉目标类别置信度。

9.一种基于多传感器融合机器人拆垛系统，其特征在于，所述系统包括：

移动机器人，设于机器人上的相机及激光雷达，与相机及激光雷达通讯连接的处理器，处理器基于权利要求1至8任一权利要求所述基于多传感器融合机器人拆垛方法来控制移动机器人抓取堆垛。

Images