CN117197241B - 一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法 - Google Patents

Abstract

本发明公开了一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，包括以下步骤：获取机器人的低精度位姿信息；对多目相机***进行标定，获取标定参数；基于所述低精度位姿信息对人工标靶进行自标定，获取靶标点之间的相对位置关系；基于所述低精度位姿信息、所述标定参数和所述相对位置进行位姿跟踪，获取所述机器人的目标位姿，完成基于多目视觉的机器人末端绝对位姿高精度跟踪。本发明提出一种自动化的靶标组标定方法，能够方便地获取靶标组之间的较高精度的相对位置关系，为后续的跟踪过程提供可靠初值；通过在机器人末端加装测量设备，可以利用该方法进行机器人末端位姿补偿，并实现在绝对坐标系下的较高精度特征测量。

Description

一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法

技术领域

本发明属于视觉测量领域，尤其涉及一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法。

背景技术

在使用视觉测量检测汽车船舶飞机零件的加工过程中，涉及到各类局部元素之间的位姿关系、测量对象与设计尺寸的偏差关系时，需要建立起全局的视觉测量***。常用的解决手段是在移动测头***外部布置高精度的位姿跟踪***，将局部测量任务与全局测量任务分开，由外部位姿跟踪***保证全局测量精度。

目前已有的外部位姿跟踪***大多是由双目视觉***组成，结构简单，能够实现三角测量获取深度信息的最基本要求。但是双目视觉***对同一世界点只能给出一个测量结果，对测量噪声十分敏感，所以在测量精度方面还有待提升。同时，双目视觉***存在共视点少，匹配鲁棒性差的问题。而目前现用的多目位姿跟踪的技术研究，普遍使用加权最小二乘法来进行标识点的三维重建。主要的问题在于权值的确定，目前的研究中的权值分配策略仍比较朴素，缺少一种依据科学定量模型的权值分配策略。

在生产加工过程中，测量精度要求高、现场环境复杂、干扰因素多，现有的双目位姿跟踪方式效果不理想。因此，本发明提出一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，以解决现有技术中的不足之处。

发明内容

本发明的目的在于提出一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，以解决现有的双目位姿跟踪方式效果不理想的问题。

为实现上述目的，本发明提供了一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，包括以下步骤：

获取机器人的低精度位姿信息；

对多目相机***进行标定，获取标定参数；

基于所述低精度位姿信息对人工标靶进行自标定，获取靶标点之间的相对位置关系；

基于所述低精度位姿信息、所述标定参数和所述相对位置进行位姿跟踪，获取所述机器人的目标位姿，完成基于多目视觉的机器人末端绝对位姿高精度跟踪。

可选的，对所述多目相机***进行标定，获取标定参数包括：

利用张正友标定法对所述多目相机***进行标定，获取初始标定参数；

以最小化每个标定点重构误差为优化目标，利用非线性优化方法LM算法对所述初始标定参数进行优化，获取标定参数；

所述标定参数包括内参、外参和畸变参数。

可选的，基于所述低精度位姿信息对人工标靶进行自标定，获取所述靶标点之间的相对位置关系包括：

S1、获取所述人工标靶的初始状态，利用所述多目相机***对所述人工标靶进行拍摄，利用Zernike矩方法确定每个椭圆圆心的亚像素坐标，并利用双目***的三维重建，获得每个靶标点的三维信息，两两相机组成所述双目***；

S2、基于所述低精度位姿信息和所述每个靶标点的三维信息，利用KD-Tree进行邻点搜索，确定当前靶标点的索引号；

S3、所述机器人带动所述人工标靶进行旋转和移动，并利用所述多目相机***对刚性变换后的人工靶标进行采集，重复S1、S2的三维重建过程，获取每个靶标点的索引号以及深度信息；

S4、重复S3，直至每个人工标靶都被拍摄若干次；

S5、获取初始图像中点云，并计算每两帧点云前后的位姿变换关系，累乘得到所述初始图像点云到基准点云的旋转矩阵和平移向量；

S6、基于所述旋转矩阵和所述平移向量，获取非线性优化方法LM算法的优化目标，进而获取所述初始图像中点云到基准点云的变换关系；

S7、利用所述初始图像中点云到基准点云的变换关系，将每一幅图的部分点云统一至所述基准点云，获取所述靶标点之间的相对位置关系。

