[发明专利]基于点云模型的互感器接线端子定位位姿获取方法及系统

权利要求书

1.一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，包括：

S1）采集数据并生成点云模型；

S2）对点云模型进行预处理得到理想目标点云模型；

S3）获取理想目标点云模型各个表面区域；

S4）求解得到各个表面区域的法向量估计；

S5）根据各个表面区域的法向量估计，确定接线端子包围盒主轴方向并生成接线端子最优包围盒，具体包括：

S501）将各个表面区域的法向量估计反变换回空间直角坐标系；

S502）当两个表面区域法向量估计之间角度最接近时，取较大的表面区域的法向量估计为第一法向量，取较小的表面区域的法向量估计为第二法向量，将第二法向量映射到第一法向量所在表面区域拟合的平面并进行正交化，将正交化后的第二法向量乘以第一法向量得到第三法向量，对第一法向量、第二法向量和第三法向量进行交叉正交化运算；

S503）得到相互正交的三个法向量估计，且满足右手定则时，选择较大的表面区域的法向量估计作为接线端子位姿的主轴方向；

S504）根据接线端子包围盒主轴方向生成接线端子最优包围盒；

S6）计算接线端子最优包围盒在机器人基坐标系下的坐标姿态和中心点空间坐标，得到接线端子定位位姿。

2.如权利要求1所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述S1）具体包括：通过3D相机从多个角度采集点云数据并生成3D点云模型。

3.如权利要求1所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述S2）具体步骤包括：

S201）读取点云模型中的点云数据；

S202）通过体素滤波器对点云数据进行降采样；

S203）将降采样后的点云数据移至机器人坐标系下完成粗略分割；

S204）将粗略分割后的点云数据进行去噪得到理想目标点云模型。

4.如权利要求1所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述S3）具体包括：采用区域生长的聚类方式提取点云模型法线方向近似的各个表面区域。

5.如权利要求4所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述采用区域生长的聚类方式提取点云模型法线方向近似的各个表面区域，具体包括：

S301）依据点云模型中点的曲率值对点进行排序，选择曲率值最小的点作为初始种子点，将初始种子点加入种子点序列；

S302）搜索当前种子点的近域点；

S303）当近域点的法线与当前种子点的法线之间的夹角小于平滑阀值时，将当前种子点加入聚类区域；

S304）当近域点的曲率值小于曲率阈值时，将近域点加入种子点序列，并删除当前种子点；

S305）判断种子点序列是否为空，如果是，则判定聚类完成，提取得到点云模型法线方向近似的各个表面区域，否则执行S302）。

6.如权利要求1所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述S4）具体包括：通过球坐标系转换和最小二乘法求解得到各个表面区域的法向量估计。

7.如权利要求1所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，所述S6）具体包括：通过空间仿射变换，计算接线端子最优包围盒在机器人基坐标系下的坐标姿态和中心点空间坐标，得到接线端子定位位姿。

8.如权利要求7所述的一种基于点云模型的互感器接线端子定位位姿获取方法，其特征在于，根据接线端子定位位姿，通过对X、Y、Z三轴进行以为单位的归一化旋转得到统一的接线端子包围盒的最终定位位姿。

9.一种基于点云模型的互感器接线端子定位位姿获取系统，其特征在于，包括：

采集模块，用于采集数据；

模型生成模块，用于根据采集数据生成点云模型；

预处理模块，用于对点云模型进行预处理得到理想目标点云模型；

定位模块，用于获取理想目标点云模型各个表面区域；用于求解得到各个表面区域的法向量估计；用于根据各个表面区域的法向量估计，确定接线端子包围盒主轴方向并生成接线端子最优包围盒；用于计算接线端子最优包围盒在机器人基坐标系下的坐标姿态和中心点空间坐标，得到接线端子定位位姿；

所述根据各个表面区域的法向量估计，确定接线端子包围盒主轴方向并生成接线端子最优包围盒，具体包括：

S501）将各个表面区域的法向量估计反变换回空间直角坐标系；

S502）当两个表面区域法向量估计之间角度最接近时，取较大的表面区域的法向量估计为第一法向量，取较小的表面区域的法向量估计为第二法向量，将第二法向量映射到第一法向量所在表面区域拟合的平面并进行正交化，将正交化后的第二法向量乘以第一法向量得到第三法向量，对第一法向量、第二法向量和第三法向量进行交叉正交化运算；

S503）得到相互正交的三个法向量估计，且满足右手定则时，选择较大的表面区域的法向量估计作为接线端子位姿的主轴方向；

S504）根据接线端子包围盒主轴方向生成接线端子最优包围盒。