Improved registration algorithm for spatial non-cooperative target point cloud clustering

随着人类对太空探索的深入，越来越多的卫星投入使用，但也有越来越多的卫星发生故障或者报废。空间在轨服务技术成为空间自主操控领域的研究热点^[1-2]。与空间站或三轴稳定卫星等运动状态已知或可控的合作目标不同，空间非合作目标在空间摄动力的作用下，随着能量耗散可能绕本体最大惯量轴慢旋，处于失稳状态^[3-4]。其动力学表现为转动惯量和旋转轴不确定的失控刚体，在无法提供用于辅助测量的人工标记或特征光标的条件下，解算此类目标的相对位姿具有较大难度。激光雷达技术^[5]因其抗干扰性能较好，能够直接获取目标高精度三维点云信息等特点成为测量非合作目标相对位姿的有效手段。对激光雷达技术获取的非合作目标三维数据进行点云处理可获取目标的相对位姿，解算目标的运动特性^[6]。

在激光遥感领域，良好的聚类分割结果可以有效降噪，降低点云规模，为点云配准以及目标识别提供可靠数据。张溪溪^[7]等人提出了基于特征线拟合的微型复杂曲面点云分割方法，通过以边界为约束的区域生长算法和三角形叉积的算法对模型点云分割，但对于可视点云效果不理想。郑少开^[8]等使用三维网络获取模型先验知识，通过先验知识和混合聚类算法对点云模型进行分割聚类，但是无法应用于先验信息缺失的空间非合作目标。王帅^[9]等使用谱聚类按照几何特征结构对各视角点云区域进行分割，但是聚类结果受相似矩阵影响较大，适用于均衡分类问题，对于空间非合作目标的效果有待研究。张良培^[10]等采用简单的几何图元的数学模型为先验信息，通过验证点云对模型参数的符合程度将点云划分到相应的图元中，但是几何图元信息对空间非合作目标描述欠佳。黄镇^[11]等将场景点云超体素聚类分割为邻接图，依据邻接图的边缘进行分类，但超体素聚类依赖目标颜色信息，在太空光照条件复杂的情况下，获得颜色信息具有较大的干扰性。张楠^[12]等使用RANSAC算筛选聚类点法向量的方向，提高建立法向量的准确性，该方法应用于具有较多平面特征的模型时效果较好，对于空间非合作目标此类具有复杂特征的模型时，效果有待验证。

综上所述，文中提出了一种改进区域生长的空间非合作目标点云聚类配准算法。对三维点云降维后，通过深度信息与RGB颜色值之间的数学映射关系减少因降维导致信息缺失的影响，引入RGB图像来辅助区域生长聚类，得到更加细致的显著特征类，同时，使用布尔擦除法剔除一般特征点云，细化突出聚类边界，并将处理后的点云输入配准算法中解算空间非合作目标的相对位姿。实验结果证明，文中算法能够较好完成显著特征聚类分割，降低点云规模，使得ICP算法运算速率提升约3.5倍，配准综合误差由3.6749降低到3.4945。

在可视点云中，由于视点与非合作目标相对位置的变化，使得具有显著特征的部位（如姿控喷嘴曲面特征、主体支架弯曲曲面特征以及前置小型信号接收天线弧形侧边特征等）在某个可视化时刻下，与一般特征（如太阳能帆板平面特征以及主体侧面）的相似度增高，进而导致在聚类过程中被隐藏，无法被分割提取。但显著特征与一般特征在空间中的相对位置是不变量，因此，建立深度值与颜色值之间的映射关系，引入三维点云降维生成的RGB图像，根据颜色梯度突变，将高识别度部位的点云从整体可视点云中分离出来，擦除一般特征点云，达到细化聚类种类，提高聚类效果的目的。

在进行空间非合作目标相对位姿测量的过程应具有较好的实时性，但是曲面重建技术不仅需要一定的时间，而且重建结果对最终法线的估计有着较大影响。因此，从点云数据入手，近似推断表面法线，进行点云数据的法线估计。通过分析协方差矩阵的最大、最小特征值和特征向量，完成对点云数据的法线估计。首先以初始种子点为查询点，通过搜索方式确定邻近点，即对于每一个点P_i所对应的协方差矩阵C如公式(1)：

