基于点云法矢方向聚类伺服的机器人定位控制方法及系统与流程
本发明属于视觉伺服技术领域，更具体地，涉及一种基于点云法矢方向聚类伺服的机器人定位控制方法及系统。
背景技术：
机器人定位与伺服控制在机器人化加工制造、智能物流仓储、医疗辅助等领域有广泛的应用需求。视觉伺服是一种利用视觉信息控制机器人运动到期望位置以实现机器人定位的视觉控制技术。
现有的视觉伺服技术一般分为基于位置的视觉伺服(pbvs)和基于图像的视觉伺服(ibvs)。pbvs需要获得目标的几何模型，相机的内部参数以及观测到的图像特征信息，然后估计相机与目标的相对位姿，减小位姿误差是pbvs系统的主要任务。与此相比，ibvs系统中图像的特征信息直接用于控制机器人，通过构造图像雅克比矩阵建立控制方程实现伺服任务。
ibvs和pbvs两种伺服控制技术都需要在图像中提取特征信息，特征提取的精度很大程度上决定了伺服控制的精度，但是在很多场景应用中(比如对光滑的风电叶片，高铁壳体等构件的寻位加工)，目标的纹理特征缺失，很难在图像空间中提取出稳定性和精度较高的图像特征，这给伺服控制带来了极大的挑战。
技术实现要素：
针对现有技术的缺陷和改进需求，本发明提供了一种基于点云法矢方向聚类伺服的机器人定位控制方法及系统，其目的在于，解决现有的视觉伺服技术应用于弱纹理技术时，存在的定位稳定性差、精度低的技术问题。
为实现上述目的，按照本发明的一个方面，提供了一种基于点云法矢方向聚类伺服的机器人定位控制方法，其中，机器人具有六自由度运动，且与一个rgbd深度相机固定连接；该基于点云法矢方向聚类伺服的机器人定位控制方法包括如下步骤：
(s1)计算机器人相对于待定位目标处于理想位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的理想点云图像；
(s2)计算理想点云图像中各点的法矢量后，进行聚类，得到理想点云图像的聚类质心集合
(s3)计算机器人相对于待定位目标处于实际位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的实际点云图像；
(s4)计算实际点云图像中各点的法矢量后，进行聚类，得到实际点云图像的聚类质心集合mc；
(s5)根据理想点云图像的聚类质心集合
(s6)判断速度vc是否已经收敛到0，若是，则伺服控制结束；否则，使机器人按照速度vc移动预设时间后，转入步骤(s3)。
本发明将rgbd深度相机与具有六自由度运动的机器人固定连接，在视觉伺服的过程中，利用rgbd深度相机获取机器人相对于待定位目标在理想位置以及实际位置时，待定位目标的点云图像，分别计算两个点云图像中各点的法矢量并进行聚类，基于聚类得到的两个聚类质心集合计算相应的旋转矩阵和平移变换向量，并进一步计算速度，使机器人按照计算得到的速度移动，直至机器人移动至理想位置。在此过程中，无需提取待定位目标的图像特征，因此，本发明不依赖于待定位目标的图像特征，对于弱纹理目标同样具有稳定、高精度的定位结果。
本发明仅利用聚类得到聚类质心集合计算rgbd深度相机在当前位置相对于理想位置的旋转矩阵r和平移变换向量c*tc，实现了点云配准，在此过程中，无需对点云图像中的所有点进行计算，有效提高了点云配准的时间效率。
进一步地，步骤(s5)中，根据理想点云图像的聚类质心集合
进一步地，步骤(s2)和步骤(s4)中，按照如下步骤对点云图像中各点的法矢量进行聚类，得到聚类质心集合：
(t1)选择k个法矢量作为初始法矢质心向量；
(t2)对于点云图像中的每一个点，计算其法矢量与每一个法矢质心向量的余弦相似性，从而确定与该点的法矢量具有最大余弦相似性的法矢质心向量，将该点聚入该法矢质心向量所在的法矢聚类集合；
(t3)点云图像中所有点聚类完成后，更新每个法矢聚类集合的法矢质心向量；
(t4)若各法矢质心向量更新前、后的角度变化均小于等于预设的阈值ε，则判定聚类过程收敛，转入步骤(t5)；否则，转入步骤(t2)；
(t5)将当前聚类得到的k个法矢聚类集合作为聚类结果，计算各法矢聚类集合的聚类质心的空间坐标，得到聚类质心集合；
其中，k为预设正整数，ε＞0。
进一步地，步骤(t3)中，按照
其中，j＝1,2...k，
进一步地，步骤(t5)中，按照
其中，j＝1,2...k；mj表示第j个法矢聚类集合对应的点集，各点以空间坐标的形式表示；|mj|表示点集mj中点的个数；pj表示第j个法矢聚类集合的聚类质心的空间坐标。
进一步地，步骤(s5)中，按照vc＝-λrtc*tc计算rgbd深度相机的速度vc；
其中，λ为尺度参数。
按照本发明的另一个方面，提供了一种基于点云法矢方向聚类伺服的机器人定位控制系统，包括：定位控制模块和视觉平台；
视觉平台由具有六自由度运动的机器人及与其固定连接的rgbd深度相机构成；
定位控制模块包括：理想点云图像获取单元、第一聚类质心集合获取单元、实际点云图像获取单元、第二聚类质心集合获取单元、速度获取单元以及移动控制单元；
理想点云图像获取单元，用于计算机器人相对于待定位目标处于理想位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的理想点云图像，之后触发第一聚类质心集合获取单元；
第一聚类质心集合获取单元，用于计算理想点云图像中各点的法矢量后，进行聚类，得到理想点云图像的聚类质心集合
实际点云图像获取单元，用于计算机器人相对于待定位目标处于实际位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的实际点云图像，之后触发第二聚类质心集合获取单元；
第二聚类质心集合获取单元，用于计算实际点云图像中各点的法矢量后，进行聚类，得到实际点云图像的聚类质心集合mc，之后触发速度获取单元；
速度获取单元，用于根据理想点云图像的聚类质心集合
移动控制单元，用于判断速度vc是否已经收敛到0，若是，则控制结束；否则，使机器人按照速度vc移动预设时间后，触发实际点云图像获取单元。
总体而言，通过本发明所构思的以上技术方案，能够取得以下有益效果：
(1)本发明利用rgbd视觉技术，获得机器人相对于待定位目标在理想位置和实际位置时，待定位目标的点云图像，分别计算两个点云图像中各点的法矢量并进行聚类，基于聚类得到的两个聚类质心集合计算相应的旋转矩阵和平移变换向量，并进一步计算速度，使机器人按照计算得到的速度移动，直至机器人移动至理想位置；在此过程中，无需提取待定位目标的图像特征，因此，本发明对于弱纹理目标同样具有稳定、高精度的定位结果，具有更好的适应性。
(2)本发明仅利用聚类得到聚类质心集合计算rgbd深度相机在当前位置相对于理想位置的旋转矩阵和平移变换向量，实现了点云配准，在此过程中，无需对点云图像中的所有点进行计算，有效提高了点云配准的时间效率。
附图说明
图1为本发明实施例提供所涉及的结构示意图；
图2为图1中各结构的坐标系示意图；
图3为本发明实施例提供的基于点云法矢方向聚类伺服的机器人定位控制方法流程图；
在所有附图中，相同的附图标记用来表示相同的元件或者结构，其中：
1为视觉平台，2为待定位目标。
具体实施方式
为了使本发明的目的、技术方案及优点更加清楚明白，以下结合附图及实施例，对本发明进行进一步详细说明。应当理解，此处所描述的具体实施例仅仅用以解释本发明，并不用于限定本发明。此外，下面所描述的本发明各个实施方式中所涉及到的技术特征只要彼此之间未构成冲突就可以相互组合。
在本发明中，本发明及附图中的术语“第一”、“第二”等(如果存在)是用于区别类似的对象，而不必用于描述特定的顺序或先后次序。
不失一般性地，以下以工件加工场景为例，对本发明的技术方案做进一步解释，在该场景下，通过视觉伺服与定位控制，需要以待加工工件实际所在位置为参考，将机器人移动至指定的理想加工位置，以进行加工。以下为实施例。
实施例1：
一种基于点云法矢方向聚类伺服的机器人定位控制方法，其中，机器人具有六自由度运动，且与一个rgbd深度相机固定连接，由该机器人及与其固定连接的rgbd深度相机构成图1中所示的视觉平台1，待定位目标2为图1所示的叶片；
为了利用rgbd深度相机获取待定位目标的点云图像，分别构建待定位目标的坐标系和rgbd深度相机的坐标系，如图2所示；
如图3所示，本实施例提供的基于点云法矢方向聚类伺服的机器人定位控制方法，包括如下步骤：
(s1)计算机器人相对于待定位目标处于理想位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的理想点云图像；
理想点云图像可根据离线扫描到的待定位目标的点云和根据此点云规划出的理想加工位置通过坐标转换计算得到，与实际位置获取的点云不同的是该理想点云图像是通过计算得到的虚拟点云；
(s2)计算理想点云图像中各点的法矢量后，进行聚类，得到理想点云图像的聚类质心集合
本实施例可采用任意一种方法求解法矢量，在此不作赘述；
在本实施例中，步骤(s2)中，对理想点云图像中各点的法矢量聚类得到理想点云图像的聚类质心集合
(t1)选择k个法矢量作为初始法矢质心向量；k为预设正整数，将点云经过聚类后，得到k个法矢聚类集合
(t2)对于理想点云图像中的每一个点，计算其法矢量
(t3)点云图像中所有点聚类完成后，更新每个法矢聚类集合的法矢质心向量；
本实施例中，按照
其中，
(t4)若各法矢质心向量更新前、后的角度变化均小于等于预设的阈值ε，则判定聚类过程收敛，转入步骤(t5)；否则，转入步骤(t2)；其中，ε＞0；ε的具体取值可根据实际的配准精度要求设定，可选地，在本实施例中，ε＝0.04；
(t5)将当前聚类得到的k个法矢聚类集合作为聚类结果，计算各法矢聚类集合的聚类质心的空间坐标，得到聚类质心集合
本实施例中，按照
其中，mj表示第j个法矢聚类集合对应的点集，各点以空间坐标的形式表示；|mj|表示点集mj中点的个数；pj表示第j个法矢聚类集合的聚类质心的空间坐标；
(s3)计算机器人相对于待定位目标处于实际位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的实际点云图像；
实际点云图像就是深度相机实时拍摄得到的，是真实的成像结果；
(s4)计算实际点云图像中各点的法矢量后，进行聚类，得到实际点云图像的聚类质心集合mc；
步骤(s4)中，对实际点云图像中各点的法矢量进行聚类，得到实际点云图像的聚类质心集合mc的具体步骤，可参考上述步骤(s2)中的描述，在此将不作赘述；
(s5)根据理想点云图像的聚类质心集合
步骤(s5)中，速度vc表示为：
其中，(vc,wc)分别表示rgbd深度相机的移动速度和角速度；λ为尺度参数，θu表示所述旋转矩阵r所对应的旋转轴角，旋转轴角可通过罗德里格斯公式与旋转矩阵相互转化。
(s6)判断速度vc是否收敛到0，若是，说明此时机器人已运动至理想位置，则伺服控制结束；否则，使机器人按照速度vc移动预设时间后，转入步骤(s3)；
机器人移动过程中，rgbd深度相机会随之相应移动。
本实施例利用rgbd深度相机获取机器人相对于待定位目标在理想位置以及实际位置的点云图像，对点云图像中各点的法矢量进行聚类，并基于聚类结果实现点云配准，在此过程中，无需提取待定位目标的图像特征，对于弱纹理目标同样具有稳定、高精度的定位结果；并且在点云配准过程中，仅利用聚类质心集合进行计算，有效提高了点云配准的时间效率。
作为一种优选的实施方式，本实施例中，步骤(s5)中，根据理想点云图像的聚类质心集合
实施例2：
一种基于点云法矢方向聚类伺服的机器人定位控制系统，包括：定位控制模块和视觉平台；
视觉平台由具有六自由度运动的机器人及与其固定连接的rgbd深度相机构成；
定位控制模块包括：理想点云图像获取单元、第一聚类质心集合获取单元、实际点云图像获取单元、第二聚类质心集合获取单元、速度获取单元以及移动控制单元；
理想点云图像获取单元，用于计算机器人相对于待定位目标处于理想位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的理想点云图像，之后触发第一聚类质心集合获取单元；
第一聚类质心集合获取单元，用于计算理想点云图像中各点的法矢量后，进行聚类，得到理想点云图像的聚类质心集合
实际点云图像获取单元，用于计算机器人相对于待定位目标处于实际位置时，待定位目标上各点在rgbd深度相机的坐标系下的三维坐标，得到待定位目标的实际点云图像，之后触发第二聚类质心集合获取单元；
第二聚类质心集合获取单元，用于计算实际点云图像中各点的法矢量后，进行聚类，得到实际点云图像的聚类质心集合mc，之后触发速度获取单元；
速度获取单元，用于根据理想点云图像的聚类质心集合
移动控制单元，用于判断速度vc是否收敛到0，若是，则伺服控制结束；否则，使机器人按照速度vc移动预设时间后，触发实际点云图像获取单元；
本实施例中，各模块的具体实施方式，可参考上述方法实施例中的描述，在此将不作复述。
本领域的技术人员容易理解，以上所述仅为本发明的较佳实施例而已，并不用以限制本发明，凡在本发明的精神和原则之内所作的任何修改、等同替换和改进等，均应包含在本发明的保护范围之内。
- 还没有人留言评论。精彩留言会获得点赞！