基于测地线距离的空间非合作目标点云配准

2023-01-31陈斌郗厚印张晓东罗敏郭治栋

航空学报 2023年1期

陈斌，郗厚印，张晓东，罗敏，郭治栋

1．北京邮电大学人工智能学院，北京 100876

2．北京空间飞行器总体设计部，北京 100094

近年来，空间资源已成为各国竞相争夺的关键，世界空间技术的研究态势已从探索、利用空间逐步拓展到控制空间。在轨目标抓捕技术水平直接反映了各国控制空间能力的高低。随着各国在轨服务计划的迅速展开，不断面临新的、更具挑战性的复杂抓捕任务需求，如抓捕失效卫星、故障航天器和太空垃圾等。由于微重力、太阳辐射和残余角动量的影响，此类目标往往表现为非合作特性，且伴随着复杂的翻滚、自旋或章动。如何利用视觉信息辨识空间翻滚目标的动态特性是实现在轨可靠抓捕的难点或关键所在。

针对空间非合作目标动态特性辨识问题，主要有强几何特征法、模板匹配法和点云配准法［1］。强几何特征和模板匹配方法通常需要已知空间非合作目标的部分特性［2］，如卫星天线［3］、对接环［4］等。相比之下，点云配准法无需空间目标的先验信息即可辨识其运动状态［5］，具有较好的自主性、适应性等优势，引起学者们广泛关注。

最典型的点云配准算法是由Besl和Mckay［6］提出的最近点迭代算法（Iterative Closest Point，ICP），通过最小化模型点云与场景点云间的欧氏距离进行点对匹配，但当场景点云与模型点云间非对齐时易导致点对匹配错误或配准失效。为此，Medioni和Chen［7］提出一种点到平面的距离度量方法，将场景点云的点按其法向量方向投影到模型点云。随后，Zhang和Xie［8］提出一种曲面到曲面的距离度量方法，达到了更精细的配准效果；Suominen和Gotchev［9］提出采用旋转点云的轨迹确定点对，从而避免频繁构造曲面。针对传感器引入的噪声问题，Sharp等［10］提出广义ICP配准方法，通过自动设置特征和位置的最佳相对贡献选择点对；Ahmed等［11］提出虚拟兴趣点的配准方法对噪声点云进行处理，所提算法在点云重叠时的配准精度明显优于传统的ICP算法。

若场景点云和模型点云间初始距离较远，标准ICP及其改进算法容易陷入局部最优解［12-13］，为此，一些学者提出基于特征的三维点云预配准方法［14-19］。Darom和Keller［14］引入自旋图像和尺度不变特征变换2个尺度不变的描述子来匹配两帧点云；Zhao等［16］提出基于直线和深度图匹配策略的点云粗配准方法，可以选择更具鲁棒性的特征估计相邻点云间的变化，但直线特征不能准确地反映目标的局部特性。随后，Hattab和Taubin［17］假设点云中至少存在3对曲面，通过检测和匹配基本形状（如平面、圆柱体和圆锥体）寻找最佳的对齐方式。此外，Kleppe等［18］综合利用点和曲面特征，提出基于局部曲率的点云粗对准方法，降低了点云匹配误差；Attia等［19］融合线与面特征提出基于凸包粗配准的改进ICP算法（Convex Hull Aided Coarse Registrations re⁃fined by ICP，CHACR_ICP），将三维模型投影到二维平面，继而提取投影图像的外轮廓预配准三维点云。

然而，实际应用中空间目标可能存在局部结构相似情形。例如，为了维持通信卫星轨道运行的平稳性和供电要求，需要在两侧加装相同结构的太阳能帆板，导致场景点云和模型点云间存在多组相似的局部帆板结构点对。受限于空间极端光照条件、所用相机视场以及目标翻滚自旋运动等因素的影响，往往仅能获取空间目标的部分点云，直接采用现有的点云配准方法难以准确辨识空间目标的动态特性。为此，提出一种基于疏密度指标与全局测地线距离的空间非合作目标点云配准方法，实现局部结构相似与翻滚遮挡下目标准确配准。

1 双目相机成像模型

