[发明专利]含螺纹目标物的位姿计算方法

摘要

本发明公开了一种含螺纹目标物的位姿计算方法，包括：获取含螺纹目标物点云数据，拟合初始轴线，作N个平面；将含螺纹目标物上的点投影到当前平面上，将点数量最多的平面记为基准平面，其他平面记为待匹配平面；将所有平面旋转至平行于xoy平面；得到基准螺纹点集和待匹配点集；利用插值算法、曲线拟合方法得到基准曲线；将每个待匹配点集均平移至最匹配的位置；得到目标点集、寻找牙顶点集和牙底点集；进行圆柱拟合，得到轴线，记牙顶点集拟合出的圆柱轴线视为正确轴线，计算位姿；该方法计算出含螺纹目标物的位姿数据，准确、实时性高。

主权项

1.一种含螺纹目标物的位姿计算方法，其特征在于，包括如下步骤：1)获取含螺纹目标物被投影一侧的点云数据，根据所述点云数据拟合圆柱，得到初始轴线和初始半径，分别以螺纹目标物上的一点与所述初始轴线做平面，得到N个平面，所述N个平面满足相邻两平面之间的夹角为5～20°；2)选一个平面，计算螺纹目标物上点到此平面的距离，当距离小于预设阈值T时，将点投影到对应平面并将其标记为此平面上的螺纹点；采用相同方法分别求取所述N个平面上的螺纹点；统计各平面上螺纹点的数量，将数量最多的平面记为基准平面，其他平面记为待匹配平面；将所有平面旋转至平行于xoy平面；原属于所述基准平面上的螺纹点构成的点集被标记为基准螺纹点集，原属于同一待匹配平面上的螺纹点组成的点集标记为待匹配点集C_i，i＝1，2……N‑1；3)利用插值算法对所述基准螺纹点集进行插值、拟合，得到基准曲线；4)将待匹配点集C_i进行平移，至与基准曲线最匹配的位置；以所述基准曲线的长度方向为纵轴，所述待匹配点集与基准曲线最匹配的位置为：同一时刻，所述待匹配点集中所有点与所述基准曲线上在同水平方向上的点在纵向的距离平方之和最小；当所有待匹配点集C_i均位于最匹配的位置时，将其与基准曲线上的所有点合计为目标点集；5)遍历所述目标点集中的所有点，分别寻找对应于螺纹牙顶、牙底位置的点，标记为牙顶点集和牙底点集；6)分别将所述牙顶点集和牙底点集内的点对应的三维空间坐标进行圆柱拟合，得到两个空间圆柱，分别计算空间圆柱的参数，该参数包括轴线参数和圆柱半径；通过将两个半径与初始半径进行比较，大于初始半径认为是牙顶点集拟合出的圆柱，小于初始半径则认为是牙底点集拟合出的圆柱；记牙顶点集拟合出的圆柱轴线视为正确轴线，计算正确轴线与底面的夹角及交点坐标，作为含螺纹目标物位姿；底面为含螺纹目标物所固定的平面。