基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法

专利主权项内容

1.一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述跟踪方法包括如下步骤：S1，首先针对源点云进行数据预处理，采用统计滤波对源点云进行去噪，再采用基于体素网格重心邻近点的降采样方法进行点云稀疏，降低源点云的数据量，获得预处理后的源点云；S2，进行点云配准，先对预处理后的源点云进行特征点提取，利用不同邻域半径估计预处理后的源点云的法向量，根据法向量邻域夹角提取特征点，得到预处理后的源点云的特征点云；然后对所述特征点云与目标点云采用基于Kd-tree邻近搜索算法改进后的ICP算法进行点云配准；所述步骤S2具体包括：S21，对获得的经过去噪和降采样后的源点云进行特征点提取；计算不同邻域半径估计点云的法向量：对预处理后的源点云任意一点的法线估计近似于估计表面的一个相切面法线，即最小二次拟合平面，该最小二次拟合平面的法向量为对应点处的法向量；计算所述法向量邻域夹角：定义所述预处理后的源点云某一点与其邻近点法向量夹角的算术平均值：/>，其中，/>为点云/>与邻近法向量的夹角；根据法向量邻域夹角区分特征点和非特征点选取夹角阈值，根据点云与其邻近点法向量的夹角来提取特征点，选取夹角阈值/>，当/>时，点/>处弯曲程度大，则定义此/>为特征点；当/>时，点/>处平坦，则定义此/>为非特征点，得到源点云的特征点云；S22，采用基于KD-tree改进的ICP算法对有效特征点云进行点云配准：首先读入源点云的特征点云和目标点云，在两片点云上预先建立Kd-tree空间拓扑，对于各点云集中所有的点分别计算在x，y，z方向上坐标值方差，方差最大的方向为分割方向；再确定根结点云：对点云数据按分割方向上坐标数值的大小排序，处于中间大小的点云作为为KD-Tree的根节点云；接着确定左、右子空间：将点云在分割方向的坐标小于根节点云的划分到左子树，大于根节点的划分到右子树，在左右子树空间分别在计算方差求分割方向，重复上述步骤直至叶子节点仅余一个节点；然后采用KD-tree优化的ICP算法对两片特征点云进行配准：先对建立了Kd-tree的特征点云P和目标点云P的每个点进行最邻近搜索，即局部最近点搜索；再计算出旋转矩阵R和平移向量，得到初代变换矩阵M1；12将初代变换矩阵M1作用于目标点云P，变换目标点云P的坐标，得到变换后的目标点云P’；计算变换后的目标点云P’与原始目标点云的距离误差；当所述距离误差收敛或得到最大迭代次数时，完成目标点云到特征源点云的配准，否则将读入对象更新为P和P’，重复上述步骤即可求出变换矩阵M；221S23，评估点云配准算法效果，采用配准完成的两帧点云数据对应点的距离差的平方和，即均方误差MSE作为配准精度的评价指标；S3，计算出源点云和目标点云之间的刚体变换矩阵，获取点云的平移矩阵和旋转矩阵，从而得到不同时刻下两片非合作目标点云的位姿测量结果，实现对空间非合作目标近距离相对位姿的跟踪。 百度搜索马 克 数 据 网