CN113034600B - 基于模板匹配的无纹理平面结构工业零件识别和6d位姿估计方法 - Google Patents

Abstract

本发明属于机器视觉、图像处理技术领域，尤其是基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，针对现有的方式的适用范围小柔性化程度低，无法实现无序堆叠工件的上下料的问题，现提出如下方案，其首先通过RGB‑D相机采集堆叠工件的RGB图像和深度图，基于2D模板匹配对RGB图像进行工件识别分类和2D图像定位；将深度图转换3D点云，对点云图像进行滤波处理剔除背景点云和噪声点云干扰，并对滤波处理后的点云进行欧式聚类分割，获得单个工件实例点云；对单一零件点云进行RANSAC平面拟合，利用拟合平面的参数方程和工件中心的2D图像像素坐标估计物体在相机坐标系下的6D位姿。

Description

基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计 方法

技术领域

本发明涉及机器视觉、图像处理技术领域，尤其涉及基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法。

背景技术

在机器人自动化应用中，利用机器人实现工业零件的自动化上下料作业是一个非常重要的应用场景。

现有的自动化上下料广泛采用的是示教-在线模式，机器人通过示教编程对固定工位上的物体进行抓取等操作，这样方式的适用范围小柔性化程度低，无法实现无序堆叠工件的上下料。

发明内容

本发明的目的是为了解决现有的方式的适用范围小柔性化程度低，无法实现无序堆叠工件的上下料的缺点，而提出的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法。

为了实现上述目的，本发明采用了如下技术方案：

基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，基于2D模板匹配对RGB图像进行工件识别分类和2D图像定位；结合图像多分辨率表达和分层模板库的模板匹配提升算法的运算速度；基于3D点云平面特征和模板匹配结果实现工件的6D位姿估计；

上述方法具体包括以下步骤：

S1：利用双目结构光相机和工业机械臂搭建机器人手眼系统，通过双目结构光相机采集堆叠零件RGB图像和点云图像；

S2：构建待匹配RGB图像的高斯金字塔多分辨率表达，同时构建模板图像的分层模板库，结合多分辨率空间搜索策略和分层模板搜索策略对RGB图像进行模板匹配，进行工件类型识别和2D图像定位；

S3：对模板匹配结果进行2D-ICP优化，得到精确的工件中心2D 定位；

S4：对3D点云进行滤波处理，并通过欧式聚类分割获得单个工件实例点云；

S5：对单个实例点云进行RANSAC平面拟合，得到工件的拟合平面方程；

S6：利用拟合平面的法向量、相机内参矩阵计算工业零件的6D 位姿估计。

优选的，所述S2中多分辨率空间搜索策略，多分辨率图像空间构建：

