[发明专利]一种挖掘机周边障碍物三维点云的快速配准方法及系统

权利要求书

1.一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，包括：

获取挖掘机周边障碍物三维点云并进行预处理；

提取源点云和目标点云的特征点进行匹配；

基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准。

2.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述获取挖掘机周边障碍物三维点云并进行预处理，具体为：

利用多线激光雷达获取挖掘机周边场景点云数据；

利用半径滤波器对周边场景点云数据进行快速滤波；

利用体素网格法对滤波后的周边场景点云数据下采样；

基于法向量对采样后周边场景点云数据进行精简。

3.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述提取源点云和目标点云的特征点进行匹配，包括：

(1)利用快速点特征直方图FPFH，为查询点求得它和其k邻域内每个点之间的三个特征元素值统计成一个简化点特征直方图SPFH；

(2)分别对k邻域中的每个点确定k领域，按(1)中的方式分别形成自己的SPFH，对邻域中的各个SPFH进行加权统计：

(3)通过匹配点对间的FPFH特征确定与点q_i唯一对应的点w_i，确定对应点云集；

(4)利用OpenMP提高FPFH的计算速度；

(5)剔除错误匹配点对，利用双向近邻点方法，获得候选匹配点对；

(6)利用分裂法，以标准差和距离为评价标准，将候选匹配点对中的错误的匹配点对全部剔除，最终获得用于ICP计算的初始目标点云集和源点云集。

4.如权利要求3所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述对领域中的各个SPFH进行加权统计为

其中θ_k表示查询点w与给定量空间中的近邻点w_k之间的距离，FPFH(w)表示查询点w的最终直方图，SPFH(w)表示查询点w的简化点特征直方图，SPFH(w_k)表示查询点w的k近邻点的简化点特征直方图。

5.如权利要求1所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述基于迭代最近点ICP算法，引入质心高权重，利用奇异向量分解SVD算法求解旋转矩阵和平移矩阵，完成配准，具体为：

构建将源点云集P和初始目标点云集M中的点对应关系，用以下公式描述为：m_i＝Rp_i+t*+E_i,i＝1,2,...,n(n3)；

通过缩小源点云与目标点云中对应点之间的欧氏距离，建立误差函数使误差函数最小，即minΔE(R,t*)；

求其关于t*的导数，令导数为零，经过计算和化简后得到两帧点云质心，分别为和

构建协方差矩阵

对协方差矩阵U做奇异值分解，U＝HΛJ^T,(Λ＝diag(d_i),d₁＝d₂＝d₃＝0)，根据H，J得到旋转矩阵R＝JH^T，平移变换矩阵

通过预设阈值，进行迭代，完成点云精确配准。

6.如权利要求5所述的一种挖掘机周边障碍物三维点云的快速配准方法，其特征在于，所述求其关于t*的导数，令导数为零，经过计算和化简后得到两帧点云质心，分别为和具体为：

求其关于t*的导数，令导数为零，关于R求最小值，得到下式：

式中，R表示3*3的旋转矩阵；t*表示平移矩阵；E_i表示对应误差；n表示控制点对数目，m_i表示初始目标点云集M中点，p_i表示与点m_i对应源点云集P中的点；

经过计算和化简后得到两帧点云质心，分别为：