可选的，利用所述多目相机***对所述人工标靶进行拍摄包括：

所述机器人带动所述人工标靶走预定轨迹时，所述多目相机***对所述人工标靶进行拍摄。

可选的，利用双目***的三维重建，获得每个靶标点的三维信息包括：

构建测量误差模型；

根据所述测量误差模型进行三维重建，获取所述每个靶标点的三维信息。

可选的，所述测量误差模型包括：X方向测量误差概率、Y方向测量误差概率和Z方向测量误差概率；

所述X方向测量误差概率为：

当D≥v时：

当D<v时：

其中，v为左相机上像点在垂直方向上的坐标；所述Y方向测量误差概率为：

当0≤h＜D时：

当h＞D时：

其中，h为J_r，R＝f/dv，

所述Z方向测量误差概率为：

其中，τ_z是给定的误差界限，D为视差。

可选的，根据所述测量误差模型进行三维重建，获取所述每个靶标点的三维信息包括：

设定误差界限，并基于所述测量误差模型，确定每组双目***三维重建的权重；

对所述每组双目***三维重建的权重进行归一化，并加权最小二乘获取所述每个靶标点的三维信息。

可选的，所述非线性优化方法LM算法的优化目标为：

其中，E为重投影误差，m、n分布为第m、n帧点云序号，K表示一共K帧点云；m和n帧点云中有N_mn个共视点；R_m、R_n与t_m、t_n分别为第m、n帧点云转换到基准坐标系下的旋转矩阵和平移向量，P_mi为第m帧点云中第i个点的三维坐标，P_ni为第n帧点云中第i个点的三维坐标。

可选的，基于所述低精度位姿信息、所述标定参数和所述相对位置进行位姿跟踪，获取所述机器人的目标位姿包括：

基于所述低精度位姿信息和所述标靶之间的相对位置关系，确定每个圆光斑对应的索引号；

对多目相机***进行三维重建，利用所述每个圆光斑对应的索引号对所述机器人进行位姿跟踪，获取部分靶标点的三维坐标；

对所述部分靶标点的三维坐标进行优化，获取所述部分靶标点的三维优化坐标；

利用所述部分靶标点的三维优化坐标和所述标靶之间的相对位置关系，获取所述机器人的目标位姿。

可选的，对所述部分靶标点的三维坐标进行优化，获取所述部分靶标点的三维优化坐标包括：

利用LM算法，优化当前位姿到人工靶标测定位姿的旋转矩阵R和平移向量t，具体优化目标如下：

其中，E为重投影误差，N为当前位姿下成功完成三维重建的特征点总数，旋转矩阵R和平移向量t即为输出的绝对位姿，P_i为第i个点的三维坐标，为第i个点的初始三维坐标。

本发明具有以下有益效果：

(1)本发明依赖于合理加权，有效利用多视图的冗余信息能够提高三维重建算法的精度，同最朴素的基于重心法的多视图三维重建算法精度提升11％，同行业内最常用的最小二乘法相比提升4.5％；

(2)本发明提出一种自动化的靶标组标定方法，能够方便地获取靶标组之间的较高精度的相对位置关系，为后续的跟踪过程提供可靠初值；还提出了一种位姿跟踪方法，适用于大场景下可动对象的6自由度绝对位姿测量；

(3)本发明通过在机器人末端加装测量设备，可以利用该方法进行机器人末端位姿补偿，并实现在绝对坐标系下的较高精度特征测量。

附图说明

构成本申请的一部分的附图用来提供对本申请的进一步理解，本申请的示意性实施例及其说明用于解释本申请，并不构成对本申请的不当限定。在附图中：

图1为本发明实施例一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法的流程示意图；

图2为本发明实施例提出的相机xz平面测量误差概率模型图。

具体实施方式

需要说明的是，在不冲突的情况下，本发明中的实施例及实施例中的特征可以相互组合。下面将参考附图并结合实施例来详细说明本申请。

需要说明的是，在附图的流程图示出的步骤可以在诸如一组计算机可执行指令的计算机***中执行，并且，虽然在流程图中示出了逻辑顺序，但是在某些情况下，可以以不同于此处的顺序执行所示出或描述的步骤。

如图1所示，本实施例中提供一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，包括以下步骤：

