并联机构实现舱段对接的位姿测量方法及试验研究

2021-06-10李世其王峻峰

工程科学与技术 2021年3期

李世其，陈栋，王峻峰

(华中科技大学机械科学与工程学院，湖北武汉 430074)

舱段总装对接是确保航空航天产品制造准确性与质量一致性的关键环节。传统串联对接技术存在耗时耗力及对接质量一致性差的缺点。因此，研究高质高效舱段自动对接技术已成为航空航天制造领域的发展趋势。

随着航空航天产品种类的多样化，舱段类部件在外形尺寸及结构特征等方面产生了较大差异。通常需要设计具有不同构型特征与对接形式的装置，并研究相应的对接方法实现不同产品的舱段对接。例如：Mei等[1]介绍了应用于飞机大部件装配的多种柔性装配夹具及对接方法。其中，在飞机机身对接或翼身对接中，通常采用多个POGO柱相结合的支撑装置或专用大型型架来实现部件调姿及定位。郭志敏等[2]设计了精密三坐标POGO柱用于飞机大部件调姿，并提出三点支撑的姿态调整方法。Zhu等[3]设计了飞机翼身对接6自由度POGO型调姿机构，提出了一种基于四元数的5次多项式轨迹规划算法。在体型相对较小的舱段类部件装配中，机器人以灵活、精确和易于控制等优点受到了广泛研究与应用，多数自动对接方法都以位姿测量及位姿拟合技术为基础[4]。Jayaweera等[5]提出了对测量数据进行最优拟合的最佳装配算法，利用测量辅助装配技术指导机器人实现对航空发动机部件的装配。邹冀华等[6]提出了3–RPS并联机构的法向调整算法，控制该机构进行飞机翼身壁板蒙皮法向钻铆。金贺荣等[7–8]研究了双目视觉测量技术，由双目相机对舱段表面喷涂靶点进行测量匹配，通过矢量位姿法求解舱段自动装配位姿；此外，还提出了两点定位调姿法，通过改变对接舱段上两个关键点的位置来引导串联对接机构调整舱段的空间位姿。Liu等[9]设计了一种由距离传感器和CCD相机组成的位姿测量系统，指导由多个3自由度定位器构成的6自由度定位系统对AGV车辆调姿[9]。陈冠宇等[10]提出了多传感器协同测量的舱段位姿估计和调整方法，通过激光轮廓传感器扫描舱体获取位姿3维点云信息，通过CCD图像传感器获取舱段对接孔位置，再将测量与拟合结果反馈给控制系统进行舱段调姿对接。Wen等[11]在舱段对接研究中，采用基于统一坐标换算思想的位姿测量方法，首先建立舱段对接的全局坐标系，再通过特征点测量等方法将舱段、对接机构等局部坐标系换算到全局坐标系下。温晶晶等[12]研究了Stewart平台实现导弹舱段自动对接的方法，利用激光跟踪仪测量舱段上多个特征点，采用模式搜索法进行特征点集匹配，以获取舱段之间的位姿偏差；再通过测量Stewart平台上的特征点，实现平台坐标系与测量坐标系的统一；最后，根据位姿偏差控制平台实现舱段调姿。在以上舱段对接技术的位姿测量方法研究中，通常需要在舱段或对接机构上选取事先加工的特征点或特殊位置点，由3维模型获取这些特征点在局部坐标系下的坐标后，才能进一步实现坐标系统及位姿换算。这类方法对舱段或对接机构的加工工艺及制造精度，甚至对测量时采点的准确性及采点的测量精度等提出了较高的要求。

作者提出了一种通用的并联机构实现舱段对接的位姿测量及换算方法。该方法不需要通过更改产品现有工艺来额外加工测量特征点，也摆脱了对舱段与并联机构3维模型的依赖，仅利用和测量实物对象的已有特征即可构造对象各种相对位姿关系，并借助位姿传递转化关系有效获取并联机构实现舱段对接的运动目标位姿。

1 舱段对接的位姿测量问题