将待检测的原图像I(x,y)作为多分辨率空间的第0层，使用高斯滤波器(式2)对其进行滤波得到平滑图像I_s(x,y)((式1)，然后进行降采样操作，使得图像分辨率将为原来的1/4，得到多分辨率空间的下一层图像，迭代进行高斯滤波和降采样操作，即得到图像的多分辨率表达(图1)。

I_s(x,y)＝G(x,y,σ)*I(x,y) (1)

优选的，所述S2中分层模板搜索策略，分层模板库构建：

第一，利用Sobel卷积核(式1)分别计算零件模板图像三通道的梯度幅值，选取三通道中最大梯度值作为像素点梯度，将梯度幅值超过一定阈值的像素点作为有效特征点；进一步计算这些特征点梯度方向，得到梯度方向特征图I_G(x)；再对模板的梯度方向特征图进行非极大值抑制操作，使特征点均匀分布，获得最终的模板；

图像梯度幅值梯度方向/>

第二，2D模板匹配中，模板和原图像中的待检测物体之间的变换关系为2D图像仿射变换

为了能在待检测图像中匹配到仿射变换后的物体，需要在2D 仿射空间进行采样得到所有可能的仿射变换组合，并生成所有仿射变换下的模板；为了解决大量模板匹配带来的算法运行时间增长，本发明提出了基于分层模板搜索的匹配策略，首先需要构建分层模板库； 2D仿射空间的分层模板库构建步骤如下，首先生成l＝0模板库，对限定范围后的仿射空间σ_min≤s≤σ_max，θ_min≤θ≤θ_max，以采样步长Δs和Δθ进行细分，得到所有可能的2D仿射变换组合：

A(s_min+mΔs,θ_min+nΔθ)，m∈[0,(σ_max-σ_min)/Δs]，n∈[0,(θ_max-θ_min)/Δθ]

第三，对原始模板图像进行2D仿射变换并提取梯度方向特征。在l＝1层，采用2Δs和2Δθ对参数空间进行细分，该层的模板数量减少为l＝0层的1/4；同理，对于l＝k层，采样步长将增加至2^kΔs和2^kΔθ，生成该层的模板库，重复上述步骤生成分层模板库(图2)。

优选的，所述结合图像多分辨率表达和分层模板库的模板匹配策略(图3)：

第一，首先从图像多分辨率金字塔的最高层开始进行匹配。利用滑窗法依次将模板库中的模板和待检测图像进行匹配并计算相似性得分(式4)，保留相似度大于设定阈值的匹配结果；对高尺度空间的匹配结果在下一层尺度空间进行精匹配，同样保留相似度最高的匹配结果，重复进行这一步骤直到搜索到达图像金字塔最低层，得到最终的精细匹配结果。

待匹配的模板定义为T＝{(x₁,d₁),(x₂,d₂),L,(x_m,d_m)}，其中x_i是特征点像素点坐标，d_i是特征点的梯度方向，对待检测图像提取梯度方向特征图，以I＝{(p₁,e₁),(p₂,e₂),…,(p_n,e_n)}集合表示，其中p_i像素点坐标， e_i是该点的梯度方向，I和模板T在图像匹配点c处的相似性得分可以用它们梯度方向的向量点积之和表达:

式中，R(i+c,T)定义为以I上的像素点p_i+c为中心，大小为 T×T的邻域，ori()代表梯度方向；

第二，在滑窗法进行图像空间搜索的过程中，利用分层搜索的匹配策略进一步对匹配过程进行加速；

首先在分层模板库l_max开始，将该层所有模板和待检测图像进行匹配，并计算相似性得分，保留得分大于阈值的模板，同时采用非极大值抑制剔除重复匹配，得到在l_max层的待检索模板集合；完成l_max层匹配后，将得到的待检索模板集合在l_max-1层进行匹配；步骤如下：设l_max层匹配得到待检索模板参数为(s₀,θ₀)，该组合表示为2D仿射变换空间中的一个点，将l_max-1层以该点为中心的一个邻域内的所有模板和待检测图像重新匹配，该邻域可以表示为直积：

[s₀-m·2^l-1Δs,s₀+m·2^l-1Δs]×[θ₀-m·2^l-1Δθ,θ₀+m·2^l-1Δθ]

2^l-1Δs和2^l-1Δθ为l_max-1层采样步长，m为控制邻域大小的参数；同样计算相似度得分，并保留得分最高的模板，得到在l_max-1层的待检索模板集合；由于l_max-1层的采样步长更小，所以匹配结果更为精确；迭代进行上述分层搜索操作，直到所有l_max层待检索模板均回溯到分层模板库的的最底层，即得到最终的精细匹配结果。

优选的，所述S3中模板匹配结果进行2D-ICP优化：

通过模板匹配得到待检测图像中工件中心的像素坐标t＝[t_x,t_y]^T和其2D仿射变换矩阵A；利用2D-ICP对匹配结果进行优化得到更为精确的结果；对模板T任一特征点x_i，在待检测图像I中搜索欧氏距离最近的点q_i作为其对应点；最优化的目标即使得模板特征点到图像对应点所在边缘切线的距离平方和损失函数最小，图像点q_i切线方程可以表示为：(q_i-x)^T·e_i＝0，则待优化的损失函数为：

精确的位姿利用基于LM算法迭代求解上述非线性最小二乘问题，得到最终2D模板匹配结果。

优选的，所述S6中工件6D位姿估计：

第一，首先对场景3D点云进行直通滤波和降采样操作，剔除背景点云干扰并降低点云密度，进一步进行统计滤波剔除噪声点云；

第二，基于欧式聚类分割获取单个工件实例点云；

第三，基于RANSAC方法拟合点云平面方程n^T·x+d＝0；

第四，通过2D模板匹配得到工件中心点像素坐标t＝[t_x,t_y]^T，齐次坐标为p＝[t_x,t_y.1]^T，设工件中心在相机坐标系下的坐标为 p_c＝[x_c,y_c,z_c]^T，根据相机模型，已知内参矩阵K，有：z_cp＝K·p_c；

第五，p_c满足平面方程：n^T·p_c+d＝0，联立4)中等式可以求解z_c＝-d/n^T·K^-1·p，进一步可以求得p_c＝z_c·K^-1·p，根据2D姿态角θ，计算出工件长轴(x轴)在相机坐标系下的方向向量r_x,工件平面z 轴方向向量r_z＝n/||n||，y轴方向向量为r_y＝r_z×r_x，则工件坐标系相对于相机坐标系的旋转矩阵r＝[r_x,r_y,r_z]，由此确定工件的6D位姿参数为:

