[0001] 本发明涉及图像识别处理、点云处理，尤其涉及一种锚孔定位方法及系统。

[0002] 在隧道掘进过程中需要对隧道表面进行锚护作业，当前机械设备在锚护作业过程中采用人工锚护方式，通过人工定位锚点进行施工，工作效率低且工作环境恶劣，并存在一定的危险性。

[0003] 专利202211519958.2提供一种钻锚孔识别方法、系统、电子设备及介质。使用视觉解决方案，只获取中心点位置信息，没有提供锚孔的法向量。专利202310239053.8提供一种钻锚机器人钢带锚孔定位方法、系统及电子设备缺，适用于单一目标的定位，在需要同时处理多个锚孔时，需要进行多次操作，效率低。激光光斑在表面纹理起伏时效果差。专利202211537950.9一种锚孔定位方法、装置、掘锚一体机以及存储介质。需要前处理获取待定位锚孔点云数据，只获取中心位置信息，没有锚孔法向量信息。专利202211531596.9一种钻锚孔中心定位方法、系统、电子设备及介质，二值化处理需要选择合适阈值，阈值可能需要根据具体的应用场景进行调整。阈值选择不当影响最终的中心点定位。同时没有提供法向量。

[0004] 因此，迫切需要研究出一种融合的锚孔定位方法及系统，用于解决锚护作业过程中通过人工定位锚点进行施工，工作效率低且存在危险性的问题。

[0060] 为使本发明实施例的目的、技术方案和优点更加清楚，下面将结合本发明实施例中的附图，对实施例中的技术方案进行清楚、完整地描述，以下实施例用于说明本发明，但不用来限制本发明的范围。

[0061] 下面结合附图1对本发明的具体实施方法进一步阐述。如图1所示，本公开实施例提供了一种锚孔定位方法，该方法包括以下步骤：

[0062] 步骤S100：获取需处理目标对齐的视觉和点云信息数据。

[0063] 具体的，使用RGB深度相机获得对齐的视觉和点云信息。如果无法直接获得点云信息，可以使用激光雷达和视觉传感器，但是需要进行联合标定。

[0064] 步骤S200：通过基于视觉的目标检测获得目标框坐标，并对目标框进行膨胀处理。

[0065] 进一步，目标检测算法使用yolov5，对目标框进行处理膨胀目标框，获得处理后的目标框。

[0066] 膨胀目标框的方法是沿着矩形目标框的长边和短边进行膨胀，边长分别乘以参数α1，α2，其中α1>1,α2>1。膨胀目标框是为了便于获得法向量。

[0067] 步骤S300：在膨胀处理后处理后的目标框范围内获得圆端矩形锚点点云，并对点云进行预处理，获得预处理后的点云。

[0068] 使用对齐的视觉和点云信息，直接获取目标框范围内的点云信息，可以减少点云信息处理量，降低计算量。

[0069] 进一步，使用RGB深度相机可以直接获得目标框范围内的点云信息，也可以在获取内参基础上进行计算，计算公式为：

[0070] X＝(u‑cx)×Z/fx

[0071] Y＝(u‑cy)×Z/fy

[0072] Z＝depth_value×depth_scale

[0073] fx,fy为焦距；cx，cy为主点图像像素中心点坐标；depth_value为图像中像素点的深度信息，depth_scale为比例因子。X，Y，Z为转换后的点云信息，使用ROS系统以及PCL库可以非常方便的处理激光雷达和点云数据，为使用PCL库进行处理，可以将点云转化成ROS标准点云格式。使用视觉传感器和激光雷达可以直接获取目标框范围内的点云信息。

[0074] 在本申请实施例中，对点云进行预处理包括：

[0075] 步骤S310：下采样和去除离群点。

[0076] 使用体素网格滤波进行下采样，需要设置体素网格尺寸；去除离群点使用统计滤波方法，需要设置邻域点个数，距离均值的标准差乘数阀值。

[0077] 步骤S320：通过旋转矩阵Rp和平移向量tp进行转换，将点云坐标系转换到初始全局坐标系。

[0078] 步骤S330：通过定位系统初始化全局坐标系原点。

[0079] 初始全局坐标系原点可以通过定位系统进行初始化，可使用基于激光雷达的slam算法、基于视觉的定位算法或者IMU融合算法。目前采用T265定位相机进行定位结合惯性测量单元IMU和视觉惯性里程计进行定位。

[0080] 步骤S400：使用预先扫描的标准圆端矩形锚点点云，对预处理后的点云进行相似度匹配处理，获得点云匹配度。