利用红外标定技术对多目相机***进行标定，包括内参、外参、畸变参数。具体的标定方法可以利用张正友标定法获得标定参数初值，进一步地利用非线性优化方法LM算法，以最小化每个标定点重构误差为优化目标，进一步提高多目***的标定参数。多目相机***标定完成后，让机器人带动人工靶标走一个特定轨迹，在这个过程中通过多目相机***对人工靶标完成自标定，即获取靶标点之间的相对位置关系。为了进一步地减少累计误差对自标定过程的影响，可以用全局ICP算法进行优化。在位姿跟踪过程中，每两个相机组成双目***利用加权最小二乘法进行靶标点的三维重建，能够获得一部分靶标点的三维坐标，将这部分点云与前面人工靶标自标定的结果进行配准，即可获得当前机器人的绝对位姿。

利用全局ICP优化算法进行人工靶标自标定流程：

第一步，选择一个初始状态作为基准状态，用多目***对人工靶标进行拍摄，利用Zernike矩方法确定每个椭圆圆心的亚像素坐标，进一步地利用双目***的三维重建可以获得每个靶标点的三维信息。在此基础上，进行非线性优化，以最小二每个靶标点的重构误差为优化目标，提高靶标点三维重建的精度。同时输入当前机器人的低精度位姿信息，利用KD-Tree进行最邻点搜索，可以进一步确定当前靶标点的索引号，以便进行三维重建后能够对应到正确点物理靶标上。

基于Zernike矩实现亚像素边缘检测是一种图像处理方法，利用Zernike矩来提取图像中物体边缘的亚像素级位置信息，基本的步骤框架如下：

(1)图像预处理：对输入图像进行预处理，如平滑、噪声去除等，确保提取到的边缘信号质量较高；

(2)边缘检测：使用适当的边缘检测算anny边缘检测)对图像进行边缘检测操作，生成二值化边缘图像；

(3)提取边缘片段：根据需要选择感兴趣的边缘区域或目标物体，通过图像分割或ROI提取等技术，获得边缘片段；

(4)Zernike矩计算：对边缘片段应用Zernike多项式生成函数计算其Zernike矩。Zernike矩在边缘形状描述方面具有旋转、尺度和平移不变性；

(5)亚像素拟合：基于Zernike矩的特性，通过对Zernike矩的插值和拟合，计算出边缘的亚像素级别位置信息。常见的拟合方法包括最小二乘法、最大似然估计等；

(6)边缘位置显示：利像素位置信息，可以将更精确的边缘位置标记在原始图像上，用于可视化展示或后续分析。

需要注意的是，基于Zernike矩的亚像素边缘检测方法在某些情况下可能对噪声敏感，并且对边缘形状的复杂性有一定限制。在实践中，需要权衡算法复杂度、数据质量和处理效果等因素，并根据具体应用进行调整和改进。

第二步，由机器人带动人工靶标进行一个小角度的旋转和移动，并用多目***对刚性变换后的人工靶标进行拍摄，重复第一步和第二步中的三维重建过程，获得在人工标靶不同位姿下每个靶标点的索引号以及高精度的三维信息。

第三步，重复第二步，直到每个人工靶标点都被拍摄到多次。

第四步，以第一幅图像中点云为基准，计算每两帧点云前后的位姿变换关系，累乘得到每一幅图像点云到基准点云的旋转矩阵R和平移向量t。这一步中得到的R和t存在累计误差，所以需要通过后续的全局优化算法来减少。

第五步，写出非线性优化方法LM算法的优化目标：

其中，E为重投影误差，m、n分布为第m、n帧点云序号，K表示一共K帧点云；m和n帧点云中有N_mn个共视点；R_m、R_n与t_m、t_n分别为第m、n帧点云转换到基准坐标系下的旋转矩阵和平移向量，P_mi为第m帧点云中第i个点的三维坐标，P_ni为第n帧点云中第i个点的三维坐标。

最终得到优化后的结果每一幅图像中的点云到基准点云的变换关系即R_i和t_i(i＝1…K)，由此可以把每幅图的部分点云都统一到基准点云上，得到在同一基准下，各靶标点之间高精度的相对位置，完成人工靶标的自标定。

基于多目视觉的靶标点高精度三维重建算法流程：

第一部分，对坐标测量误差建立数学模型。