与现有技术相比，本发明的优点在于：

本方案，将视觉传感器引入机器人上下料任务中，和机器人构建手眼系统，通过基于视觉的物体识别和位姿估计算法，可以识别场景中目标物体的类别并估计物体相对于相机的6D位姿，从而引导机器人完成无序堆叠工件的抓取，分拣等更加智能自动化的操作，有效解决传统示教-在线方法存在的问题；

本发明采用结合图像多分辨率空间和分层模板空间的模板匹配策略，有效提升了传统模板匹配方法的匹配速度；通过2D-ICP优化使2D图像定位精度更高；最后结合3D平面特征的位姿估计算法能实现对平面结构工业零件准确的6D位姿估计。

附图说明

图1为图像高斯金字塔多分辨率表达原理图；

图2为分层模板库生成原理图；

图3为结合图像多分辨率表达和分成模板库搜索的快速模板匹配策略原理图；

图4为基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法流程图。

具体实施方式

下面将结合本发明实施例中的附图，对本发明实施例中的技术方案进行清楚、完整地描述，显然，所描述的实施例仅仅是本发明一部分实施例，而不是全部的实施例。

参照图1-4，基于模板匹配的无纹理平面结构工业零件识别和6D 位姿估计方法，基于2D模板匹配对RGB图像进行工件识别分类和2D 图像定位；结合图像多分辨率表达和分层模板库的模板匹配提升算法的运算速度；基于3D点云平面特征和模板匹配结果实现工件的6D位姿估计；

上述方法具体包括以下步骤：

S1：利用双目结构光相机和工业机械臂搭建机器人手眼系统，通过双目结构光相机采集堆叠零件RGB图像和点云图像；

S2：构建待匹配RGB图像的高斯金字塔多分辨率表达，同时构建模板图像的分层模板库，结合多分辨率空间搜索策略和分层模板搜索策略对RGB图像进行模板匹配，进行工件类型识别和2D图像定位；

S3：对模板匹配结果进行2D-ICP优化，得到精确的工件中心2D 定位；

S4：对3D点云进行滤波处理，并通过欧式聚类分割获得单个工件实例点云；

S5：对单个实例点云进行RANSAC平面拟合，得到工件的拟合平面方程；

S6：利用拟合平面的法向量、相机内参矩阵计算工业零件的6D 位姿估计。

本实施例中，S2中多分辨率空间搜索策略，多分辨率图像空间构建：