[0081] 在本申请实施例中，预先扫描的标准圆端矩形锚点点云，该点云需要进行预处理，将点云坐标变换到圆端矩形中心点坐标。

[0082] 具体方法为：采用手动标定方法获得变换矩阵，将零件放置在距离激光雷达坐标原点的已知坐标上进行扫描，获得旋转矩阵和平移向量。

[0083] 对于预先扫描的标准圆端矩形锚点点云，需要预先准备标准圆端矩形锚点点云作为标准进行相似度匹配。对预处理后的点云进行相似度匹配处理，使用ICP迭代最近点算法进行配准，可获得变换矩阵T，包括旋转矩阵R和平移向量t，通过计算配准误差来判断匹配度m，配准误差使用均方根误差进行计算获得匹配度。

[0084] 均方根误差计算方法：

[0085]

[0086] 其中，N为点对的数量，di为第i个对应点对之间的欧式距离。

[0087] 步骤S500：对处理后的点云进行法向分析，获得点云平面法向评价度和平面参数。

[0088] 对处理后的点云进行法向分析，进行PCA主成分分析获得平面法向量n1,PCA主成分分析使用SVD奇异值分解方法获得点云法向量n1。对预处理后的点云信息进行RANSAC随机采样一致性分析，获得平面法向量n2和平面参数。

[0089] 平面参数形式：

[0090] Ax+By+Cz+D＝0

[0091] A、B、C是平面法向量，D是平面到原点距离。

[0092] 获得点云平面法向评价度和平面参数，点云平面法向评价度为n1和n2的夹角βn。

[0093] PCA主成分分析获得的n1代表了整个点云的法向量，n2为点云中拟合出的一个平面的法向量。通过两者夹角，可以判断点云预处理后法向量获取的质量。

[0094] 步骤S600：对点云匹配度m和点云法向评价度βn加权获得总代价，并根据总代价来处理圆端矩形锚点点云，获得圆端矩形锚点法向量。

[0095] fcost＝γ1m+γ2βn

[0096] 其中，fcost为总代价，γ1和γ2为点云匹配度和点云法向评价度的加权值，加权值和为1，可选取为γ1＝0.5，γ2＝0.5。

[0097] 如果小于阀值处理圆端矩形锚点点云，获得圆端矩形锚点法向量。如果大于阀值，返回到步骤S100选取其他目标重新开始。

[0098] 通过计算配准误差来判断匹配度m，配准误差使用均方根误差进行计算获得匹配度。如果m越大，表示点云离预先扫描的标准圆端矩形锚点点云较远，也就是匹配度越低。

[0099] 获得点云平面法向评价度和平面参数，点云平面法向评价度为n1和n2的夹角βn。夹角越大表示两者偏差较大。所以加权之后的代价越大代表匹配度低，法向也不匹配。

[0100] 步骤S700：对获得圆端矩形锚点法向量进行坐标转换，计算出圆端矩形中心坐标。

[0101] 步骤S400中通过ICP配准获得的平移向量t为相对于初始全局坐标系的中心点坐标。通过S400中ICP配准可获得变换矩阵T，变换矩阵T包括旋转矩阵R和平移向量t。

[0102]

[0103] 其中x、y、z为预先扫描的标准圆端矩形锚点中心点坐标，坐标值为(0，0，0)，步骤S400中使用标定的方法已经将点云坐标变换为以中心点为原点的坐标系。x’、y’、z’为锚点中心相对于初始全局坐标系下的坐标，获得中心点坐标。

[0104] 步骤S800：根据平面参数和膨胀目标框对平面进行随机撒点，利用随机撒点获得的点云对高斯过程回归模型进行训练。

[0105] 根据给定的平面参数和膨胀目标框，在平面上随机撒点生成点云数据集，利用生成的点云数据集(X,Y,Z)，使用高斯过程模型进行训练，其中X，Y作为输入，Z作为输出，学习点的分布和特性。

[0106] 高斯过程模型选择径向基函数核(RBF Kernel)，参数包括长度尺度，是核函数对输入数据的敏感程度；较小的长度尺度意味着核函数对输入变化较为敏感，能够捕捉到较小的变化；较大的长度尺度表示核函数对输入变化较不敏感，更适合捕捉全局趋势。

[0107] 进一步，核函数为：

[0108]

[0109] 其中l为长度尺寸参数，||xi‑xj||是输入数据点之间的欧式距离。

[0110] 步骤S900：通过训练后的高斯过程回归模型判断中心点是否符合标准，来确定获得圆端矩形锚孔点中心坐标。