如图2所示，P1-P8是几根光线的交点，具体地，P1、P2所在的光线，是由左像素的右边界通过左相机的光心发出的；P3、P4所在的光线是由左像素的左边界通过左相机的光心发出的；P5、P6所在的光线是在当前n_l测量不确定下，被测点和左光心的连线。类似的，P1、P4所在的光线是由右像素的右边界通过右光心发出的，P2、P3所在的光线是由右像素的左边界通过右光心发出的。考虑相机像元离散对测量结果的影响，其中点P1、P2、P3、P4围成的四边形区域对应的就是左右两个匹配像素，所以左右相机中匹配像素对应的不是空间中的一个点而是一块区域。直观地，直线P5P6的长度越长，则被测点在直线P5、P6上的概率越大。写出直线P5、P6长度关于左相机测量不确定度n_l的表达式：

其中，Δ为基线距离，J_l为左像素所在的列坐标，J_r为右像素所在的列坐标，f代表相机焦距，d为视差。

对右相机的测量不确定度取微元δn_r进一步得到直线P7、P8长度关于δn_r的表达式：

由此用四边形P1、P2、P3、P4的面积和直线P5、P6的长度进行归一化得到左图像不确定度N_l关于n_l的概率密度分布和右图像不确定度N_r关于n_l和n_r的条件概率分布密度。

其中，n_l和n_r都是无法确定的测量不确定量，因此考虑通过给定阈值和积分的方式将概率分布密度转化为测量误差在给定阈值范围内的概率。Z方向测量误差概率分布如下：

其中，ε_z用于表示Z方向实际误差大小，τ_z是Z方向给定的误差界限，D为视差。可以看出z方向的测量误差概率只与视差相关，且视差越大，测量误差概率越小，这也是与事实相符的。类似的可以给出X方向和Y方向的测量误差概率。

X方向测量误差概率如下：

当D≥v时：

当D<v时：

其中，τ_x是X方向给定的误差界限，ε_x用于表示X方向实际误差大小，v为左相机上像点在垂直方向上的坐标；

Y方向的测量误差概率如下：

当0≤h＜D时：

当h＞D时：

其中，h为J_r，R＝f/dv，

其中，τ_y为是Y方向给定的误差界限，ε_y用于表示Y方向实际误差大小。

可以看出Y方向的测量误差除了和视差相关以外，和像素坐标与主点偏离程度也相关，与主点偏离越大，即越靠近图像边缘的被测点保持测量误差较低的概率越小。这同样是与测量事实相符合的。

第二部分，根据测量误差模型进行加权最小二乘的三维重建

利用多目***对固连着人工靶标的机器人末端进行图像拍摄。两两相机组成双目***进行三维重建，选择0.05mm作为给定的误差界限，根据应用场景的视场大小，可以更改给定的绝对误差界限，根据第一部分中的测量误差模型确定每组双目***三维重建的权重大小，进一步地权重归一化并加权最小二乘得到单个靶标点的三维信息。

利用LM算法，优化当前位姿到人工靶标测定位姿的旋转矩阵R和平移向量t，具体优化目标如下：

其中，P_i为第i个点的三维坐标，为第i个点的初始三维坐标，N为当前位姿下成功完成三维重建的特征点总数，旋转矩阵R和平移向量t即为输出的绝对位姿。

以上所述，仅为本申请较佳的具体实施方式，但本申请的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本申请揭露的技术范围内，可轻易想到的变化或替换，都应涵盖在本申请的保护范围之内。因此，本申请的保护范围应该以权利要求的保护范围为准。

Claims (7)

1.一种基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，包括以下步骤：

获取机器人的低精度位姿信息；

对多目相机***进行标定，获取标定参数；

基于所述低精度位姿信息对人工标靶进行自标定，获取靶标点之间的相对位置关系；

基于所述低精度位姿信息对人工标靶进行自标定，获取所述靶标点之间的相对位置关系包括：

S1、获取所述人工标靶的初始状态，利用所述多目相机***对所述人工标靶进行拍摄，利用Zernike矩方法确定每个椭圆圆心的亚像素坐标，并利用双目***的三维重建，获得每个靶标点的三维信息，两两相机组成所述双目***；

S2、基于所述低精度位姿信息和所述每个靶标点的三维信息，利用KD-Tree进行邻点搜索，确定当前靶标点的索引号；

S3、所述机器人带动所述人工标靶进行旋转和移动，并利用所述多目相机***对刚性变换后的人工靶标进行采集，重复S1、S2的三维重建过程，获取每个靶标点的索引号以及深度信息；