双目相机成像模型描述了非合作目标像素坐标系到世界坐标系的映射过程，如图1所示。Ow-XwYwZw为世界坐标系；Ocl-XclYclZcl为左目相机坐标系，Ocr-XcrYcrZcr为右目相机坐标系；Oxy-xlyl为成像平面坐标系，描述了像素点的物理位置；Ouv-uv为像素坐标系，表征了像素点的像素位置。

图1 双目相机成像原理Fig. 1 Imaging principle of binocular camera

下面以左目相机为例说明世界坐标系下点Pw(Xw，Yw，Zw)映射至像素坐标系的过程。首先，将点Pw(Xw，Yw，Zw)变换至相机坐标系：

式中：Rl和Tl分别为转换过程中旋转矩阵和平移矩阵。经过相机投影变换后，可得在成像左平面坐标系下的坐标Pl(xl，yl)：

其中：f为相机焦距，同理可得在成像右平面坐标系下的坐标Pr(xr，yr)。

因成像平面坐标系与像素坐标系仅存在坐标原点位置及图像大小差异，可通过平移及伸缩变换得到Pl(xl，yl)对应的像素坐标Pl(ul，vl)：

式中：dx、dy分别表示图像行、列的像素大小，单位为mm；u0、v0分别为图像中心像素坐标与原点像素坐标之差的横向、纵向像素数。世界坐标系下点映射至像素坐标系的完整变换过程可以表示为

式中：Zcl表示目标到相机成像平面的距离。式（4）可以等价表示成矩阵向量形式：

其中：s为缩放因子，表征目标距离相机的距离；A、T分别表示内参矩阵与外参矩阵。

受缩放因子的影响，像素坐标系下目标点在三维空间存在不同深度的点与之对应，造成单目成像无法得到真实空间下目标三维点云，为此，采用双目相机采集像素点的深度值Zi，计算可得目标点云的三维坐标：

2 基于测地线距离的点云配准算法

假设空间目标的场景点云到模型点云为刚性变换或二者表征的目标全局形状不变，本文算法主要包括目标点云球面流形映射、球面点云子集划分、测地线距离矩阵计算及配准矩阵估计。

2.1 不规则目标点云的球面流形映射

测地线距离最早提出用于测量目标尺寸与形状。以图2所示的带太阳能帆板的立方星为例，卫星上两点P1与P3间的虚线、实线分别表示欧氏距离及测地线距离。相比于欧氏距离，测地线距离能够更为精细地刻画目标的全局形状特性。考虑球面域具有连续、全局和旋转不变性的特点［20-21］，且投影结果不受点云位姿变换的影响［22-23］，选择将不规则的空间翻滚目标点云映射到规则的球面流形上。

图2 球面投影Fig. 2 Spherical projection

图2描述了场景点云中任意点的球面投影过程。以原始点云的平均坐标值作为场景点云的重心O(xo，yo，zo)，依据重心至相机原点的偏移量将场景点云平移至原点处，则平移后点云中点Pi(xi，yi，zi)在相机坐标下的极坐标为

式中：i=1，2，...，M，M表示球面点云的数量；θi、φi分别表示点的方位角与天顶角。

投影球体的半径大小直接决定了投影点间距，间距过大或过小均不利于球面点云全局形状的测地线距离描述，可取原始场景点云的重心至最远处点Pc(xc，yc，zc)的距离作为球体半径：

依据球体半径、方位角及天顶角，可确定原始点云上任意点Pi(xi，yi，zi)的球面映射点Si(x′i，y′i，z′i)的空间坐标：

2.2 球面局部点云子集评价与划分

投影后的球面点云可能存在疏密分布不均情形，如图3所示。从全局的角度看，稠密区域A、B、C与稀疏区域D的离散程度存在明显差异，且不同稠密区域间的点云疏密分布不均，同时，局部稠密区域内各点的疏密程度不同。为了准确计算投影后点云的疏密度，需综合考虑点云整体分布特性以及局部区域内相邻点间的空间位置关系，为此，将球面点云划分为多层次邻域空间并进行平面投影，设计一种基于全局与局部点云离散程度相结合的疏密度评价指标，将球面点云按照疏密程度划分为不同的局部点云子集，提升测地线距离矩阵感知目标局部形状的能力。

图3 球面点云分布Fig. 3 Distribution of spherical point cloud