[0111] 通过训练的高斯过程回归模型，判断中心点的可信度。通过模型的预测值point_pred和不确定性(标准差)sigma来评估中心点的可信度。

[0112] 高斯过程回归结果为预测值point_pred和不确定性(标准差)sigma，假如已经在S7中获得中心点坐标(x’、y’、z’),通过高斯过程回归模型进行预测，其中输入值为(x’、y’)，预测值为z即高度，以及对应的不确定性(标准差)sigma。

[0113] 通过输出的预测值确定95％置信度区间(point_pred‑1.96*sigma,point_pred+1.96)，如果z’在95％置信度区间内，那么确定获得中心点坐标(x’,y’,z’)，获得圆端矩形锚点中心点坐标和法向量并进行输出。否则返回步骤S100重新开始，处理下一个目标。

[0114] 下述为本发明锚孔定位装置实施例，可以用于执行本发明锚孔定位方法实施例。对于本发明锚孔定位装置实施例中未披露的细节，请参照本发明锚孔定位方法实施例。

[0115] 在一个实施例中，提出了一种锚孔定位装置，该装置包括：

[0116] 获取模块，用于获取需处理目标对齐的视觉和点云信息数据；

[0117] 膨胀模块，用于通过基于视觉的目标检测获得目标框坐标，并对目标框进行膨胀处理；

[0118] 预处理模块，用于在膨胀处理后的目标框范围内获得圆端矩形锚孔点点云，并对点云进行预处理；

[0119] 匹配度模块，用于使用预先扫描的标准圆端矩形锚点点云，对预处理后的点云进行相似度匹配处理，获得点云匹配度；

[0120] 法向评价模块，用于对预处理后的点云信息进行法向分析，获得点云平面法向评价度和平面参数；

[0121] 综合加权评价模块，用于对点云匹配度和点云法向评价度加权获得总代价，根据总代价阀值判断是否符合要求；如果符合要求继续处理圆端矩形锚点点云，获得圆端矩形锚点法向量；如果不符合要求，返回处理下一个目标；

[0122] 中心坐标计算模块，用于对获得圆端矩形锚点法向量进行坐标转换，计算获得圆端矩形中心坐标；

[0123] 训练模块，用于根据平面参数和膨胀目标框对平面内进行随机撒点，利用随机撒点获得的点云对高斯过程模型进行训练；

[0124] 判断模块，用于判断中心点进行高斯过程回归是否符合标准；如果符合标准，确定获得圆端矩形锚孔点中心坐标；如果不符合标准，则处理下一个目标。

[0125] 需要说明的是，上述实施例提供的锚孔定位装置在执行锚孔定位方法时，仅以上述各功能模块的划分进行举例说明，实际应用中，可以根据需要而将上述功能分配由不同的功能模块完成，即将设备的内部结构划分成不同的功能模块，以完成以上描述的全部或者部分功能。另外，上述实施例提供的锚孔定位装置与锚孔定位方法实施例属于同一构思，其体现实现过程详见锚孔定位方法实施例，这里不再赘述。

[0126] 如图2所示，在一个实施例中提出了一种锚孔定位系统，该系统包括传感器、计算平台以及移动底盘，传感器包括深度相机和双目视觉定位相机，计算平台使用ubuntu系统和ROS系统运行2D‑3D融合锚孔定位方法，确定锚点中心坐标。计算平台根据锚点中心坐标规划路径，使用MPC控制器输出控制信号包括速度和转向角速度，通过CAN总线和线控底盘通信实现移动。

[0127] 在此处所提供的说明书中，说明了大量具体细节。然而，能够理解，本发明的实施例可以在没有这些具体细节的情况下实践。在一些实例中，并未详细示出公知的方法、结构和技术，以便不模糊对本说明书的理解。

[0128] 此外，本领域的技术人员能够理解，尽管在此所述的一些实施例包括其它实施例中所包含的某些特征而不是其它特征，但是不同实施例的特征的组合同样意味着处于本发明的保护范围之内并且形成不同的实施例。例如，在上面的实施例中，本领域技术人员能够根据获知的技术方案和本申请所要解决的技术问题，以组合的方式来使用。

[0129] 以上所述仅是本发明的较佳实施例而已，并非对本发明作任何形式上的限制，虽然本发明已以较佳实施例揭露如上，然而并非用以限定本发明，任何熟悉本专利的技术人员在不脱离本发明技术方案范围内，当可利用上述提示的技术内容做出些许更动或修饰为等同变化的等效实施例，但凡是未脱离本发明技术方案的内容，依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化与修饰，均仍属于本发明方案的范围内。