式中：K为点P_i邻近点数量；$\bar P$为满足搜索条件范围内最邻近点的三维质心坐标；λ_j为点P_i对应协方差矩阵的第j个特征值；V_j为第j个特征值对应的特征向量。其中λ_min对应的特征向量V_min的几何意义为拟合切平面的法向量。为提高法线估计的精度，需进行法线方向归一化处理^[13]，通过公式(2)判断点P_i的法向量${ {\boldsymbol{n}}_i}$与视点V_P指向点P_i的方向是否一致，同向保留，反向取反，完成对法向量进行有序化。

之后通过公式(3)输出拟合切平面的表面曲率来近似表征点的曲率σ：

加载点的法线和曲率之后，计算初始种子点和其邻域内的点的法线夹角，小于平滑度阈值，即两点法线偏差的允许范围，便将该邻近点加入当前区域。之后再进行曲率值的检查，若小于曲率阈值，即两点曲率偏差的允许范围，那满足阈值条件的点加入当前区域。通过曲率的计算，进行二次判别，细化聚类种类，提高聚类精度。

可视点云是线阵激光雷达通过线扫描方式获得的，点云数据集中仅存储每个可视点的位置信息。在进行可视点云的降维和显示过程中，深度信息是必要且不可丢失的，因此将深度信息转换成颜色信息进行存储。建立深度信息与颜色信息之间的映射关系，便于深度信息和颜色信息之间的转换。

首先，应通过线阵激光雷达获取空间非合作目标深度信息。雷达初始分辨率为m×n，像素宽度为t，雷达视场长L、宽H。点云降维投影后的外接矩形长M、宽N。激光雷达测量目标时，非合作目标表面可测点在雷达坐标系O-xyz下的位置关系如图1所示。

Figure 1.  Positional relationship of measurable points in radar coordinate system

点P_xOz和P_yOz为点P在xOz、yOz平面中的投影点，R_f为激光雷达在当前工况下的测量距离，点云深度值为z，τ_x为线段OP_xOz与z轴夹角∠P_xOzOP_z，同理，τ_y为线段OP_yOz与z轴夹角∠P_yOzOP_z，则深度值按照公式(4)解算：

获得空间非合作目标的深度信息后，按照深度值的大小将可视点云中的离散数据点进行排序，将原有无序点云按照空间延伸顺序转换成有序点云，方便寻找深度信息与颜色值之间的映射关系。

在颜色值与深度值之间映射关系建立的过程中，选择使用颜色空间与深度信息进行对应。设置距离视点最近的点P_nearest的RGB颜色坐标为（255, 0, 0），最远的点P_farthest的RGB颜色坐标为（0, 0, 255）。则位于点P_nearest和点P_farthest之间的点P_i的RGB颜色赋值按公式(5)进行赋值：

式中：μ (P_nearest, P_i)和μ (P_i, P_farthest)为点P_i的着色比例因子，与点P_nearest到点P_i之间的距离d (P_nearest, P_i)和点P_i到点P_farthest之间的距离d (P_i, P_farthest)有关，按公式(6)计算为：

原始可视点云数据格式仅保存了点P_i的空间位置信息。在对点P_i完成RGB赋值后，需要对原有点云数据进行维数上的扩展，进行点云的可视化即可得到含有位置信息和RGB颜色信息的点云。

在复杂空间环境中，空间非合作目标的运动主要由自旋和章动复合而成。因此，非合作目标特征部位的出现或消失会随其自身的运动特性在时间顺序上的推移而变化。从视点处对可视点云进行投影后，某些部位特征信息被削减，导致区域生长聚类算法无法将该部位聚类出来。在RGB图像中，RGB颜色梯度突变的边界，可能是显著特征部位与一般部位的分界。因此，在获得RGB图像后，可以根据颜色梯度突变获取聚类边界，据此可以确定在可视点云中，该部位的位置，即该部位点云所对应的位置坐标x、y值。最后，利用深度信息和RGB颜色值的映射关系反演原始可视点云深度信息。但是在进行反求的过程中，相同的x、y值存在对应多个z值的情况，此时需要引入误差δ及判定条件，按公式(7)计算：

式中：z’_Pi为反求值；z_n为相同x、y值对应的n个z值。此时，δ_min对应的z’_Pi将被对应的z值刷新，确保反求后，可以对应到原始点云中的原始数据。

聚类所得结果为相似特征的点的集合，但是在配准过程中，较为单一的特征无法得到很好的应用，所以需要将具有较好特征的聚类结果进行合并，完成特征的叠加，组成特征明显、数据量小的类。同时使用布尔擦除法剔除一般特征，突出特征边界，弱化一般特征对特征匹配的干扰。