取球面任意一点与其余点间弧长相反数的指数函数的平均值刻画其在球面点云上全局离散程度：

式中：Li，j表示球面点云上任意两点Si(x′i，y′i，z′i)和Sj(x′j，y′j，z′j)间的弧长：

为计算球面点云局部区域的离散程度，选取球面上任意一点Si，如图3所示，其与相邻点Sij间的弧长为li，j，将距离Si小于弧长τt的球面点所构成的区域作为第t邻域，建立统一量化的多层次邻域空间。弧长τt的计算公式如下：

以任意球面点Si为坐标原点，将邻域内点投影至与Si、球心构成向量垂直的各平面上，其平面映射方程为

式中：γ表示弧长τK对应的圆心角。

若Si投影后位于平面点云内部，则四象限间点的数量差异较小；反之，Si位于平面点云边缘时的数量差异较大。为此，利用象限内平面投影点数量的标准差确定Si在球面局部点云中的分布位置，结合Si投影后的各邻域内任意两点间的平均距离共同刻画球面点云的局部离散程度：

式中：α表示邻域内球面局部点云分布的权重系数，可用邻域内局部点数m占全部球面点数M的比值衡量，其取值范围为（0，1）；K代表邻域总数；表示第t邻域内点的总数表示邻域内任意两点间的距离；Cv为第v象限内平面投影点的数量；Cˉt表示第t邻域下四象限内平面投影点数的均值。

综合全局与局部点云离散程度可得任意点在球面点云上的疏密度评价指标：

归一化球面点的疏密度指标，取疏密度值大于阈值η的点构造局部点云集合H。采用基于密度的文本聚类算法将点云集合划分为h类局部稠密点云子集H={H1，H2，…，Hi，…，Hh}，其余稀疏点云记作集合H͂。

2.3 基于点云子集分布的测地线距离矩阵计算

针对非合作目标点云损失导致场景点云和模型点云的全局测地线距离矩阵间存在差异问题，依据球面点所属点云子集类型赋予不同权重，从而减小信息缺失导致测地线距离矩阵中两点间距离偏差，提高点云配准算法的精度。

采用最近邻算法寻找球面点云上目标点附近F个最近点并连接得到邻域图，重复计算可得所有点邻域图构成的无向图G。基于此，利用弗洛伊德最短路径算法计算G中任意两顶点Gi、Gj间最短路径，则Gi与Gj间的测地线距离可以表示为

式中：d′(Gg，Gg+1)为最短路径中点Gg到点Gg+1的欧氏距离；βg，g+1表示最短路径中相邻两顶点间权重。

为了减小场景点云与模型点云的最短路径差异，根据最短路径中相邻两顶点所属点云子集的分布特性赋予权重，具体方法如下：

1）若相邻两顶点属于同一局部稠密点云子集内，即Gg，Gg+1∈Hi，则缺失部分点对测地线距离矩阵的影响较小，可结合球面点云子集分布及内部点数赋予权重：

式中：N(Hi)为点云子集Hi内部点数

2）若相邻两顶点分别属于不同的局部稠密点云子集，即Gg∈Hi，Gg+1∈Hj，依据子集间分布差异及内部点数赋予权重：

3）若相邻两顶点分别属于局部稠密与全局稀疏点云子集，即Gg∈Hi，Gg+1∈H͂，依据内部点数的占比赋予权重：

将权重代入式（18），可得描述球面点云全局形状特性的测地线距离矩阵D。由于任意两顶点Gi与Gj间测地线距离dij与dji相同，所以，D为实对称矩阵。

2.4 点云配准矩阵估计

通过投影变换，可分别得原始场景点云与模型点云的全局测地线距离矩阵DP、DQ，如图4所示。考虑到点云扫描的无序性导致无法确定点云配准过程中实对称矩阵DP、DQ间的对应点对，利用特征值矩阵间接估计场景点云和模型点云间的配准矩阵。首先，对矩阵DP、DQ进行特征分解：

图4 配准流程Fig. 4 Registration process

式中：Λp、Λq分别表示分解后的特征值矩阵；Up、Uq分别表示DP、DQ的特征向量矩阵。由于目标点云翻转过程中任意两相同球面点间最短路径保持不变且DP、DQ均为实对称矩阵，因此，二者可通过初等行列变换相互转换：

