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

权利要求书

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)分别将所述牙顶点集和牙底点集内的点对应的三维空间坐标进行圆柱拟合，得到两个空间圆柱，分别计算空间圆柱的参数，该参数包括轴线参数和圆柱半径；通过将两个半径与初始半径进行比较，大于初始半径认为是牙顶点集拟合出的圆柱，小于初始半径则认为是牙底点集拟合出的圆柱；记牙顶点集拟合出的圆柱轴线视为正确轴线，计算正确轴线与底面的夹角及交点坐标，作为含螺纹目标物位姿；底面为含螺纹目标物所固定的平面。

2.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：步骤4)寻找最匹配的位置的方法如下：

通过整体平移先将待匹配点集C_i中的位于顶部的点移至与基准曲线的顶点的坐标重合，再沿基准曲线的长度方向将待匹配点集C_i中所有点按照步长进行平移，同时计算所述待匹配点集中所有点与所述基准曲线上在同水平方向上的点在纵向的距离平方和；当此距离平方和最小时，记此位置为最匹配的位置，其中，步长为0.1mm～0.5mm。

3.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：步骤5)寻找对应于螺纹牙顶、牙底位置的点采用如下方法：

首先标记目标点集中在上下边缘处的所有顶点，拟合边缘线，分别以每条边缘线为中心线，标记与中心线之间的距离值小于阈值d的点为牙顶点集/牙底点集中的点。

4.如权利要求3所述含螺纹目标物的位姿计算方法，其特征在于：所述阈值d＝0.05～0.2mm。

5.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：步骤3)中插值算法为三次样条插值法或分段线性插值法。

6.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：所述拟合圆柱的方法为最小二乘法。

7.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：所述含螺纹目标物为螺柱、螺栓、螺杆或螺孔。

8.如权利要求1所述含螺纹目标物的位姿计算方法，其特征在于：所述螺纹为三角形螺纹、矩形螺纹、梯形螺纹或锯齿形螺纹。