在完成聚类后，需对点云进行配准，以求解空间非合作目标的位置和姿态。将RGB图像辅助区域生长聚类嫁接在迭代最近点算法前端，为配准提供显著特征点云，解算空间非合作目标的相对位姿。

首先，将聚类完成的点集作为点集P，激光雷达获得的下一帧的点集作为待配准点集Q。计算点云P、Q中的点的特征向量，并通过KD-tree的搜索方式确定两组点云中的对应点，对应点对数量为N。此时在点云P中点为P_i在点云Q中的对应点为Q_i。最后，通过最小二乘法迭代计算最优的坐标变换，及旋转矩阵R和平移矩阵T，迭代收敛条件为公式(8)：

ICP算法简便直接，可视配准具有较好的精度，但是算法的运行速度以及向全局最优的收敛性却在很大程度上依赖于初值和在迭代过程中对应关系的确立。文中聚类分割算法对配准过程的正反馈作用体现在：仅保留目标搭载的机动性载荷以及测量性载荷等显著特征，将对应点的搜索范围限制在目标主体，降低一般特征，如太阳能帆板等平面特征对配准的干扰。若太阳能帆板角点与主体侧楞点建立了错误的对应关系，将极大地影响配准精度，甚至导致配准失败。因此，较好的聚类结果能够缩小搜索范围，提高特征提取与计算的速度，从而提高ICP算法的实用性。

使用模拟现实工况下的线阵激光雷达扫描获取的空间非合作目标多帧可视点云，应用Visual Studio 2013与点云库PCL （Point Cloud Library）验证文中算法相对于传统ICP算法在效率及精度方面的提高效果，考虑了任务要求和技术指标（任务技术指标：配准角度误差为±5°）。空间非合作目标模型与实物比例1∶1，运动状态为自旋和章动的复合运动，运动仿真参数设定：章动角速度为5.1037 (°)/s，雷达扫描帧率为1 Hz，目标运动周期时长约为72 s，即73帧数据。在线阵激光雷达完整捕捉非合作目标运动周期的条件下，即非合作目标形貌可完全获得，卫星支架仅第21~50帧数据存在，其余时刻被主体及太阳能帆板遮挡。

使用可视化数据结构和组件，以加载原始空间非合作目标仿真第20帧及第50帧可视点云数据为例，如图2所示。

Figure 2.  Spatial non-cooperative target simulation visual point cloud

对空间非合作目标可视点云进行传统区域生长聚类。经过大量实验与计算，在保证阈值设定最大可能满足所有可视点云数据聚类的前提下，选取最优泛用平滑度阈值为30，曲率阈值为0.03，点云最小规模阈值$reg.setMinClusterSize = 50$，点云最大规模阈值$reg.setMaxClusterSize = 10\;000$。在该参数下，可视点云特征保留较为全面，如主体上方的姿控喷嘴、前置小型信号接收天线的弧形侧边和部分支架，区域生长聚类结果显示如图3所示。但是聚类达到极限，支架无法完全被聚类分割出来，如图3右图所示。

Figure 3.  Regional growth clustering results

其中颜色相同的是一类。从图中可以发现，所有支架并未被聚类分割出来，不满足参数的点云会出现丢失的情况，同时，主体的平面和太阳能帆板的平面不属于显著特征部分，会对配准造成一定的干扰，影响配准精度。经分析，造成聚类不够细致的原因是可视点云自身遮挡特性造成的。空间非合作目标自身的遮挡会导致部分特征发生改变甚至缺失，例如曲面的曲率发生改变、特征角点缺失等。

为解决上述问题，将已有深度值转换成较为直观的颜色信息，进行平滑着色，并在原有点云位置信息的基础上融合颜色信息。特征部位的空间相对位置及深度值是不变的，所以转换后的颜色信息也具有一定的不变性。平滑着色后的可视点云效果如图4所示。

平滑着色后，支架与太阳能帆板及背景具有明显色差，便可以将支架从太阳能帆板的背景色中提取出来，并将目标边界内的部分弱化，突出边界。使用颜色梯度的下降突变对RGB图像进行边界提取，提取出的边界存储在边界信息存储矩阵中，并输出边界坐标，提取效果如图5所示。

Figure 4.  Color visual point cloud

Figure 5.  Boundary extraction result