S4、重复S3，直至每个人工标靶都被拍摄若干次；

S5、获取初始图像中点云，并计算每两帧点云前后的位姿变换关系，累乘得到所述初始图像中点云到基准点云的旋转矩阵和平移向量；

S6、基于所述旋转矩阵和所述平移向量，获取非线性优化方法LM算法的优化目标，进而获取所述初始图像中点云到基准点云的变换关系；

S7、利用所述初始图像中点云到基准点云的变换关系，将每一幅图的部分点云统一至所述基准点云，获取所述靶标点之间的相对位置关系；

利用双目***的三维重建，获得每个靶标点的三维信息包括：

构建测量误差模型；

根据所述测量误差模型进行三维重建，获取所述每个靶标点的三维信息；

所述测量误差模型包括：X方向测量误差概率、Y方向测量误差概率和Z方向测量误差概率；

所述X方向测量误差概率为：

当D≥v时：

当D<v时：

其中，τ_x是X方向给定的误差界限，ε_x用于表示X方向实际误差大小，τ_y是Y方向给定的误差界限，ε_y用于表示T方向实际误差大小，v为左相机上像点在垂直方向上的坐标，R为旋转矩阵；

所述Y方向测量误差概率为：

当0≤h＜D时：

当h＞D时：

其中，h为J_r，J_r为右像素所在的列坐标，R＝f/dv，f代表相机焦距，d为视差；

所述Z方向测量误差概率为：

其中，ε_z用于表示Z方向实际误差大小，τ_z是Z方向给定的误差界限，D为视差；

基于所述低精度位姿信息、所述标定参数和所述相对位置进行位姿跟踪，获取所述机器人的目标位姿，完成基于多目视觉的机器人末端绝对位姿高精度跟踪。

2.如权利要求1所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，对所述多目相机***进行标定，获取标定参数包括：

利用张正友标定法对所述多目相机***进行标定，获取初始标定参数；

以最小化每个标定点重构误差为优化目标，利用非线性优化方法LM算法对所述初始标定参数进行优化，获取标定参数；

所述标定参数包括内参、外参和畸变参数。

3.如权利要求1所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，利用所述多目相机***对所述人工标靶进行拍摄包括：

所述机器人带动所述人工标靶走预定轨迹时，所述多目相机***对所述人工标靶进行拍摄。

4.如权利要求1所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，根据所述测量误差模型进行三维重建，获取所述每个靶标点的三维信息包括：

设定误差界限，并基于所述测量误差模型，确定每组双目***三维重建的权重；

对所述每组双目***三维重建的权重进行归一化，并加权最小二乘获取所述每个靶标点的三维信息。

5.如权利要求1所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，所述非线性优化方法LM算法的优化目标为：

其中，E为重投影误差，m、n分布为第m、n帧点云序号，K表示一共K帧点云；m和n帧点云中有N_mn个共视点；R_m、R_n与t_m、t_n分别为第m、n帧点云转换到基准坐标系下的旋转矩阵和平移向量，P_mi为第m帧点云中第i个点的三维坐标，P_ni为第n帧点云中第i个点的三维坐标。

6.如权利要求1所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，基于所述低精度位姿信息、所述标定参数和所述相对位置进行位姿跟踪，获取所述机器人的目标位姿包括：

基于所述低精度位姿信息和所述标靶之间的相对位置关系，确定每个圆光斑对应的索引号；

对多目相机***进行三维重建，利用所述每个圆光斑对应的索引号对所述机器人进行位姿跟踪，获取部分靶标点的三维坐标；

对所述部分靶标点的三维坐标进行优化，获取所述部分靶标点的三维优化坐标；

利用所述部分靶标点的三维优化坐标和所述标靶之间的相对位置关系，获取所述机器人的目标位姿。

7.如权利要求6所述的基于多目视觉的机器人末端绝对位姿高精度跟踪方法，其特征在于，对所述部分靶标点的三维坐标进行优化，获取所述部分靶标点的三维优化坐标包括：

利用LM算法，优化当前位姿到人工靶标测定位姿的旋转矩阵R和平移向量t，具体优化目标如下：

其中，E为重投影误差，N为当前位姿下成功完成三维重建的特征点总数，旋转矩阵R和平移向量t即为输出的绝对位姿，P_i为第i个点的三维坐标，为第i个点的初始三维坐标。