一种面向空间非合作目标位姿测量应用的三维点云滤波算法

顾营迎，王 立，华宝成，刘 达，吴 云，徐云飞

(北京控制工程研究所，北京 100190)

引言

随着航天技术的进步和空间任务的多样化发展，对于空间非合作目标位置和姿态的测量需求越来越迫切。非合作目标上没有配合测量的靶标等人为设施，目标的运动状态、形状、材质各异，这给目标的位姿测量以及进一步的非合作目标抵近、绕飞、导航带来了新的挑战[1]。

在非合作目标测量中，应用激光雷达获取目标点云数据，经处理、计算后得到非合作目标的位姿是当前研究热点。对于空间目标点云数据处理来说，关键在于如何实现点云数据的降采样、空间噪声滤波以及提取目标的特征点云，以相对少的点云数量，最大程度表征非合作目标的几何形态和位姿状态。在点云降采样方面，常规的算法有角度偏差法、最小距离法、均匀网格法、包围盒子法、空间随机抽样法[2]等，但是这些方法仅从点云精简的角度研究问题。在点云去噪方面，郭博宁提出一种基于RANSAC分割的点云数据K-近邻去噪算法[3]；廖昌粟研究了根据密度滤波和点法矢量信息对离群噪声点进行去除的算法[4]；吴禄慎等人给出了基于特征信息分类的三维点云数据去噪方法[5]等。这些方法多从三维精确建模角度开展点云去噪研究。然而空间非合作目标的点云降采样和噪声滤除的目标是精确、高效获得目标的空间位姿，同时算法不宜过度复杂,以适应星上计算机有限的处理能力，保证位姿测量的实时性。

因此，为了解决以上问题，本文给出了一种用于空间非合作目标位姿测量的点云处理算法，并对算法进行了仿真分析和实验验证。通过本算法可以实现对空间非合作目标边沿特征点云的提取，同时达到点云降采样、滤除激光敏感器空间随机噪声的目的。

1 点云滤波与特征提取算法

1.1 点云特征分析

激光位姿敏感器扫描方式的不同，会导致获得的非合作目标点云分布特征不尽相同[6-7]，如图1所示。对于非合作目标的位姿测量，一般需要选取合适的点云数量和具有显著姿态表征的点云参与计算，也就是需要对激光敏感器获得的原始点云进行噪声滤波、降采样和特征点云提取工作[8-9]。

图1 激光位姿敏感器的不同扫描方式形成的点云Fig.1 Point cloud formed by different scanning methods of laser P&A sensor

利用点云解算目标位姿时，同二维图像的特征点类似，一般希望三维点云的特征点具有如下特性[10-12]：

1) 对噪声的影响不敏感，对观测位姿的变化不敏感；

2) 点云的分布具有较大的目标位姿表征度。

如图2所示，处于平面或体目标边沿、或形状顶点的点云具有较好的目标位姿表征度，同时处于边沿上的点云能够最大限度支撑起形状或立体目标的形体。通过计算分析各个点云与邻域点云间的几何关系，提取处于目标边沿的点云作为目标的特征点云，用特征点云进行目标的位姿解算。

图2 目标点云与边沿特征点云Fig.2 Target point cloud and edge feature point cloud

1.2 算法原理与实现

本文算法实现的目标，一是有效降低实际配准应用的点云数量，提升位姿解算处理速度；二是提取如图2所示的具有边沿特征的点云，实现参与配准点最大程度的表征空间目标的位姿；同时使提取的点云具有可区分的局部特征，易于后期进行点云对的配准[13]。

图3 边沿特征点空间关系示意图Fig.3 Schematic diagram of spatial relationship between edge feature points

基本算法流程如下：

如图所示，假设通过激光位姿敏感器获得了点云集P={pi|i=1,2,…,n}。

1)对点云进行K-D Tree划分，以便于后续对点云的近邻查找。

2)对点云集P中的每个点云pi查找半径为r的近邻点pj。

7)点pi的以半径为r的近邻点大于等于N个(通常可取N=3)，且求出的θj均小于θth(通常可取θth≤90°)，则将点pi作为提取的特征点保留，其他不满足条件的pi点则予以滤除，不参与后续的点云位姿匹配。实际应用中，邻域半径r、邻近点数N、以及θth参数需要根据测量工况进行合理设定。

