LSFP下的机器人视觉定位技术研究_魏永超.pdf

第 44 卷 第 4 期2023 年 4 月 激光杂志LASER JOURNALVol.44,No.4April,2023http /收稿日期:2022-08-21基金项目:国家自然科学基金(No.U1633127)、西藏科技厅重点研发计划(No.XZ202101ZY0017G)、中国民航飞行学院科技基金(No.CJ2020-01)、大学生创新创业训练计划项目(No.S202110624114)作者简介:魏永超(1981-),男,博士,教授,研究方向为光电信息处理、机器视觉。E-mail:mylife001 通讯作者:邓毅(1998-),男,硕士研究生,研究方向为机器视觉,点云处理。E-mail:dengyi_00 LSFP 下的机器人视觉定位技术研究魏永超,邓 毅,李 涛,李 锦,朱姿翰,徐未其中国民用航空飞行学院,四川 德阳 618307摘 要:针对当前点云的定位算法中,多平面分割效果差、误匹配以及定位精度不够高等问题,提出 LSFP的定位思想和 C-S 分割算法,对目标物体进行定位。在深度相机场景下,利用点云处理算法初步获取目标的位姿,经手眼标定后,进行初步引导;再使用 3D 扫描仪进一步定位目标物体。结果表明:(1)C-S 分割算法对多平面分割效果良好,能更准确地去除多余平面,减少点云数量。(2)在垂直于 Z 轴且 Z 轴坐标值为 171 mm 和-10 mm 的平面上,仅 PrimeSense 的定位距离的均方根误差为 7.3 mm 和 8.2 mm,LSFP 下的定位距离的均方根误差为 2.6 mm 和3.2 mm。(3)LSFP 定位思想能扩大场景定位的范围,提高物体的定位精度,具有良好的准确性。关键词:三维点云;位姿估计;LSFP;C-S 分割算法;手眼标定中图分类号:TN209 文献标识码:A doi:10.14016/ki.jgzz.2023.04.025Research on robot vision positioning technology under LSFPWEI Yongchao,DENG Yi,LI Tao,LI Jin,ZHU Zihan,XU WeiqiCivil Aviation Flight University of China,Deyang Sichuan 618307,ChinaAbstract:At the problems of poor multi-plane segmentation,mis-matching and insufficient localization accuracy in the current localization algorithm of point clouds,the localization idea of LSFP and C-S segmentation algorithm are proposed to localize the target object.In the depth camera scene,the point cloud processing algorithm is used to ini-tially obtain the bit position of the target,which is initially guided by hand-eye calibration;then the 3D scanner is used to further localize the target object.The results show that:(1)the C-S segmentation algorithm works well for multi-planar segmentation and can more accurately remove redundant planes and reduce the number of point clouds.(2)On planes perpendicular to the Z-axis and with Z-axis coordinate values of 171 mm and-10 mm,the root mean square error of the localization distance is 7.3 mm and 8.2 mm for PrimeSense only,and 2.6 mm and 3.2 mm for LS-FP.(3)The LSFP localization idea can expand the range of scene localization and improve the object localization ac-curacy with good accuracy.Key words:3D point cloud;posture estimation;LSFP;C-S segmentation algorithm;hand-eye calibration01 引言在人工智能时代,人类通过视觉可以获取众多的外部信息,若将这一特点应用到机器人上,其应用领域更加广泛。视觉定位技术在其中扮演着重要的角色,机器人通过机器视觉实现对物体的识别,从而进行后续操作。深度相机获取三维信息是目前机器视觉和自动驾驶等领域的主流手段,且基于三维信息的视觉定位方法已经被广泛研究。吴柳生1通过引入霍夫变换的投票策略对得到的多个位姿进行聚类,并以 ICP 算法进行优化。陈廷炯2使用语义分割网络进行粗匹配,用 K-D 树加快运行速度并引入对应点采样机制加速 ICP 算法迭代,以应对在复杂环境下对无纹理、相互间有遮挡的机器人零件抓取任务。魏玉峰3提出了一种多模态信息的视觉定位与识别,通过二维图像定位物体位置信息、点云图像定位物体方向http /信息,最终完成物体的识别与定位。Charles R.Qi4-5等人提出 PointNet,并研究了 Frustum PointNets 在大规模场景中如何有效地估计目标物体的位姿。同样研究的还有 Anshul Paigwar6等人,提出基于注意机制新的三维定位网络模块,减少了要处理的点的数量和推理时间。在场景定位过程中,场景分割的效果直接决定定位的精度。魏玉锋和吴柳生等人在物体识别与定位过程中,利用欧式聚类分割在实验平台进行定位实验,并利用包围盒大小将其删除无效点云,取得较好效果。郭交通7在 RANSAC 算法和区域生长算法的基础上提出一种过分割再融合的平面点云分割方法,通过迭代方式分割场景平面点云。在应用方面,如哈尔滨工程大学和北京领邦仪器公司设计研发的用于零件检测的视觉分拣系统。尽管视觉定位技术相关研究火热,但在许多工厂实际应用中仍采用示教器或者单目相机,不能满足多样化的工厂作业。基于以上研究,发现机器视觉定位对适应多样化工业任务具有重大意义。但随着场景及目标物体的变化,对视觉定位的准确性和可操作性要求较高。结合深度相机与 3D 扫描仪的视场特点,提出一种“大视场引导,小视场定位”的思想,即 LSFP(Large and Small FOV Positioning);针对欧式聚类分割等平面分割的不完全,提出 C-S 分割算法。并搭建基于 LSFP的手眼标定硬件系统,完成对目标物体的识别和定位,验证了所提出的思想和分割算法对物体进行识别与定位的准确性和可行性。2 点云位姿估计算法对原始点云采用滤波、采样算法,减少整体场景点云的数量;采用分割等算法,获取目标物体的点云信息。提出 C-S 分割算法,针对不同场景都有较好的分割效果。使用 CVFH 描述子,在有遮挡的情况也可以有较好的识别效果;结合 FPFH 特征描述子,获取目标物体的位姿信息。并提出 LSFP 的定位思想,即视场大的深度相机和视场小但精度高的 3D 扫描仪结合,当场景大且杂时,利用深度相机定位目标物体或附近明显的标志物体,即进行位姿的初步引导;当在3D 扫描仪视场范围内时,再使用 3D 扫描仪获取高精度的场景点云进行定位。算法整体结构如图 1 所示。2.1 点云获取将从深度相机得到的原始场景点云进行滤波、降采样处理,如图2 所示,2(a)左为原始点云,2(a)右为采样后点云,数据量由原本的 74 519 个下降到 7 426个;针对噪声等杂乱点云进行滤波处理,统计滤波器用于去除明显离群点的效果如图 2(b)所示;半径滤波器用于去除明显离群点如图 2(c)所示;直通滤波如 2(d)所示,可以根据深度相机或 3D 扫描仪与目标物体的相对位置设置点云在 x,y,z 方向上的范围,较快减除离群点。图 1 算法整体结构图(a)(b)(c)(d)图 2 点云滤波效果62魏永超,等:LSFP 下的机器人视觉定位技术研究http /2.2 C-S 分割算法不同的点云数据,它们的空间、几何、颜色以及纹理特征不同,点云分割以上述特征为依据对点云数据进行划分。在 PCL 库中实现有欧式聚类分割8、基于随机采样一致性分割9、区域生长分割10等。为了增强分割效果,提高后续配准精度,提出 C-S 分割算法,如图 3 所示,基本流程如下:1)原始点云数据为输入点云集 P,用 K-D 数建立离散点之间的拓扑关系,并新建一空的聚类Qi和点队列 M;2)对每个点 pn(pnP),用 k 近邻搜索找到其最近的 n 个点,计算欧氏距离,将这 n 个点中到 pi的距离小于阈值 r 的点放入中;pi-Mi2r,(Mi Mni)3)对所有 pnP 的点云,均执行第二步,当 Mni中不再有新的点加入时,一个聚类完成,判断聚类中点云数量是否小于设定聚类簇数量的阈值下限 1和阈值上限 2,若大于 1且小于 2则将其加入 Qi中,并在剩余点云中选取新的种子点,继续执行以上步骤,直到遍历完所有点。4)当 Qi=0 时,设置一个空的种子点序列 N 和一个空的聚类数组 pn,选择种子点,加入到 N 中。5)搜索当前种子点的邻域点 pn,计算邻域点的法线与当前种子点的法线之间的夹角 i,小于平滑阈值的邻域点加入到当前区域;即 i(为夹角阈值)。图 3 C-S 算法流程图6)检查每一个邻域点 pn的曲率,小于曲率阈值的邻域点加入到 N 中,并删除当前种子点,以新的种子点继续生长,smooth为曲率阈值。smooth,ismooth7)重复进行 4),5),6)步骤,直到种子点序列被清空。此时,一个区域生长完成,并将其加入到聚类数组 pn中;8)对剩余点重复 1)8)的步骤,直到遍历完所有点,复制Qi到。9)再进行一次场景平面分割和聚类分割,若剩余的点云数量 Num2Num(根据自己的实验选取数量),则用于后续配准;否则Qi=R,再代入配准。2.3 点云配准算法点云分割和提取特征描述子后,使用目标物体的模板点云与场景中的点云分割簇进行配准。点云配准可分为粗配准和精配准,粗配准指在点云相对位姿完全未知的情况下对点云进行配准,为精配准提供良好的初始值,使用 SAC-IA 采样一致性初始配准算法11。精配准是在粗配准的基础上,让点云之间的空间位置差异最小化,使用 ICP 迭代最近点算法12。2.4 位姿估计算法的实现目标物体的位姿估计算法如图 1、图 3 所示,深度相机获取场景点云,经过点云去噪、降采样、分割以及特征描述子的计算,获取场景点云的聚类特征信息。如图 5 和 6 所示,实验平台有一个人物模型及其他杂物,通过第一步平面模型分割,尽可能去除场景中的大平面,并得到聚类集Qi。簇间距 d 使得场景点尽可能聚集,当 Qi2时,则去除;其余的Qi形成新的场景点云。在实验中发现多平面点时,分割效果较差,且不能形成聚类簇。为保证算法的有效性,额外对 Qi进行判断,若 Qi0,则进行区域生长分割。通过 smooth、数量阈值上限和下限得到 Qi,复制 Qi到R。再次进行聚类分割,若剩余 Qi的点云数量 Num2Num,则 Qi=R。图 4 原始点云分割后得到模板点云图 5 C-S 分割场景图 6 普通欧式聚类分割场景72魏永超,等:LSFP 下的机器人视觉定位技术研究http /图 7 区域生长分割场景图 4 为 C-S 分割算法得到点云目标模板,由图 5可知人物模型所在平面及不相干平面明显被去除,去除效果及保留精度由 C-S 算法的参数确定;由图 6 可知分割后的点云还保留有平面,分割效果不理想。图7 所示的区域生长分割,法线夹角阈值分别为 3、5、8、11,曲率阈值为 1,表明在场景平面中以局部为凸进行分割,分割效果如图所示,对目标物体的分割存在过分割不完全(法线夹角阈值 8、11)以及分割不完全(法线夹角 3、5)等问题。C-S 算法的第一次聚类分割,理论上可以去除场景中的平面,但实际上对参数很敏感,实际场景效果不佳。通过第一次分割簇点云个数的判断,对每一个点云分割簇进行第二次区域生长分割,并设定最大分割簇数以去除杂乱场景。在此基础上再次聚类,通过 Num2 判断剩余场景点云是否满足需求,极大提高算法的容错率和有效性。对目标物体的模板点云 Si进行 CVFH13描述子提取;对分割后的点云簇 Qi提取 CVFH 特征,将 Qi的 CVFH 与模板点云 Si的 CVFH 逐一进行比较,如图8 所示,如小于当前设定相似度阈值,则判断当前场景有目标物体。图 8 CVFH 描述子的识别对当前 Qi和模板点云 Si分别计算表面法线和FPFH 特征,采用 1.3 小节的 SAC-IA 和 ICP 算法进行配准,最终输出目标物体在场景中的位姿矩阵。在其中一个场景点云进行配准,SAC-IA 算法的获取的初始位姿结果如下:TCobj1=0.96-0.110.240.035 40.060.980.210.029 9-0.26-0.190.950.472 50001ICP 算法后获取的最终位姿结果如下:TCobj2=0.99-0.050.130.034 70.010.990.010.027 6-0.13-0.010.990.475 200013 实验结果与分析为验证 LSFP 定位思想及分割算法的可靠性,搭建 JAKA 机械臂实验平台,平台包括 PrimeSense 深度相机,3D 扫描仪,JAKA 机械臂,深度相机的视野范围为 58H,45V,70D;视距为 80 cm350 cm;3D 扫描仪视距范围约为 10 cm120 cm,精度为 0.02 mm;机械臂最大载重 7 kg。在 Windows10 下用 PCL1.8 实现点云图像的处理,实验对象为 73 mm72 mm24 mm人物头像模型。机械臂基底坐标系与相机坐标系不同,首先需要对机械臂与深度相机 PrimeSense 和 3D扫描仪组成 LSFP 的视觉系统进行手眼标定,获得机器人坐标系与两个相机坐标系的转换关系。3.1 实验环境的硬件结构设计并搭建了一套机器人视觉定位硬件系统。根据所需要的视场大小及精度要求,利用深度相机视场大以及 3D 扫描仪精度高的特点,搭建出一套基于深度相机的大视场初步引导、基于 3D 扫描仪的精确定位的硬件系统。采用 Eye-in-hand14的方式,得到当前位置下目标物体的相对相机的位姿信息,并利用矩阵转换得到相对机械臂基底坐标系的位姿。如图 9所示。图 9 大小视场的系统结构图82魏永超,等:LSFP 下的机器人视觉定位技术研究http /3.2 手眼标定及精度分析手眼标定即视觉系统坐标系到机器人基坐标位姿变换矩阵的求解。在扫描过程中,相机坐标系会随着拍摄过程发生位置变化,机器人的基坐标始终保持不变。假设标定板的左上角角点位置为 Obj,在相机视觉坐标系为C。机器人末端坐标系G到机器人基坐标系B的平移和旋转变换关系表示为 Tbg(44);相机视觉坐标系C到机器人末端坐标系E的平移和旋转变换关系表示为 Tgc(44),即手眼标定求解的关系矩阵。相机对准固定的棋盘格标定板进行拍照,如图 10 所示。图 10 手眼标定数据每一次拍照后,获取示教器中机械臂末端在机械臂基底坐标系的位姿信息。得到相机和扫描仪相对于标定板的位姿信息和图片的像素误差,深度相机拍摄得到的图片像素误差为 0.39,手眼转换矩阵误差较大,且对表面光滑的目标物体配准效果较差,对最终的定位精度有较大影响。3D 扫描仪镜头拍摄得到的图片像素误差为 0.049,误差小,得到的手眼转换矩阵误差较小,能得到更加准确的位姿信息。从 Matlab 中导入相机相对标定板的位姿信息(外参矩阵)带入15,最终得到相机坐标系与机器人末端坐标系的位姿关系。Tirgc=0.66 -0.752 0.02 16.380.75 0.66 0.01 -77.34-0.16 -0.02 0.99 10.010 0 0 13D 扫描仪标定同深度相机标定过程,最终得到3D 扫描仪的相机坐标系与机器人末端坐标系的位姿关系。Tdvp gc=0.98 0.01 0.06 -33.350.01 0.99 -0.01 -125.570.06 0.012 0.99 86.750 0 0 1通过 t(x,y,z)=ECTTirEOT1,1,1,0获得机械臂基底坐标系下标定板的 XYZ 如表 1 所示。数据分析得到 XYZ 的精度,深度相机 X 轴上的标准差是 0.46 mm,Y 轴上的标准差是 1.28 mm,Z 轴上的标准差是 0.54 mm;3D 扫描仪 X 轴的标准差是0.73 mm,Y 轴上的标准差是 1.34 mm,Z 轴上的标准差是 0.36 mm,均满足实验要求。3.3 检测识别与分析使用深度相机和 3D 扫描仪在实验场景中对模型进行识别和定位实验,对比仅深度相机视觉定位3结果与 LSFP 下的视觉定位结果。若当前场景没有识别到目标点云,则机械臂移动变换场景;部分场景如图11 所示。实验所测位置坐标均以机械臂基底坐标系为基坐标,在识别目标物体的场景中,定位场景分别为垂直于 Z 轴且 Z 轴坐标值为 171 mm 和-10 mm 的平面。图 11 场景信息(左图为深度相机,右图为扫描仪)表 1 标定板在机械臂基底坐标的位姿深度相机下的位姿信息3D 扫描仪下的位姿信息序号X/mmY/mmZ/mm序号X/mmY/mmZ/mm1-995.08-203.42360.971-995.74-203.34 361.832-996.99-203.87360.932-995.65-203.31361.803-996.28-204.89360.723-996.08-203.22361.874-996.66-205.40360.724-996.14-202.87360.9017-996.29-205.95361.3815-996.99-204.33361.4792魏永超,等:LSFP 下的机器人视觉定位技术研究http /目标物体摆放不同位置进行多次定位,最终定位信息如表 2 所示。第一组到第五组位置信息为同一个位置(绕目标物体 Z 轴旋转 5)得到,由第一组到第五组(xdep,ydep,zdep)分析得到前后五次运动结果的定位误差;其中在 X 轴方向上的误差范围为 9 mm,其中在 Y 轴方向上的误差范围为 7 mm,其中在 Z 轴方向上的误差范围为 14 mm;与 Z 轴方向的实际距离相比,测量数据与真实值的波动如图 12 左所示,其均方根误差 RMSE 为 7.3 mm。该类误差范围及 RMSE 对初步定位的引导是可行的。由第一组到第五组(xLSFP,yLSFP,zLSFP)分析得到前后五次运动结果的定位误差;其中在 X 轴方向上的误差范围为 03 mm,其中在 Y 轴方向上的误差范围为 03 mm,其中在 Z 轴方向上的误差范围为 05 mm,与 Z 轴方向的实际距离相比,数据与真实值的波动如图 12 左所示,其均方根误差 RMSE 为 2.6 mm。对定位误差来说,误差范围相对于物体体积 5%以内都可接受,实验所得结果在 4%以内,表明相对误差较小,目标物体的定位是合理的,不对后续的操作造成影响。图 12 Z 轴上定位值与真实地面值对比图表 2 目标物体的位姿估计序号深度相机获取的目标位置LSFP 下获取的目标位置实际位置xdep/mmydep/mmzdep/mmxLSFP/mmyLSFP/mmzLSFP/mm184.5760.90168.68108.3848.51169.43171288.6560.17170.41107.4948.36173.61171386.3463.57182.35108.5350.13172.79171493.2656.51181.37110.0747.55173.54171586.4162.74176.56107.5848.96174.931716-17.9429.01223.67-23.3410.94223.942247422.04-445.66-18.61434.10-464.93-6.39-108428.71-476.92-13.94423.56-496.33-5.99-109435.66-476.36-17.43442.25-474.15-5.49-1010458.28-488.34-20.99461.43-473.79-8.67-1011442.09-440.62-21.44451.67-454.30-10.36-1012449.85-443.15-19.23458.78-465.75-6.28-1013455.61-523.91-15.67460.53-530.40-7.52-1014449.67-565.22-15.51456.71-586.29-6.45-10 第六组到第十四组位置信息是在场景中任意摆放得到的目标物体位置信息,通过算法分别得到仅深度相机下目标物体位置信息和在 LSFP 下目标物体位置信息的计算结果。如表 2 所示,整体上看,深度相机定位的物体位置在 Z 轴上的偏差在 6.0 mm 内,与Z 轴方向的实际距离相比,测量数据与真实值的波动如图 12 右所示,其均方根误差 RMSE 为 8.2 mm。LSPF 下定位的物体位置在 Z 轴的偏差在 4.0 mm 内,与 Z 轴方向的实际距离相比,测量数据与真实值的波动如图 12 右所示,其均方根误差 RMSE 为 3.2 mm。分析偏差产生的原因主要是深度相机获取的点云因为距离的不同,其表面的点密度不均匀,另外随着检测距离增加,精度也会变差;滤波、采样、分割后得到目标模型点云与实际目标点云形状大小有些许不同,导致最终配准有所偏差;另外根据采样的密度不同,目标模型质心也有变化。就 LSFP 的定位思想来说,是可行的。与仅使用深度相机定位相比,LSFP 不仅扩大了场景定位范围,也提高了定位精度;(xdep,ydep,03魏永超,等:LSFP 下的机器人视觉定位技术研究http /zdep)可以作为目标物体定位的初步引导,得到更加精确的位置信息(xLSFP,yLSFP,zLSFP)。4 结束语利用点云图像进行物体位姿估计,在多平面的场景进行目标物体的定位时,提出 C-S 算法极大提高算法的容错率和有效性,可以有效分割场景点云。受硬件条件限制,深度相机对平滑的目标物体配准效果较差,对最终的定位精度有较大影响。3D 扫描仪得到的手眼转换矩阵误差较小,能得到更加准确的位姿信息,且对目标物体没有太多限制,更能适应定位物体的泛化性。所提出的 LSFP 的思想,扩大了定位场景的范围,具有良好的准确性和定位精度。该分割算法和 LSFP 思想同样适用于其他的目标物体的位姿估计。参考文献1 吴柳生.基于点对特征描述子的三维物体识别和位姿估计方法研究D.武汉:华中科技大学,2019.002759.2 陈廷炯,秦威,邹德伟.基于语义分割和点云配准的物体检测与位姿估计J.电子技术,2020,(1):36-40.3 魏玉锋,梁冬泰,梁丹,等.基于多模态信息的机器人视觉识别与定位研究J.光电工程,2018,45(02):73-84.4 Qi C R,Su H,Mo K,et al.Pointnet:Deep learning on point sets for 3d classification and segmentationC/Pro-ceedings of the IEEE conference on computer vision and pattern recognition.2017:652-660.5 Qi C R,Liu W,Wu C,et al.Frustum pointnets for 3d ob-ject detection from rgb-d dataC/Proceedings of the IEEE conference on computer vision and pattern recogni-tion.2018:918-927.6 Paigwar A,Erkent O,Wolf C,et al.Attentional poin-tnet for 3d-object detection in point cloudsC/Proceedings of the IEEE/CVF Conference on Computer Vision and Pattern Recognition Workshops,2019.7郭交通.基于点云的目标识别和位姿估计方法研究D.南京:南京航空航天大学,2019.8 李文,刘德儿,王有毅,等.基于超体素的区域聚类的复杂场景分割J.激光与红外,2021,(11):1425-1432.9 Shi X,Peng J,Li J,et al.The iterative closest point regis-tration algorithm based on the normal distribution transfor-mationJ.Procedia Computer Science,2019,147:181-190.10 Fan Y,Wang M,Geng N,et al.A self-adaptive segmen-tation method for a point cloudJ.The Visual Computer,2018,34(5):659-673.11 Li W,Cheng H,Zhang X.Efficient 3D Object Recognition from Cluttered Point CloudJ.Sensors,2021,21(17):5850.12 Li P,Wang R,Wang Y,et al.Evaluation of the ICP algo-rithm in 3D point cloud registration J.IEEE Access,2020,8:68030-68048.13 He L,Wang X,Zhang H.M2DP:A novel 3D point cloud descriptor and its application in loop closure detectionC/2016 IEEE/RSJ International Conference on Intelli-gent Robots and Systems(IROS).IEEE,2016:231-237.14 Torabi L,Gupta K.An autonomous six-DOF eye-in-hand system for in situ 3D object modelingJ.The International Journal of Robotics Research,2012,31(1):82-100.15 Zhang Z,Zhang L,Yang G Z.A computationally efficient method for hand-eye calibrationJ.International journal of computer assisted radiology and surgery,2017,12(10):1775-1787.13魏永超,等:LSFP 下的机器人视觉定位技术研究