0 引言
空间机械臂完成一系列具有复杂接触特征的精细作业，是在轨服务和维护的重要技术基础[1]。电连接器是航天装备的基础元件，需要完成在轨装配、拆卸、检测和维修等基本操作。航天常用的电连接器种类较多（矩形或圆形等），每种类型下不同规格系列电连接器的插拔操作差异大，给装备的更换与维修带来较大难度。此外，受制于苛刻的空间环境以及舱外航天员动作与视野约束，人工完成电连接器的装配面临较大风险，而在轨无人飞行器的日常维护也需要实现自动化、智能化的电连接器装拆操作。航天用电连接器的接入过程可分为对中、找平、插入、旋转、按压等基本动作，基于机器视觉和力控关键技术，采用机械臂对上述动作进行模拟和训练，可实现电连接器的自动物理接入。
1 国内外研究现状
目前空间机器人的在轨作业，在物件搬运、目标抓取和模块拆卸等操作方面已日趋成熟，譬如：加拿大开发的专用灵巧机械臂（Special Purpose Dexterous Manipulator，SPDM），主要用于搬运或装卸小型载荷等任务[2]；德宇航开发的机器人技术验证系统（space RObot Technology EXperiment，ROTEX），用于规则形状物体的抓取和插拔等试验任务[3]；2020年美国国家航空宇航局（National Aeronautics and Space Administration，NASA）研发的燃料加注空间机器人，成功实现了剪断电缆、拧开阀门、对接和插拔软管等操作任务[4]。NASA也计划于2024年通过在轨应用微创机器人（MIniaturized in vivo Robotic Assistant，MIRA），实现模拟切割人体皮肤的太空微创手术[5]。尽管国外已开展很多空间机器人在轨作业研究，但针对复杂接触特征的航天用电连接器等非结构化目标的在轨精细装拆操作尚未有相关报道。
轴孔装配搜索与控制策略方面，考虑到电连接器接入过程中需要一定的操作力进行旋拧，目前大多采用力控传感器间接对接入的操作力进行检测，通常采用柔顺（导纳）或阻抗模型对机械臂进行控制[6]。Ortner等[7]提出了一种机械臂轴孔对齐盲搜索策略，通过阿基米德曲线搜索实现了多级电插头和插座的柔顺化装配；Bdiwi等[8]提出了一种基于螺旋线的盲搜索策略，通过力和位置补偿策略实现充电插头的盲搜孔与插拔作业。Jasim等[9]提出了采用高斯混合模型进行期望最大化建模，并与螺旋搜孔路径相结合，构建出识别装配过程中孔位置的策略。Vecerik等[10]采用基于神经网络和深度强化学习的自适应控制方法，实现了两极电插头和插座的柔顺轴孔对齐。David等[11]提出了融合视觉粗定位和触觉精细操作的策略，采用力-位控制对齐，实现了针对不规则形状的轴孔类插拔任务。目前轴孔柔顺装配的研究大多集中在简单几何形状（圆形、矩形等）且间隙配合下的接触装配，复杂几何形状的柔顺装配研究较少。
视觉点云匹配算法方面，夏坎强[12]提出了采用双目结构光相机进行点云信息的初始配准和精配准策略，其中初始配准将位置差距较大的两片点云对齐到大致正确的位置，精细配准使两片点云的空间位置差距最小化。任镤等[13]从点集选择、对应点选择、点对加权、错误点对去除、误差度量与最小化等不同角度对传统的迭代邻近点算法（Iterative Closest Point，ICP）进行了改进。赵明富等[14]提出了一种融合采样一致性算法和ICP算法的方法，用于提取原始点云数据的快速点特征直方图（Fast Point Features Histograms，FPFH）特征。该方法使用采样一致性初始配准算法（SAmple Consensus Initial Alignment，SAC-IA）确定对应点对关系，并找到较好的初始配准位置。张崇军等[15]提出了一种改进权重的加权ICP算法，该算法根据每个点对的配准结果计算相应的初始权重，应用选权迭代法获得满足条件的权重，有效抵御了粗差点。王庆闪等[16]提出了基于正态分布变换（Normal Distributions Transform，NDT）与ICP结合的配准算法，对点云数据预处理后，利用NDT算法对相邻两帧点云数据进行配准。苏本跃等[17]使用颜色几何特征与最近点结合形成特征描述子，以此进行特征匹配关系的迭代和变换矩阵的计算，实现了更高精度的点云配准。上述算法虽然对ICP算法进行了改进，但仍存在对应点对选择不佳、配准时间较长和精度较低的问题。
2 电连接器自动物理接入
以灵巧作业为代表的空间机器人在轨操作一般分为抓握、插拔、旋拧、剪切四大类，而旋拧装配电连接器、紧固螺钉等多点多域接触的无标识空间目标是集导向、定位、压紧、锁定、拧紧、解锁等动作序列为一体的最复杂空间作业。
以航天用电连接器等轴孔类装配为例（见图1 ），相比于地面条件，在轨操作任务受到光照多变、空间狭小、视线遮挡、接触受限等约束，空间机器人面临目标识别能力低、运动范围窄、控制精度差等困难，传统遥操作的工作模式难以有效应对，需要研究基于接触搜索和视觉模板匹配的电连接器创新装配方法。
针对视场受限（狭小空间约束）、障碍遮挡（如电缆线）等在轨复杂环境下多点耦合接触电连接器的精细装配难题，研究团队前期创新提出了基于力/力矩不等式约束的任务优先级控制策略，引入力/扭矩-位置混合阻抗控制算法，创建了“两点接触−柔性滑动−姿态校正−对准插入”的融合接触搜索和力柔顺控制的圆形电连接器自适应装配方法[18-20]。对机器人在轨使用的J599电连接器进行了41次地面试验验证，装配任务成功率达到93%以上，平均成功装配时间为101 s，验证了控制策略和装配方法的有效性，为空间机器人的精细操作奠定了理论和技术基础。尽管该类型方法在自主装配电连接器方面取得了较大进展，但对机械臂视觉传感器的利用有限，仅用于装配过程的监测。随着RGB-D相机的像素分辨率极大提升（像素分辨率达到0.02 mm），基于深度点云信息并采用模板匹配方法进行电连接器插头和插座的精确定位，能够大大提高装配效率，已成为电连接器自动物理接入技术的新发展趋势。本文在地面条件下进行了测试，考虑了机械臂末端的六维力/力矩传感器数据补偿、采用研发的通用末端工具稳定夹持电连接器（避免尾部电缆线束干涉），并主要针对相对复杂的装配环节（拆卸环节相对容易）进行地面模拟试验，以验证点云模板匹配方法实现精确定位的有效性。
在对点云进行滤波降噪和初步的区域分割处理后，采用基于尺度不变特征变换（Scale Invariant Feature Transform，SIFT）特征点提取的ICP点云配准方法（Source-Target Iterative Closest Point，ST-ICP）。在进行ICP配准前，先对原始点云进行SIFT特征点提取，获取FPFH特征后，通过前述SAC-IA算法搜索对应点对进行初始配准，解决因特征点对选择不佳而影响配准精度的问题，并缩短配准时间、提高配准效率，其主要算法流程如图2 所示。
2.1 FPFH特征描述子算法
FPFH特征提取是一种快速且有效的三维点云特征描述方法，用于捕捉每个点的局部几何特征，并生成用于点云配准的特征描述子。在对所有点云进行法线估计后，得到每个点云与其对应的法向量数据。而后对于每个关键点，FPFH首先考虑计算简化的点特征直方图（Simple Point Feature Histograms，SPFH）并考虑关键点的直接邻居。然后将更远邻居的SPFH加权平均，最终形成FPFH。
（1）计算简化点特征直方图（SPFH）。每个查询点的SPFH是基于其直接邻域点的这些三元组$ (\alpha ,\varphi ,\theta ) $ 计算得到的，计算方法见式（1）~式（3）。
式中，$ \mathop {\boldsymbol{n}}\nolimits_p $ 是点$ p $ 的法向量；$ {p_i} $ 是点$ p $ 的邻域点；$ {{\boldsymbol{n}}_i} $ 是邻域点$ {p_i} $ 的法向量。
这些角度特征用于描述点$ p $ 与其邻域点之间的几何关系。将每个角度特征分成若干区间（例如11个区间），生成直方图，即
（2）加权平均SPFH得到FPFH特征直方图：对于每个查询点$ p $ ，将其直接邻域点的SPFH进行加权平均，生成最终的FPFH特征，如式（5）所示。
式中，$ k $ 是邻域点数量；$ {w_i} $ 是权重，通常为邻域点到查询点的距离的倒数。
在FPFH算法的设计和实验中，11个区间被证明在大多数情况下能够有效地描述点云的局部几何特征。FPFH特征提取算法将每个特征值分成11份，形成一个33维的直方图。因此FPFH特征向量是一个33维的向量，用于描述点云中每个点的几何特征信息。
2.2 RANSAC粗配准
随机采样一致 （RANdom SAmple Consensus，RANSAC）是从一组包含噪声的观测数据中拟合几何模型的迭代算法。通过随机采样和模型验证，找到最优模型参数，使模型能够最大程度地解释数据中的点。RANSAC粗配准在点云配准中的主要目标是通过匹配两个点云中的特征点，估计初始的刚体变换矩阵。
（1）匹配点对并采样。依据之前得到的描述子，用KD树（K-Dimensional tree，KD）检索找到模板点云与场景点云的对应点对。KD树是一种空间分割的数据结构，用于在多维空间中进行高效的最近邻搜索。在模板点云中随机抽取若干点云，并且找到场景点云中与之对应的点集。
（2）估计模型参数。使用选取的特征点对，估计点云之间的刚体变换矩阵。
（3）验证模型。将估计的变换矩阵应用于源点云中的所有点，计算变换后的点与目标点云的匹配度。计算符合该变换矩阵的内点数量，即变换后与目标点云中最近邻点距离小于某个阈值的点的数量。
（4）迭代。重复进行随机采样与步骤（2）~（3），直到最大迭代次数，并在其中选出内点数量最多的模型作为输出。
2.3 ICP精配准
ICP算法通过迭代的方式，找到源点云和目标点云之间的最优刚体变换矩阵。通过不断调整源点云的位置，使其与目标点云的对应点对之间的距离最小化。
（1）将由RANSAC粗配准算法得到的初始变换矩阵，对模板点云进行变换，将模板点云与场景中对应的部分初步对齐。
（2）对模板点云中的每个点，通过距离计算找到目标点集中与其最相近的点，以此形成新的对应点对。
（3）采用奇异值分解法，基于对应点对再计算刚体的变化矩阵。
（4）迭代：应用变换矩阵后计算总体均方根误差（Root Mean Squared Error，RMSE）以衡量对准程度，若未达到最大阈值则在当前位姿重复步骤（2）~（3），直到满足阈值或达到最大迭代次数终止并输出变换矩阵，可输出均方根误差RMSE作为评分标准，如式（6）所示。
2.4 算法验证
如图3 所示，输入模板点云与场景点云，在场景点云中找到与模板点云相匹配的点云位置进而找到插座的位姿信息。经过预处理后，在模板点云中构建直角坐标系，去除场景点云的环境杂点。在此基础上进行法线计算、特征提取等操作后，两点云的形状特征已被描述为数字信息，依赖以上信息再进行RANSAC粗配准与ICP精配准后可得到场景点云以及被对齐的模板点云（绿色），模板点云中的坐标也被一同移动，最终输出场景中目标插座口的位姿信息。
3 电连接器的自动接入测试
电连接器自动物理接入硬件系统安装如图4 所示。为了提高测试系统的适应性，先后测试了J599、Y27、J36A、J30和Y2等类型的电连接器，开发了专用末端夹板和电连接器安装转换器等。考虑到自动接入系统中涉及多个硬件和软件模块，且彼此之间存在较强的关联关系，如图5 所示，因此需要分别对每个模块进行测试和验证，以确保整个系统运行的准确性和可靠性。
Y2-50TK/ZJ作为常规使用的圆形电连接器，其接入特点相比J30J-31TJ/ZK和J30J-15TJ/ZK等矩形电连接器更复杂，主要体现在，接入过程需要3个关键步骤，即插头与插座对齐、小槽对齐、螺旋旋入，需要实现两次抓取。本文采用高精度相机完成对齐工作，然后采用力控传感器控制小槽对齐，最后采用机械臂二次抓取实现旋入。接入过程如图6 所示，力控传感器的数据如图7 所示。
4 结语
本文基于电连接器的自动接入需求，构建了基于视觉和力控信息的电连接器自动接入平台，建立了相机标定、手眼标定、软硬件连接、轴孔搜索和对中控制算法，构建了基于位置的机械臂末端柔顺控制算法，实现了Y2-50TK/ZJ、J30J-31TJ/ZK和J30J-15TJ/ZK等圆形和方形电连接器的自动物理接入，为其他系列的电连接器的自动物理接入奠定了较好的研究基础。其中，基于视觉的识别和精确定位方法可以泛化应用到其他类型的电连接器；大孔搜索和孔内调整可根据不同电连接器的接入需求进行微调，孔内插入过程依据不同接触方式可以采用本项目柔顺控制算法实现。