2 算法特性仿真验证

该算法的主要特点是，可以同时对点云目标实现空间噪声滤波、点云数量降采样以及目标边沿特征的提取，从而在保证解算精度的情况下，最大限度的提升在轨非合作目标位姿解算的实时性和数据处理的效率。

为了验证算法在点云滤噪、降采样、特征提取稳定性方面的性能，以12 m×2 m×2 m的某卫星为目标，仿真生成在距离目标100 m处，通过扫描激光雷达获得的点云，并应用本文的算法对点云进行处理。

2.1 算法点云滤噪、降采样的有效性

仿真中，在目标重心10 m×10 m×10 m的空间中，随机生成100个空间随机噪声点云，模拟激光测量敏感器在测量中受到杂光影响所产生的空间随机分布噪声。用李萨如模式扫描获得的原始点云数量共计1 941个，其中100个点是随机分布的空间噪声。

如图4所示，应用本文的算法，对原始点云处理，获得了特征点云，滤除了原始点云中空间随机分布噪声，在保留目标空间形态特征的同时，处理后的点云数量降为222个。算法实现了1∶9的降采样，同时在保留了目标空间形态特征的前提下，实现了空间随机分布噪声的滤除。

而常规的点云降采样方法，如均匀网格法、空间随机抽样法等，在降采样目标点云的同时，难以实现对空间随机分布噪声的滤除。

图4 带有空间随机分布噪声的点云与 算法提取的特征点(粗点)Fig.4 Point cloud with spatially random distributed noise and feature points extracted by algorithm (rough points)

2.2 算法提取的特征对位姿变化的鲁棒性

为了有利于点云帧间特征点的对应匹配，方便后期利用特征点云帧间变化的信息，要求提取的特征点云应具有一定的位姿变化稳定性。如图5所示，利用本算法分别对原始点云和绕Y轴旋转2°的点云进行处理，获取边沿特征点云。从图中可看出，相应的特征点云具有很好的一致性，点云位姿的变化对算法特征点云的提取无明显影响。

进一步对算法进行验证，保持激光位姿敏感器不动，控制卫星目标绕Y轴分别旋转2°、4°、6°、8°和10°，利用激光位姿敏感器获得卫星目标的原始点云，并用本算法提取卫星目标的边沿特征，试验结果如图6所示。从试验结果中，可以看到随着卫星旋转姿态的变化，由于卫星投影面积的缩小，导致激光位姿敏感器获得的卫星目标原始点云数目略有下降，但是通过算法提取到的目标边沿特征点的数目基本保持不变，进一步说明本算法的边沿特征提取不受点云位姿变化的影响，算法边沿特征的提取对目标位姿变化具有鲁棒性。由于篇幅所限，本文只给出了卫星目标绕Y轴旋转的结果，绕其他轴旋转后，改变卫星目标的姿态，可以得出同样的结论。

图5 绕Y旋转2°前、后，卫星原始点云(细点)及 算法提取的特征点云(粗点)Fig.5 Before and after rotating 2° around Y-axis, satellite raw point cloud (fine points) and algorithm-extracted feature point cloud (rough points)

图6 卫星不同旋转角度下，原始点云与算法处理后对应 的边沿特征点数量Fig.6 Numbers of edge feature points and original points at different satellite rotation angles

2.3 算法提取的特征对点云高斯噪声的鲁棒性

在点云获取的过程中，激光测量敏感器除了有外界杂光引入的空间分布的噪声外，另外一类和测量相关的噪声，可以用高斯分布模型来仿真[14]。通常现有激光敏感器的测距精度可以达到0.09 m(3σ)。为了验证算法提取的特征对点云高斯噪声的鲁棒性，本仿真实验中，在卫星的原始点云上，在x、y、z三个方向上分别叠加均值μ=0，标准差σ=0.01 m、σ=0.02 m、σ=0.03 m、σ=0.04 m、σ=0.05 m的高斯噪声，模拟激光测量敏感器的测量噪声。