空间非合作目标与激光雷达的空间位置相对固定，因此在运动过程中，主体及支架边界颜色在一定范围内发生周期性变化。设置颜色阈值为R=15，G=255，B=255和R=15，G=255，B=15内的边界，剔除不需要的边界，保留目标轮廓，并输出轮廓值范围内的点的位置信息。在轮廓范围内的点，按照赋值规则进行反演，结合误差判定条件完成判定后使用原始点云数据中的深度值刷新反求的计算值，以颜色与深度信息为双重判定，输出对应原始点云数据中的x、y值。聚类结果如图6所示。与图3区域生长的聚类结果相比，帆板与支架得到了较好的分割，支架完整度提高。

Figure 6.  Clustering results of the proposed algorithm

使用布尔擦除法将空间非合作目标主体平面和太阳能帆板点云剔除的同时，将支架和主体上方的喷管以及前置小型信号接收天线点云合并，形成用于配准的具有较高识别度的小规模点云，如图7所示。

Figure 7.  Small-scale distinctive feature point cloud

与原始可视点云相比，可见点云规模明显缩小，较好地保留了特征显著的部位的点云，同时空间非合作目标主体侧楞部位的点云由于其法线特征存在二义性，故得以保存，并为后续配准建立对应关系提供更多点对，对位姿解算具有矫正作用。

实验程序的运行条件及环境如下：CPU为Intel(R)Core(TM)i5-5200U @ 2.20GHz，在内存为4 GB的Windows 10操作系统PC机上运行。实验使用30组可视点云数据，每组数据包括当前帧全部可视点云、当前帧聚类点云和下一帧全部可视点云。

空间非合作目标仿真聚类配准实验使用文中算法与传统ICP配准算法进行配准对比实验：

(1) 使用下一帧全部可视点云和文中算法处理后的聚类点云进行逆序配准；

(2) 使用下一帧全部可视点云和当前帧的全部可视点云进行逆序配准。

对三轴平移距离及旋转角度、算法过程耗时进行统计，并依据技术指标重点对配准旋转角度结果进行误差分析。其中点特征计算环节属于前端数据处理且受硬件影响极大，在聚类过程中可直接加载调用，实际任务时可进行星上数据下载至地面大型服务器，进行离线处理，聚类、细化以及点云数据配准环节耗时稳定，因此，算法耗时包括聚类细化及配准时间。文中算法配准结果如表1所示，其中dx、dy以及dz为三轴平移距离，ax、ay以及az为三轴旋转角度。其中，三轴平移距离真值为0 mm，围绕x、z轴旋转角度真值为0°，绕y轴旋转角度真值为5.1037°。

ICP算法配准结果如表2所示。

文中算法及ICP算法配准耗时结果如表3所示。

依据技术指标要求，三轴旋转误差容忍度为±5°，三轴平移误差容忍度为±5 mm，对结果及误差进行统计与分析，统计结果如图8~11所示。

Figure 8.  Pitch angle registration error

Figure 9.  Yaw angle registration error

Figure 10.  Roll angle registration error

Figure 11.  Algorithm consumes time

由于配准过程存在方向性，且配准结果直接作用于目标三维重建，因此使用累积误差表征结果准确性，结果如表4所示：在三轴旋转角度误差为±5°的容忍范围以及三轴平移±5 mm的容忍范围内，文中算法在x、z轴方向上的累积距离误差以及围绕x、z轴旋转累积角度误差上略优于ICP算法，依据旋转和平移对整体的影响，较好旋转角度会使得平移误差减小，因此对六个参数误差赋予权重，其中三轴平移权重均为1/9，三轴旋转权重均为2/9，综合表征配准效果。文中算法综合误差为3.4945，ICP算法为3.6749。同时，文中聚类配准算法平均耗时为16.4733 s，ICP算法平均耗时为57.4536 s，因此文中聚类配准算法较ICP算法效率稳定且提高约3.5倍，整体配准效果略有提升。

论文以解决空间非合作目标位姿测量问题为目的，在传统基于区域生长的点云聚类分割算法的基础上，建立RGB颜色值和点云深度信息之间的映射关系，引入RGB图像，通过轮廓提取及反演，合并显著特征点云，擦除一般特征点云，优化点云聚类，使得因点云可视化造成部分显著特征点云无法被聚类出来的问题得以解决，最终得到规模小、特征明显的聚类点云数据集。使用文中算法与ICP算法进行配准对比实验，验证在配准角度误差为±5°条件下，使用聚类点云进行配准，可有效提高配准效率，约为ICP算法的3.5倍，配准综合误差从3.6749降低为3.4945，配准精度略有提升，为解决空间非合作目标相对位姿测量难题和配准算法的实际应用提供了技术支持。