其中：E1是第一类初等变换矩阵。

显然，矩阵DP与DQ相似，因此，分别对特征值矩阵Λp、Λq降序排列可转化为相同的特征值矩阵Λ：

式中：R1、R2分别表示矩阵DP、DQ到Λ的变换矩阵，可由Λ与DP、DQ的广义逆相乘得到。测地线距离矩阵DP、DQ间转换关系为

因为DP、DQ分别由场景点云与模型点云投影变换得到，等同于测地线距离矩阵间转换关系，场景点云矩阵P与模型点云矩阵Q间转换关系为

由变换矩阵R1、R2，可得场景点云到模型点云的配准矩阵：

基于测地线距离矩阵的点云配准方法可避免配准过程中对应点对误选问题，达到提升点云配准精度的目的。

3 仿真分析

3.1 数据集描述

为了验证所提方法的有效性，以斯坦福公共数据集中具有局部相似结构的通信卫星的三维点云为对象。该点云包含2 132个点、12条相似的轮廓线特征、2个相似的对接机构特征以及4个相似的本体面特征。为了模拟空间目标的翻滚运动，将卫星点云分别以10 (°)/s、10 (°)/s、45 (°)/s的初始速度绕x、y和z轴旋转，旋转加速度均为5 (°)/s2。图5给出卫星点云在第0、1、2、3、4、5、6 s的位姿。可以看出，第1 s点云与初始点云存在一定程度的重合，且旋转加速度的存在导致相邻2 s间点云的相对位姿差异逐渐增大。为了模拟空间目标受遮挡的特殊情形，随机删除点云数据集中的部分点，创建不同缺失程度（1%~10%）的点云缺失数据集，在此基础上，选择缺失率为3%、6%、9%的数据集，验证本文算法在不同遮挡程度且目标翻滚下的点云配准效果。

图5 点云位姿变化Fig. 5 Position change of point clouds

3.2 翻滚运动下点云配准效果比较

图6 卫星点云配准结果Fig. 6 Registration results of satellite point cloud

定义任意连续两帧点云的前一帧为场景点云，后一帧为模型点云。图6给出了连续两帧（第0 s、1 s）点云下ICP、CHACR_ICP和本文算法的配准结果，其中红色点云为场景点云，蓝色点云为模型点云乘配准矩阵的结果。可以看出：① ICP算法配准后两帧点云在x、y、z轴方向上的重合性均较差；② 相比于ICP算法，尽管CHACR_ICP算法在y方向上仍然存在点对匹配误差，但在x、z方向有较好的重合度，这是因为CHACR_ICP算法采用投影后三视图的点云外轮廓初始化变换矩阵；③ 采用本文算法配准后两帧点云在三轴方向上均基本重合，表明场景点云和模型点云的空间位置一致或二者配准成功，主要因为算法中测地线距离矩阵描述了点云的全局特性，可有效避免局部结构相似带来的配准影响。

为了定量评估3种配准算法的性能，定义场景点云和模型点云间的配准误差ε：

式中：R*表示连续两帧点云间旋转矩阵；F(⋅)是将矩阵转换为对应欧拉角的函数。实际上，配准误差是旋转矩阵与配准矩阵间对应欧拉角的绝对值之差的和。

图7 变速翻滚条件下点云配准误差Fig. 7 Errors of point cloud registration with variable speed rolling

图7给出了ICP、CHACR_ICP和本文算法在卫星点云连续变速翻滚8 s内的配准误差曲线。可以看出：① 在第2 s时，CHACR_ICP算法的配准误差略大于ICP算法，主要因为场景点云和模型点云的初始位姿较为接近，ICP算法所采用的最近点准则不易匹配错误，而CHACR_ICP算法所使用的点云三视图会在目标翻滚初期存在偏差；② 第2 s以后，随着时间累积，连续两帧点云的翻滚角度逐渐增大，致使ICP及CHACR_ICP算法的配准误差随之增加，且同一时刻下ICP的配准误差明显大于CHACR_ICP；③ 相比于ICP与CHACR_ICP算法，本文算法在连续两帧间的点云配准误差仍然几乎为零且不受翻滚角度的影响，更适合于空间非合作目标翻滚运动下点云配准。

