基于平面点云配准与相对位姿控制的工业件装配方法
1 引言
具有自主感知决策能力的机械臂装配是工业自动化生产的重要组成部分,其对空间中目标定位与机械臂自身运动规划有着较高要求。目前在现代化生产中主要通过机器视觉系统控制机械臂完成装配任务:首先对相机、机械臂、夹具等部分进行离线标定;之后使用相机对机械臂携带的目标物体进行拍照,并对图像中的物体特征进行提取和分析,获取到物体在空间中的绝对位姿;最后控制机械臂到达指定位置完成装配任务[1-3]。然而,工业现场中目标工件多为平面弱几何轮廓结构。平面弱几何轮廓结构主要是指整体以平面特征为主的被测物,其轮廓几何特征单一、表面法向量变化有限且表面纹理较少,具体可定义为由平面组成的弱纹理、少几何特征结构体。平面弱几何轮廓结构难以只通过其图像灰度信息获取准确的位姿信息。同时机械臂自身结构参数、相机工件标定结果、系统开环控制等存在偏差,往往无法完成严格公差下的装配任务。而基于点云信息的目标定位技术具有精度高、信息丰富、无视弱纹理结构等优势,可最大程度采集物体三维结构信息并极大改善光源等环境因素带来的不利影响[4-6]。同时基于视觉伺服的机械臂控制方法通过对目标位姿结果误差进行闭环反馈控制,可以极大减小系统标定带来的误差,规划出的运动轨迹也将更加合理[7]。这二者为解决上述问题提供了有效思路。
目前基于点云信息目标定位技术的研究主要分为基于点云局部特征定位与全局特征定位两大类。对于全局特征方法:Mellado等[8]提出一种基于随机采样一致性(RANSAC)算法框架的4PCS配准方法,通过构建两组点云中全等四点对的方式来减少算法复杂度、加速匹配过程;Xu等[9]提出一种基于branch and bound(BnB)的配准方法,实现迭代最近点(ICP)框架下的全局最优配准。然而,基于全局特征配准定位的方式虽然有较快的匹配速度,但精度不高。对于局部特征方法,比较有代表性的工作为Rusu等[10-12]提出的基于fast point feature histogram(FPFH)的配准策略,通过快速点特征直方图得到的统计特征求解出对应点之间的变换关系,从而获取目标物体的空间位姿。基于局部特征配准的方法精度较全局配准有所提高,但在面对以平面为主要特征的目标时,点云中各部分特征描述子差异性弱,易匹配到错误的对应点,导致点云匹配失败。
基于视觉伺服的机械臂控制方法利用视觉信息作为反馈,对机械臂的工作环境和对象进行非接触式的测量,提高了机械臂的感知能力与工作精度。但由于视觉伺服系统将视觉误差信号转换为机械臂的速度信号,所以对视觉信息采样频率与处理效率有较高的要求。特别是对于高精度的双目结构光传感器而言,需要对同一位置的对象进行多次投影,导致传统的视觉伺服不再适合这种测量方式。另外在机械臂装配过程中,一个重要的误差来源是装配件的抓握位姿,对于无法保证抓握到理想位置的情况通常需要通过传感器二次校准进行位姿补偿,但额外添加测量设备往往会导致更多的误差,对最终机械臂定位精度造成影响。
综上所述,针对新环境下存在的点云配准稳定性不足与机械臂伺服控制失效等问题,在充分发挥结构光传感器点云信息精度高、信息丰富等优势的前提条件下,本文提出一种基于平面点云配准与相对位姿控制的装配方法,解决了在以平面特征为主的工业件在装配过程中由于定位误差较大导致的装配失败问题,为平面特征工件抓取方法研究提供新的思路。设计了一种基于点云平面特征(平面法向量与轮廓最大距离关系)的点云配准纠正方法,该方法可有效避免平面点云弱几何轮廓特征导致的点云配准陷入局部最优问题,提升点云配准精度和成功率。构建了一种基于相对位姿的机械臂控制方法,改善了对工件抓握位置的精度要求,同时减少了由于系统标定不准所带来的误差。提出了一种融合高精度点云配准与相对位姿控制的工业件装配方法,使用大型机械臂进行实验,完成了公差为0.5 mm的高精度装配任务。
2 所提方法
所提方法流程如
首先对装配底板进行离线标定以获取待装配件目标位姿,其次通过模型中的已知信息推算出工件成功装配时的位姿状态,依据该位姿可以生成目标点云。之后使用结构光传感器重建出装配件当前点云,并通过基于FPFH特征点的RANSAC配准方法对当前、目标点云进行粗配准,根据主平面法向量方向以及距离关系判断并纠正粗配准的结果,为ICP提供更加可靠鲁棒的初值,最终得到装配件从当前位置到目标位置的转换矩阵。
将上述转换矩阵作为系统输入来控制机械臂运动补偿位姿偏移量。由于转换矩阵中提供的是位姿的相对变化量,因此无需确定机械臂抓握装配件的绝对位姿,不再需要通过其他传感器进行绝对位姿补偿,简化了实验步骤,减少了误差来源。同时由于位姿误差计算始终基于同一坐标系,故装配结果误差评定不受手眼标定误差影响,可反映出更准确的精度范围。
3 基于平面点云匹配的相对位姿估计
基于点云配准的位姿估计方法本质上是将两个不同坐标系下的相同物体进行平移和旋转操作,使得点云重合,最终得到两个坐标系之间的旋转和平移关系。但是,传统点云配准方法仅针对几何轮廓结构信息丰富物体,在处理单一平面结构的物体时容易出现非线性优化解算振荡及无法收敛问题。为此,提出一种结合平面法向量和轮廓信息的鲁棒性点云配准方法。
3.1 基于FPFH特征点的RANSAC-ICP配准策略
首先通过点云FPFH特征和RANSAC进行粗配准,再而利用ICP算法进行精配准。通过任意特征点p在k邻域范围内的点位置以及法向量信息,建立p与邻域点之间的局部坐标系,计算关于距离和角度的特征信息,统计生成点特征直方图。为降低计算复杂度,将p邻域点也作为特征点进行计算,并通过引入距离加权平均的方式求出特征点p的k邻域FPFH。在FPFH特征空间中查询最近邻可以得到不同组点云中具有相似局部几何结构的点,将其作为对应点用于RANSAC全局配准。
采用RANSAC进行全局配准时:首先不断对点云进行采样,根据采样点在不同点云中欧氏距离不变这一特性对采样点形成的多边形边长之间的比值进行检查,计算对应边的相异值,凭此确定两组点云的对应点;之后估计变换矩阵并应用至当前点云,依据点云欧氏距离设定阈值,迭代挑选出包含局内点最多的参数模型;最后使用内点重新估计变换矩阵,计算变换后内点对应点之间的误差,确定最终的变换结果。
但是上述过程难以满足更高精度的配准要求,需要通过ICP算法进行点云精配准。在依靠RANSAC获取到高质量位姿初值的前提下,ICP通过迭代最近点的方法得到对应点对距离误差最小的最佳变换关系,最终得到高精度的点云配准结果。
3.2 基于法向量与最大距离关系的位姿纠正
由于上述配准方法依靠点云特征描述子获得对应点,在应对以平面为主要特征的点云时往往会出现以下两种错误匹配的情况——上下颠倒和前后相反。且这些错误配准无法通过调整精配准阈值纠正,很容易陷入局部最优。为解决这一问题,提出一种基于平面点云法向量以及轮廓最大距离关系的配准位姿纠正方法。
针对配准结果上下颠倒的问题,可以通过点云法向量夹角进行判定:首先基于局部表面拟合的方法进行法向量估计,点云中某点的法向量通过拟合该点局部邻域平面法向量来表示。即对点云中的某个点
式中:
平面
正确配准的结果中两点云主平面法向量夹角近似为零,但错误配准结果中这一夹角往往会远远大于这一数值,可通过粗配准结果中法向量夹角判断是否达到精度要求。值得一提的是法向量的方向,点云法向量方向通常是由点云中的点到空间中某一选定点的向量方向来确定的,为保证法向量方向不受选定点的位置影响,将计算得到的点云质心作为选定点,使得点云法向量始终指向点云模型内部,不受点云位置以及姿态的影响。在获取到点云法向量后,若相差角度过大,需要通过刚体变换使得两个法向量重合,得到法向量方向一致的配准结果。
若粗配准点云主平面法向量为
式中:
由罗德里格斯公式可以求得旋转矩阵:
式中:
由于待配准的点云质心不是坐标系原点,在进行刚体变换的过程中需要将点云质心平移至坐标系原点进行旋转操作后再平移回原有位置。该过程中旋转向量并不是点云的对称轴,会导致点云在主平面上产生一定角度的旋转,旋转角度依据旋转向量而定。这一结果会导致出现前后相反这一问题,最终会使得后续精配准陷入局部最优,下面来解决这一问题。
针对配准结果前后相反这一问题,提出一种旋转配准的方法:由于两个点云主平面已经重合,那最终的配准状态一定是经过绕主平面法线旋转某一角度后的位置。这里在每次旋转单一角度后,通过计算点云对应点欧氏距离来挑选出最佳粗配准结果,理论上单次旋转角度设定得越小,则作为精配准初始位姿效果越好,但也会导致时间成本的增加,本实验每次旋转5°来获取最佳粗配准结果。其中,旋转操作还是要遵循平移—旋转—平移这一流程,而旋转中心的选取需要额外注意,若将其设定为点云质心往往只能保证两组点云质心位置相差不大时效果较好,但点云扫描得到的当前点云并不是完整的,且可能会由于夹具或环境障碍物的遮挡导致缺失部分特征,这表明两组点云质心往往不是对应点,在这种情况下使用质心作为旋转中心的话,后续旋转操作没有任何意义。因为无论如何旋转,都不会到达正确配准位姿,所以需要找到一组正确的对应点作为旋转中心。采用查找轮廓上距离最大的两个点的中点作为旋转中心的方法,通过提取轮廓、遍历轮廓点,求取任意两点间欧氏距离,可得到轮廓最大距离:
将
4 基于相对位姿的机械臂伺服控制
上文论述的点云配准结果是基于结构光传感器下的两组点云的位姿变换关系,为保证最终的装配精度,依靠相对位姿变换矩阵直接控制机械臂末端进行运动。首先在4.1节中着重阐述了相对位姿控制的原理,从理论上证明了刚体在空间运动中的位姿变换一致性。其次在4.2节中结合现实工业机械臂装配模型,建立基于相对位姿的机械臂伺服控制系统,因为未对工具坐标系进行标定,从根本上解决了由待装配件抓握位置不准带来的误差。同时,整体系统装配误差只来源于传感器坐标系下位姿结果,从一定程度上避免了机械臂手眼标定误差对结果的影响。
4.1 基于相对位姿控制的原理分析
在经典动力学中,刚体被定义为在外力作用下,形状与大小都不发生改变的物体,其基本的运动方式包括平动与转动。在平动过程中,刚体中所有点的运动轨迹都保持一致,运动状态也相同,所以可以选择刚体上任意一点来代表整个刚体的运动;而对于转动,本研究着重讨论的是相对变换关系不变的两个坐标系的旋转一致性。
假设空间基坐标系为
设
依据角标相消法则,有
进而可以得到
因为两坐标系之间转换关系保持不变,
等式两边同时右乘
上面证明过程说明两个固连在一起的坐标系在空间运动时具有相同的变换关系,具有变换一致性。
4.2 基于相对位姿的机械臂控制
在装配过程中,待装配件往往通过夹具或是吸盘牢牢固定在工具坐标系下,此时,在不发生碰撞的情况下,装配件、夹具、机械臂末端可视为一个刚体,依据上节证明结果可知,装配件、夹具、机械臂末端所对应的工件坐标系、工具坐标系、机械臂末端坐标系具有相同的变换关系。又因为由结构光传感器得到的相对位姿变换是待装配件当前点云与目标点云之间的转换关系,则该转换关系同时也是机械臂末端的转换关系,所以可以通过控制机械臂末端完成相对位姿的补偿达到工件装配的效果,这样省略了工件坐标系与工具坐标系的标定,并对抓件位姿有极大的包容性,提高了装配精度。
5 实验与分析
5.1 实验任务
待装配对象为9块轮廓各异的平面装配件,每块上面固定有两个定位销。装配底板为400×320×15的方形槽体,槽体中分布有定位孔,依据两孔一面原理实现装配件的定位。其中,底板圆孔尺寸公称值为6 mm,装配件上定位销直径公称值为5 mm,若装配成功,则证明该装配可实现0.5 mm装配精度。装配实验中装配零部件如
图 3. 装配实验零部件。(a)装配件;(b)底板;(c)装配结果
Fig. 3. Assemble experimental parts。(a) Assemblies; (b) base plate; (c) assembly results
5.2 实验环境
分别搭建工件形貌扫描配准实验平台以及真实工业机械臂装配平台进行验证。
5.2.1 工件形貌扫描及配准平台
通过搭建双目结构光传感器对工件形貌进行三维重建,如
图 4. 双目结构光系统。(a)双目结构光传感器整体结构图;(b)结构光传感器内部连接图
Fig. 4. Binocular structured light system. (a) Overall structure diagram of the binocular structured light sensor; (b) structured light sensor internal connection diagram
5.2.2 工业机器人装配平台
搭建了包含六自由度工业机械臂(型号ABB-2600)、硅胶吸盘(型号DP-50)、双目结构光传感器及待装配工业件的实验平台,如
将结构光传感器固定在工作台一侧,属于眼在手外工作模式。为此,需要通过激光跟踪仪-机器人标定法与efficient perspective-n-point(EPNP)匹配算法联合标定的手眼标定方法将结构光传感器坐标系与机械臂坐标系统一起来。结构光传感器扫描重建出工件三维点云后将点云传输给上位机进行点云配准,上位机计算相对位姿并将变换矩阵传输到机械臂控制系统中,随后控制系统在机器人操作系统(ROS)中发布定位信息并将规划出的运动轨迹传输给机械臂控制台,使得机械臂可以运动到指定位姿完成装配。实验最终精度评判标准通过工件装配的成功率来验证。
5.3 实验结果及分析
通过使用多种点云配准算法对不同种类装配件点云进行配准实验来验证所提基于平面特征的点云配准方法的精度优势。同时,在实际机械臂场景下进行装配实验来验证基于相对位姿变换的机械臂控制方法的可行性和配准鲁棒性。
5.3.1 点云配准实验
首先,对9块不同形状的窄边平面装配件进行配准实验,分别使用RANSAC+ICP、4PCS+ICP、GO-ICP、TEASER++以及所提配准算法,实验结果如
图 6. 不同样本点云配准结果。(a)装配件1;(b)装配件2;(c)装配件3;(d)装配件4;(e)装配件5;(f)装配件6;(g)装配件7;(h)装配件8;(i)装配件9
Fig. 6. Point cloud registration results for different samples. (a) Assembly 1; (b) assembly 2; (c) assembly 3; (d) assembly 4; (e) assembly 5; (f) assembly 6; (g) assembly 7; (h) assembly 8; (i) assembly 9
图 7. 装配件3点云配准重复性实验。(a)实验1;(b)实验2;(c)实验3;(d)实验4;(e)实验5
Fig. 7. Assembly 3 point cloud registration repeatability experiment. (a) Experiment 1; (b) experiment 2; (c) experiment 3; (d) experiment 4; (e) experiment 5
从
5.3.2 工件装配实验
对不同装配件以及装配件在不同初始位姿的情况下进行装配实验,使用结构光传感器对装配件进行扫描重建,然后将计算得到的相对位姿矩阵输入ROS中控制机械臂执行操作,实验结果如
图 8. 实验结果。(a)装配件点云配准结果;(b)(c)真实场景装配结果
Fig. 8. Experimental results. (a) Assembly point cloud registration results; (b) (c) real-world scene assembly results
在点云配准过程中,所提基于平面特征的点云配准纠正算法对装配成功率有明显的提升,在多个姿态下对不同装配件采用相对位姿的机械臂控制算法进行装配实验。使用RANSAC-ICP点云配准算法与基于平面特征的点云配准算法分别进行40次实验,RANSAC+ICP配准方法只成功10次,成功率为25%,但所提点云配准算法成功率为100%。另外,从
表 1. 装配实验结果
Table 1. Assembly experiment results
在装配过程中,所提基于相对位姿控制机械臂运动的方法较传统装配方法在装配精度上也有很大的提升,传统装配方法通过标定工件、工具坐标系计算绝对坐标控制机械臂完成装配。实验中使用这两种装配方法分别对多个姿态下的不同装配件进行40次装配实验,其中,配准方法均采用基于平面特征的点云配准方法。传统装配成功率仅为15%,而基于相对位姿控制的方法成功率高达100%,有85个百分点的提升。上述实验结果充分证明了所提装配方法可稳定实现对不同平面装配件的相对位姿估计并完成高精度的装配任务。
6 结论
提出一种基于平面点云与相对位姿估计的平面弱几何轮廓结构的装配件自动化解决方法,通过三维点云配准得到的相对位姿变换关系来指导机械臂按照正确变换关系实现精准装配。为解决配准过程中可能出现的上下颠倒和前后相反的点云配准二义性问题。首先提出基于点云主平面特征及轮廓距离关系的精配准方法,进而使用不依赖于机械臂标定结果和不受时间形变振动影响的相对位姿而非绝对位姿来控制机械臂精确运动,极大提高了系统长时间装配系统性能。最后,通过实验验证了所提方法的可行性、正确性及优势。
[1] 董云龙, 李祥飞, 刘行, 等. 航空航天领域机器人化智能装配技术综述[J]. 人工智能, 2022, 9(3): 6-20.
Dong Y L, Li X F, Liu X, et al. Overview of robotic intelligent assembly technology in the aerospace field[J]. AI-View, 2022, 9(3): 6-20.
[2] 黄海松, 张松松. 机器人协同装配的发展与应用研究综述[J]. 贵州大学学报(自然科学版), 2022, 39(6): 17-24.
Huang H S, Zhang S S. Development and application of robot collaborative assembly[J]. Journal of Guizhou University (Natural Sciences), 2022, 39(6): 17-24.
[4] 韩家杰, 周建平, 薛瑞雷, 等. 线结构光管道焊缝表面形貌重建与质量评估[J]. 中国激光, 2021, 48(14): 105-114.
Han J, Zhou J P, Xue R L, et al. Surface morphology reconstruction and quality evaluation of pipeline weld based on line-structured light[J]. Chinese Journal of Lasers, 2021, 48(14): 105-114.
[5] Diao S P, Yang H D, Xiang Y, et al. Research on splicing method of point clouds with insufficient features based on spatial reference[J]. Journal of Electronic Imaging, 2021, 30(4): 043008.
[6] Mehranfar M, Arefi H, Alidoost F. Knowledge-based 3D reconstruction of bridge structures using UAV-based photogrammetric point cloud[J]. Journal of Applied Remote Sensing, 2021, 15(4): 044503.
[7] 陶波, 龚泽宇, 丁汉. 机器人无标定视觉伺服控制研究进展[J]. 力学学报, 2016, 48(4): 767-783.
Tao B, Gong Z Y, Ding H. A survey of uncalibrated robot visual servoing control[J]. Chinese Journal of Theoretical and Applied Mechanics, 2016, 48(4): 767-783.
[8] Mellado N, Aiger D, Mitra N J. Super 4PCS fast global pointcloud registration via smart indexing[J]. Computer Graphics Forum, 2014, 33(5): 205-215.
[9] Fan Y F, Yao X F, Xu X F. Automatic surface-matching spatial registration method for neuronavigation using the globally optimal iterative closest point[J]. Journal of Medical Imaging and Health Informatics, 2017, 7(5): 1099-1102.
[10] RusuR B, BlodowN, MartonZ C, et al. Aligning point cloud views using persistent feature histograms[C]∥2008 IEEE/RSJ International Conference on Intelligent Robots and Systems, September 22-26, 2008, Nice, France. New York: IEEE Press, 2008: 3384-3391.
[11] RusuR B, BlodowN, BeetzM. Fast point feature histograms (FPFH) for 3D registration[C]∥2009 IEEE International Conference on Robotics and Automation, May 12-17, 2009, Kobe, Japan. New York: IEEE Press, 2009: 3212-3217.
[12] Rusu R B. Semantic 3D object maps for everyday manipulation in human living environments[J]. KI - Künstliche Intelligenz, 2010, 24(4): 345-348.
Article Outline
魏博言, 杜弘志, 张瀛, 任永杰, 孙岩标. 基于平面点云配准与相对位姿控制的工业件装配方法[J]. 激光与光电子学进展, 2024, 61(4): 0415001. Boyan Wei, Hongzhi Du, Ying Zhang, Yongjie Ren, Yanbiao Sun. Planar Point Cloud Registration and Relative Pose Control-Based Assembly Method of Industrial Parts[J]. Laser & Optoelectronics Progress, 2024, 61(4): 0415001.