将待检测的原图像I(x,y)作为多分辨率空间的第0层，使用高斯滤波器(式2)对其进行滤波得到平滑图像I_s(x,y)((式1)，然后进行降采样操作，使得图像分辨率将为原来的1/4，得到多分辨率空间的下一层图像，迭代进行高斯滤波和降采样操作，即得到图像的多分辨率表达(图1)。

I_s(x,y)＝G(x,y,σ)*I(x,y) (1)

本实施例中，S2中分层模板搜索策略，分层模板库构建：

第一，利用Sobel卷积核(式1)分别计算零件模板图像三通道的梯度幅值，选取三通道中最大梯度值作为像素点梯度，将梯度幅值超过一定阈值的像素点作为有效特征点；进一步计算这些特征点梯度方向，得到梯度方向特征图I_G(x)；再对模板的梯度方向特征图进行非极大值抑制操作，使特征点均匀分布，获得最终的模板；

图像梯度幅值梯度方向/>

第二，2D模板匹配中，模板和原图像中的待检测物体之间的变换关系为2D图像仿射变换

为了能在待检测图像中匹配到仿射变换后的物体，需要在2D仿射空间进行采样得到所有可能的仿射变换组合，并生成所有仿射变换下的模板；为了解决大量模板匹配带来的算法运行时间增长，本发明提出了基于分层模板搜索的匹配策略，首先需要构建分层模板库；2D 仿射空间的分层模板库构建步骤如下，首先生成l＝0模板库，对限定范围后的仿射空间σ_min≤s≤σ_max，θ_min≤θ≤θ_max，以采样步长Δs和Δθ进行细分，得到所有可能的2D仿射变换组合：

A(s_min+mΔs,θ_min+nΔθ)，m∈[0,(σ_max-σ_min)/Δs]，n∈[0,(θ_max-θ_min)/Δθ]

第三，对原始模板图像进行2D仿射变换并提取梯度方向特征。在l＝1层，采用2Δs和2Δθ对参数空间进行细分，该层的模板数量减少为l＝0层的1/4；同理，对于l＝k层，采样步长将增加至2^kΔs和2^kΔθ，生成该层的模板库，重复上述步骤生成分层模板库(图2)。

本实施例中，结合图像多分辨率表达和分层模板库的模板匹配策略(图3)：

第一，首先从图像多分辨率金字塔的最高层开始进行匹配。利用滑窗法依次将模板库中的模板和待检测图像进行匹配并计算相似性得分(式4)，保留相似度大于设定阈值的匹配结果；对高尺度空间的匹配结果在下一层尺度空间进行精匹配，同样保留相似度最高的匹配结果，重复进行这一步骤直到搜索到达图像金字塔最低层，得到最终的精细匹配结果。

待匹配的模板定义为T＝{(x₁,d₁),(x₂,d₂),L,(x_m,d_m)}，其中x_i是特征点像素点坐标，d_i是特征点的梯度方向，对待检测图像提取梯度方向特征图，以I＝{(p₁,e₁),(p₂,e₂),…,(p_n,e_n)}集合表示，其中p_i像素点坐标， e_i是该点的梯度方向，I和模板T在图像匹配点c处的相似性得分可以用它们梯度方向的向量点积之和表达:

式中，R(i+c,T)定义为以I上的像素点p_i+c为中心，大小为 T×T的邻域，ori()代表梯度方向；

第二，在滑窗法进行图像空间搜索的过程中，利用分层搜索的匹配策略进一步对匹配过程进行加速；

首先在分层模板库l_max开始，将该层所有模板和待检测图像进行匹配，并计算相似性得分，保留得分大于阈值的模板，同时采用非极大值抑制剔除重复匹配，得到在l_max层的待检索模板集合；完成l_max层匹配后，将得到的待检索模板集合在l_max-1层进行匹配；步骤如下：设l_max层匹配得到待检索模板参数为(s₀,θ₀)，该组合表示为2D仿射变换空间中的一个点，将l_max-1层以该点为中心的一个邻域内的所有模板和待检测图像重新匹配，该邻域可以表示为直积：

[s₀-m·2^l-1Δs,s₀+m·2^l-1Δs]×[θ₀-m·2^l-1Δθ,θ₀+m·2^l-1Δθ]

2^l-1Δs和2^l-1Δθ为l_max-1层采样步长，m为控制邻域大小的参数；同样计算相似度得分，并保留得分最高的模板，得到在l_max-1层的待检索模板集合；由于l_max-1层的采样步长更小，所以匹配结果更为精确；迭代进行上述分层搜索操作，直到所有l_max层待检索模板均回溯到分层模板库的的最底层，即得到最终的精细匹配结果。

本实施例中，S3中模板匹配结果进行2D-ICP优化：

通过模板匹配得到待检测图像中工件中心的像素坐标t＝[t_x,t_y]^T和其2D仿射变换矩阵A；利用2D-ICP对匹配结果进行优化得到更为精确的结果；对模板T任一特征点x_i，在待检测图像I中搜索欧氏距离最近的点q_i作为其对应点；最优化的目标即使得模板特征点到图像对应点所在边缘切线的距离平方和损失函数最小，图像点q_i切线方程可以表示为：(q_i-x)^T·e_i＝0，则待优化的损失函数为：

精确的位姿利用基于LM算法迭代求解上述非线性最小二乘问题，得到最终2D模板匹配结果。

本实施例中，S6中工件6D位姿估计：

第一，首先对场景3D点云进行直通滤波和降采样操作，剔除背景点云干扰并降低点云密度，进一步进行统计滤波剔除噪声点云；

第二，基于欧式聚类分割获取单个工件实例点云；

第三，基于RANSAC方法拟合点云平面方程n^T·x+d＝0；

第四，通过2D模板匹配得到工件中心点像素坐标t＝[t_x,t_y]^T，齐次坐标为p＝[t_x,t_y.1]^T，设工件中心在相机坐标系下的坐标为 p_c＝[x_c,y_c,z_c]^T，根据相机模型，已知内参矩阵K，有：z_cp＝K·p_c；

第五，p_c满足平面方程：n^T·p_c+d＝0，联立4)中等式可以求解z_c＝-d/n^T·K^-1·p，进一步可以求得p_c＝z_c·K^-1·p，根据2D姿态角θ，计算出工件长轴(x轴)在相机坐标系下的方向向量r_x,工件平面z 轴方向向量r_z＝n/||n||，y轴方向向量为r_y＝r_z×r_x，则工件坐标系相对于相机坐标系的旋转矩阵r＝[r_x,r_y,r_z]，由此确定工件的6D位姿参数为:

