点击下方卡片，关注「3D视觉工坊」公众号
选择星标，干货第一时间送达
来源：3D视觉工坊
星球内有20多门3D视觉系统课程、3DGS独家系列视频教程、顶会论文最新解读、海量3D视觉行业源码、项目承接、求职招聘等。想要入门3D视觉、做项目、搞科研，欢迎加入！
传统的小型飞行器舱段对接定位与对接方法通常依赖人工操作，需要多名操作人员协同完成对准与对接过程，导致其在对接与装配精度方面存在不足。基于视觉的小型飞行器舱段位姿估计系统在成本效益、数据维度丰富性及测量效率方面表现出显著优势。然而，仅依赖相机实现舱段完整的相对位姿估计仍具有较大挑战，其往往需要额外的辅助测量装置，或仅能获取部分参数。本研究提出了一种面向小型飞行器舱段对接的三维重建与位姿估计方法，将单目结构光相机与机械臂结合，通过点云数据处理、舱段位姿的迭代估计以及六自由度平台位姿求解三个阶段，实现对舱段的全面位姿估计。定义了小型飞行器舱段关键定位特征，使用随机抽样一致（RANSAC）方法提取特征，得到关键定位参数。通过对关键定位参数教学解析得到舱段相对位姿，将舱段间的相对位姿转换为平台的控制输入参数，从而实现对接控制。在真实小型飞行器舱段对接实验中验证了该方法的可行性，结果表明本文方法适用于不同尺寸小型飞行器舱段的对接场景，具有良好的通用性与实用性。
0 引言
在航天领域，舱段对接操作通常需要专用的定位与对接设备。传统的定位与对接方法多依赖人工操控机械装置，需多名操作人员协同完成对准及对接过程，存在对接精度和装配质量不足等问题。自动化对接技术可有效提升对接的精度与质量[2]，并已被广泛应用。精准测量舱段的空间位置与姿态是实现其自动化对接的关键环节，目前主流的测量方法包括接触式[3-4]与非接触式[5-7]两类。接触式测量方法借助探针或放置的靶球获取被测物体表面多个点的空间坐标，并通过数值拟合算法推算出其空间位姿。然而，该方法需与物体发生物理接触，可能引起姿态偏移，因而在高精度测量场景中应用受限。
非接触式测量技术则利用光学传感设备，在无需物理接触的条件下完成测量，已被广泛应用于舱段位姿测量中。Qiao等[8]采用四台激光跟踪仪实现了舱段位姿的实时非接触测量，成功完成对接。与激光跟踪仪相比，基于视觉的测量系统在成本、数据维度多样性和测量效率方面具有明显优势，因此，基于视觉的舱段位姿估计方法已成为研究热点。Wang等[9]利用结构光相机获取舱段点云数据，并结合迭代最近点（Iterative Closest Point，ICP）算法与随机抽样一致（Random Sample Consensus，RANSAC）方法实现相对姿态估计。然而，该方法忽略了关键平移参数和轴向旋转测量，在全面性上存在局限。
由于舱段结构具有轴对称特性，其相对滚转角的测量通常需依赖对接界面的特征参照[10]。Ma等[11]在实现舱段同轴对准的基础上，针对滚转角对准问题，采用两台单目相机对称布置，用于捕捉角度参考。Xu等[12]设计了一种由单目相机与激光跟踪仪组成的测量系统，通过激光跟踪仪识别凹槽特征来测量滚转角，利用单目相机识别对接界面上的标志点来测量相对位置、俯仰角与偏航角。这些方法均无法仅依靠相机实现完整的相对位姿估计，需辅以额外测量设备，或只能获取部分姿态参数。此外，视觉系统本身的工作范围受限，也使其难以适用于不同尺寸舱段的对接需求。
为解决舱段位姿测量问题，本文提出了一种基于结构光相机的三维重建与舱段位姿估计方法。该方法可直接计算舱段的完整位姿，具有非接触、高效率和鲁棒性强等优点，同时避免了固定视角相机所带来的视野受限问题，可适用于不同尺寸舱段的对接过程。
本文的主要工作包括以下几个方面：设计了一套用于舱段对接任务的测量与对接系统，包括六轴机械臂、六自由度平台、结构光相机及单自由度滑轨；提出了一种基于结构光相机的三维重建与舱段位姿估计方法，可仅依赖一台结构光相机实现舱段完整位姿的直接估计；通过实验验证了所提出系统与方法的可行性，实验结果表明该方法适用于不同尺寸的舱段，具有良好的适应性与实用性。
1 测量和对接系统介绍
本研究设计的系统如图1所示，主要包括一个六自由度平台、一条单自由度滑轨、一台六轴机械臂以及一台结构光相机。两个舱段分别固定于单自由度滑轨与六自由度平台上。
本文涉及的坐标系包括：相机坐标系、机械臂基座坐标系以及六自由度平台控制坐标系，测量和对接系统结构如（图1位置）所示。本文将机械臂基座坐标系定义为世界坐标系。
本文提出的算法结构如图2所示，整体分为三个部分。第一部分为点云处理：利用结构光相机采集舱段的局部点云数据，经过重建后获得舱段的全局点云信息。第二部分为舱段位姿估计：从全局点云中提取关键特征，用于计算两个舱段之间的相对位姿关系。第三部分为六自由度平台位姿求解：通过建立数学关系，将舱段间的相对位姿转换为平台的控制输入参数，从而实现对接控制。
2 三维重建与位姿估计方法
2.1 点云处理方法
本系统采用结构光相机进行点云采集。通过控制机械臂将相机移动至多个不同位置，获取舱段的多个局部点云数据。在完成局部点云采集后，采用重建方法对舱段进行重建。局部点云需从相机坐标系转换至机械臂基座坐标系，转换过程采用如下形式的齐次变换矩阵完成：
式中： 为在世界坐标系中的点坐标； 为在相机坐标系中的点坐标； 为从机械臂末端坐标系到世界坐标系的变换矩阵，可通过读取机械臂末端位姿信息获得； 为从相机坐标系到机械臂末端坐标系的变换矩阵，可通过相机外参标定获得。
2.2 舱段位姿估计方法
基于舱段的全局点云，计算两个舱段之间的相对位置与姿态关系。在本节中，从全局点云中提取关键特征，并利用这些特征的参数信息计算舱段间的相对位姿。
2.2.1 关键特征识别
在对接过程中，本文将舱段的关键特征定义为：轴线、法兰面以及定位销（或孔）的轴线。为提取这些关键特征的位置，采用RANSAC算法进行拟合。
在所提出的算法中，利用RANSAC算法提取以下几何特征：两条轴线的方向向量 和 、两个法兰面的中心位置 和 、定位销位置 和定位孔位置 。小型飞行器舱段几何特征如 图3 所示。
RANSAC算法通过预先确定的特征模型，使用随机抽取的最小样本集（能唯一确定模型参数所需的最少数据点）拟合出特征模型候选参数，将整个数据集代入模型验证候选参数，在多次迭代后选取验证效果最好（整个数据集中符合模型的样本最多）的模型候选参数作为拟合结果。在本文中，用于确定小型飞行器舱段相对位置的特征模型包括空间圆柱模型与空间圆模型，它们分别表达如下：
式中： 和 是特征模型参数，分别代表圆柱轴线上的任一点坐标、圆柱轴线方向和圆柱半径； 和 是特征模型参数，分别代表空间圆圆心和空间圆所在平面法向量。
通过在RANSAC方法中使用空间圆柱模型，可以拟合得到两条轴线的方向向量 和 ；通过在RANSAC方法中使用空间圆模型，可以拟合得到两个法兰面的中心位置 和 、定位销位置 和定位孔位置 。
2.2.2 位置计算
为了生成六自由度平台的控制输入，需要计算两个舱段之间的相对位置与姿态。本文将每个舱段的距离参考点定义为其轴线与法兰面的交点。舱段之间的距离通过两个参考点的位置差计算获得：
对于舱段间的相对姿态，首先计算舱段轴线之间的相对姿态关系，在轴线对齐后确定相对姿态。舱段轴线的相对方向可通过它们之间的旋转关系获得，旋转轴 垂直于两个舱段轴线的方向，其可通过如下方式计算：
式中： 为六自由度平台一侧舱段的轴线方向； 为另一侧舱段的轴线方向。
在绕轴线 旋转的方向上，为使两个舱段的轴线共线，所需的旋转角度 即为两个方向向量 与 之间的夹角，该角度可按如下方式计算：
计算出旋转轴和旋转角度后，可通过罗德里格斯旋转公式求得对应的旋转矩阵，具体方程如下：
式中： 为六自由度平台一侧的舱段到另一侧舱段的旋转矩阵； 为根据旋转轴构造的反对称矩阵； 和 分别为旋转轴 在 和 方向上的分量。
为了确定舱段轴线对齐后的相对姿态，本文将舱段的初始姿态定义为：从定位销（或孔）参考点到对应舱段轴线的最短有向线段的方向。该初始姿态可按如下方式计算获得：
式中： 和 分别为从定位销（或孔）参考点到其对应舱段轴线的最短有向线段的方向。
对于第二次旋转，其旋转轴 为第一次旋转后舱段轴线的方向，旋转角度 为方向向量 与 之间的夹角，计算方式如下：
计算出旋转轴和旋转角度后，可利用罗德里格斯旋转公式求得对应的旋转矩阵，具体表达式如下：
式中： 为第二旋转矩阵； 为根据旋转轴构造的反对称矩阵； 和 分别为旋转轴 在 和 方向上的分量。
由此，两舱段之间的旋转矩阵可以表示为如下形式：
2.3 舱段对接六自由度平台位姿确定
六自由度平台的控制输入是在其控制坐标系下定义的。因此，在世界坐标系中计算得到的旋转矩阵与相对位移无法直接用于平台控制，必须通过坐标变换转换到平台的控制坐标系中。
假设在某一运动后六自由度平台当前坐标系相对于其控制坐标系的姿态变换矩阵为 ，则根据矢量关系，可得如下转换公式：
式中： 为当前控制输入为零时六自由度平台控制坐标系相对于世界坐标系的旋转矩阵； 为 在六自由度平台控制坐标系下的表达形式。
结合式（16）~式（18），可以得到世界坐标系下的旋转矩阵与六自由度平台控制坐标系下旋转矩阵之间的变换关系：
根据旋转矩阵和姿态角的定义，平台的角度控制输入依次按照 姿态角顺序计算，表达式如下：
式中： 和 为控制坐标系下 轴、 轴和 轴的旋转角； 为控制旋转矩阵 在第 行第 列的元素值。
对于位置控制输入，为保证控制后能够完成对接，可得到如下计算公式：
式中： 为 在六自由度平台控制坐标系下的表达形式。
结合式（23）和式（24），位置输入 可以用下式表示：
3 实验验证
为验证所提出方法的有效性，本文设计并开展了相关实验，利用该方法实现舱段对接。实验所用测量和对接硬件系统如图4所示，包括一个六自由度平台、一条单自由度可移动导轨、一台六轴机械臂以及一台结构光相机。两个舱段分别固定在单自由度导轨和六自由度平台上。
3.1 舱段三维重建
采用迁移科技的EpicEyeS-MAir型结构光相机来获取舱段的局部点云，相机关键参数见表1。
在完成局部点云采集后，对点云进行配准处理。随后对两个舱段的点云进行关键特征提取实验。三维重建与关键特征识别结果如图5所示，展示了舱段的外壁点云、法兰面点云、定位销以及定位孔等关键结构特征。
特征提取误差评估在仿真中进行，通过计算特征提取结果与真值的偏差得到，其中轴线特征的误差定义为特征提取得到的轴线方向与真实方向的角度偏差，法兰面中心位置和定位销孔位置的误差定义为特征提取得到的位置与真实位置的距离偏差。通过仿真验证，特征提取的轴线方向误差为 ，位置误差为 。
3.2 舱段对接实验
利用提取的特征参数，结合所提出的方法计算出保证两舱段顺利对接所需的六自由度平台控制参数，并将其输入至六自由度平台进行控制。对接实验结果如图6所示。
实验中对对接误差进行了分别评估，对接误差主要由位姿测量误差来决定。位姿测量误差利用本文方法测量结果与三维扫描仪精准测量结果的偏差得到，其值为 。位姿测量误差产生的主要原因是：使用的结构光相机自身在单点采集上具有 的位置误差，这导致采集的点云在特征模型验证阶段使用的阈值不能小于 ，从而降低了特征参数的精度，最终使得计算的相对位姿与实际相比具有一定偏差。
实验在不同初始姿态角条件下进行（滚转角： ，俯仰角： ，偏航角： ），在所有实验条件下均实现了舱段的成功对准与对接。实验结果验证了本文方法在舱段对接中的有效性和实用性。
4 结论
仅依靠相机实现舱段间的完整相对位姿估计具有较大的挑战，通常需辅以其他测量装置，或只能获得部分位姿参数。为此，本文提出了一种基于三维重建的舱段对接位姿估计方法，仅依赖单台相机与机械臂即可实现舱段全面位姿的估算。
该方法通过获取舱段点云，实现三维重建，随后提取舱段关键特征位置，最终完成相对位姿的计算。实验结果验证了所提方法的可行性，该方法适用于不同尺寸的舱段对接任务。
本文仅做学术分享，如有侵权，请联系删文。
。
添加微信：cv3d001，备注：姓名+方向+单位，邀请入群。
