CN109458994B - 一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 - Google Patents
一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 Download PDFInfo
- Publication number
- CN109458994B CN109458994B CN201811241997.4A CN201811241997A CN109458994B CN 109458994 B CN109458994 B CN 109458994B CN 201811241997 A CN201811241997 A CN 201811241997A CN 109458994 B CN109458994 B CN 109458994B
- Authority
- CN
- China
- Prior art keywords
- point cloud
- space
- cooperative target
- laser
- pose
- 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.)
- Active
Links
Images
Classifications
-
- G—PHYSICS
- G01—MEASURING; TESTING
- G01C—MEASURING DISTANCES, LEVELS OR BEARINGS; SURVEYING; NAVIGATION; GYROSCOPIC INSTRUMENTS; PHOTOGRAMMETRY OR VIDEOGRAMMETRY
- G01C15/00—Surveying instruments or accessories not provided for in groups G01C1/00 - G01C13/00
- G01C15/002—Active optical surveying means
-
- G—PHYSICS
- G06—COMPUTING; CALCULATING OR COUNTING
- G06F—ELECTRIC DIGITAL DATA PROCESSING
- G06F30/00—Computer-aided design [CAD]
- G06F30/20—Design optimisation, verification or simulation
Abstract
一种空间非合作目标激光点云ICP位姿匹配正确性判别方法及系统，开展地面物理仿真试验，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；得到ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；进行直方图分布统计，得到点云距离的标准直方图分布H2；判断两个直方图分布趋势是否一致。本发明的算法数据处理简单、计算复杂度低，适应空间非合作目标位姿匹配的工程应用，充分考虑了点云测量噪声的影响，可实时判定ICP位姿匹配是否达到一致性位姿匹配，进而有效提高位姿匹配的测量精度。
Description
技术领域
本发明涉及一种空间非合作目标激光点云ICP(Iterative Closest Point，最近点迭代)位姿匹配正确性判别方法及系统，适用于在有测量噪声的情况下，将测量激光点云作为输入，进行空间非合作目标位姿解算过程中，判别ICP算法是否实现了测量点云与模型点云间的最佳位姿匹配。
背景技术
随着航天技术的进步和空间任务的多样化发展，对于空间非合作目标位置和姿态的测量需求越来越迫切。应用激光获取非合作目标的点云，具有作用距离远、测距精度高、受外部光照环境影响小，具备全天时工作能力等特点，是空间非合作目标位姿测量的常用测量体制。利用激光的方法获取空间非合作目标点云后，需要对点云进行匹配计算处理，获得测量点云与模型点云之间的位姿关系，或者是获得当前帧测量点云与前一时刻测量点云之间的位姿关系，以此来获得空间非合作目标的相对空间位姿。
实际上，空间非合作目标的位姿解算，在数学方法上等效于对两片相同或局部相同的点云进行配准，如果能够实现点云的精确配准，则目标的位姿解算也更加精确。为了提高点云匹配精度，避免点云误匹配，空间非合作目标位姿解算中的点云匹配在方法上通常分为点云粗配和点云精匹配两个过程。在两片点云位姿差异较大的情况下，首先应用粗匹配方法，如主成份分析法、哈希查表法等对点云进行粗匹配；通过粗匹配，将点云位姿差异缩小，再利用精匹配方法，使两片点云高度匹配、重合，从而提高位姿解算精度。目前ICP算法是公认的高精度点云匹配方法，常用于点云精匹配阶段。但是常规的ICP算法存在如下几个问题：一个是点云迭代匹配过程中，逐个寻找最近点集过程非常耗时；一个是在点云迭代过程中容易陷入局部极小值，导致位姿匹配不准确，同时这种陷入局部极小值的情况在计算过程中难以有效识别；最后一个是在实际应用中，有噪声的测量点云和理想模型点云匹配计算时，最佳的一致匹配有可能并不是ICP的全局极小值。
针对ICP的耗时问题有很多学者提出改进算法，如基于KD-tree最近点搜索方法来提高对应最近点的搜索速度；AKD-tree(Approximate KD-tree)方法通过省去回溯查询来节省搜索时间；基于分层块状全局搜索到临近点局部搜索的改进ICP算法等等。针对陷入局部极小值问题，相应的算法有Fitzgibbon的LM-ICP算法，Sandhu的基于粒子滤波算法，Jiaolong的全局优化ICP(Globally Optimal ICP)算法等等。
发明内容
本发明的技术解决问题是：克服现有技术不足，适应空间非合作目标位姿匹配的工程应用，提供一种空间非合作目标激光点云ICP位姿匹配正确性判别方法及系统，该方法能够在ICP匹配计算后，判别测量的有噪声点云与模型点云是否达到了正确的一致匹配，工程应用效果好。本发明解决ICP算法存在的问题，尤其是在空间非合作目标位姿匹配应用中，如何判定测量的有噪声点云与模型点云在ICP匹配后是否达到了正确的一致匹配，以利于后续处理，进一步提高位姿匹配的精度。
本发明的技术解决方案是：一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，步骤如下：
(1)开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
(2)根据步骤(1)获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
(3)采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得空间中真实的空间非合作目标的测量激光点云；
(4)根据空间中真实的空间非合作目标的标准激光点云和步骤(3)测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
(5)获得步骤(4)得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
(6)对步骤(5)每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
(7)根据步骤(2)激光位姿敏感器的标准直方图分布H1与步骤(6)点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定步骤(4)ICP位姿匹配正确，否则判定步骤(4)ICP位姿匹配不正确。
步骤(1)开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得非合作目标的测量激光点云和空间非合作目标靶标的已知模型点云，具体如下：
地面物理仿真试验是指：在激光位姿敏感器工作距离范围内，放置靶标，该靶标的位置和姿态相对于激光位姿敏感器测量坐标系已知，靶标的已知模型即已知位置和姿态的靶标，用于模拟空间非合作目标，并形成靶标的已知模型点云，用激光位姿敏感器，对靶标进行扫描测量，得到靶标的测量激光点云，即在地面获得模拟实际工况的空间非合作目标靶标的激光点云和靶标的已知模型点云。
步骤(2)根据步骤(1)获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1，具体如下：
对空间非合作目标靶标的测量点云中的每个点，在空间非合作目标靶标的标准点云中，找到与其对应的距离其最近的点，形成对应的点云对，对每一组点云对，求三维欧几里德空间距离，即用空间非合作目标靶标的测量点云中的点坐标减去空间非合作目标靶标的标准点云中的点坐标，并求其绝对值。
步骤(3)采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云，具体如下：
将激光位姿敏感器运用到空间非合作目标位姿测量的实际工作中，激光位姿敏感器通过对空间非合作目标发射激光，并接收被空间非合作目标表面反射回来的激光回波，测量激光从发射到接收到激光回波之间的时间，进而根据激光的传播速度，以及激光发射的方位角和俯仰角，计算得到空间非合作目标的相对于激光位姿敏感器测量坐标系的一个空间位置点，根据这一原理，在一个测量周期内，对激光位姿敏感器的整个测量视场依次完成激光的发射和激光回波的接收工作，即可形成一帧空间非合作目标的测量激光点云。
步骤(4)根据空间中真实的空间非合作目标的标准激光点云和步骤(3)测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云，具体如下：
应用ICP匹配算法，对空间非合作目标的测量激光点云和空间非合作目标的标准激光点云进行位姿匹配迭代，以空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和最小为迭代目标，每进行一次位姿匹配迭代，空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和都会缩小一点，同时空间非合作目标的测量激光点云的位姿也进一步接近空间非合作目标的标准激光点云的位姿，直到以上两片点云每组点的距离绝对值平方的和达到预定的停止位姿匹配迭代的门限值，此时经位姿匹配迭代后具有新的位姿状态的空间非合作目标的测量激光点云，即定义为接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云。
步骤(5)获得步骤(4)得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离，具体如下：
每一组对应点的三维欧几里德空间距离的计算方法是，每一组对应的两点的三维坐标值分别相减并取平方，然后对平方后的结果求和，然后再对求和的结果开平方，即得到每一组对应点的三维欧几里德空间距离。
步骤(6)对步骤(5)每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2，步骤如下：
首先求取步骤(5)每一组对应点的三维欧几里德空间距离的最大值，并用此最大值分别与步骤(5)每一组对应点的三维欧几里德空间距离相除，得到标准化的步骤(5)每一组对应点的三维欧几里德空间距离，依据实际需求设定直方图分布统计的等采样数值，在零到一的数值区间内，统计标准化的步骤(5)每一组对应点的三维欧几里德空间距离落入各个数值区间的数目，求取落入各个数值区间的数目的最大值，用其与落入各个数值区间的数目值相除，得到标准化的每一组对应点的三维欧几里德空间距离落入各个数值区间的数目的归一化数值，经以上计算处理后即得到点云距离的标准直方图分布H2。
步骤(7)根据步骤(2)激光位姿敏感器的标准直方图分布H1与步骤(6)点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，优选方案如下：
求激光位姿敏感器的标准直方图分布H1，与点云距离标准直方图分布H2，之间落入对应区间的三维欧几里德空间距离数目的差的平方的求和均值，当差的平方的求和均值小于等于设定阈值，判定取得位姿匹配一致性，当差的平方的求和均值大于设定阈值，判定未取得位姿匹配一致性。
步骤(7)根据步骤(2)激光位姿敏感器的标准直方图分布H1与步骤(6)点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，优选方案如下：
分别对激光位姿敏感器的标准直方图分布H1和点云距离标准直方图分布H2，依据落入各个区间的距离数目大小进行排序，并记录其对应的区间序号，如果激光位姿敏感器的标准直方图分布H1和点云距离标准直方图分布H2经排序后，两直方图分布所对应的前m个区间序号完全相同，m取3～5，则判定取得位姿匹配一致性；否则判定未取得位姿匹配一致性。
空间非合作目标，是指：外太空中，无法与其建立通讯联系，未事先在其表面构建合作标识物或标识基准，而广泛存在的一类空间目标。
激光位姿敏感器需要有非合作目标点云测量的能力，同时在进行非合作目标测量，并进行非合作目标激光点云ICP位姿匹配正确性判前，要求通过步骤(1)和步骤(2)已经获得了激光位姿敏感器的标准直方图分布H1。
一种空间非合作目标激光点云ICP位姿匹配正确性判别系统，包括：地面物理仿真试验模块、点云确定模块、测量模块、位姿匹配模块、距离确定模块、统计模块、判断模块；
地面物理仿真试验模块，开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
点云确定模块，根据地面物理仿真试验模块获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
测量模块，采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云；
位姿匹配模块，根据空间中真实的空间非合作目标的标准激光点云和测量模块测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
距离确定模块，获得位姿匹配模块得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
统计模块，对距离确定模块中每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
判断模块，根据点云确定模块中的激光位姿敏感器的标准直方图分布H1与统计模块中点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定位姿匹配模块中ICP位姿匹配正确，否则判定位姿匹配模块中ICP位姿匹配不正确。
本发明与现有技术相比的优点在于：
(1)本发明考虑了点云测量的仪器噪声，将激光雷达点云测量的测量噪声分布与ICP后两点云对应点间的距离差分布的一致性，作为评价点云匹配是否达到一致匹配的标准，可以有效判别ICP算法陷入局部最小或在噪声干扰下未达到位姿最佳匹配的情况，使后续纠正匹配成为可能，从而提高位姿匹配精度。本发明方法可行，考虑了测量噪声对算法的影响，因此具有实用性。
(2)本发明考虑了工程应用中，ICP算法不可避免的受到测量噪声的影响，导致点云的一致匹配并不是ICP全局极小值的情况，利用本发明方法可以及时判别出这种情况，具有工程实际应用价值。
(3)本发明中提供的两种判别两个直方图分布趋势是否一致的方法，具有方法简单、计算复杂度低、分布趋势一致性判定有效的特点，适合应用于空间在轨计算资源受限的条件下。
(4)由于计算复杂度低，本发明方法可以实现实时的ICP位姿匹配正确性判别，及时发现ICP匹配未达到最佳匹配的情况，有利于后续算法及时进行位姿匹配纠正，避免出现粗大的位姿匹配误差。
附图说明
图1为本发明的流程图；
图2的(a)为经ICP后的测量点云与标准激光点云达到一致最优位姿匹配的情况示意图；(b)为经ICP后的测量点云与标准激光点云达到ICP的最优目标函数极值示意图；
图3为采用本发明方法获得仪器标准直方图分布H1示意图；
图4为采用本发明方法获得点云距离标准直方图分布H2示意图；
图5为采用本发明方法判断直方图分布H1和H2是否分布相同的方法的示意图。
图6为本发明激光位姿敏感器的标准直方图分布H1示意图；
图7为本发明点云距离标准直方图分布H2示意图。
具体实施方式
下面结合附图和具体实施例进行详细说明。
本发明一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，开展地面物理仿真试验，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；得到ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；进行直方图分布统计，得到点云距离的标准直方图分布H2；判断两个直方图分布趋势是否一致。本发明的算法数据处理简单、计算复杂度低，适应空间非合作目标位姿匹配的工程应用，充分考虑了点云测量噪声的影响，可实时判定ICP位姿匹配是否达到一致性位姿匹配，进而有效提高位姿匹配的测量精度。
在工程应用中，ICP算法用于非合作目标位姿匹配的精细配准环节，可以有效提高位姿的配准精度，但是ICP算法不可避免的受到测量噪声的影响，导致点云的一致匹配并不是ICP全局极小值的情况时有发生，使位姿匹配出现粗大的位姿匹配误差，在位姿匹配实时解算中，这种情况如果得不到及时的判别发现，会使位姿匹配的精度降低，达不到预期的测量精度。
在噪声的影响下，点云的一致匹配并不是ICP全局极小值的情况时有发生的原理依据如下：
在空间非合作目标位姿测量中，有两类点云集，一类是目标的模型点云集，记为Y＝{yj},j＝1,…,N，一类是激光雷达测量获得的带噪声的点云集，记为X＝{xi},i＝1,…,M，其中，
全体有序三元实数的集合。通常ICP算法是寻找或估计旋转矩阵R和平移矩阵t，R∈SO(3)，SO(3)为空间三维正交群，
为全体有序三元实数的集合，使得核函数在L2范数下的误差E最小，如下公式所示：
式中，ei(R,t)是点集X中每个点与对应
点的残差。
通过观察上式，如果测量点云集X没有噪声干扰，只与对应的模型点云集
相差一个旋转矩阵R∈SO(3)空间三维正交群和平移矩阵
全体有序三元实数的集合，则使误差E为最小的估计旋转矩阵和平移矩阵
必然一致接近真实的旋转矩阵R和平移矩阵t，所以在理想情况下，ICP算法上的目标是搜索使误差E最小时的
认为此时的
最接近真实的旋转矩阵R和平移矩阵t。
在工程应用中，激光雷达测量获得的测量点云是有噪声的，也就是说测量点云集X与对应的模型点云集
除了相差一个旋转矩阵R∈SO(3)空间三维正交群和平移矩阵
全体有序三元实数的集合外，还相差一个测量误差K＝{ki}，且误差K＝{ki}的分布与测量仪器相关，因此引入测量误差K＝{ki}后，不能保证使误差E最小时的
估计最接近真实的旋转矩阵R和平移矩阵t，也就是说在噪声的影响下，点云的一致匹配并不一定是ICP全局极小值时的情况。
图2的(a)为经ICP后的测量点云与标准激光点云达到一致最优位姿匹配的情况，此时ICP的目标函数为0.266，两点云的位姿匹配误差为零度(b)为经ICP后的测量点云与标准激光点云达到ICP的最优目标函数极值，但未达到一致最优位姿匹配，此时ICP的目标函数为0.199，两点云的位姿匹配误差为3.6度。
举例如图2的(a)、(b)所示展示了在测量噪声的影响下，ICP位姿匹配算法的全局极小值发生时，并不能保证测量点云与模型点云间的匹配位姿达到一致匹配。图2(b)的ICP最优目标函数极值为0.199，小于图2(a)的0.206，但是图2(b)的位姿匹配角度误差为3.6度，大于图2(a)的0度，此时图2(a)达到了位姿一致匹配，但ICP的最优目标函数并不是最小的。
如图1所示，本发明按照如下步骤进行实施：
(1)开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
地面物理仿真试验是指：在激光位姿敏感器工作距离范围内，放置相对于激光位姿敏感器测量坐标系来说，已知位置和姿态的靶标，用于模拟空间非合作目标，并形成靶标的已知模型点云，记为Pi＝(xi,yi,zi),i∈Ω自然数集合，用激光位姿敏感器，对靶标进行扫描测量，得到靶标的测量激光点云，即在地面获得模拟实际工况的空间非合作目标靶标的激光点云和靶标的已知模型点云，记为Qi＝(xxi,yyi,zzi),i∈Ω自然数集合，其中，xxi,yyi,zzi分别为姿敏感器测量坐标系下的x、y、z三轴分量。
(2)根据步骤(1)获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
对空间非合作目标靶标的测量点云中的每个点，在空间非合作目标靶标的标准点云中，找到与其对应的距离其最近的点，形成对应的点云对，对每一组点云对，求三维欧几里德空间距离，即用空间非合作目标靶标的测量点云中的点坐标减去空间非合作目标靶标的标准点云中的点坐标，并求其绝对值。
获得模拟实际工况的空间非合作目标靶标的激光点云与靶标的已知模型点云的距离的计算公式为：
自然数集合，对距离d(Qi,Pi)进行直方图统计，并进行标准化，方法为：
首先获得最大距离d(Qi,Pi)max，对距离归一化处理d(Qi,Pi)/d(Qi,Pi)max,i∈Ω自然数集合，在[0,1]区间内分成n等份，统计落入各区间的距离数目λn，对各区间的距离数目λn归一化处理λn/max(λn)，max(λn)表示λn中的最大值，形成激光位姿敏感器的标准直方图分布H1，如图3所示。实际应用中n根据需要确定，通常n取10或15。
(3)采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云；
将激光位姿敏感器运用到空间非合作目标位姿测量的实际工作中，激光位姿敏感器通过对空间非合作目标发射激光，并接收被空间非合作目标表面反射回来的激光回波，测量激光从发射到接收到激光回波之间的时间，进而根据激光的传播速度，以及激光发射的方位角和俯仰角，计算得到空间非合作目标的相对于激光位姿敏感器测量坐标系的一个空间位置点，根据这一原理，在一个测量周期内，对激光位姿敏感器的整个测量视场依次完成激光的发射和激光回波的接收工作，即可形成一帧空间非合作目标的测量激光点云。
(4)根据空间中真实的空间非合作目标的标准激光点云和步骤(3)测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
应用ICP匹配算法，对空间非合作目标的测量激光点云和空间非合作目标的标准激光点云进行位姿匹配迭代，以空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和最小为迭代目标，每进行一次位姿匹配迭代，空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和都会缩小一点，同时空间非合作目标的测量激光点云的位姿也进一步接近空间非合作目标的标准激光点云的位姿，直到以上两片点云每组点的距离绝对值平方的和达到预定的停止位姿匹配迭代的门限值，此时经位姿匹配迭代后具有新的位姿状态的空间非合作目标的测量激光点云，即定义为接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云。
空间中真实的空间非合作目标的标准激光点云通过非合作目标的机械模型进行点云采样抽取获得，应用的抽取软件可以是MeshLab，然后将采样抽取的点云事先写入或上传注入到激光位姿敏感器的控制计算机中，形成真实的空间非合作目标的标准激光点云供目标位姿匹配测量应用。
(5)获得步骤(4)得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
每一组对应点的三维欧几里德空间距离的计算方法是，每一组对应的两点的三维坐标值分别相减并取平方，然后对平方后的结果求和，然后再对求和的结果开平方，即得到每一组对应点的三维欧几里德空间距离。用数学公式的方法表达为：
(6)对步骤(5)每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
直方图统计，并进行标准化方法同步骤(2)，直方图n等分的数目与仪器标准直方图分布H保持相同。点云距离直方图分布H2如图4所示。
(7)根据步骤(2)激光位姿敏感器的标准直方图分布H1与步骤(6)点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定步骤(4)ICP位姿匹配正确，否则判定步骤(4)ICP位姿匹配不正确。
方法1为：求激光位姿敏感器的标准直方图分布H1与点云距离标准直方图分布H2对应区间内距离数目的差的平方的求和均值Δrms＝[(γ1-λ1)2+(γ2-λ2)2+…+(γn-λn)2]/n，设定阈值ρ，当Δrms≤ρ，判定取得位姿匹配一致性；当Δrms＞ρ，判定未取得位姿匹配一致性。
方法2为：分别对激光位姿敏感器的标准直方图分布H1与点云距离标准直方图分布H2中落入各区间的距离数目从大到小排序，并记录其对应的区间序号，如果激光位姿敏感器的标准直方图分布H1与点云距离标准直方图分布H2中距离数目前m个大的距离数目所对应的区间序号完全相同，优选m取3～5，则判定取得位姿匹配一致性；否则判定未取得位姿匹配一致性。如图5所示。
本发明一种空间非合作目标激光点云ICP位姿匹配正确性判别系统，包括：地面物理仿真试验模块、点云确定模块、测量模块、位姿匹配模块、距离确定模块、统计模块、判断模块；
地面物理仿真试验模块，开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
点云确定模块，根据地面物理仿真试验模块获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
测量模块，采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云；
位姿匹配模块，根据空间中真实的空间非合作目标的标准激光点云和测量模块测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
距离确定模块，获得位姿匹配模块得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
统计模块，对距离确定模块中每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
判断模块，根据点云确定模块中的激光位姿敏感器的标准直方图分布H1与统计模块中点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定位姿匹配模块中ICP位姿匹配正确，否则判定位姿匹配模块中ICP位姿匹配不正确。
本发明的核心思路在于利用激光位姿敏感器的点云测量误差分布在地面试验条件下和在真实的激光点云测量过程中均保持分布的一致性不变的特性。在经ICP位姿匹配后，如果点云距离的标准直方图分布H2的分布与在地面试验条件下获得的激光位姿敏感器的标准直方图分布H1仍然保持这种分布的一致性，则判定为取得位姿匹配一致性，否则判定未取得位姿匹配一致性。该方法可以有效判别ICP算法陷入局部最小或在噪声干扰下未达到位姿最佳匹配的情况，使后续纠正位姿匹配成为可能，从而提高位姿匹配精度。
利用图2的(a)、(b)中的点云仿真数据进行本发明算法的一个测试验证：
首先，获得了激光位姿敏感器的标准直方图分布H1，如图6所示；
然后，如图2(b)所示，应用ICP算法对一次测量得到的非合作目标的测量激光点云和空间非合作目标的标准激光点云进行位姿匹配，得到ICP的最优目标函数极值为0.199，此时，根据ICP匹配后激光点云与空间非合作目标的标准激光点云，进点云距离标准直方图分布H2的绘制，如图7所示。
利用判断两个直方图分布趋势是否一致方法1进行计算，得到
Δrms＝[(0.94-1)2+(0.81-0.91)2+(1-0.69)2
+(0.68-0.50)2+(0.36-0.34)2+(0.20-0.25)2
+(0.20-0.20)2+(0.18-0.18)2+(0.10-0.10)2+(0.10-0.10)2]/10
＝0.0145
设定阈值为0.01，阈值的设定参考依据为激光位姿敏感器的测量精度的平方，本仿真中设定激光位姿敏感器的测量精度为0.1m，因此设定阈值为0.01。因此判定图2(b)经ICP匹配后的两组点云未达到位姿匹配一致性。通过图2的(a)、(b)可以看到，此方法的判断是有效的，在ICP的最优目标函数达到极致0.199的情况下，两点云的位姿匹配角度误差仍有3.6度，而点云的最优匹配位姿为0度，如果没有本发明的判定方法，则在位姿匹配解算中会引入3.6度的粗大位姿匹配误差，从而降低了测量系统的位姿匹配精度。
同样在图2的(a)、(b)的数据基础上，利用判断两个直方图分布趋势是否一致方法2进行计算，也可以得到相同的判断结果，即判定图2(b)经ICP匹配后的两组点云未达到位姿匹配一致性。
本发明方法经过了计算机仿真测试验证和空间非合作目标绕飞、抵近、捕获全物理试验平台的物理试验验证，试验结果表明本发明方法，将激光雷达点云测量的测量噪声分布与ICP后两点云对应点间的距离差分布的一致性，作为评价点云匹配是否达到一致匹配的标准，可以有效判别ICP算法陷入局部最小或在噪声干扰下未达到位姿最佳匹配的情况，使后续纠正匹配成为可能，从而提高位姿匹配精度。
本发明考虑了点云测量的仪器噪声，将激光雷达点云测量的测量噪声分布与ICP后两点云对应点间的距离差分布的一致性，作为评价点云匹配是否达到一致匹配的标准，可以有效判别ICP算法陷入局部最小或在噪声干扰下未达到位姿最佳匹配的情况，使后续纠正匹配成为可能，从而提高位姿匹配精度。本发明方法可行，考虑了测量噪声对算法的影响，因此具有实用性。本发明考虑了工程应用中，ICP算法不可避免的受到测量噪声的影响，导致点云的一致匹配并不是ICP全局极小值的情况，利用本发明方法可以及时判别出这种情况，具有工程实际应用价值。
本发明中提供的两种判别两个直方图分布趋势是否一致的方法，具有方法简单、计算复杂度低、分布趋势一致性判定有效的特点，适合应用于空间在轨计算资源受限的条件下。由于计算复杂度低，本发明方法可以实现实时的ICP位姿匹配正确性判别，及时发现ICP匹配未达到最佳匹配的情况，有利于后续算法及时进行位姿匹配纠正，避免出现粗大的位姿匹配误差。
Claims (10)
1.一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于步骤如下：
(1)开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
(2)根据步骤(1)获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
(3)采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得空间中真实的空间非合作目标的测量激光点云；
(4)根据空间中真实的空间非合作目标的标准激光点云和步骤(3)测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
(5)获得步骤(4)得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
(6)对步骤(5)每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
(7)根据步骤(2)激光位姿敏感器的标准直方图分布H1与步骤(6)点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定步骤(4)ICP位姿匹配正确，否则判定步骤(4)ICP位姿匹配不正确。
2.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(1)开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得非合作目标的测量激光点云和空间非合作目标靶标的已知模型点云，具体如下：
地面物理仿真试验是指：在激光位姿敏感器工作距离范围内，放置靶标，该靶标的位置和姿态相对于激光位姿敏感器测量坐标系已知，靶标的已知模型即已知位置和姿态的靶标，用于模拟空间非合作目标，并形成靶标的已知模型点云，用激光位姿敏感器，对靶标进行扫描测量，得到靶标的测量激光点云，即在地面获得模拟实际工况的空间非合作目标靶标的激光点云和靶标的已知模型点云。
3.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(2)根据步骤(1)获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1，具体如下：
对空间非合作目标靶标的测量点云中的每个点，在空间非合作目标靶标的标准点云中，找到与其对应的距离其最近的点，形成对应的点云对，对每一组点云对，求三维欧几里德空间距离，即用空间非合作目标靶标的测量点云中的点坐标减去空间非合作目标靶标的标准点云中的点坐标，并求其绝对值。
4.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(3)采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云，具体如下：
将激光位姿敏感器运用到空间非合作目标位姿测量的实际工作中，激光位姿敏感器通过对空间非合作目标发射激光，并接收被空间非合作目标表面反射回来的激光回波，测量激光从发射到接收到激光回波之间的时间，进而根据激光的传播速度，以及激光发射的方位角和俯仰角，计算得到空间非合作目标的相对于激光位姿敏感器测量坐标系的一个空间位置点，根据这一原理，在一个测量周期内，对激光位姿敏感器的整个测量视场依次完成激光的发射和激光回波的接收工作，即可形成一帧空间非合作目标的测量激光点云。
5.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(4)根据空间中真实的空间非合作目标的标准激光点云和步骤(3)测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云，具体如下：
应用ICP匹配算法，对空间非合作目标的测量激光点云和空间非合作目标的标准激光点云进行位姿匹配迭代，以空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和最小为迭代目标，每进行一次位姿匹配迭代，空间非合作目标的测量激光点云和空间非合作目标的标准激光点云相对应的每组点的距离绝对值平方的和都会缩小一点，同时空间非合作目标的测量激光点云的位姿也进一步接近空间非合作目标的标准激光点云的位姿，直到测量激光点云和标准激光点云这两片点云每组点的距离绝对值平方的和达到预定的停止位姿匹配迭代的门限值，此时经位姿匹配迭代后具有新的位姿状态的空间非合作目标的测量激光点云，即定义为接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云。
6.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(5)获得步骤(4)得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离，具体如下：
每一组对应点的三维欧几里德空间距离的计算方法是，每一组对应的两点的三维坐标值分别相减并取平方，然后对平方后的结果求和，然后再对求和的结果开平方，即得到每一组对应点的三维欧几里德空间距离。
7.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：步骤(6)对步骤(5)每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2，步骤如下：
首先求取步骤(5)每一组对应点的三维欧几里德空间距离的最大值，并用此最大值分别与步骤(5)每一组对应点的三维欧几里德空间距离相除，得到标准化的步骤(5)每一组对应点的三维欧几里德空间距离，依据实际需求设定直方图分布统计的等采样数值，在零到一的数值区间内，统计标准化的步骤(5)每一组对应点的三维欧几里德空间距离落入各个数值区间的数目，求取落入各个数值区间的数目的最大值，用其与落入各个数值区间的数目值相除，得到标准化的每一组对应点的三维欧几里德空间距离落入各个数值区间的数目的归一化数值，经以上计算处理后即得到点云距离的标准直方图分布H2。
8.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：空间非合作目标，是指：外太空中，无法与其建立通讯联系，未事先在其表面构建合作标识物或标识基准，而广泛存在的一类空间目标。
9.根据权利要求1所述的一种空间非合作目标激光点云ICP位姿匹配正确性判别方法，其特征在于：激光位姿敏感器需要有非合作目标点云测量的能力，同时在进行非合作目标测量，并进行非合作目标激光点云ICP位姿匹配正确性判别，要求通过步骤(1)和步骤(2)已经获得了激光位姿敏感器的标准直方图分布H1。
10.一种空间非合作目标激光点云ICP位姿匹配正确性判别系统，其特征在于包括：地面物理仿真试验模块、点云确定模块、测量模块、位姿匹配模块、距离确定模块、统计模块和判断模块；
地面物理仿真试验模块，开展地面物理仿真试验，采用激光位姿敏感器，在地面模拟实际工况对空间非合作目标靶标进行扫描测量，获得空间非合作目标靶标的测量点云和空间非合作目标靶标的已知模型点云；
点云确定模块，根据地面物理仿真试验模块获得的空间非合作目标靶标的测量点云和空间非合作目标靶标的标准点云，确定激光位姿敏感器的标准直方图分布H1；
测量模块，采用激光位姿敏感器，测量空间中真实的空间非合作目标，获得非合作目标的测量激光点云；
位姿匹配模块，根据空间中真实的空间非合作目标的标准激光点云和测量模块测量得到的空间中真实的空间非合作目标的测量激光点云，进行ICP位姿匹配，得到接近于空间中真实的空间非合作目标的标准激光点云的ICP匹配后激光点云；
距离确定模块，获得位姿匹配模块得到的ICP匹配后激光点云与空间中真实的空间非合作目标的标准激光点云上，每一组对应点的三维欧几里德空间距离；
统计模块，对距离确定模块中每一组对应点的三维欧几里德空间距离，进行直方图分布统计，得到点云距离的标准直方图分布H2；
判断模块，根据点云确定模块中的激光位姿敏感器的标准直方图分布H1与统计模块中点云距离标准直方图分布H2，判断两个直方图分布趋势是否一致，若一致则判定位姿匹配模块中ICP位姿匹配正确，否则判定位姿匹配模块中ICP位姿匹配不正确。
Priority Applications (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811241997.4A CN109458994B (zh) | 2018-10-24 | 2018-10-24 | 一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 |
Applications Claiming Priority (1)
| Application Number | Priority Date | Filing Date | Title |
|---|---|---|---|
| CN201811241997.4A CN109458994B (zh) | 2018-10-24 | 2018-10-24 | 一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 |
Publications (2)
| Publication Number | Publication Date |
|---|---|
| CN109458994A CN109458994A (zh) | 2019-03-12 |
| CN109458994B true CN109458994B (zh) | 2021-04-13 |
Family
ID=65608286
Family Applications (1)
| Application Number | Title | Priority Date | Filing Date |
|---|---|---|---|
| CN201811241997.4A Active CN109458994B (zh) | 2018-10-24 | 2018-10-24 | 一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 |
Country Status (1)
| Country | Link |
|---|---|
| CN (1) | CN109458994B (zh) |
Families Citing this family (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| CN110287873B (zh) * | 2019-06-25 | 2021-06-29 | 清华大学深圳研究生院 | 基于深度神经网络的非合作目标位姿测量方法、系统及终端设备 |
| CN111337946B (zh) * | 2020-04-23 | 2023-06-06 | 湖南云箭格纳微信息科技有限公司 | 一种旋转式全视野激光雷达扫描系统 |
| CN112100900B (zh) * | 2020-06-30 | 2024-03-26 | 北京控制工程研究所 | 一种空间非合作目标点云初始姿态测量方法 |
| WO2022031226A1 (en) * | 2020-08-01 | 2022-02-10 | Curium Pte. Ltd. | Method, system and computer readable medium for calibration of cooperative sensors |
| CN113822892B (zh) * | 2021-11-24 | 2022-02-08 | 腾讯科技（深圳）有限公司 | 仿真雷达的评测方法、装置、设备及计算机存储介质 |
Family Cites Families (5)
| Publication number | Priority date | Publication date | Assignee | Title |
|---|---|---|---|---|
| KR100930626B1 (ko) * | 2007-08-23 | 2009-12-09 | 전자부품연구원 | 스테레오 카메라를 구비한 로봇의 물체 자세 인식 방법 |
| CN104598675B (zh) * | 2015-01-07 | 2017-11-24 | 北京卫星环境工程研究所 | 一种基于实测数据的航天器部组件的装配仿真方法 |
| CN105931234A (zh) * | 2016-04-19 | 2016-09-07 | 东北林业大学 | 一种地面三维激光扫描点云与影像融合及配准的方法 |
| CN107067469B (zh) * | 2016-12-27 | 2022-03-22 | 中国人民解放军装甲兵工程学院 | 获取损伤零件缺损部位模型的系统 |
| CN108226938B (zh) * | 2017-12-08 | 2021-09-21 | 华南理工大学 | 一种agv小车的定位系统和方法 |
-
2018
- 2018-10-24 CN CN201811241997.4A patent/CN109458994B/zh active Active
Also Published As
| Publication number | Publication date |
|---|---|
| CN109458994A (zh) | 2019-03-12 |
Similar Documents
| Publication | Publication Date | Title |
|---|---|---|
| CN109458994B (zh) | 一种空间非合作目标激光点云icp位姿匹配正确性判别方法及系统 | |
| CN106548462B (zh) | 基于薄板样条插值的非线性sar图像几何校正方法 | |
| CN111145227B (zh) | 一种地下隧道空间多视点云的可迭代整体配准方法 | |
| CN110716194B (zh) | 同时参考球与面的多波束三维激光成像系统的检校方法 | |
| CN109508709B (zh) | 一种基于机器视觉的单指针仪表读数方法 | |
| US20220230348A1 (en) | Method and apparatus for determining a three-dimensional position and pose of a fiducial marker | |
| CN110942077B (zh) | 基于权重局部变化度和l1中值优化的特征线提取方法 | |
| CN114137562B (zh) | 一种基于改进全局最邻近的多目标跟踪方法 | |
| CN111815681A (zh) | 基于深度学习和判别式模型训练的目标跟踪方法及存储器 | |
| CN109856616B (zh) | 一种雷达定位相对系统误差修正方法 | |
| CN113706588B (zh) | 基于改进四点快速鲁棒匹配算法的环形锻件点云配准方法 | |
| Wang et al. | Quantitative evaluation of impacts of random errors on ALS accuracy using multiple linear regression method | |
| CN112652003A (zh) | 一种基于ransac测度优化的三维点云配准方法 | |
| CN109856619B (zh) | 一种雷达测向相对系统误差修正方法 | |
| CN109670557B (zh) | 一种基于杆状构筑物的公路点云自动配准方法 | |
| CN108106634B (zh) | 一种直接观星的星敏感器内参数标定方法 | |
| CN108595373A (zh) | 一种无控制dem配准方法 | |
| CN112884057B (zh) | 基于点云数据的三维曲面质量分类方法、系统及存储介质 | |
| CN115359124A (zh) | 用于复杂环境条件下的高精度双目视觉测量方法和装置 | |
| CN114842074A (zh) | 一种基于模型匹配的无人机影像定位方法 | |
| CN113933798A (zh) | 一种基于相似性原理的全局传感器系统误差分区配准算法 | |
| Wang et al. | A binocular vision method for precise hole recognition in satellite assembly systems | |
| CN117788537B (zh) | 一种基于Pointnet的飞机蒙皮点云配准方法 | |
| CN111595289A (zh) | 一种基于图像处理的三维角度测量系统及方法 | |
| CN113920180B (zh) | 一种基于正态分布变换假设校验的点云配准优化方法 |
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 |