对卫星原始点云和叠加了高斯测量噪声的点云，分别应用本文提出的算法提取特征点。如图7所示，叠加了不同水平高斯噪声的点云，提取的边沿特征点与原始点云提取的边沿特征点类似，均很好地将卫星目标的边沿特征点提取出来，说明算法提取边沿特征的特性基本不受点云高斯噪声的影响。但是从图8结果来看，不同高斯噪声水平，虽然基本不影响边沿特征的提取，但是高斯噪声水平影响边沿特征提取的数目。随着高斯噪声水平逐渐增大，点云的边沿有各向分散的趋势，且越来越接近算法中的邻域半径参数r的阈值，导致算法将部分分散了的高斯噪声点也误判为边沿特征。因此在工程应用中，要根据激光测量敏感器的测量噪声水平，来合理设置特征提取算法的邻域半径参数r，通常使6σ

图7 不同高斯噪声水平下，卫星点云(细点)及 算法提取的边沿特征点云(粗点)Fig.7 Satellite point cloud(fine points) and algorithm- extracted feature point cloud(rough points) at different Gaussian noise levels

图8 不同高斯噪声水平下，算法提取的 边沿特征点的数目Fig.8 Number of edge feature points extracted by algorithm at different Gaussian noise levels

3 算法物理实验验证

工程应用中，点云的误差源是多方面的，既有内部电路噪声、定时量化误差，也有来自外界温度、杂光、目标反射特性等因素的影响[15]。为了用实际的、夹杂噪声的非合作目标点云数据作为算法的输入，验证算法在特征提取、降采样和空间噪声滤除方面的性能。以空间非合作目标绕飞、抵近、捕获全物理试验平台(图9)为依托，应用激光位姿敏感器获取了由10 m距离向非合作目标卫星接近过程中的实际激光点云原始数据。应用该数据作为原始输入，测试本文算法在实际应用中的性能。

图9 空间非合作目标绕飞、抵近、捕获全物理试验平台Fig.9 Full physical test platform for space non-cooperative targets fly around, approach and capture

如图10所示，激光位姿敏感器获得的原始点云有4 012个点，应用本文算法，设定计算搜索的邻域半径50 mm，对原始点云进行特征计算、提取，最终获得了280个特征点。从图10中可见，算法获取的特征点具有降采样、噪声滤除和保留目标星主要轮廓特征的作用。

图10 全物理试验获取的目标星实测点云(细点)与 算法提取的特征点云(粗点)Fig.10 Target star point cloud measured by full physical test platform(fine points) and feature point cloud extracted by algorithm(rough points)

如表1所示，应用点云数量、位姿求解(ICP)计算量、ICP距离均值和ICP距离标准差4个参数，定量比较本文特征点提取算法的性能。其中位姿求解计算量，为不考虑空间检索加速的情况下，取ICP计算迭代次数为15，获得的ICP点比较次数的估算值。ICP距离的均值为参与计算的点云与模型点云之间经过ICP匹配后，各个计算点云与模型最近点云的空间距离的平均值，该变量可以间接估计ICP计算后位姿匹配的精确程度。

表1 特征点提取算法性能Table 1 Performance of feature point extraction algorithm

ICP距离的标准差为参与计算的点云与模型点云之间经过ICP匹配后，各个计算点云与模型最近点云的空间距离的标准差，该变量可从另一角度间接估计ICP计算后位姿匹配的精确程度。如表1所示，通过实验验证的结果表明，本文的点云特征提取算法在实验中实现了原始点云93.1%的降采样，节省了92.9%的位姿解算时间，而ICP距离均值和距离标准差相对于原始点云却只增加了7.9%和4.6%，在保证位姿测量精度的条件下，实现了对原始点云进行滤噪、降采样，有效提升了星上数据处理的效率和解算的实时性。

4 结论

本文给出了一种用于空间非合作目标位姿测量的点云处理算法，应用仿真的方法验证了算法滤除空间随机噪声和点云降采样的有效性，验证了提取的特征点对目标位姿变化和高斯测量噪声的鲁棒性。

依托空间非合作目标绕飞、抵近、捕获全物理试验平台，开展实验，以获取的原始非合作目标点云作为输入，验证算法在实际空间目标位姿测量中的有效性。实验结果表明，在实验中本文的点云特征提取算法实现了原始点云93.1%的降采样，节省了92.9%的位姿解算时间，而ICP距离均值和距离标准差只增加了7.9%和4.6%，有效实现了滤噪、降采样和非合作目标位姿表征的预期目的。