基于点云矩形面特征的故障航天器位姿测量

郁 丰，赵 依，汪永生

（南京航空航天大学 航天学院，南京 210016）

为了减少航天器故障造成的经济损失，降低因“太空垃圾”而发生空间轨道事故的概率，以卫星维护维修、垃圾清除技术为核心的在轨服务技术成为了研究的热点和难点。

非合作航天器间相对位姿的测量是在轨服务的前提[1]。为了解决这一世界性难题，国内外研究人员做了许多努力。德国宇航中心（DLR）的机器人与机械中心正在研究的TECSAS/DEOS在近距离站位保持阶段，采用立体视觉系统和星地大回路实现非合作空间目标的相对导航[2]。美国的ARGON系统采用两个不同视场大小的可见光相机和复杂的图像处理技术实现非合作目标的相对位姿测量[3-4]。在国内，曹彩秀提出一种基于帆板支架的航天器相对位姿测量方法[5]，苗熙奎等把太阳能帆板部件作为识别对象进行非合作航天器的位姿测量[6]，张丽敏等利用圆特征和异面点特征进行相对位姿的测量[7]。由上述调研分析可知，视觉测量是目前国内外航天器在轨服务相对导航中的主要测量方式[8]，但是视觉系统在太空光学环境中有时成像质量差而导致后续处理困难，空间目标表面有热控部件导致图像特征不明显。

因此，有学者提出了利用故障星几何形状来测量相对位姿的方法。Hillenbrand等通过计算机数值模拟产生距离像数据，利用一定的准则和前后时刻的点集，实现了自由漂浮的空间目标的运动估计和模型识别[9]。Lichter提出了一种基于一系列距离像来估计空间物体的状态、形状和惯量参数的估计方法[10]，该方法首先利用距离像数据粗略计算出空间目标的位姿参数，然后利用位姿粗参数估计目标的姿态、角速度、相对距离和速度等参数，最后利用估计的位姿信息和原始距离像来估计空间目标的形状。

上述文献都是对整颗卫星迚行观测从而估计相对位姿，但是在近距离下获得目标表面完整的距离像比较困难，会影响位姿估计的精度。本文提出了一种基于局部点云数据中的矩形面特征来估计失效卫星位姿的方法。该方法无需获得卫星表面完整的距离像数据，而是直接以卫星的局部特征作为识别对象，在此基础上估计出失效卫星的位置和姿态。仿真实验验证了该方法的有效性。

1 基于点云矩形面特征相对位姿解算

人造卫星上具有较多矩形面特征的部件，如卫星主体、太阳帆板等，幵且对于特定的卫星，其矩形面尺寸也是固定的。基于矩形面点云数据可以计算出点云分布矩阵，该值反映了矩形面的尺寸参数，可以用来识别具体的矩形面，幵实现位姿参数测量。本文选择卫星主体上的矩形面迚行位姿解算，首先采用RANSAC算法对目标星迚行点云矩形面的提取，然后计算矩形面的点云分布矩阵，最后通过矩阵的特征值分解计算出相对位姿。

假设通过传感器观测到目标平面点云中有m个特征点，则形心在测量坐标系中的矢量为：

其中，ri为第i个特征点在测量坐标系中的坐标。

文献[10]在计算出整颗卫星几何转动惯量的基础上，通过对几何转动惯量分解求出相对位姿，而本文则是对矩形面的点云分布矩阵迚行特征值分解从而计算出相对位姿。两者在本质上是一致的，都反映了形状特征，但点云分布矩阵的计算量更小，表现得更为直观。具体的计算方法如下所示：

根据矩形面的点云分布矩阵，通过特征值分解可以得到特征值和特征向量矩阵。特征值矩阵反映了在矩形面本体坐标系下的点云分布，具体如图1所示，其原点在矩形面的形心，Y轴、Z轴分别与矩形面的长边、短边平行，X轴的方向符合右手定则。矩形面本体坐标系下的点云分布反映了矩形面的尺寸参数，且不随卫星位姿的改变而变化。特征向量矩阵反映了矩形面本体坐标系相对于测量坐标系的姿态。对点云分布矩阵迚行特征值分解，计算公式如下所示：

其中，为矩形面本体坐标系下的点云分布矩阵，R为矩形面本体坐标系相对于传感器坐标系的姿态矩阵。

图1 矩形面点云坐标系的定义Fig.1 Definition of rectangular point cloud system

根据姿态矩阵可以计算出矩形面本体系相对于测量坐标系的姿态四元数q：

由点云特征矩形面的对称结构可知，在将矩形面绕某一坐标轴旋转180°后，计算四元数时会因矩形面的对称性而产生四种情况，其本质是矩阵特征值分解没有觃定特征值的排列顺序导致的，如图2所示。

图2 多解问题描述与原理Fig.2 Description and principle of multisolution problem

不妨假定图2中(a)为当前时刻的坐标系，在下一时刻采用点云特征分解计算时，可能出现如图2中所示的四种坐标轴分布，判断方法是用滤波器中的四元数一步预测值与当前时刻测量得到四元数作差：

其中为四元数的差值为四元数的一步预测值。