以上所述，仅为本发明较佳的具体实施方式，但本发明的保护范围并不局限于此，任何熟悉本技术领域的技术人员在本发明揭露的技术范围内，根据本发明的技术方案及其发明构思加以等同替换或改变，都应涵盖在本发明的保护范围之内。

Claims (6)

1.基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，基于2D模板匹配对RGB图像进行工件识别分类和2D图像定位；结合图像多分辨率表达和分层模板库的模板匹配提升算法的运算速度；基于3D点云平面特征和模板匹配结果实现工件的6D位姿估计；

上述方法具体包括以下步骤：

S1：利用双目结构光相机和工业机械臂搭建机器人手眼系统，通过双目结构光相机采集堆叠零件RGB图像和点云图像；

S2：构建待匹配RGB图像的高斯金字塔多分辨率表达，同时构建模板图像的分层模板库，结合多分辨率空间搜索策略和分层模板搜索策略对RGB图像进行模板匹配，进行工件类型识别和2D图像定位；

S3：对模板匹配结果进行2D-ICP优化，得到精确的工件中心2D定位；

S4：对3D点云进行滤波处理，并通过欧式聚类分割获得单个工件实例点云；

S5：对单个实例点云进行RANSAC平面拟合，得到工件的拟合平面方程；

S6：利用拟合平面的法向量、相机内参矩阵计算工业零件的6D位姿估计。

2.根据权利要求1所述的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，所述S2中多分辨率空间搜索策略，多分辨率图像空间构建：

将待检测的原图像I(x,y)作为多分辨率空间的第0层，使用高斯滤波器对其进行滤波得到平滑图像I_s(x,y)，然后进行降采样操作，使得图像分辨率将为原来的1/4，得到多分辨率空间的下一层图像，迭代进行高斯滤波和降采样操作，即得到图像的多分辨率表达：

I_s(x,y)＝G(x,y,σ)*I(x,y)

3.根据权利要求1所述的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，所述S2中分层模板搜索策略，分层模板库构建：

第一，利用Sobel卷积核分别计算零件模板图像三通道的梯度幅值，选取三通道中最大梯度值作为像素点梯度，将梯度幅值超过一定阈值的像素点作为有效特征点；进一步计算这些特征点梯度方向，得到梯度方向特征图I_G(x)；再对模板的梯度方向特征图进行非极大值抑制操作，使特征点均匀分布，获得最终的模板；

图像梯度幅值梯度方向/>

第二，2D模板匹配中，模板和原图像中的待检测物体之间的变换关系为2D图像仿射变换：

在待检测图像中匹配到仿射变换后的物体，在2D仿射空间进行采样得到仿射变换组合，并生成仿射变换下的模板；基于分层模板搜索的匹配策略，构建分层模板库；2D仿射空间的分层模板库构建步骤如下，首先生成l＝0模板库，对限定范围后的仿射空间σ_min≤s≤σ_max，θ_min≤θ≤θ_max，以采样步长Δs和Δθ进行细分，得到所有可能的2D仿射变换组合：

A(s_min+mΔs,θ_min+nΔθ)，m∈[0,(σ_max-σ_min)/Δs]，n∈[0,(θ_max-θ_min)/Δθ]

第三，对原始模板图像进行2D仿射变换并提取梯度方向特征；在l＝1层，采用2Δs和2Δθ对参数空间进行细分，该层的模板数量减少为l＝0层的1/4；同理，对于l＝k层，采样步长将增加至2^kΔs和2^kΔθ，生成该层的模板库，重复上述步骤生成分层模板库。

4.根据权利要求1所述的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，所述结合图像多分辨率表达和分层模板库的模板匹配策略：

