基于点云的空间非合作目标初始相对位姿获取

刘智远，郭延宁，梁维奎，徐 杭

(1. 哈尔滨工业大学控制科学与工程系，哈尔滨 150001；2. 上海宇航系统工程研究所，上海 201109)

0 引 言

空间非合作目标范指不能提供有效合作信息的空间目标[1]，由于其不具备已提前安装的定位识别标志，在近距离解算相对位姿时只能依赖服务航天器的观测信息[2]。对空间非合作目标进行相对位姿解算，是服务航天器相对导航、进而实施在轨服务或空间碎片清除等空间操作任务的基本前提。

根据所采用的传感器不同，已有的非合作目标相对位姿解算研究主要分为视觉传感器与激光雷达两类。Hu等[3]使用单目视觉传感器完成对非合作目标星舰对接环的识别与相对位姿建模；Christan等[4]使用视觉传感器与结构光深度传感器，进行传感器融合以解算非合作目标相对位姿并完成自主导航。视觉传感器成本低，可适应复杂的工作环境，但对空间观测光照条件要求较高；激光雷达则避免了由于复杂光照条件带来的影响，成为空间近距离观测与相对位姿解算的主流方法：Woods和Christan[5]使用扫描式激光雷达获取目标点云数据，并使用点云全局特征拟合以获取ICP迭代初值，提高了点云配准与位姿解算的鲁棒性；Liu[6]使用闪光式激光雷达以获取目标点云数据来解算相对位姿，并设计数字仿真实验与物理实验完成对非合作目标模型的相对位姿建模与接近。

由于激光雷达的观测不受空间光照条件影响，在弱光、逆光、阴影等观测条件下均具有较高的鲁棒性[7]，使用其进行通过点云构建解算相对位姿具有显著的应用前景。然而，由于需使用点云全局特征进行点云主方向拟合以获取初始位姿，对非合作目标点云模型与观测点云完整度要求较高。鉴于此，如何使用目标外轮廓点云，在初始位姿未知的情况下完成迭代最近点位姿解算，成为一项关键工作。

在使用点云进行迭代最近点解算目标相对位姿时，需要使用ICP算法[8]拟合当前帧点云与目标点云模型。然而，由于ICP算法对初值要求较高[9]，当前帧点云与目标模型点云之间实际为错误点对时，直接使用ICP算法将使得逐步迭代到模型的错误位置，进而产生误匹配，得到错误的位姿解算，如图1所示。

图1 ICP初始位姿对匹配结果的影响示意图Fig.1 Iteration result infected by initiol ICP relative pose

考虑到非合作目标可能具有的高度对称性，如典型的通信卫星都具有两个对称的太阳帆板等，简单地仅使用ICP进行当前帧点云配准极有可能将其匹配到模型的另一对称部分或对称角度，因此容易造成误匹配。

目前已有的点云ICP初值选取算法包括两种：使用点云局部特征及点云关键点匹配处理算法，以及使用点云全局特征的点云拟合算法[10]。其中，点云FPFH特征是使用较为广泛的点云局部特征，但其计算结果受点云质量及计算参数等影响较大[11]，其应用场合多为静态物体三位点云拼接与SFM[12]；VFH与更为稳定的OUR-CVFH是两种常用的点云全局特征[13]，目前已有多名学者使用OUR-CVFH特征对待匹配点云进行预处理，并得到良好的匹配结果与初始位姿。然而，针对外表高度对称的非合作目标点云模型，不同角度的观测点云计算得到的OUR-CVFH特征将具备相当高的相似度，亦会产生一定概率的误匹配。

然而，非合作目标可能携带不同的表面载荷，如雷达、星敏感器、轨控发动机、姿控喷嘴等。这些载荷增加了目标表面的不对称度，并由于其构型位于目标本体之外，其点云模型也将成为目标本体点云之外的点云凸包。考虑到非合作目标携带不同表面载荷带来的不对称性与唯一性，通过对载荷点云进行提取，结合所占面积较大的目标太阳能帆板共同构成核心点云，并使用此点云求解位姿变换矩阵，进而以此矩阵作为ICP位姿解算的预处理结果进行输入，将会给ICP提供良好的迭代初值。

