CN117541614A - 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 - Google Patents
基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 Download PDFInfo
- Publication number
- CN117541614A CN117541614A CN202410031283.XA CN202410031283A CN117541614A CN 117541614 A CN117541614 A CN 117541614A CN 202410031283 A CN202410031283 A CN 202410031283A CN 117541614 A CN117541614 A CN 117541614A
- Authority
- CN
- China
- Prior art keywords
- point cloud
- point
- points
- characteristic
- source
- Prior art date
- Legal status (The legal status is an assumption and is not a legal conclusion. Google has not performed a legal analysis and makes no representation as to the accuracy of the status listed.)
- Granted
Links
- 238000004422 calculation algorithm Methods 0.000 title claims abstract description 43
- 238000000034 method Methods 0.000 title claims abstract description 35
- 239000013598 vector Substances 0.000 claims abstract description 70
- 230000005484 gravity Effects 0.000 claims abstract description 13
- 238000001914 filtration Methods 0.000 claims abstract description 10
- 238000005259 measurement Methods 0.000 claims abstract description 9
- 238000005070 sampling Methods 0.000 claims abstract description 6
- 238000010845 search algorithm Methods 0.000 claims abstract description 4
- 239000011159 matrix material Substances 0.000 claims description 40
- 230000009466 transformation Effects 0.000 claims description 19
- 238000013519 translation Methods 0.000 claims description 16
- 238000007781 pre-processing Methods 0.000 claims description 8
- 230000011218 segmentation Effects 0.000 claims description 7
- 230000000694 effects Effects 0.000 claims description 4
- 238000005452 bending Methods 0.000 claims description 3
- 238000006243 chemical reaction Methods 0.000 claims description 3
- 238000011156 evaluation Methods 0.000 claims description 3
- 230000006872 improvement Effects 0.000 claims description 3
- 230000001131 transforming effect Effects 0.000 claims description 2
- 238000000605 extraction Methods 0.000 description 5
- 238000010586 diagram Methods 0.000 description 4
- 230000036544 posture Effects 0.000 description 4
- 238000004088 simulation Methods 0.000 description 4
- 238000012800 visualization Methods 0.000 description 4
- 238000004364 calculation method Methods 0.000 description 3
- 210000001503 joint Anatomy 0.000 description 2
- 238000012986 modification Methods 0.000 description 2
- 230000004048 modification Effects 0.000 description 2
- 238000000513 principal component analysis Methods 0.000 description 2
- 238000012795 verification Methods 0.000 description 2
- 238000013459 approach Methods 0.000 description 1
- 230000009286 beneficial effect Effects 0.000 description 1
- 230000008859 change Effects 0.000 description 1
- 230000007547 defect Effects 0.000 description 1
- 238000012217 deletion Methods 0.000 description 1
- 230000037430 deletion Effects 0.000 description 1
- 238000011161 development Methods 0.000 description 1
- 230000018109 developmental process Effects 0.000 description 1
- 238000005516 engineering process Methods 0.000 description 1
- 238000005286 illumination Methods 0.000 description 1
- 238000003384 imaging method Methods 0.000 description 1
- 238000012423 maintenance Methods 0.000 description 1
- 230000008569 process Effects 0.000 description 1
- 230000009467 reduction Effects 0.000 description 1
- 238000011160 research Methods 0.000 description 1
- 238000012163 sequencing technique Methods 0.000 description 1
Classifications
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/20—Analysis of motion
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T7/00—Image analysis
- G06T7/30—Determination of transform parameters for the alignment of images, i.e. image registration
- G06T7/33—Determination of transform parameters for the alignment of images, i.e. image registration using feature-based methods
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06T—IMAGE DATA PROCESSING OR GENERATION, IN GENERAL
- G06T2207/00—Indexing scheme for image analysis or image enhancement
- G06T2207/10—Image acquisition modality
- G06T2207/10028—Range image; Depth image; 3D point clouds
Landscapes
- Engineering & Computer Science (AREA)
- Computer Vision & Pattern Recognition (AREA)
- Physics & Mathematics (AREA)
- General Physics & Mathematics (AREA)
- Theoretical Computer Science (AREA)
- Multimedia (AREA)
- Image Analysis (AREA)
Abstract
本发明公开了一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法。首先采用统计滤波进行点云去噪；再通过基于体素网格重心邻近点的降采样方法进行点云稀疏；然后利用法向量邻域夹角特性提取特征点，得到特征点云，采用Kd‑tree邻近搜索算法改进后的ICP算法实现对特征点云和目标点云的配准；最终解算得到不同时刻下两片非合作目标点云的位姿测量结果。
Description
技术领域
本发明属于导航制导与控制技术领域，具体涉及一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法。
背景技术
空间非合作目标超近距离相对位姿测量作为在轨服务的关键技术之一，能够为非合作目标的交会对接、抓捕维修等提供精确的相对位姿信息，是开展后续在轨操作的前提。目前，空间非合作目标的相对位姿的测量常采用立体视觉相机、3D 闪光激光雷达等。随着对于空间非合作目标相对位姿测量系统小型化、实时性及测量精度的要求越来越高，近年来，TOF( time-of-flight)原理的3D相机作为一种新型的、小型化立体成像设备，发展迅速。TOF 相机具有功耗低、结构紧凑、实时性高且对光照不敏感等优点，无须扫描便可实时高效地捕捉动态目标的深度信息和灰度图像，适合空间非合作目标超近距离测量。
随着两个航天器之间距离的接近，点云数量变得过于密集，使得位姿跟踪算法难以满足实时性的要求。因此，在点云配准之前，往往需要在尽量保留点云数据特征信息的前提下，对需要对庞大的采样点云模型进行精简，去除大量冗余数据，提高配准效率。
同时，实际采集的点云数据中不可避免的包含着噪声点，影响点云配准的精度和效率。因此需要点云数据进行降噪处理
基于非合作目标三维点云数据的位姿估计算法核心是使用点云配准算求取TOF相机对目标表面采样获得的目标点云与源点云之间齐次变换矩阵。迭代最近点算法（Iterative Closest Point，简称ICP）是解决目标点云与源点云之间对应点的配准以及变换矩阵的求解的经典算法。但是直接采用ICP算法，计算量太大，并且对数据噪声的鲁棒性差，精度也较低；而且传统ICP算法在寻找最邻近点时，将欧氏距离最小的点作为最邻近点，这种暴力检索方式花费时间长，会带来大量的错误匹配点对。
发明内容
针对于上述现有技术的不足，本发明的目的在于提供一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，在空间非合作目标超近距离交会对接的背景下，以TOF相机在不同时刻获取的非合作目标高密集三维点云数据作为研究对象，提出了一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪。首先进行数据预处理：先采用统计滤波去除点云数据的噪声，然后进行点云降采样；然后进行点云配准阶段：首先基于法向量邻域夹角特性提取特征点，然后在寻找最近点过程中应用 Kdtree 数据结构优化ICP算法，提升点云配准效率；最后获取两片非合作目标点云之间的刚体变换矩阵，得到相对位姿测量结果。本文算法在提高配准精度的同时提高配准效率，满足空间非合作目标超近距离交会对接的实时性和高精度要求。
为达到上述目的，本发明采用的技术方案如下：
一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，所述跟踪方法包括如下步骤：
S1，首先针对源点云进行数据预处理，采用统计滤波对源点云进行去噪，再采用基于体素网格重心邻近点的降采样方法进行点云稀疏，降低源点云的数据量，获得预处理后的源点云；
S2，进行点云配准，先对预处理后的源点云进行特征点提取，利用不同邻域半径估计预处理后的源点云的法向量，根据法向量邻域夹角提取特征点，得到预处理后的源点云的特征点云；然后对所述特征点云与目标点云采用基于Kd-tree邻近搜索算法改进后的ICP算法进行点云配准；
S3，计算出源点云和目标点云之间的刚体变换矩阵，获取点云的平移矩阵和旋转矩阵，从而得到不同时刻下两片非合作目标点云的位姿测量结果，实现对空间非合作目标近距离相对位姿的跟踪。
进一步的，所述步骤S1具体包括：
S11，首先采用统计滤波对源点云进行去噪操作：将源点云中的任一点作为查询点，通过近邻搜索找到相对应的领域点集合，统计该查询点与邻域点集合之间的距离，设置第一阈值，将平均领域距离低于或高于第一阈值的点分别划分为内值点或离群点；遍历整个源点云后，将所有离群点滤除实现对源点云的去噪；
S12，然后采用基于体素网格重心邻近点的降采样方法对滤波后的源点云进行降采样：计算出每个点云数据的体素重心后，寻找重心的邻近点来代替体素中的所有点，得到高质量的稀疏点云。
进一步的，所述步骤S2具体包括：
S21，对获得的经过去噪和降采样后的源点云进行特征点提取；
计算不同邻域半径估计点云的法向量：对高质量的稀疏点云任意一点的法线估计近似于估计表面的一个相切面法线，即最小二次拟合平面，该最小二次拟合平面的法向量为对应点处的法向量；
计算所述法向量邻域夹角：定义所述高质量的稀疏点云某一点与其邻近点法向量夹角的算术平均值：/>，其中，/>为点云/>与邻近法向量的夹角；
根据法向量邻域夹角区分特征点和非特征点选取夹角阈值，根据点云与其邻近点法向量的夹角来提取特征点，选取夹角阈值/>，当/>时，点/>处弯曲程度大，则定义此为特征点；当/>时，点/>处平坦，则定义此/>为非特征点，得到源点云的特征点云；
S22，采用基于KD-tree改进的ICP算法对有效特征点云进行点云配准：
首先读入源点云的特征点云和目标点云，在两片点云上预先建立Kd-tree空间拓扑，对于各点云集中所有的点分别计算在x，y，z方向上坐标值方差，方差最大的方向为分割方向；再确定根结点云：对点云数据按分割方向上坐标数值的大小排序，处于中间大小的点云作为为KD-Tree的根节点云；接着确定左、右子空间：将点云在分割方向的坐标小于根节点云的划分到左子树，大于根节点的划分到右子树，在左右子树空间分别在计算方差求分割方向，重复上述步骤直至叶子节点仅余一个节点；
然后采用KD-tree优化的ICP算法对两片特征点云进行配准：先对建立了Kd-tree的特征点云P1和目标点云P2的每个点进行最邻近搜索，即局部最近点搜索；再计算出旋转矩阵R和平移向量，得到初代变换矩阵M1；
将初代变换矩阵M1作用于目标点云P2，变换目标点云P2的坐标，得到变换后的目标点云P’；计算变换后的目标点云P’与原始目标点云的距离误差；当所述距离误差收敛或得到最大迭代次数时，完成目标点云到特征源点云的配准，否则将读入对象更新为P1和P’，重复上述步骤即可求出变换矩阵M。
S23，评估点云配准算法效果，采用配准完成的两帧点云数据对应点的距离差的平方和，即均方误差MSE作为配准精度的评价指标。
进一步的，所述步骤S3具体包括：
S31，建立旋转矩阵与欧拉角之间转换关系：
所述变换矩阵M包括旋转矩阵R和平移向量，表示为：/>，设定源点云和目标点云经过三次旋转实现重合：先绕z轴旋转，产生的旋转角度为滚转角/>；再绕 y 轴旋转，产生的旋转角度为角度偏航角/>；最后绕 x 轴旋转，产生的旋转角度俯仰角/>；以欧拉角的形式按照z-y-x的旋转顺序，旋转矩与欧拉角之间的关系表示为：
平移向量定义为源点云原点到目标点云原点之间的向量，用来表征两个点云之间的相对位置关系；
S32，根据矩阵R和相对平移坐标，获得源点云与目标点云的相对姿态估计结果。
进一步的，所述步骤S21中，对求得的法向量进行方向调整，使之满足，其中，/>为源点云法向量，/>为目标点云法向量。
进一步的，所述步骤S21中，设定法向量夹角阈值=15，源点云的法向量夹角均值大于阈值即为特征点，反之为非特征点；保留特征点，删除非特征点，得到源点云的特征点云。
本发明的有益效果：
本发明提出了一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，同时保证位姿估计的实时性和高精度，实现了空间非合作目标近距离实时位姿跟踪。
本发明采用数据预处理，在保留原始点云特征信息的同时降低点云数量，接着采用基于法向量估计的特征点提取及KD-tree改进的ICP算法进行点云配准，在提高配准精度的同时提高了配准速度。
附图说明
图1为本发明方法的流程图；
图2a为源点云可视化图；
图2b为目标点云可视化图；
图3为源点云数据预处理结果图；
图4为源点云特征提取结果图；
图5a为未配准的源点云与目标点云位置示意图；
图5b为源点云与目标点云的配准结果图。
具体实施方式
为了便于本领域技术人员的理解，下面结合实施例与附图对本发明作进一步的说明，实施方式提及的内容并非对本发明的限定。
参照图1所示，本发明的一种空间非合作目标近距离实时位姿跟踪方法，步骤如下：
S1，首先针对源点云进行数据预处理。先采用统计滤波进行点云去噪，再采用基于体素网格重心邻近点的降采样方法进行点云稀疏，降低点云数据量，获得预处理后的源点云。
在该步骤中，首先采用统计滤波对源点云进行去噪操作。将源点云中的某一点作为查询点，通过近邻搜索找到相对应的领域点集合，统计查询点与邻域点集合之间的距离，设置一个阈值，将平均领域距离低于或高于阈值的点分别划分为内值点或离群点。遍历整个源点云后，即可将所有离群点滤除进而实现对源点云的去噪。
然后采用基于体素网格重心邻近点的降采样方法对滤波后的源点云进行降采样。计算出每个体素的重心后，寻找重心的邻近点来代替体素中的所有点，从而减少点云数据特征点的误删，提高点云数据的精确性，得到高质量的稀疏点云。
S2，然后进行点云配准。先对预处理后的源点云进行特征点提取，利用不同邻域半径估计预处理后的源点云的法向量，并根据法向量邻域夹角提取特征点，降低非特征点对点云配准的影响，得到预处理后的源点云的特征点云；然后对特征点云与目标点云采用Kd-tree邻近搜索算法改进后的ICP算法进行点云配准；
S21，对所述步骤S1中获得的经过去噪和降采样后的源点云进行特征点提取。
首先使用主成分分析（Principal Component Analysis，PCA）的方法，利用不同邻域半径估计点云的法向量。点云数据任意一点的法线估计近似于估计表面的一个相切面法线，即最小二次拟合平面，该平面的法向量为对应点处的法向量。为了保证该点处切平面为最小二乘拟合平面，分析一个协方差矩阵的特征矢量和特征值，则协方差矩阵的最小特征值对应的特征向量就是所求的法向量。为保证点云表面法向量方向的一致性，提取特征点更准确，需要对求得的法向量进行方向调整，使之满足公式，其中，/>为源点云法向量，/>为目标点云法向量。
而后计算法向量邻域夹角。局部区域点云法向量夹角变化较大，则表明该区域较为起伏；法向量夹角变化不大则表明该区域较为平坦。因此，定义点云某一点/>与其邻近点法向量夹角的算术平均值：/>，其中，/>为点云/>与邻近法向量的夹角。
接着选取适当的夹角阈值，根据法向量邻域夹角区分特征点和非特征点。根据点与其邻近点法向量的夹角来提取特征点，选取适当的夹角阈值/>，当/>时，点/>处弯曲程度较大，故/>为特征点；当/>时，点/>处较为平坦，故/>为非特征点。
最后得到特征点云。通过计算空间非合作目标点云数据与邻域之间的法向量夹角，设定法向量夹角阈值=15，其法向量夹角均值大于阈值即为特征点，反之为非特征点。保留特征点，删除非特征点，得到源点云的特征点云。
S22，采用KD-tree改进的ICP算法对有效特征点云进行点云配准。
首先读入源点云的特征点云和目标点云，并在两片点云上预先建立Kd-tree空间拓扑，先确定分割方向，对于点云集中所有的点，分别计算在x，y，z方向上坐标值方差，方差最大的方向为分割方向；再确定根结点云，对点云数据按分割方向上坐标数值的大小排序，处于中间大小的点云作为为KD-Tree的根节点云；接着确定左、右子空间：确定好根结点，将点云在分割方向的坐标小于根节点的划分到左子树，大于根节点的划分到右子树，在左右子树空间分别在计算方差求分割方向，重复上述步骤直至叶子节点仅余一个节点。优化了传统ICP算法的检索方式，提高了ICP算法的计算速度，减低了计算的迭代次数。
然后采用KD-tree优化的ICP算法对两片特征点云进行配准。先对建立了Kd-tree
的特征点云P1和目标点云P2的每个点进行最邻近搜索，即局部最近点搜索；再计算出旋转矩
阵R和平移向量，即初代变换矩阵M1；将初代变换矩阵M1作用于目标点云P2，变换P2的坐
标；计算变换后的目标点云P’与原始目标点云的距离误差；当误差收敛或得到最大迭代次
数时，完成目标点云到特征源点云的配准，否则将读入对象更新为P1和P’，重复上述步骤即
可求出变换矩阵M。
S23，评估点云配准算法效果，采用配准完成的两帧点云数据对应点的距离差的平方和，即均方误差MSE（mean squared error），作为配准精度的评价指标，以此评估点云配准算法效果。均方误差MSE，值越小，匹配精度越高。
S3，完成点云配准后，计算出源点云和目标点云之间的刚体变换矩阵，进一步获取点云的平移矩阵和旋转矩阵，从而得到不同时刻下两片非合作目标点云的位姿测量结果，实现对空间非合作目标近距离相对位姿的跟踪。
所述步骤S3具体包括：
S31，旋转矩阵与欧拉角之间转换关系的建立。
由所述步骤S22可以获得源点云和目标点云之间的变换矩阵M。为了更直观的描述源点云和目标点云之间的位姿变化，将位姿变化的表示方法由变换矩阵转换为欧拉角和平移向量的方式。变换矩阵M包括旋转矩阵R和平移向量，可表示为：/>。现假定，源点云和目标点云经过三次旋转实现重合：先绕z轴旋转，产生的旋转角度为滚转角/>；再绕y轴旋转，产生的旋转角度为角度偏航角/>；最后绕 x 轴旋转，产生的旋转角度俯仰角/>。以欧拉角的形式按照z-y-x的旋转顺序，旋转矩与欧拉角之间的关系可以表示为：
平移向量定义为源点云原点到目标点云原点之间的向量，用来表征两个点云之间的相对位置关系。
S32，求取欧拉角表示的相对位姿结果。矩阵R中含有表征两个坐标系刚性旋转关系的旋转角、/>、/>，因此只需采用点云配准算法求解出R和/>， 就可以获得三个相对旋转角(/>,/>,/>)和相对平移坐标 />，从而获得源点云与目标点云的相对姿态估计结果。
以下通过具体实例说明算法的使用流程：
采用Blensor模拟TOF相机采集点云生成点云数据集，采用C++进行仿真验证，可以获得图2a、图2b、图3、图4、图5a以及图5b的仿真结果。其中，表1为本发明的仿真验证数据结果：
表1
源点云与目标点云的原始点云数分别为393580、393524，(∆α,∆β,∆γ)是源点云与目标点云相对姿态的误差，计算公式为相对姿态的测量值减标准值，源点云与目标点云相对姿态标准值为(1°,1°,10°)；(∆X,∆Y,∆Z)是源点云与目标点云相对位置的误差，计算公式为相对位置测量值减去标准值，源点云与目标点云相对姿态标准值为(0,0,0)。
从表1中可看出，点云配准的速度和精度很高，位姿估计的误差在合理范围内，符合位姿跟踪的精度及实时性要求。
图1为本发明方法的流程图；图2a为源点云可视化图；图2b为目标点云可视化图；图3为源点云数据预处理结果图；图4为源点云特征提取结果图；图5a为未配准的源点云与目标点云位置示意图；图5b为源点云与目标点云的配准结果图。从图2a、图2b中可以看出，原始的源点云与目标点云点云数据密集，存在噪声点。从图3中可看出，数据预处理能够滤除噪声点，在保留原始点云特征信息的同时降低点云数量。从图4中可以看出，特征点提取能够删除非特征点，保留有效特征点，得到特征点云。对比图5a、图5b可看出，本说明提出的改进的ICP算法能够精确实现点云配准。
本发明具体应用途径很多，以上所述仅是本发明的优选实施方式，应当指出，对于本技术领域的普通技术人员来说，在不脱离本发明原理的前提下，还可以作出若干改进，这些改进也应视为本发明的保护范围。
Claims (6)
1.一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述跟踪方法包括如下步骤：
S1，首先针对源点云进行数据预处理，采用统计滤波对源点云进行去噪，再采用基于体素网格重心邻近点的降采样方法进行点云稀疏，降低源点云的数据量，获得预处理后的源点云；
S2，进行点云配准，先对预处理后的源点云进行特征点提取，利用不同邻域半径估计预处理后的源点云的法向量，根据法向量邻域夹角提取特征点，得到预处理后的源点云的特征点云；然后对所述特征点云与目标点云采用基于Kd-tree邻近搜索算法改进后的ICP算法进行点云配准；
S3，计算出源点云和目标点云之间的刚体变换矩阵，获取点云的平移矩阵和旋转矩阵，从而得到不同时刻下两片非合作目标点云的位姿测量结果，实现对空间非合作目标近距离相对位姿的跟踪。
2.根据权利要求1所述的一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述步骤S1具体包括：
S11，首先采用统计滤波对源点云进行去噪操作：将源点云中的任一点作为查询点，通过近邻搜索找到相对应的领域点集合，统计该查询点与邻域点集合之间的距离，设置第一阈值，将平均领域距离低于或高于第一阈值的点分别划分为内值点或离群点；遍历整个源点云后，将所有离群点滤除实现对源点云的去噪；
S12，然后采用基于体素网格重心邻近点的降采样方法对滤波后的源点云进行降采样：计算出每个点云数据的体素重心后，寻找重心的邻近点来代替体素中的所有点，得到高质量的稀疏点云。
3.根据权利要求1所述的一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述步骤S2具体包括：
S21，对获得的经过去噪和降采样后的源点云进行特征点提取；
计算不同邻域半径估计点云的法向量：对预处理后的源点云任意一点的法线估计近似于估计表面的一个相切面法线，即最小二次拟合平面，该最小二次拟合平面的法向量为对应点处的法向量；
计算所述法向量邻域夹角：定义所述预处理后的源点云某一点与其邻近点法向量夹角的算术平均值：/>，其中，/>为点云/>与邻近法向量的夹角；
根据法向量邻域夹角区分特征点和非特征点选取夹角阈值，根据点云与其邻近点法向量的夹角来提取特征点，选取夹角阈值/>，当/>时，点/>处弯曲程度大，则定义此/>为特征点；当/>时，点/>处平坦，则定义此/>为非特征点，得到源点云的特征点云；
S22，采用基于KD-tree改进的ICP算法对有效特征点云进行点云配准：
首先读入源点云的特征点云和目标点云，在两片点云上预先建立Kd-tree空间拓扑，对于各点云集中所有的点分别计算在x，y，z方向上坐标值方差，方差最大的方向为分割方向；再确定根结点云：对点云数据按分割方向上坐标数值的大小排序，处于中间大小的点云作为为KD-Tree的根节点云；接着确定左、右子空间：将点云在分割方向的坐标小于根节点云的划分到左子树，大于根节点的划分到右子树，在左右子树空间分别在计算方差求分割方向，重复上述步骤直至叶子节点仅余一个节点；
然后采用KD-tree优化的ICP算法对两片特征点云进行配准：先对建立了Kd-tree的特征点云P1和目标点云P2的每个点进行最邻近搜索，即局部最近点搜索；再计算出旋转矩阵R和平移向量，得到初代变换矩阵M1；
将初代变换矩阵M1作用于目标点云P2，变换目标点云P2的坐标，得到变换后的目标点云P’；计算变换后的目标点云P’与原始目标点云的距离误差；当所述距离误差收敛或得到最大迭代次数时，完成目标点云到特征源点云的配准，否则将读入对象更新为P1和P’，重复上述步骤即可求出变换矩阵M；
S23，评估点云配准算法效果，采用配准完成的两帧点云数据对应点的距离差的平方和，即均方误差MSE作为配准精度的评价指标。
4.根据权利要求3所述的一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述步骤S3具体包括：
S31，建立旋转矩阵与欧拉角之间转换关系：
所述变换矩阵M包括旋转矩阵R和平移向量，表示为：/>，设定源点云和目标点云经过三次旋转实现重合：先绕z轴旋转，产生的旋转角度为滚转角/>；再绕 y 轴旋转，产生的旋转角度为角度偏航角/>；最后绕 x 轴旋转，产生的旋转角度俯仰角/>；以欧拉角的形式按照z-y-x的旋转顺序，旋转矩与欧拉角之间的关系表示为：
；
平移向量定义为源点云原点到目标点云原点之间的向量，用来表征两个点云之间的相对位置关系；
S32，根据矩阵R和相对平移向量，获得源点云与目标点云的相对姿态估计结果。
5.根据权利要求3所述的一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述步骤S21中，对求得的法向量进行方向调整，使之满足，其中，/>为源点云法向量，/>为目标点云法向量。
6.根据权利要求3所述的一种基于改进ICP算法的空间非合作目标近距离相对位姿跟踪方法，其特征在于，所述步骤S21中，设定法向量夹角阈值=15，源点云的法向量夹角均值大于阈值即为特征点，反之为非特征点；保留特征点，删除非特征点，得到源点云的特征点云。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410031283.XA CN117541614B (zh) | 2024-01-09 | 2024-01-09 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN202410031283.XA CN117541614B (zh) | 2024-01-09 | 2024-01-09 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN117541614A true CN117541614A (zh) | 2024-02-09 |
| CN117541614B CN117541614B (zh) | 2024-03-29 |
Family
ID=89794307
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN202410031283.XA Active CN117541614B (zh) | 2024-01-09 | 2024-01-09 | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN117541614B (zh) |
Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| US20180332480A1 (en) * | 2017-05-10 | 2018-11-15 | Facebook, Inc. | Multi-Hop Wireless Network Planning |
| US20190080503A1 (en) * | 2017-09-13 | 2019-03-14 | Tata Consultancy Services Limited | Methods and systems for surface fitting based change detection in 3d point-cloud |
| CN112070832A (zh) * | 2020-08-07 | 2020-12-11 | 清华大学 | 基于点云dca特征的非合作目标位姿跟踪方法及装置 |
| CN113706619A (zh) * | 2021-10-21 | 2021-11-26 | 南京航空航天大学 | 一种基于空间映射学习的非合作目标姿态估计方法 |
| CN114371485A (zh) * | 2022-03-21 | 2022-04-19 | 中汽研（天津）汽车工程研究院有限公司 | 一种基于icp与多特征数据关联的障碍物预测与跟踪方法 |
| CN114677418A (zh) * | 2022-04-18 | 2022-06-28 | 南通大学 | 一种基于点云特征点提取的配准方法 |
| CN114898041A (zh) * | 2022-05-09 | 2022-08-12 | 南京理工大学 | 一种基于光度误差的改进icp方法 |
| CN115797418A (zh) * | 2022-09-27 | 2023-03-14 | 湖南科技大学 | 一种基于改进icp的复杂机械零件测量点云配准方法及系统 |
| CN116681733A (zh) * | 2023-08-03 | 2023-09-01 | 南京航空航天大学 | 一种空间非合作目标近距离实时位姿跟踪方法 |
-
2024
- 2024-01-09 CN CN202410031283.XA patent/CN117541614B/zh active Active
Patent Citations (10)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN105976353A (zh) * | 2016-04-14 | 2016-09-28 | 南京理工大学 | 基于模型和点云全局匹配的空间非合作目标位姿估计方法 |
| US20180332480A1 (en) * | 2017-05-10 | 2018-11-15 | Facebook, Inc. | Multi-Hop Wireless Network Planning |
| US20190080503A1 (en) * | 2017-09-13 | 2019-03-14 | Tata Consultancy Services Limited | Methods and systems for surface fitting based change detection in 3d point-cloud |
| CN112070832A (zh) * | 2020-08-07 | 2020-12-11 | 清华大学 | 基于点云dca特征的非合作目标位姿跟踪方法及装置 |
| CN113706619A (zh) * | 2021-10-21 | 2021-11-26 | 南京航空航天大学 | 一种基于空间映射学习的非合作目标姿态估计方法 |
| CN114371485A (zh) * | 2022-03-21 | 2022-04-19 | 中汽研（天津）汽车工程研究院有限公司 | 一种基于icp与多特征数据关联的障碍物预测与跟踪方法 |
| CN114677418A (zh) * | 2022-04-18 | 2022-06-28 | 南通大学 | 一种基于点云特征点提取的配准方法 |
| CN114898041A (zh) * | 2022-05-09 | 2022-08-12 | 南京理工大学 | 一种基于光度误差的改进icp方法 |
| CN115797418A (zh) * | 2022-09-27 | 2023-03-14 | 湖南科技大学 | 一种基于改进icp的复杂机械零件测量点云配准方法及系统 |
| CN116681733A (zh) * | 2023-08-03 | 2023-09-01 | 南京航空航天大学 | 一种空间非合作目标近距离实时位姿跟踪方法 |
Non-Patent Citations (2)
| Title |
|---|
| 李运川;王晓红;: "一种用于点云配准的改进迭代最近点算法", 软件导刊, no. 09, 15 September 2020 (2020-09-15) * |
| 胡加涛;吴晓红;何小海;王正勇;龚剑;: "一种基于几何特征由粗到细点云配准算法", 科学技术与工程, no. 05, 18 February 2020 (2020-02-18) * |
Also Published As
| Publication number | Publication date |
|---|---|
| CN117541614B (zh) | 2024-03-29 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN110223348B (zh) | 基于rgb-d相机的机器人场景自适应位姿估计方法 | |
| CN109410321B (zh) | 基于卷积神经网络的三维重建方法 | |
| CN108038906B (zh) | 一种基于图像的三维四边形网格模型重建方法 | |
| CN109493372B (zh) | 大数据量、少特征的产品点云数据快速全局优化配准方法 | |
| CN109903319B (zh) | 一种基于多分辨率的快速迭代最近点配准算法 | |
| CN101082988A (zh) | 自动的深度图像配准方法 | |
| CN112001926B (zh) | 基于多维语义映射rgbd多相机标定方法、系统及应用 | |
| CN115372989A (zh) | 基于激光雷达的越野自动小车长距离实时定位系统及方法 | |
| CN114677418A (zh) | 一种基于点云特征点提取的配准方法 | |
| CN113421291A (zh) | 利用点云配准技术和三维重建技术的工件位置找正方法 | |
| CN111860651A (zh) | 一种基于单目视觉的移动机器人半稠密地图构建方法 | |
| CN114463396B (zh) | 一种利用平面形状和拓扑图投票的点云配准方法 | |
| Yuan et al. | 3D point cloud recognition of substation equipment based on plane detection | |
| CN114529703A (zh) | 一种基于熵增优化的大型复杂构件点云全局匹配方法 | |
| CN107610216B (zh) | 基于粒子群优化多视角立体点云生成方法及应用的摄像机 | |
| CN111765883B (zh) | 机器人蒙特卡罗定位方法、设备及存储介质 | |
| CN117197333A (zh) | 基于多目视觉的空间目标重构与位姿估计方法及系统 | |
| CN116894876A (zh) | 基于实时图像的6-dof的定位方法 | |
| CN117541614B (zh) | 基于改进icp算法的空间非合作目标近距离相对位姿跟踪方法 | |
| CN116883590A (zh) | 一种三维人脸点云优化方法、介质及系统 | |
| CN114563000B (zh) | 一种基于改进型激光雷达里程计的室内外slam方法 | |
| CN113436235B (zh) | 一种激光雷达与视觉点云的初始化自动配准方法 | |
| CN114004900A (zh) | 一种基于点线面特征的室内双目视觉里程计方法 | |
| CN114897967A (zh) | 一种面向挖掘装备自主作业的物料形态识别方法 | |
| Li et al. | An SLAM Algorithm Based on Laser Radar and Vision Fusion with Loop Detection Optimization |
Legal Events
| Date | Code | Title | Description |
|---|---|---|---|
| PB01 | Publication | ||
| PB01 | Publication | ||
| SE01 | Entry into force of request for substantive examination | ||
| SE01 | Entry into force of request for substantive examination | ||
| GR01 | Patent grant | ||
| GR01 | Patent grant |