舱段对接中，通常将体积或质量较大的舱段固定在架车上作为基准舱段，称为固定舱段；将通过支架或其他方式固定在对接设备上的待对接舱段，称为移动舱段。作者设计了6自由度并联机构作为舱段对接机构，以连接动平台铰点连线所构成的几何中心为原点建立 {M}，以移动舱段和固定舱段端面中心点为原点分别建立{D}与 {F}。舱段对接前后的相对位姿关系变化如图1所示。

图1中，虚线代表对接前移动舱段{D}和并联机构{M} 所处的初始位姿，实线代表对接后{D}和 {M}的目标位姿。在目标位姿处，移动舱段与固定舱段实现对接，即{D}与 {F}在理论上实现重合。

当移动舱段借助于某种支撑装置与并联机构固连后，两者存在未知的相对位姿关系，记{D}与 {M}的相对位姿为 [R,T]，理论上该相对位姿量在移动舱段调姿过程中保持不变；{F}与 {D}之间也存在未知的相对位姿关系，记{F}与 {D}的相对位姿[R,T]，理论上该相对位姿量在移动舱段调姿过程中会逐渐趋于0。

图1 舱段对接中的相对位姿关系Fig. 1 Relative pose relations in cabin docking

2 舱段对接系统的结构组成

舱段对接系统主要包括位姿测量子系统、对接执行子系统和运动控制子系统，各子系统信息传递与配合关系如图2所示。

1）位姿测量子系统采用的测量设备为FARO–Vantage激光跟踪仪及直径为2.222 5 cm（0.875英寸）的光学靶球，配套的测量数据处理软件为CAM2 measure10。FARO–Vantage最新标定的测距精度为（8.0±0.4） µm/m，测角精度为（10.0±2.5） µm/m。

图2 舱段对接系统的结构组成Fig. 2 Composition of cabin docking system

2）对接执行子系统采用的对接设备为6–UPU并联机构，主要由动平台、静平台及通过虎克铰分别连接动、静平台的6个折返式电动缸组成。定义6个上铰点连线的几何中心为动平台 {M}的原点，沿导轨运动方向为x轴正向，竖直向上为z轴正向，由右手定则确定y轴方向。从零位起，末端（动平台）在x、y、z轴方向上各具有±100 mm的行程，在绕x、y、z轴方向上各具有±10°的转动范围。标定后，并联机构的平移重复定位误差小于0.1 mm，转动重复定位误差小于0.05°，满足舱段对接的精度要求。

3）运动控制子系统的硬件包括控制柜和计算机等；软件集成了位姿拟合、调姿规划、运动控制等核心算法。主要处理位姿测量子系统发送的测量数据，并向对接执行子系统发送调姿规划及运动控制等指令。

3 相对位姿关系的测量及构造

以z–y–x方式描述欧拉角 [ α,β,γ]T的旋转次序，旋转矩阵R可记为：

当已知R时，可以反求出按照z–y–x方式旋转的欧拉角分别为：

式中，Rij表示旋转矩阵R中第i行、第j列的元素，i,j≤3。

3.1 移动舱段与激光跟踪仪坐标关系匹配

理论上，在移动舱段上选取一定数量的特征点，并利用激光跟踪仪测量，即可建立特征点集关于移动舱段与激光跟踪仪的坐标系匹配关系，前提是根据3维模型确定各特征点在移动舱段坐标系下的局部坐标。这种借助数模关系匹配坐标系的方法通常需要事先在测量对象的3维模型上设计特征点，并确保实物对象上的特征点加工精度。实际上，在一些航天产品生产过程中，因产品性能及工艺条件限制而不具备加工测量特征点的条件，导致数模关系匹配坐标系的方法不可行。因此，采用特征测量的方法实现移动舱段与激光跟踪仪的坐标系匹配。

初始状态下，激光跟踪仪坐标测量的虚拟坐标系{L}位于系统默认位置。首先，手持光学靶球在移动舱段的内侧圆柱面上轻轻滑动，通过激光跟踪仪采点测量，采点范围超过内圆柱面截面圆周长的一半，采点数量不少于100个；在剔除粗差点后，利用其余测点拟合出内侧圆柱面。然后，手持光学靶球在移动舱段的对接端面光滑处采点不少于6个，通过激光跟踪仪测量，同理拟合出对接端面；内圆柱面与对接端面交线中心点为{D}的原点Od，在CAM2 measure10中，设定{L}原点Ol与Od重合。最后，通过测量移动舱段的定位孔坐标，定义Od与定位孔连线矢量为{D}的z轴，定义对接端面法矢量方向为{D}的x轴，并设定{L}的x、y、z轴方向与{D}对应各方向重合。

利用激光跟踪仪测量移动舱段上的关键特征，构造并拟合{D}系各坐标轴矢量后，间接建立了{L}与{D}的换算关系，实现了坐标系匹配。其优点和便捷性在于：简化了后续相对位姿测量任务对所测量坐标的坐标系换算，提升了运算效率。换言之，两坐标系经特征测量方法实现关系匹配后，利用激光跟踪仪测量的任一点坐标也表示该点在移动舱段坐标系下的局部坐标。

3.2 移动舱段与动平台的相对位姿测量

移动舱段与动平台相对位姿测量的原理是采用控制变量的方式，使并联机构在载有移动舱段的情况下，从起始零位处分别沿自身坐标系各轴方向平移，构建移动舱段与动平台的相对姿态关系；使并联机构在载有移动舱段的情况下从起始零位处分别绕自身坐标系各轴方向转动，构建移动舱段与动平台的相对位置关系。

3.2.1 {D}与{M}的相对姿态测量

以广义位姿矢量[MxD,MyD,MzD,MαD,MβD,MγD]T表示 {D}相对于{M}的位姿。平移矢量T=[MxD,MyD,MzD]T为Od在 {M}下的坐标，[MαD,MβD,MγD]T为{D}旋转变换到 {M} 的欧拉角，旋转矩阵为DM R。

在相对姿态测量前，首先，利用第3.1节方法实现{D}与 {L}的匹配。然后，将拟合得到的{D}原点Od定义为参考标记点K，K的初始坐标为DK=[0,0,0]T，如图3所示。

图3 移动舱段坐标系下的参考标记点KFig. 3 Reference marker K in the coordinate system of movable platform

控制载有移动舱段的并联机构从起始零位起，沿自身 {M} 的x轴方向平移一定距离dx，K点坐标发生变化，如图4所示。

由于 {D} 与 {M} 的固连约束关系，{M}下K点坐标的变化可等价为 {M} 下的坐标点 [dx,0,0]T按某个欧拉角a的旋转变换。记并联机构运动后激光跟踪仪测得的K点坐标为D，根据{D}下同一参考标记点的两种坐标变换的表达方式得：

调整并联机构回零归位后，再次控制并联机构沿着 {M}的y轴方向平移一定距离dy，K点坐标再次变化，如图5所示。

图4 沿x轴平移后的K点坐标变化Fig. 4 Coordinate of point K changes after translation along the x axis

图5 沿y轴平移后K点坐标变化Fig. 5 Coordinate of point K changes after translation along the y axis

{M} 下K点坐标变化亦可等价为 {M}下的坐标点[0,dy,0]T按某个欧拉角b的旋转变换。记并联机构沿y轴运动后由激光跟踪仪测得的K点坐标为DKy′，则：

并联机构沿z轴方向运动dz时，K点坐标变化如图6所示。同理可得：

3.2.2 {D}与 {M}的相对位置测量

在相对位置测量中，同样选取移动舱段{D}系原点Od为参考标记点，并记为k以示区分，k点初始坐标值为Dk=[0,0,0]T。控制载有移动舱段的并联机构从起始零位按照已知欧拉角 [a1,b1,c1]T旋转后，k点坐标将发生变化，如图7所示。

例如，机构按照第1组欧拉角旋转后， {M} 下的k点由初始坐标Mk1=Dk+MD T变为Mk′1=R(a1,b1,c1)·(DkT)。同时，由激光跟踪仪测得旋转后的k点坐标为D。

图6 沿 z轴平移后K点坐标变化示意图Fig. 6 Coordinate of point K changed after translation along the z axis

图7 按照已知欧拉角旋转后k点坐标变化Fig. 7 Change of coordinates of point k after rotation according to the known Euler angle

根据同样的步骤，利用i(i≥3)组已知的欧拉角控制并联机构从起始零位处进行旋转，再利用激光跟踪仪分别测量出每组欧拉角对应的变化后的k点坐标D。

根据同一矢量kk′在{M} 和{L}下的模量相等的原理，建立多组等式关系：

3.3 固定舱段与移动舱段的相对位姿测量

理论上，利用激光跟踪仪分别对固定舱段及移动舱段进行位姿测量，可计算两者的相对位姿偏差。但这种方式需要测量多组关键特征并涉及多次拟合换算，容易造成误差累积。考虑到舱段对接的必要条件，即在加工精度有保证时，至少确保移动舱段上3个销孔与固定舱段上各对应销轴对齐。因此，本节基于上述3点定位的对接原理，通过选取并测量移动舱段与固定舱段端面上的3组特征点，构建两者的相对位姿关系。示{F}相对于{D}的位姿。平移矢量=[DxF,DyF,DzF]T为{F}原点在{D}下的坐标，[DαF,DβF,DγF]T为{F}旋转

以广义位姿矢量[DxF,DyF,DzF,DαF,DβF,DγF]T表变换到{D} 的欧拉角，旋转矩阵为。

首先，利用第3.1节的方法在舱段相对位姿测量前实现 {D} 与 {L}的匹配；然后，在两舱段对接端面上分别选择3处销孔及销轴末端作为测量特征点，如图8中的1、2、3所示。

图8 对接端面上3组特征点位置分布Fig. 8 Location distribution of three groups of feature points on the end face

当并联机构位于起始零位时，将光学靶球紧贴移动舱段上3个销孔，利用激光跟踪仪分别测量，记各特征点为Dpi(i=1,2,3)；然后，测量固定舱段上对应的3个销轴末端，记各特征点为Dti(i=1,2,3)。根据3点定位对接原理，建立{D}与 {F}的相对位姿关系：

4 相对位姿关系的等价变换及求解

受环境因素及人工操作等测量误差影响，式（3）～（5）中包含了复杂三角函数关系，在恒等条件下难以取得解析解。因此，本节根据相对位姿关系构造自定义标量函数，将相对位姿求解转换为标量函数的无约束优化问题，并利用单纯形法求解。

4.1 移动舱段与动平台相对位姿等价变换

将式（3）～（5）所描述的移动舱段与动平台的相对姿态关系方程移项构造成矩阵形式：

为求解使得两矩阵具有最大相似度的相对姿态参数，本文通过构造关于上述偏差矩阵A的标量函数，求解使标量函数取最小值时的相对姿态参数作为最优解。

考虑到并联机构动平台在沿各坐标轴方向上的运动误差，将式（8）中动平台沿各坐标轴方向设定的运动距离dx、dy、dz用参考标记点K的实际运动距离（即动平台在相应方向上运动前后的K点坐标值之差）替代：

构造的第1个标量函数为式（8）所表示的3行3列矩阵A中各元素aij的平方和：

同理，利用式（8）所描述的相对位置关系，以矢量kk′在 {M}系和{L}系下的模长之差构造第2个标量函数：

4.2 舱段之间的相对位姿等价变换

将式（7）展开为方程组形式，按照两两相减后消去方程的常数项：

式（12）展开项为包含3个未知量的6组等式方程组，由于3组特征点均含有测量误差，式（12）在实数域内无法得到解析解。为避免求解超定方程组，将式（12）移项后，构造成第3个关于欧拉角参数的标量函数：

4.3 相对位姿参数优化的初值确定及步骤

单纯型（Nelder–Mead）法是一种通用的搜索算法，是针对多维无约束非线性优化问题的一种数值解法，又称为下山单纯形法，其基本思想为：利用非线性模型的n个待估参数，以n+1个顶点构成一个多胞体，通过比较所有顶点的目标函数值来判断极值点的搜索方向，再利用一定的换点原则进行迭代，使单纯形向最优解区域逼近。因此，单纯型法是一种从试验角度来寻找函数最优解的实用方法。

在利用单纯形法优化标量函数极值时，初值对优化结果影响较大，当初值选择不当时，单纯型法难以获得满意的解。

1）在利用单纯形法求解式（10）和（11）的标量函数f1和f2极小值时，需要用到{D} 与{M}的相对位姿初值。

试验过程中，利用卷尺测量的移动舱段中心附近到动平台几何中心点附近在水平径向、水平横向及垂直方向的距离作为相对位置的迭代初值。吊装时会粗调移动舱段姿态，使其不在某个方向明显倾斜，因此相对姿态角的迭代初值为[ 0,0,0]T。

2）在求解{F}与 {D}的相对位姿时，将相对姿态角的迭代初值取 [0,0,0]T。然后，用单纯型法求得使式（11）中标量函数取极小值时的最佳姿态旋转矩阵。

合理选择初值后，可利用单纯型法求得使各标量函数取最小值时的最佳相对位姿。单纯型法优化标量函数的主要步骤为：

步骤1：排序。按照函数值的大小，将单纯形中的点从最低f(x1) 到最高f(xn+1) 排序。其中，xi(i=1,2,···,n+1)表示当前单纯形中的点列表，也表示未知位姿向量。

步骤3：扩展。如果f(xr)

步骤4：收缩。如果f(xr)≥f(xn)，在x¯与xr和xn+1之间的更优者之间执行收缩：

a）外缩。如果f(xr)

b）内缩。如果f(xr)≥f(xn+1)，计算xci=x¯+(xn+1−x¯)/2，并计算函数值f(xci)。如果f(xci)

步骤5：整体收缩。按照vi=x1+(xi−x1)/2计算剩余n个点及函数值f(vi)，下一个单纯形中，迭代的点为x1,v2,···,vn+1。

按照以上步骤将求得的DFR代入式（7），以取平均值的方法求解最佳位置矢量：

{F}相对于{D}的平均最佳位置矢量：

5 并联机构目标位姿的换算方法

根据求得的相对位姿参数，采用位姿传递转换法计算并联机构实现舱段对接时运动到的目标位姿。

根据图1简化出各坐标系间的传递换算关系，如图9所示。描述固定舱段位姿的{F}系决定了与之实现对接匹配的移动舱段目标位姿 {D′} 的位姿，{D}与目标位姿 {D′} 之间的相对位姿偏差由 {M}按照一定的调姿量运动，并在接近目标位姿{M′}的过程中逐渐消除。动平台初始零位坐标为 [0,0,0,0,0,0]T，因此，该调姿量大小等于并联机构动平台调姿运动的目标位姿。

图9 舱段对接中各坐标系传递换算关系Fig. 9 Transfer conversion relation of each coordinate system in cabin docking problem

根据移动舱段与动平台的固连约束关系，{D}与{M} 的相对位姿量等价于{F} 与 {M′}的相对位姿量。{F}下任一点坐标可按照图9所示的两种方式统一换算到{M} 下，并由此建立出求解{M′}的等式关系。

设{F}下任意一参考点p的坐标为：

根据所求的{F}到 {D} 的位姿关系 [DF R,DFT]，将点p变换到{D}下的坐标为：

根据所求{D}到 {M} 的位姿关系[R,T]，将点p变换到{M}下的坐标为：

移动舱段和动平台同时到达各自目标位姿时，固连约束确保两者之间的相对位姿不变，因此：

此时，{F}下的参考点p在动平台到达{M′}时，在{M′}下的坐标为：

将式（19）中的等价关系代入式（20）得：

联立式（17）和（18），得到{F}下的点p经{D}换算到{M}下的坐标为：

联立式（21）和（22），得到{F}下的p点经{M′}换算到{M}下的坐标为：

联立式（23）和（24），得到动平台由 {M}运动到{M′}时的姿态及位置调整量分别为：

最后，根据式（2）可反解出实现 {M}姿态调整的欧拉角角度。

6 位姿测量及换算方法的试验验证

为验证上述方法的有效性及计算结果的正确性，搭建了如图10所示的舱段对接模拟试验场景。其中，固定舱段安装在梯形支架上，移动舱段借助简易支架与并联机构动平台固连。试验前，两舱段的端面最大间距未超过并联机构沿轴向的有效行程。将激光跟踪仪调整到距离两舱段对接端面的对称面约2 m的位置处固定。

图10 舱段对接模拟试验实物场景Fig. 10 Physical scene of cabin docking simulation

根据第3.2节所描述的位姿测量方法：在测量移动舱段与动平台相对姿态时，根据预先设定的位移量，控制动平台每次从初始零位分别沿x、y、z轴方向平移100 mm，激光跟踪仪测得平移后的参考标记点K坐标如表1所示。

在测量移动舱段与动平台的相对位置时，根据预先设定的欧拉角，控制并联机构从初始零位分别进行了6次旋转，激光跟踪仪测得旋转后参考标记点k的坐标如表2所示。

表1 3次平移的参考标记点K坐标测量值Tab. 1 Measurement coordinate values of reference marker K for three times translations

表2 6次旋转的参考标记点k坐标测量值Tab. 2 Measurement coordinate values of reference marker k for six times rotations

在两舱段相对位姿测量过程中，根据第3.3节的测量方法，利用激光跟踪仪测得图8中的3组特征点坐标，如表3所示。

表3 对接前参考特征点及目标特征点坐标值Tab. 3 Coordinate values of reference feature points and target feature points before docking

利用第4.3节方法拟合得到移动舱段与动平台的相对位置及姿态旋转矩阵分别为：

利用第4.3节方法拟合得到固定舱段与移动舱段的相对位置及姿态旋转矩阵分别为：

根据以上计算结果，利用第5节的位姿传递转换法，求出动平台目标位姿相对初始位姿的旋转矩阵为：

利用式（2）求得动平台需要调整的姿态角为[2.45◦,−2.19◦,−0.29◦]T。

对接前，将固定舱段调整在任意位置处，在运动控制程序中输入动平台调姿量[101.34,−67.93,−119.84,2.45◦,−2.19◦,−0.29◦]T，驱动并联机构对移动舱段调姿，与固定舱段实现对接。对接前、后的结果如图11所示。

图11 舱段对接前与对接后的场景Fig. 11 Scene before and after cabin docking

对接试验结果显示，两舱段对接端面对正，且移动舱段上各销孔与固定舱段上各销轴基本对齐，定位销部分倒角已略微进入定位孔中。

模型舱段的加工精度较低，现场又缺乏同轴度的测量仪器，为了在一定程度上量化对接效果，利用激光跟踪仪再次测量了图8所示3组特征点坐标，如表4所示。

表4 对接后参考特征点及目标特征点坐标值Tab. 4 Coordinate values of reference feature points and target feature points after docking

表3和4分别为两舱段在对接前、后的两舱段端面上3组特征点的坐标值，以3组特征点对应坐标值之差的绝对值表示舱段对接前后各特征点在各坐标轴方向的绝对偏差，如图12、13所示。

图12与13结果对比显示，3组特征点坐标值绝对偏差在舱段对接后较对接前已大幅减小，符合对接的实际情况。舱段对接后，3组特征点在x方向绝对偏差均值为2.673 mm，与各销轴倒角略微插入销孔的试验结果较吻合；3组特征点在y、z方向绝对偏差均值分别为0.120、0.163 mm，与中各销孔与销轴基本对齐的试验结果较吻合；该结果在一定程度上表明所提出的测量方法能够满足模拟对接试验的精度要求。