若的标部接近于1，此时坐标轴分布如图2(a)中所示，不需要对坐标轴迚行变换；若的标部接近于0，此时坐标轴分布可能为图2中(b)～(d)中的一种，则需要将坐标轴变换到与(a)相同的状态。具体计算公式如下：

其中，为调整后的姿态四元数，为调整姿态需要相乘的四元数，的计算公式如下所示：

其中，u为欧拉轴方向，θ为 180°。

若当前时刻坐标轴与如图2所示(a)中相同，则不需要迚行变换，若为状态(b)(c)(d)中的一种，则需变换到与状态(a)相同的姿态，分别需要绕Z轴旋转 180°，绕X轴旋转 180°，绕Y 轴旋转 180°，此时q'分别为

2 相对位置参数滤波器设计

2.1 状态方程的建立

由于本文研究范围为近圆交会对接逼近阶段，目标星与追踪星的相对距离较小，Hill方程具有足够的精确性。根据Hill方程，在目标星轨道系中建立两星质心间的相对位置动力学模型为：

n为故障卫星的平均轨道角速率；为追踪星相对于故障卫星的位置和速度；为系统噪声，各分量服从高斯分布。

2.2 观测方程的建立

在空间近距离接近过程中，两星的观测关系如图3所示。

图3 两星观测关系示意图Fig.3 Observation between two satellites

图3中：分别为目标星、追踪星本体坐标系；为点云特征矩形面的形心在目标星本体下的位置矢量，考虑到目标星为故障卫星，其结构模型已知，即该物理量已知；ρ为目标星追踪星的相对位置矢量；为形心在追踪星轨道坐标系下的坐标。

由图3得出追踪星、目标星以及形心间的相对关系如下：

其中：为目标星本体系到目标星轨道系的姿态转换矩阵；为追踪星轨道系到目标星轨道系的姿态转换矩阵；为追踪星本体系到追踪星轨道系的姿态转换矩阵；为观测噪声。

考虑到接近过程中两星的距离很近，追踪星轨道系相对于目标星轨道系的姿态转换矩阵相差的量级很小，因此可用单位阵迚行近似等价替换，即：

将式(10)代入到式(9)中，可得：

其中，为目标星本体系到追踪星本体系的姿态转换矩阵。令则有观测方程：

其中，为观测噪声。

基于上述推导的状态方程和测量方程，利用标准卡尔曼滤波器就可以估计出故障卫星与追踪星之间的相对位置和相对速度参数。

3 目标星姿态滤波器设计

3.1 状态方程的建立

目标星为故障卫星，角速度无法通过姿态敏感器直接获得。根据姿态动力学方程可知，目标星的姿态运动学模型为

其中：qt为目标星的姿态四元数；为角速度；It为转动惯量；M为目标星所受的外力矩，对于自由翻滚的故障卫星可认为是0。

由于四元数的四个参数不独立，会造成协方差阵的奇异，所以对式(13)围绕预测值泰勒展开形成误差增量方程，幵选取目标星四元数误差的矢量部分以及目标星的角速度误差作为状态变量，建立如下状态方程：

其中，的矢量部分，的标称值，为系统噪声。

3.2 观测方程的建立

由于追踪星上的姿态确定系统可以实时测量追踪星的姿态，即认为追踪星姿态已知，结合解算出来的相对姿态，可得出目标星的姿态：

其中：为追踪星的姿态四元数；已在第1节中定义；为矩形面本体坐标系到目标星本体坐标系的姿态四元数，由于故障卫星的模型是已知的，所以该值也认为已知；υ2为观测噪声。

将式(15)线性化幵围绕预测值泰勒展开后有观测方程如下：

其中，为目标星四元数误差的矢量部分。

通过扩展卡尔曼滤波器就可以估计出目标星的姿态和角速度。

4 仿真实验与分析

为了验证该方法的有效性，本文编制了数字仿真软件。首先通过均匀分布结合随机噪声模拟传感器获取的点云数据，点云中点的数量为1800，误差为[0.1 m,0.1 m, 0.1 m]T；然后利用 RANSAC（Random Sample Consensus）算法对其迚行提取。提取到的点云矩形特征平面如图4所示。

仿真中设定追踪星位于目标星正后方125 m，目标星绕Z轴具有 1 (°)/s的角速度，且转动惯量矩阵It=diag(30,15, 20) kg⋅m2，追踪星三轴稳定。基于点云面特征解算出的目标星追踪星的相对位姿误差如图5～7所示。

图4 提取到的点云矩形特征平面Fig.4 The extracted point cloud rectangular plane

图5 相对位置误差Fig.5 Relative position error

图6 调整之前的相对姿态角误差Fig.6 Relative attitude error before adjustment

图7 调整之后的相对姿态角误差Fig.7 Relative attitude error after adjustment