3.3 点云缺失下配准误差比较

在不同的点云缺失程度下，测地线距离矩阵中两顶点间最短路径权重对点云配准效果的影响如图8所示。可以看出：① 在相同的点云缺失程度下，z轴上点云配准误差明显高于x轴与y轴，主要因为该轴方向的旋转速度大于其它两轴；② 当点云缺失程度从2%变化至9%时，引入两顶点间最短路径权重可以明显降低z轴方向的配准误差，但对x轴与y轴的影响相对较小。

图8 不同点云缺失程度下配准误差比较Fig. 8 Comparison of errors of registration with differ⁃ent degrees of point cloud missing

当卫星点云从1 s变速翻滚至8 s时，不同点云缺失程度（3%、6%和9%）下ICP、CHACR_ICP和本文算法的配准误差曲线如图9所示。可以看出： ① 在相同时刻下，ICP、CHACR_ICP和本文算法的配准误差均随着点云缺失程度的加剧逐渐增大，且当翻滚至第8 s时，ICP算法在不同缺失程度下的点云配准误差（超过35%）均明显大于CHACR_ICP及本文算法；② 相比于ICP与CHACR_ICP算法，本文算法在相同点云缺失程度下的配准误差最小且几乎保持不变，表明所提出的点云配准算法受空间目标翻滚遮挡所导致的信息缺失的影响程度较小。

图9 不同方法的配准误差比较Fig. 9 Comparison of registration errors for different methods

4 实物试验验证

为进一步验证本文算法的有效性，搭建了空间非合作目标地面捕获试验系统，由D435i深度相机构成的图像采集模块、模拟太空中微重力环境的气浮平台、参照东方红一号卫星制作的模拟目标星以及计算机处理模块构成，如图10所示。其中，深度相机通过支架固定于模拟目标星斜上方45°且试验过程中保持位置不变，负责实时采集目标的彩色图、深度图；模拟目标星在试验中分别绕x、y、z轴旋转；计算机处理模块依据采集的模拟目标星的彩色图及深度图实时计算、储存目标点云。相关试验参数设置如表1所示。

图10 试验系统框图Fig. 10 Block diagram of experimental system

表1 试验参数设置Table 1 Experimental parameter setting

为计算模拟目标星的旋转速度，需对配准矩阵进行位姿解算。设目标星绕x、y、z轴的旋转角度分别为θx、θy、θz，配准矩阵表示为

式中：Rx(θx)、Ry(θy)、Rz(θz)分别表示绕x、y、z轴旋转的旋转矩阵。依据旋转矩阵与旋转角间转换关系，采用下式计算相应的旋转角［25］：

式中：atan 2(⋅)表示反正切函数，取值范围为[0，2π)。试验中，依据前后帧的配准矩阵计算两帧间的旋转角，结合前后帧的时间间隔，可得模拟目标星在各轴上的旋转速度。

图11给出了连续采样时刻下ICP、CHACR_ICP和本文算法的点云配准结果。可以看出：①采用ICP算法配准后，在x、y、z方向上对应点位置的一致性较差，且卫星窗口及边缘处存在较大偏差；② 相比于ICP算法，尽管CHACR_ICP算法在三轴方向上仍然存在一定程度的点对匹配误差，但在卫星窗口及点云边缘处有较好的重合度，主要因为CHACR_ICP算法采用投影后三视图的点云外轮廓初始化变换矩阵，可提高算法精度；③ 相比于CHACR_ICP算法，本文算法在卫星窗口与边缘处的点对配准效果更佳。此外，依据配准矩阵，通过位姿解算可得模拟目标星分别沿x、y、z轴旋转了2.896 8°、3.918 0°、3.108 8°，与试验中设置的目标星旋转度基本一致。

图11 不同方法的配准结果Fig. 11 Registration results for different methods

图12给出了ICP、CHACR_ICP和本文算法在卫星点云连续旋转10 s内的配准误差曲线。可以看出：ICP算法配准误差超过6%且明显高于CHACR_ICP算法，同时，本文算法的配准误差（平均值为2.94%）最低。此外，受模拟目标卫星在旋转过程中光照条件、速度不稳定等因素的影响，3种算法的配准误差均存在小幅波动现象。