本文基于对ICP算法的分析，充分考虑非合作目标可能具有的高度对称性结构，设计了一种基于LCCP点云分割与点云Harris3D角点的点云匹配预处理：通过对合理降采样后的点云进行点云分割，提取太阳帆板部分并割离目标本体；进而对本体点云模型进行Harris3D角点提取，获取点云中可能代表表面不同载荷的凸包点云；以Harris3D角点为中心，衍生周边点云区域作为潜在关键载荷区域；结合太阳帆板点云并进行多角度预变换处理，分别求解各种预变换位姿下的匹配度，最终选取最佳匹配度作为匹配结果；在匹配上的Harris角点间使用SVD求解最佳位姿变换；最终，使用该位姿变换矩阵作为ICP初始迭代位姿，完成对ICP算法的预处理与优化。

1 基于点云分割与关键点的初始位姿获取方法

基于点云的非合作目标相对位姿解算流程图如图2所示。

图2 典型位姿解算流程图Fig.2 Typical relative pose estimation process

其中，ICP迭代过程为相对位姿求取的关键，且对初始迭代位姿要求较高。本文旨在通过点云处理获取较好的ICP初始迭代位姿，以保证迭代结果的可靠性。

本节将具体阐释本文提出的ICP初始迭代位姿获取方法流程。该方法具体流程如表1所示。

表1 初始位姿获取方法Table 1 Initial relative pose acquire process

现以某通信卫星模型为例进行算法流程演示。

1.1 LCCP点云分割

LCCP(Locally convex connected patches)算法，不依赖点云模型的颜色，仅使用点云的三维位置分布进行点云分割[14]。通过计算点云晶核聚类后不同超体之间的凸凹关系，进行相邻超体的重聚类并完成最终点云的划分。超体之间的凸凹关系将使用相邻超体中心连线向量与两超体法向量夹角进行计算,如图3所示。

图3 相邻超体凸凹判据Fig.3 Criterion between adjacent superbodies

且若满足

α1≥α2

(1)

则判定相邻超体为凸连接，反之为凹连接。

进而，若相邻超体判定为凸连接，使用SC判据计算相邻超体中心连线与超体临接线夹角θ进行超体连通性判断,如图4所示。

图4 相邻超体中心连线与超体临接线夹角θFig.4 The angle θ between the adjacent super-body center line and the super-body wiring

夹角θ越接近90°，则SC判据判定相邻超体为连通的可能性更大。最终，LCCP使所有连通的相邻超体跨越凸边界增长，并完成点云的分割。

使用降采样后的点云模型，设定粒子距离、晶核距离容差阈值、空间与法线权值等参数，对点云模型进行LCCP分割。分割结果如如图5所示。

图5 LCCP点云分割结果Fig.5 Point cloud segmentation result

获取点云维度最大的方向，并提取该方向上第一和最后一个点的点云标签，其对应的点云则分别为目标的太阳帆板。

1.2 Harris关键点提取及周边区域衍生

由于本文算法旨在充分利用表面载荷点云信息，避免误匹配并提高匹配精度，为避免求取点云法向量以降低计算量，不使用SIFT关键点；同时，考虑当前帧点云为非完整点云包络，点云的边缘不应作为关键点被提取出，因此按照算法原理不选择NARF关键点而选择Harris角点[15]作为关键点。

基于已经提取的Harris3D角点，可以通过提取其周围某一范围区域的点云作为关键点云凸包，而这些凸包则极有可能包含目标表面携带的关键载荷。以目标点云本体模型为例，Harris3D角点及其周边衍生区域的提取效果图如图6所示。

图6 Harris3D关键点及周边衍生区域Fig.6 Harris keypoints and peripheral derivative point cloud

1.3 关键点云多角度预变换处理

考虑到非合作目标高对称度外观，以及相对规整的几何轮廓，其不同Harris3D角点可能被从不同的类似位置提取出来，例如正多面体目标本体的多个相似角点等。因此，不论从被提取出的关键点本身的描述子及基于描述子的关键点匹配角度，抑或从单一关键点周边衍生区域分别进行模板匹配的角度，都将无法解决上述相似角点所带来的误匹配问题。因此，关键点或关键点周边衍生区域不能被拆开使用，只用对其进行统一的变换与匹配，才可以求解得到正确且有意义的变换矩阵，进而作为ICP精确配准的初始迭代位姿。将目标太阳帆板与本体角点及衍生区域组合后构成的关键点云如图7所示。

图7 目标帆板与本体关键点衍生区域Fig.7 Solar panels and peripheral derivative point cloud

为充分利用目标表面不同关键载荷所带来的不对称性与唯一性，将当前帧关键载荷潜在点云凸包进行多次整体旋转变换，以避免由于目标的高对称度而在预处理时就出现误匹配的结果。以上述点云模型为例，以俯仰-偏航-滚转的旋转顺序，绕每个轴旋转的角度分别设定为0π,1/2π,2/2π,3/2π的角度并进行排列组合，得到64种预处理旋转变换矩阵R，及相应的预变换点云：

R=Rx·Rz·Ry

(2)

(3)

式中:Rx,Rz,Ry分别为滚转、偏航、俯仰轴变换矩阵；xi,yi,zi分别为原始点云坐标，x′i,y′i,z′i分别为变换后点云坐标。

1.4 关键点云最佳匹配提取

通过对关键点云进行不同的预旋转变换处理，分别对其与目标模型关键点云凸包进行最佳匹配，以充分消除由于目标对称性与初始位姿所带来的误匹配结果。以上述点云模型为例，匹配方式使用ICP，匹配相似度变换率阈值设为5%，迭代次数阈值设为10次。预变换结果如图8所示，分别按照俯仰、偏航与滚转变换次数进行命名。

图8 不同角度预变换后的点云凸包匹配结果Fig.8 Alignment results through different pretreatment

对于不同角度预变换处理后的点云匹配结果，匹配契合度的量化结果为ICP最近邻点距。此上展示的不同预处理变换角度及最近邻点距值如表2所示。

表2 预变换角度及最近邻点距Table 2 Angles of pretreatment and closest points’ distances

ICP最近邻点距越小代表匹配契合度越高，选取最高契合度对应的匹配结果作为点云最佳匹配结果。

1.5 SVD求解关键点云配对最佳变换矩阵

选取最佳匹配的点云对，分别按空间距离求取匹配后的Harris角点配对。点云模型初始位姿下的Harris角点配对如图9所示。

图9 Harris 3D关键点配对Fig.9 Harris 3D keypoints pairs

获取全部配对Harris3D关键点的三维坐标后，构建最小二乘问题：

(4)

2 仿真校验

2.1 仿真环境搭建及坐标系定义

为验证本文基于点云分割与Harris关键点的初始位姿获取算法的可行性，设计了数字仿真实验完成对算法的校验，并对比：1)不加预处理的ICP点云配准算法；2)使用点云FPFH特征并进行点云关键点匹配的预处理算法；3)使用点云OUR-CVFH特征的预处理算法，并评估各种算法对位姿解算的可行性与准确率。算法测试与校验的相关代码使用C++编写，编译并运行于Ubuntu系统中。

使用Solidworks绘制非合作目标模型，模型缩放比例为1:10。仿真环境选用Gazebo进行构建，点云传感器选用SR4000激光雷达。

本实验已知目标点云模型，且仿真环境中的目标姿态已知，因此算法解算出的位姿误差可计算得到。

实验中使用到的坐标系，包括惯性系O-xyz，目标本体坐标系Ot-xtytzt与服务航天器坐标系Os-xsyszs。坐标系规划与设定如下图所示：

图10 惯性系、目标本体坐标系与服务航天器坐标系Fig.10 Inertial coordinate system, target body coordinate system and service spacecraft coordinate system

滚转、偏航、俯仰变换矩阵Rx；Rz；Ry分别为：

(5)

(6)

(7)

式中：φ，φ，θ分别为滚转、偏航、俯仰角度。

传感器坐标系与目标本体坐标系之间点云坐标变化如下：

(8)

式中:Δxt,Δyt,Δzt分别为目标中心在世界坐标系中的偏移量，Δxs,Δys,Δzs分别为观测器位置在世界坐标系中偏移量。

2.2 数字仿真实验

仿真实验将使目标绕zt轴旋转，测试算法的可行性并与传统ICP及其他预处理算法进行对比验证，同时对实验中出现的各种现象做出分析与临时调整，在实验中对本文算法进行了完善。

目标中心置于坐标原点(0, 0, 2)处(单位：m)，观测点位置为(2, 0, 2)；目标初始姿态(φ,φ,θ) = (0°, 0°, -180°)，其θ将以20°的间隔从-180°变化至180°。

考虑到某些特定角度范围内，由于目标可能侧向于观测点，其太阳帆板可能会无法完整观测，甚至被本体遮挡。因此，在点云分割后的太阳帆板提取步骤中，加入左右太阳帆板点云个数差的限制：

(9)

式中：nLeft，nRight分别为左、右帆板点云个数，nPoints为点云模型整体点云个数；以及太阳帆板整体点云个数阈值限制：

(10)

若不满足上述任一条件，将默认太阳帆板受到严重遮挡导致点云分割的误识别，或目标相对观测点的角度使得获取到太阳帆板的点云数量过少。此时将跳过点云分割过程，直接进行全体点云的Harris关键点提取。

基于点云模型的Harris关键点，计算关键点的FPFH描述子[16]并进行关键点匹配，匹配结果如图11所示。

图11 某帧点云FPFH描述子配对结果Fig.11 FPFH description matching results

由于点云局部特征对点云质量及计算参数鲁棒性低，同一物体不同观测的点云计算得到的FPFH描述子将具有一定差别。本文使用的点云模型并不能通过FPFH描述子进行有效的关键点匹配，亦无法进一步求解变换位姿。

图12 三种算法整体角度误差Fig.12 Overall angular error of the three algorithms

文中预处理算法、使用OUR-CVFH特征的预处理算法及传统ICP算法解算出的整体角度误差α如图12所示。

其中，整体角度误差α将按照罗德里格斯公式(Rodriguez formula)计算：

(11)

式中：R0为目标模型旋转矩阵，Rresult为预处理过程由SVD解算得到的旋转矩阵或传统ICP算法迭代求解的变换矩阵。

通过数据可以分析出，本文算法对目标点云具备大角度偏航角位姿的情况可以鲁棒地求解最佳匹配；同时可以在一定程度上有效避免由于太阳帆板无法完整观测所带来的误匹配；给出的SVD解算值也相对准确，可作为进一步ICP配准的有效初值。传统ICP在目标具备大角度偏航角的条件下易根据迭代最近邻点的算法特性，与目标大面积太阳帆板的特征，而产生180°的误匹配；在太阳帆板受到遮挡或观测点云数量较少时，由于目标本体的高度对称性，将产生极高的误匹配率；基于OUR-CVFH的预处理算法[5]在太阳帆板可良好观测的条件下可以有效地消除初始位姿带来的局部最优，但点云的高度对称性所带来的点云OUR-CVFH特征的相似性将有可能产生误匹配，产生误匹配时的特征如图13所示。

图13 不同角度观测的OUR-CVFH特征Fig.13 OUR-CVFH features get from different perspectives

由于非合作目标的高度对称性，不同观测角度下点云的OUR-CVFH特征具备相当高的相似性，因此有一定概率发生误匹配。

三种算法的平移量误差如图14-16所示，点云平移误差将按照点云重心坐标计算：

图14 本文算法x，y，z轴平移误差Fig.14 Translation error component of our algorithm

图15 传统ICP算法x，y，z轴平移误差Fig.15 Traditional ICP translation error component

图16 OUR-CVFH算法x，y，z轴平移误差Fig.16 OUR-CVFH translation error component

在不同偏航角度下，三种算法均可有效缩减点云间的距离，且位置误差均在容许范围内。卫星模型最大维度尺寸约为1.5 m，观测距离2 m，三种算法匹配结果距离误差均在0.1 m级。

当偏航角度在[-60°, 60°]范围内，三种算法均得到了正确的迭代匹配结果。在正确匹配的情况下，本文算法在三轴上角度误差如图17所示。

图17 本文算法角度误差分量Fig.17 Angular error component of our algorithm

分析数据可得出，本文算法由于需使用SVD求解变换矩阵数值解而非迭代求解，其结果精度突变性较大，但角度误差均在10°以内，可作为进一步ICP配准的有效初值。

3 结 论

本文设计了一种基于LCCP点云分割与点云Harris3D关键点的改进ICP预处理过程，该预处理过程可以有效地减小不同初始位姿情况下，由非合作目标高度对称性所引起误匹配的概率。所提出的算法针对非合作目标的高度对称性外观，提取目标表面潜在携带载荷区域，利用携带载荷所带来的不对称性与唯一性，同时考虑观测角度带来的太阳帆板受遮挡或观测不佳的情况，结合目标携带的太阳帆板进行辅助定位，求解当前帧点云与目标模型点云之间的最佳位姿，并以此位姿输入ICP求解器作为初始迭代位姿，减小由于非合作目标高度对称性带来的误匹配率。