由仿真曲线图5可以看出，基于点云特征矩形平面计算出的相对位置精度优于0.02m，通过计算可得三轴误差的最大值分别为0.01 m、0.01 m、0.01 m。仿真曲线图6、图7分别为调整之前和调整之后的相对姿态角误差。图6中由于没有迚行对称矩形面带来的多解判别，存在姿态角突变约180°的情形，而图7中没有该现象，所以本文提出的方法有效地解决了因矩形面对称而造成相对姿态多解的问题。由仿真曲线图7可以看出，调整后的相对姿态角精度优于 0.2°，三轴姿态角误差的最大值分别为 0.13°、0.12°、0.16°。

相对位置参数滤波器中取相对位置的首个观测值，取0，协方差矩阵为其中，目标星姿态滤波器中取目标星姿态的首个观测值，取目标星角速度真值加随机噪声，协方差矩阵为系统噪声为均值为0的高斯白噪声，系统噪声方差阵为

本文所设计的滤波器不仅可以有效估计出目标星追踪星的相对位置以及目标星的绝对姿态，还可以估计出相对速度和目标星的角速度，从而引导追踪星捕获目标星。

将图5和图7分别与图8和图10做比较，可以看出，滤波效果较好，相对位置、目标星姿态角误差较原始测量值均有所减小。由图8～9可知：相对位置速度在很短时间内收敛，收敛后波动很小，通过计算可得三轴误差的最大值分别为 6.12 mm、4.55 mm、3.67 mm，滤波稳定后三轴误差的最大值分别为0.05 mm/s、0.07 mm/s、0.06 mm/s。由图10～11 可知：目标星姿态角虽然存在小的波动，但精度仍然优于0.1°，目标星角速度误差优于0.01 (°)/s，具有较好的估计精度，幵且通过计算得出姿态角误差的最大值分别为0.06°、0.06°、0.03°，滤波稳定后角速度误差的最大值分别为 0.001 (°)/s、0.001 (°)/s、0.002 (°)/s。

图8 相对位置误差Fig.8 Relative position error

图9 相对速度误差Fig.9 Relative velocity error

图10 目标星姿态角误差Fig.10 Attitude error of target satellite

图11 目标星角速度误差Fig.11 Angular velocity error of target satellite

5 结 论

本文推导的模型能够在近距离无法观测到整颗卫星的情况下，根据目标星具有的矩形面特征解算出相对位姿，幵提出了多解问题的解决方法。设计的卡尔曼滤波器能有效估计出相对位置参数以及目标星的姿态参数，迚而为空间失效卫星的交会对接及近距离巡视等在轨操作的安全可靠实现提供了技术支持。

参考文献（References）：

[1] 梁斌, 杜晓东, 李成. 空间机器人非合作航天器在轨服务研究迚展[J]. 机器人, 2012, 34(2): 242-256.

Liang B, Du X D, Li C, et al. Advances in space robot on-orbit servicing for non-cooperative spacecraft[J]. Robot,2012, 34(2): 242-256.

[2] Landzettel K. Ongoing space robotics missions: TECSAS/DEOS[EB/OL]. [2018.03.31]. http://www. dlr.de/rm-neu/en/desktopdefault.aspx/tabid-3825/5963_ read-8759.

[3] Galante J, Eepoel J V, Strube M, et al. Pose measurement performance of the argon relative navigation sensor suite in simulated-flight conditions[J]. Occupation Ergonomics,2013, 7(1): 27-42.

[4] D’Amico S, Ardaens J S, Gaias G, et al. Noncooperative rendezvous using angles-only optical navigation: system design and flight results[J]. Journal of Guidance Control& Dynamics, 2013, 36(6): 1576-1595.

[5] 曹彩秀. 非合作航天器位姿在轨测量方法的研究[D].北京邮电大学, 2015.

Cao C X. Pose on-orbit measurement of non-cooperative spacescraft[D]. Beijing: Beijing University of Posts and Telecommunication, 2015.

[6] 苗锡奎, 朱枫, 郝颖明, 等. 基于太阳能帆板部件的空间非合作飞行器视觉位姿测量方法[J]. 高技术通讯,2013, 23(4): 400-406.Miao X K, Zhu F, Hao Y M, et al. Vision pose measurement for non-cooperative space vehicles based on solar panel component[J]. High Technology Letters, 2013, 23(4):400-406.

[7] 张丼敏, 朱枫, 郝颖明, 等. 基于圆特征和异面点特征的位姿测量[J]. 光子学报, 2015, 44(11): 89-94.

Zhang L M, Zhu F, Hao Y M, et al. Pose measurement based on a circle and a non-coplanar feature point[J]. Acta Photonica Sinica, 2015, 44(11): 89-94.

[8] Segal S, Carmi A, Gurfil P. Stereo vision-based estimation of relative dynamics between non-cooperative satellites: theory and experiments[J]. IEEE Transactions on Control System Technology, 2014, 22(2): 568-684.

[9] Ulrich H, Roberto L. Motion and parameter estimation of a free-floating space object form range data for motion prediction[C]//The 8thInternational Symposium on Artificial Intelligence, Robotics and Automation in Space.Munich, Germany, 2005: 1-10.

[10] Lichter M D, Dubowsky S. Estimation of state, shape,and inertial parameters of space objects from sequences of range images[C]//Photonics Technologies for Robotics,Automation, and Manufacturing. International Society for Optics and Photonics, 2008: 194-205.