基于截断最小二乘和半正定规划的空间非合作目标相对位姿估计*

潘 汉, 曹姝清, 武海雷, 敬忠良, 顿向明

（1.上海交通大学 航空航天学院·上海·200240；2. 上海航天控制技术研究所·上海·201109；3. 上海市空间智能控制技术重点实验室·上海·201109）

0 引 言

在轨服务是指在空间中通过人、空间机器人或者两者协同工作的方式，实现各类航天器在复杂空间环境下长时间稳定运行的空间操作[1-3]。具体来说，在轨服务一般包含在轨维护、在轨加注、在轨组装、模块更换等。美国、欧盟国家、日本等航天强国，以在轨演示验证项目需求为牵引，验证在轨服务技术，如轨道快车[4]、凤凰计划[5]等。一些发达国家研制了相关的航天器，如德国的“试验服务卫星”和“轨道寿命延长飞行器”等。国内的在轨服务技术，在优势单位（航天五院502所、航天八院803所、上海交通大学、哈尔滨工业大学、天津大学等）的大力攻关下，也取得了长足的发展[6-7]。然而，上述的在轨演示验证项目、空间机器人大部分面向合作目标开展关键技术攻关，无法处理空间非合作目标。具体来说，空间非合作目标是指无应答、无标识的空间目标，如失效的翻滚卫星、空间碎片等。针对空间非合作目标，空间机器人自主感知是在轨服务的核心关键技术之一。考虑到空间复杂光照环境、非合作目标相对位置、姿态与尺度变化强耦合等因素，使得空间非合作目标相对位姿估计比较困难。因此，迫切需要开展面向空间非合作目标的空间机器人自主感知技术研究。

空间非合作目标相对位姿估计是空间机器人自主感知的核心关键技术之一[7]。该技术通过多传感器测量并估计出空间非合作目标相对于服务航天器的相对位置、相对姿态、相对速度以及相对角速度等。常用的视觉传感器有：单目相机（Monocular camera）、双目相机（Stereo camera）、扫描式激光雷达（Scanning Lidar）、闪光式激光雷达（Flash Lidar），以及飞行时间（Time-of-Flight，TOF）相机。这些传感器都有各自的优缺点，在目标形态测量、3D重建以及相对运动学与动力学分析等应用场景中得到了应用[7]。目前，已有相关的研究团队设计了新的传感器以适应空间复杂光场环境的不利影响，如过曝、视场角较小、卫星表面覆盖层带来的散射光影响等[8-11]。然而，空间操作任务的复杂性，使得视觉传感器在轨迹规划、逼近过程、姿态控制、制导等方面的应用与信息处理也变得复杂[12]。例如，在轨迹规划过程中就需要考虑视觉传感器的视场角指向以及己方航天器的动力约束等因素。

此外，空间非合作目标的相对运动学与动力学的复杂性，亦使得空间非合作目标相对位姿估计比较困难。具体来说，服务航天器无法获得或只能获得较少的空间非合作目标的先验知识，如尺寸、大小、旋转角度及旋转轴等参数，从而导致服务航天器的多传感器无法准确测量并估计。现有的文献基于即时定位与地图构建（Simul-taneous Localization and Mapping，SLAM）技术框架[12-13]，研究空间非合作目标的3D建模与状态估计，并取得了一定的进展。但是，这些工作大部分使用单目相机或是双目相机，易受复杂光场的影响，因此应用场景受限。

与此同时，一些文献基于TOF相机中的点云信息，提取目标的3D几何结构特征，研究了空间非合作目标的相对位置与姿态估计方法。现有基于3D点云的相对位置与姿态估计方法可分为两种：1）基于特征匹配的方法[8-11]；2）基于同时位姿测量与匹配的方法[14-19]。基于特征匹配的方法的主要思路是首先检测局部或全局特征描述子并匹配3D特征点，然后再使用随机抽样一致性（Random Sample Consensus，RANSAC）算法或是应用鲁棒配准来估计相对位姿。同时位姿测量与匹配的方法思想是在基于特征匹配的配准以及计算最优相对位姿之间进行切换。然而，这些方法都没有考虑实际场景中传感器噪声以及目标（如立方星等目标）几何结构特性的差异化，因此，这些方法的应用潜力有限。

本文面向在轨服务任务需求，针对空间光场复杂、非合作目标尺度变化及相对姿态与位置强耦合等难题，基于TOF相机产生的点云信息，充分利用空间目标的典型几何特征（如快速点特征直方图（Fast Point Feature Histograms，FPFH）[20]）进行匹配,将相对位姿估计过程抽象为基于截断最小二乘（Truncated Least Squares，TLS）的最小化问题，通过对相对位置和相对姿态进行解耦，提取FPFH特征[20]，构造基于半正定规划的相对姿态估计方法。通过使用非合作目标的3D模型进行仿真，验证了该方法在不同强度的噪声特性下具有较好的估计特性，可以为后续的任务实施提供理论与技术支撑。

1 基于截断最小二乘的空间非合作目标相对位姿估计

（1）

（2）

需要注意的是，式（2）不对输入的异常点直接建模，而是设置了一个上界。实际上，在应用过程中可以发现无法对异常点有效地建模。因此，使用一个上界解决对异常点进行描述的问题是一种简便且可行的方法。

2 空间非合作目标的相对尺度、姿态与位置的解耦与估计

2.1 相对尺度、姿态与位置的解耦

本节基于式（2），给出了空间非合作目标的相对尺度、姿态与位置的解耦过程[21]，实现了空间非合作目标的相对位姿估计。通过观察可以发现，Pb与相对位置t相关，也就是说，相对位置具有不变性。具体来说，给定两个点bi和bj，根据式（2），可得这两个点的相对位置为

bi-bj=sR（ai-aj）+（zi-zj）+（φi-φj）

（3）

（4）

其中，zij=zi-zj具有类似的定义。

更进一步地，还可以得到位置与姿态的不变性。通过观测式（4）可知，该式仍然与姿态R相关，因此，首先计算一个相对位置不变向量的范数，也就是

（5）

对于内点来说，zij=0，且满足式（6）

（6）

（7）

2.2 相对位置、尺度与姿态的估计

这里给出空间非合作目标相对位姿估计的主要流程，如表1所示。对于大部分异常点，本文使用最大团选择法[22]（Maximal Clique Selection）。最大团选择法[23]的思想是首先进行体素下采样，获得一个图g（v,ζ）。其中，v是图的顶点，ζ是图中边的集合。然后，利用该方法计算图的最大团g（v,ζ′）（最大团对应的边的集合）。

表1 基于截断最小二乘和半正定规划的空间非合作目标相对位姿估计流程

2.2.1 相对尺度估计

首先，基于式（7）使用截断最小二乘方法，结合给定的量测sij以及上界εij情况，估计出相对尺度

（8）

其中，K代表具有不变性的量测；εk代表给定内点噪声的上限值。此外，sk=sij。

2.2.2 相对姿态估计

（9）

2.2.3 相对位置估计

在分别得到了相对尺度与相对姿态后，可以将这两个参数代入式（2），估计相对位置t，则可以得到

（10）

其中，χi代表给定的内点噪声的上限值。

3 仿真实验及结果分析

3.1 仿真测试实验条件

本文使用BlenSor[24-25]对TOF相机进行仿真。使用了斯坦福bunny、风云系列航天器和立方星（cubsat）的CAD模型渲染图，如图1所示。追踪航天器观测空间非合作目标的仿真轨迹，如图2所示。其中，追踪航天器螺旋逼近目标，目标自旋角速度5（°）/s。TOF相机的分辨率为640×570。整个仿真的实验平台采用Ubuntu 16.04操作系统，8G内存，4.7Hz Intel CPU。

3.2 相对位置估计实验结果

本节在斯坦福bunny、风云系列航天器和立方星CAD模型的基础上，开展相对位置估计精度对比分析实验。实验结果如表2所示。其中，斯坦福bunny、风云系列航天器、立方星CAD模型的期望相对位置为（x,y,z）=（5, 5, 5），单位为m。具体来说，在仿真过程中，对上述CAD模型进行刚体变换，产生位置偏移，也就是相对平移。输入高斯噪声强度均值都为0，方差分别是0.01, 0.05, 0.1。本文使用估计值与期望相对位置值的L2范数来度量相对位置估计精度。表2中，符号“—”代表算法崩溃，无法产生相对位置估计值，标注为粗体的数字代表较好的结果。本文所比较的相对位置估计算法是Open3D框架中的迭代最近点（Iterative Closest Point，ICP）算法[26]。由表2可知，所提出的算法具有较好的相对位置估计精度。

（a） 斯坦福bunny模型

（b） 风云系列航天器模型

（c） 立方星模型图1 3个CAD模型的渲染图Fig.1 Effect drawing of the three CAD models

图2 航天器观测空间非合作目标的仿真轨迹Fig.2 Simulation trajectory of spacecraft observing space non-cooperative targets

表2 相对位置估计精度对比分析

图3和图4所示为立方星模型匹配过程结果图。图3（a）给出了输入的点云数据，其中绿色代表立方星的原始点云，红色代表加了噪声（强度为0.1）以及相对位移与相对姿态变换后的点云。图3（b）给出了基于FPFH的特征匹配结果，其中，蓝色的线代表成功匹配后的结果。基于得到的相对位置与相对姿态，可以得到最终的匹配结果，如图4所示，绿色代表原模型的点云，红色代表仿真给出的点云。实验结果表明，本文算法具有较好的相对位置估计性能。

（a） 输入的点云数据

（b） 基于FPFH的特征匹配结果图3 立方星模型的特征匹配结果图Fig.3 The feature matched results on cubsat model

图4 立方星模型的匹配结果图Fig.4 The matched results on cubsat model

3.3 相对姿态估计实验结果

为了验证相对姿态估计算法的有效性，本文基于斯坦福bunny、风云系列航天器、立方星CAD模型开展相对姿态估计精度实验。实验结果如表3所示。其中，斯坦福bunny、风云系列航天器、立方星CAD模型的期望相对姿态，以旋转矩阵的形式表示为

其中，相对姿态的单位是rad，输入的高斯噪声强度分别为0.001, 0.005, 0.1。本文使用估计值与期望相对姿态值的绝对值最大值来度量相对姿态的估计精度。表3中，符号“—”代表算法崩溃，无法产生相对姿态估计值，标注为粗体的数字代表较好的结果。采用Open3D框架中的ICP算法完成相对姿态估计，实验结果表明，所提出的算法具有较好的相对姿态估计精度。

表3 相对姿态估计精度对比分析

最后，斯坦福bunny模型的匹配结果如图5所示。图5（a）给出了斯坦福bunny模型的点云。图5（b）给出了输入的点云数据，其中绿色代表立方星的原始点云，红色代表加了噪声（强度为0.1）以及相对位移与相对姿态变换后的点云。图5（c）给出了基于FPFH的特征匹配结果，其中，黑色的线代表成功匹配后的结果。

（a） 斯坦福bunny模型的点云

（b） 输入的点云数据

（c） 基于FPFH的特征匹配结果图5 斯坦福bunny模型的匹配结果图Fig.5 The matched results on Stanford bunny model

4 结 论

本文面向空间在轨服务，针对空间非合作目标中尺度、姿态与位置的强耦合特性，挖掘点云信息的内在特点，采用FPFH特征，提出了基于截断最小二乘的空间非合作目标相对位姿估计目标函数。其中，相对姿态的求解采用基于半正定规划的求解方法。本文通过TOF相机仿真，基于斯坦福bunny、风云系列航天器、立方星CAD模型，模拟了航天器逼近空间非合作目标的过程。仿真实验结果验证了本文算法的有效性，可为后续的工程应用提供理论与技术支撑。