第一，首先从图像多分辨率金字塔的最高层开始进行匹配；利用滑窗法依次将模板库中的模板和待检测图像进行匹配并计算相似性得分，保留相似度大于设定阈值的匹配结果；对高尺度空间的匹配结果在下一层尺度空间进行精匹配，同样保留相似度最高的匹配结果，重复进行这一步骤直到搜索到达图像金字塔最低层，得到最终的精细匹配结果；

待匹配的模板定义为T＝{(x₁,d₁),(x₂,d₂),L,(x_m,d_m)}，其中x_i是特征点像素点坐标，d_i是特征点的梯度方向，对待检测图像提取梯度方向特征图，以I＝{(p₁,e₁),(p₂,e₂),…,(p_n,e_n)}集合表示，其中p_i像素点坐标，e_i是该点的梯度方向，I和模板T在图像匹配点c处的相似性得分可以用它们梯度方向的向量点积之和表达:

R(i+c,T)定义为以I上的像素点p_i+c为中心，大小为T×T的邻域，ori()代表梯度方向；

第二，在滑窗法进行图像空间搜索的过程中，利用分层搜索的匹配策略进一步对匹配过程进行加速；

首先在分层模板库l_max开始，将该层所有模板和待检测图像进行匹配，并计算相似性得分，保留得分大于阈值的模板，同时采用非极大值抑制剔除重复匹配，得到在l_max层的待检索模板集合；完成l_max层匹配后，将得到的待检索模板集合在l_max-1层进行匹配；步骤如下：设l_max层匹配得到待检索模板参数为(s₀,θ₀)，该待检索模板参数为(s₀,θ₀)表示为2D仿射变换空间中的一个点，将l_max-1层以该点为中心的一个邻域内的所有模板和待检测图像重新匹配，该邻域可以表示为直积：

[s₀-m·2^l-1Δs,s₀+m·2^l-1Δs]×[θ₀-m·2^l-1Δθ,θ₀+m·2^l-1Δθ]

2^l-1Δs和2^l-1Δθ为l_max-1层采样步长，m为控制邻域大小的参数；同样计算相似度得分，并保留得分最高的模板，得到在l_max-1层的待检索模板集合；l_max-1层的采样步长更小，匹配结果更为精确；迭代进行上述分层搜索操作，所有l_max层待检索模板均回溯到分层模板库的的最底层，得到最终的精细匹配结果。

5.根据权利要求1所述的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，所述S3中模板匹配结果进行2D-ICP优化：

通过模板匹配得到待检测图像中工件中心的像素坐标t＝[t_x,t_y]^T和其2D仿射变换矩阵A；利用2D-ICP对匹配结果进行优化得到更为精确的结果；对模板T任一特征点x_i，在待检测图像I中搜索欧氏距离最近的点q_i作为其对应点；最优化的目标即使得模板特征点到图像对应点所在边缘切线的距离平方和损失函数最小，图像点q_i切线方程可以表示为：(q_i-x)^T·e_i＝0，则待优化的损失函数为：

精确的位姿利用基于LM算法迭代求解非线性最小二乘问题，得到最终2D模板匹配结果。

6.根据权利要求1所述的基于模板匹配的无纹理平面结构工业零件识别和6D位姿估计方法，其特征在于，所述S6中6D位姿估计：

第一，首先对场景3D点云进行直通滤波和降采样操作，剔除背景点云干扰并降低点云密度，进一步进行统计滤波剔除噪声点云；

第二，基于欧式聚类分割获取单个工件实例点云；

第三，基于RANSAC方法拟合点云平面方程n^T·x+d＝0；

第四，通过2D模板匹配得到工件中心点像素坐标t＝[t_x,t_y]^T，齐次坐标为p＝[t_x,t_y.1]^T，设工件中心在相机坐标系下的坐标为p_c＝[x_c,y_c,z_c]^T，根据相机模型，已知内参矩阵K，有：z_cp＝K·p_c；

第五，p_c满足平面方程：n^T·p_c+d＝0，等式可以求解z_c＝-d/n^T·K^-1·p，进一步可以求得p_c＝z_c·K^-1·p，根据2D姿态角θ，计算出工件长轴在相机坐标系下的方向向量r_x,工件平面z轴方向向量r_z＝n/||n||，y轴方向向量为r_y＝r_z×r_x，则工件坐标系相对于相机坐标系的旋转矩阵r＝[r_x,r_y,r_z]，由此确定工件的6D位姿参数为: