一种运载火箭加泄连接器自动对接控制系统

摘要

一种运载火箭加泄连接器自动对接控制系统，包括：监控单元、运动控制单元、伺服控制器、力检测单元、锁紧机构，六自由度并联机构、视觉监测单元；视觉监测单元利用摄像头对运载火箭箭上接口位置和姿态进行检测；力监测单元利用力和力矩传感器对接后产生的力和力矩进行检测；锁紧机构完成加泄连接器与箭上接口的锁紧；运动控制单元利用视觉监测单元和力检测单元对六自由并联机构的运动进行控制；伺服驱动器完成对电动机的驱动。本发明实现在不对箭体进行改动的条件下完成加泄连接器与箭上接口的自动对接与锁紧，并跟随箭上接口的运动，相比于现在人工对接的方法，节省人力，保障人员安全。

说明书

技术领域

本发明涉及一种运载火箭加泄连接器自动对接控制系统，属于运载火箭地面设备加注技术领域。

背景技术

运载火箭或导弹与地面设备之间的气、液接口设备一般称为连接器。在发射之前，运载火箭一直通过箭上的接口与连接器连接在一起，完成运载火箭的各项测试。

目前我国运载火箭的连接器与箭上的接口对接全部依靠人工完成，尤其是推进剂加注和泻出连接器(简称“加泄连接器”)，由操作人员采取手动直接操作方式进行加泄连接器与箭体接口之间的连接，而运载火箭的液态推进剂具有易燃、易爆、易挥发及强腐蚀性的显著特性，少量吸入或接触即可能导致人员中毒，出现伤亡事故。

随着我国新一代重型运载火箭发展，加泄连接器的质量也在不断增大，正在逐步超出人力的能力范围；而由于技术的发展，对发射过程提出了快速、安全、可靠等要求，人工对接的方式已经不能满足相应要求。

发明内容

本发明解决的技术问题为：克服现有技术不足，提供一种运载火箭加泄连接器自动对接控制系统，根据视觉和力检测箭上接口的位置和姿态，驱动六自由度并联机构完成对接、锁紧，并在加注过程中能够实时调整以适应箭体的晃动，加注完毕后控制加泄连接器与箭体活门解锁、分离并可靠回收到安全位置。解决了我国运载火箭连接依靠人工对接所带来的问题。

本发明解决的技术方案为：一种运载火箭加泄连接器自动对接控制系统，包括：监控单元、运动控制单元、伺服控制器、力检测单元、锁紧机构，六自由度并联机构、视觉监测单元；

六自由度并联机构，包括上表面和下表面，上表面的面积小于下表面的面积，上表面和下表面之间包括六个电动缸，六个电动缸分别为第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸。

第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的缸桶一端均通过球形铰链连接在下表面的一侧，第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的活塞杆一端通过球形铰链连接在上表面的一侧；

设定上表面上的六个电动缸活塞杆一端的球形铰链与上表面的连接点所形成的圆为上铰链分布圆，上铰链分布圆的半径为R_a；

设定下表面上的六个电动缸缸桶一端的球形铰链与下表面的连接点所形成的圆为下铰链分布圆，下铰链分布圆的半径为R_b；

第一电动缸、第三电动缸、第五电动缸活塞杆一端的球形铰链在上表面均匀分布在半径为R_a的圆的边缘，且第一电动缸、第三电动缸、第五电动缸活塞杆一端的球形铰链分别与上表面连接形成的三个连接点，连线后呈正三角形；

第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链在上表面均匀分布在半径为的R_a圆的边缘，且第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链分别与上表面连接形成的三个连接点，连线后呈正三角形；

第一电动缸活塞杆一端的球形铰链与上表面连接点与半径为R_a的圆的圆心的连线和第二电动缸活塞杆一端的球形铰链与上表面连接点与半径为R_a的圆的圆心的连线的夹角小于60°，设定第一电动缸活塞杆一端的球形铰链与上表面连接点与第二电动缸活塞杆一端的球形铰链与上表面连接点的距离为上铰链短边间距L_a；

第一电动缸、第三电动缸、第五电动缸缸桶一端的球形铰链在下表面均匀分布在半径为R_b的圆的边缘，且第一电动缸、第三电动缸、第五电动缸缸桶一端的球形铰链分别与下表面连接形成的三个连接点，连线后呈正三角形；

第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链在下表面均匀分布在半径为R_b的圆的边缘，且第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链分别与下表面连接形成的三个连接点，连线后呈正三角形；

第一电动缸的球形铰链与下表面连接点与半径为R_b的圆的圆心的连线和第二电动缸的球形铰链与下表面连接点与半径为R_b的圆的圆心的连线夹角小于60°，设定第一电动缸的球形铰链与下表面连接点与第二电动缸的球形铰链与下表面连接点的距离为下铰链短边间距L_b；

设定六自由度并联机构处于中位时，六个电动缸的活塞杆处于伸缩范围的中间位置；设定每个电动缸与上表面连接的球形铰链为上铰链，每个电动缸与下表面连接的球形铰链为下铰链，每个电动缸的上铰链中心与下铰链中心的距离L₂；

力检测单元的一侧安装在上表面的另一侧，且力检测单元的中心与上铰链分布圆的圆心重合；锁紧机构安装在力检测单元的另一侧；锁紧机构与上铰链分布圆的圆心重合；视觉监测单元固定在上表面的另一侧的边缘；

视觉监测单元安装在力检测单元旁，并与六自由度并联机构上表面固定连接；视觉监测单元，实时获取箭上的推进剂加注口的靶标图像，根据靶标图像获得视觉监测单元和靶标图像之间的位置偏差和姿态偏差，将该位置偏差和姿态偏差传送至运动控制单元，运动控制单元根据该位置偏差和姿态偏差，得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置需六个电动缸的活塞杆运动的位移，将该位移传送至伺服控制器，伺服控制器根据该位移驱动六个电动缸运动，使锁紧机构和箭上的推进剂加注口对接，当力检测单元检测到锁紧机构和箭上的推进剂加注口有力产生后，锁紧机构执行锁紧动作，即将锁紧机构和箭上的推进剂加注口锁紧；(箭上的推进剂加注口设置了靶标，靶标为几个正圆和定位点，使视觉监测单元能够通过靶标检测到箭上的推进剂加注口的图像)

执行锁紧动作后，力检测单元检测实时检测锁紧机构和箭上的推进剂加注口对接面上各个方向的力，形成力信号，将该力信号传送至运动控制单元，运动控制单元根据该力信号，得出使锁紧机构和箭上的推进剂加注口对接面上各个方向的力为零时六个电动缸的活塞杆需运动的位移，将该位移传送至伺服控制器，伺服控制器根据该位移驱动六个电动缸运动，完成锁紧机构和箭上的推进剂加注口的对接。(对接完成后，与锁紧机构一体成型连接的加泄连接器能够将地面的燃料罐中的燃料加注到箭上的推进剂加注口或将箭上的推进剂加注口内(即火箭内)的燃料泄回至地面的燃料罐中。加注或泄回过程在火箭起飞点火前完成。)

所述运动控制单元根据该位置偏差和姿态偏差，得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置需六个电动缸的活塞杆运动的位移，步骤如下：

(1)建立三维正交坐标系，即在上铰链分布圆的圆心处建立惯性坐标系O_a-X_aY_aZ_a，上铰链分布圆的圆心为原点O_a，在下铰链分布圆的圆心处建立惯性坐标系O_b-X_bY_bZ_b，下铰链分布圆的圆心为原点O_b，即静坐标系，设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸缸筒一端的球形铰链分别与下表面连接形成的六连接点分别为B₁、B₂、B₃、B₄、B₅、B₆；

设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸活塞杆一端的球形铰链分别与上表面连接形成的六连接点分别为A₁、A₂、A₃、A₄、A₅、A₆；

X_a轴的正方向为原点O_a指向A₁与A₆的中点，即X_a轴穿过A₁与A₆的中点，Y_a轴在上表面上超前X_a轴90°，即将X_a轴的正方向在上铰链分布圆的平面上顺时针转动90°为Y_a轴的正方向，根据右手定则确定Z_a轴的正方向；

X_b轴的正方向为原点O_b指向B₁与B₆的中点，即X_b轴穿过B₁与B₆的中点，Y_b轴在下表面上超前X_b轴90°，即将X_b轴的正方向在下铰链分布圆的平面上顺时针转动90°为Y_b轴的正方向，根据右手定则确定Z_b轴的正方向；

(2)在步骤(1)确定了坐标系的前提下，上表面的位置和姿态，用广义坐标向量q来表示，其中式中x,y,z分别表示上铰链分布圆的圆心在O_b-X_bY_bZ_b坐标系下的三维坐标，令t＝[x,y,z]^T为位置向量；为惯性坐标系O_a-X_aY_aZ_a在O_b-X_bY_bZ_b中的姿态角，采用Z_b-Y_b-X_b轴的依次旋转次序，即在坐标系O_a-X_aY_aZ_a与坐标系O_b-X_bY_bZ_b重合的初始条件下，φ表示先将O_a-X_aY_aZ_a坐标系先绕Z_b轴旋转的角度，θ表示再将O_a-X_aY_aZ_a绕Y_b轴旋转的角度，表示最后将O_a-X_aY_aZ_a绕X_b轴旋转得角度。

根据姿态角采用Z_b-Y_b-X_b轴的依次旋转次序，即将O_a-X_aY_aZ_a坐标系先绕Z_b轴旋转角度φ，再绕Y_b轴旋转角度θ，最后再绕X_b轴旋转角度形成旋转矩阵R为：

(3)设定第i电动缸活塞杆一端的球形铰链与上表面连接点即上铰链A_i的中心在O_a-X_aY_aZ_a坐标系中的三维坐标为设定第i电动缸缸筒一端的球形铰链与下表面连接点即下铰链B_i的中心在O_b-X_bY_bZ_b坐标系中的三维坐标为b_i＝[b_ix,b_iy,b_iz]^T，i＝1,2,…,6；当上铰链A_i随着六自由度并联机构上表面一起运动，上铰链中心在O_a-X_aY_aZ_a坐标系中的坐标不变；

由二维空间的旋转原理，得到上铰链A_i的中心在O_a-X_aY_aZ_a坐标系中坐标，所有上铰链A_i的坐标按照A₁-A₆的顺序组成的矩阵A，公式如下：

式中，H_a＝0，A中第i列表示上铰链A_i在O_a-X_aY_aZ_a中的坐标；

同理，能够得到下铰链B_i的中心在O_b-X_bY_bZ_b坐标系下的坐标，所有下铰链B_i的坐标按照B₁-B₆的顺序组成矩阵B，公式如下，

式中，H_b＝0，B中第i列表示下铰链B_i在O_b-X_bY_bZ_b中的坐标；

(4)随着六自由度并联机构的运动，O_a-X_aY_aZ_a坐标系相对于O_b-X_bY_bZ_b坐标系的位置和姿态不断变化，上铰链A_i的中心在O_b-X_bY_bZ_b坐标系中的坐标不断变化，由空间机构学的原理，上铰链A_i的中心在O_b-X_bY_bZ_b坐标系中的坐标a_i为：

式中，为矩阵A的第i列，即A_i在O_a-X_aY_aZ_a中的坐标；

(5)将步骤(4)的公式等号两边同时减去b_i，即得到第i个电动缸的长度矢量l_i，即

(6)根据步骤(5)得到的六个电动缸的长度矢量l_i，确定第i个电动缸的期望长度L_i，公式如下

(7)根据步骤(6)确定第i个电动缸的长度L_i，分别减去第i个电动缸的实际长度，得到第i个电动缸的位移，最终得到全部六个电动缸的位移。

所述运动控制单元根据该力信号，得出使锁紧机构和箭上的推进剂加注口对接面上各个方向的力为零时六个电动缸的活塞杆需运动的位移，步骤如下：

(1)建立三位坐标系，在上铰链分布圆的圆心处建立惯性坐标系O_a-X_aY_aZ_a，上铰链分布圆的圆心为原点O_a，在下铰链分布圆的圆心处建立惯性坐标系O_b-X_bY_bZ_b，下铰链分布圆的圆心为原点O_b，即静坐标系，设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的球形铰链分别与下表面连接形成的六连接点分别为B₁、B₂、B₃、B₄、B₅、B₆；

设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的球形铰链分别与上表面连接形成的六连接点分别为A₁、A₂、A₃、A₄、A₅、A₆；

X_a轴的正方向为原点O_a指向A₁与A₆的中点，即X_a轴穿过A₁与A₆的中点，Y_a轴在上表面上超前X_a轴90°，即将X_a轴的正方向在上铰链分布圆的平面上顺时针转动90°为Y_a轴的正方向，根据右手定则确定Z_a轴的正方向；

X_b轴的正方向为原点O_b指向B₁与B₆的中点，即X_b轴穿过B₁与B₆的中点，Y_b轴在下表面上超前X_b轴90°，即将X_b轴的正方向在下铰链分布圆的平面上顺时针转动90°为Y_b轴的正方向，根据右手定则确定Z_b轴的正方向；

(2)执行锁紧动作后，力检测单元实时检测锁紧机构和箭上的推进剂加注口对接面上各个方向的力F_x,F_y,F_z分别表示锁紧机构和箭上的推进剂加注口对接面在X_a、Y_a、Z_a轴的受力，T_θ,T_φ为锁紧机构和箭上的推进剂加注口对接面在X_a、Y_a、Z_a轴的力矩，即扭矩；

根据刚体的刚度计算原理得到F＝M·Δq，Δq＝M^-1·F，式中，M为六自由度并联机构的刚度矩阵；为得到六自由度并联机构上表面的位置和姿态，用广义坐标向量来表示，x,y,z分别表示上铰链分布圆的圆心在O_b-X_bY_bZ_b坐标系下的三维坐标，令t＝[x,y,z]^T；为上铰链分布圆的圆心在O_b-X_bY_bZ_b中的姿态角，即欧拉角，采用Z_b-Y_b-X_b轴的依次旋转次序，即在坐标系O_a-X_aY_aZ_a与坐标系O_b-X_bY_bZ_b重合的初始条件下，φ表示先将O_a-X_aY_aZ_a坐标系先绕Z_b轴旋转的角度，θ表示再将O_a-X_aY_aZ_a绕Y_b轴旋转的角度，表示最后将O_a-X_aY_aZ_a绕X_b轴旋转得角度。

根据姿态角采用Z_b-Y_b-X_b的旋转次序，即将O_a-X_aY_aZ_a坐标系先绕Z_b轴旋转角度φ，再绕Y_b轴旋转角度θ，最后再绕X_b轴旋转角度形成旋转矩阵R为：

(3)设定上铰链A_i的中心在O_a-X_aY_aZ_a坐标系中的三维坐标为下铰链B_i的中心在O_b-X_bY_bZ_b坐标系中的三维坐标为b_i＝[b_ix,b_iy,b_iz]^T，i＝1,2,…,6；当上铰链A_i随着上表面一起运动，上铰链中心在O_a-X_aY_aZ_a坐标系中的坐标不变；

由二维空间的旋转原理，可得到上铰链A_i的中心在O_a-X_aY_aZ_a坐标系中坐标的矩阵A，公式如下：

式中，H_a＝0，

同理，能够得到下铰链B_i的中心在O_b-X_bY_bZ_b坐标系下的坐标矩阵B，公式如下，

式中，H_b＝0，

(4)随着六自由度并联机构的运动，O_a-X_aY_aZ_a坐标系相对于O_b-X_bY_bZ_b坐标系的位置和姿态不断变化，上铰链A_i的中心在O_b-X_bY_bZ_b坐标系中的坐标不断变化，由空间机构学的原理，上铰链A_i的中心在O_b-X_bY_bZ_b坐标系中的坐标a_i为：

(5)将步骤(4)的公式等号两边同时减去b_i，即得到第i个电动缸的长度矢量l_i，即

(6)根据步骤(5)得到的六个电动缸的长度矢量l_i，确定第i个电动缸的期望长度L_i，公式如下

(7)根据步骤(6)确定第i个电动缸的长度L_i，分别减去第i个电动缸的实际长度，得到第i个电动缸的位移,最终得到全部六个电动缸的位移。

本发明与现有技术相比的优点在于：

(1)本发明提供了运载火箭加泄连接器自动对接控制的技术方案，依据此方案可实现运载火箭加泄连接器的自动对接；

(2)本发明提出的视觉监测单元，在不改动运载火箭箭体结构的情况下，完成箭上接口的位置和姿态的检测，因此也可应用在现有的运载火箭型号上，提升了通用性；

(3)本发明提出的力监测单元，在对接锁紧后，通过测量对接面的力和力矩，并转换为位置和姿态的运动量，实时调整锁紧机构的位置和姿态，有效保护了箭体接口不被损坏；

(4)本发明提出的机械结构方案为六自由度并联机构，相比于一般的串联式机械臂，具有精度高、承受载荷大的优点、更适用于运载火箭加泄连接器自动对接；

(5)本发明提出视觉监测单元和力监测单元的计算方法，具有很大的灵活性，既适合电动缸驱动的六自由度并联机构，也可以根据具体情况更改为液压等其他方案，且计算方法不变。

附图说明

图1为本发明的系统示意图；

图2为本发明的控制信号流程图；

图3为坐标系建立示意图

图4为六自由度并联机构三维示意图

具体实施方式

本发明的基本思路为：一种运载火箭加泄连接器自动对接控制系统包括：监控单元、运动控制单元、伺服控制器、力检测单元、锁紧机构，六自由度并联机构、视觉监测单元；视觉监测单元利用摄像头对运载火箭箭上接口位置和姿态进行检测；力监测单元利用力和力矩传感器对接后产生的力和力矩进行检测；锁紧机构完成加泄连接器与箭上接口的锁紧；运动控制单元利用视觉监测单元和力检测单元对六自由并联机构的运动进行控制；伺服驱动器完成对电动机的驱动。本发明实现在不对箭体进行改动的条件下完成加泄连接器与箭上接口的自动对接与锁紧，并跟随箭上接口的运动，相比于现在人工对接的方法，节省人力，保障人员安全。

下面结合附图和具体实施例对本发明进行详细说明。

一种运载火箭加泄连接器自动对接控制系统，包括：监控单元、运动控制单元、伺服控制器、力检测单元、锁紧机构，六自由度并联机构、视觉监测单元；

如图4所示，六自由度并联机构可实现三维空间中六个自由度的运动，以适应火箭在风载荷作用下的无规律晃动，包括上表面和下表面，上表面的面积小于下表面的面积，上表面和下表面之间包括六个电动缸，六个电动缸分别为第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸。

第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的缸桶一端均通过球形铰链连接在下表面的一侧，第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸的活塞杆一端通过球形铰链连接在上表面的一侧；

设定上表面上的六个电动缸活塞杆一端的球形铰链与上表面的连接点所形成的圆为上铰链分布圆，上铰链分布圆的半径为R_a；

设定下表面上的六个电动缸缸桶一端的球形铰链与下表面的连接点所形成的圆为下铰链分布圆，下铰链分布圆的半径为R_b；

第一电动缸、第三电动缸、第五电动缸活塞杆一端的球形铰链在上表面均匀分布在半径为R_a的圆的边缘，且第一电动缸、第三电动缸、第五电动缸活塞杆一端的球形铰链分别与上表面连接形成的三个连接点，连线后呈正三角形；

第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链在上表面均匀分布在半径为的R_a圆的边缘，且第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链分别与上表面连接形成的三个连接点，连线后呈正三角形；

第一电动缸活塞杆一端的球形铰链与上表面连接点与半径为R_a的圆的圆心的连线和第二电动缸活塞杆一端的球形铰链与上表面连接点与半径为R_a的圆的圆心的连线的夹角小于60°，设定第一电动缸活塞杆一端的球形铰链与上表面连接点与第二电动缸活塞杆一端的球形铰链与上表面连接点的距离为上铰链短边间距L_a；

第一电动缸、第三电动缸、第五电动缸缸桶一端的球形铰链在下表面均匀分布在半径为R_b的圆的边缘，且第一电动缸、第三电动缸、第五电动缸缸桶一端的球形铰链分别与下表面连接形成的三个连接点，连线后呈正三角形；

第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链在下表面均匀分布在半径为R_b的圆的边缘，且第二电动缸、第四电动缸、第六电动缸缸桶一端的球形铰链分别与下表面连接形成的三个连接点，连线后呈正三角形；

第一电动缸的球形铰链与下表面连接点与半径为R_b的圆的圆心的连线和第二电动缸的球形铰链与下表面连接点与半径为R_b的圆的圆心的连线夹角小于60°，设定第一电动缸的球形铰链与下表面连接点与第二电动缸的球形铰链与下表面连接点的距离为下铰链短边间距L_b；

设定六个电动缸的活塞杆全部缩回至最短长度时为零位，六个电动缸活塞杆全部伸出至最大长度时为极限位置，零位与极限位置的中间长度为中位，即处于伸缩范围的中间位置；六自由度并联机构处于中位时，设定每个电动缸与上表面连接的球形铰链为上铰链，每个电动缸与下表面连接的球形铰链为下铰链，每个电动缸的上铰链中心与下铰链中心的距离L₂；

力检测单元的一侧安装在上表面的另一侧，且力检测单元的中心与上铰链分布圆的圆心重合；锁紧机构安装在力检测单元的另一侧；锁紧机构与上铰链分布圆的圆心重合；视觉监测单元固定在上表面的另一侧的边缘；

建立三维正交坐标系，即在上铰链分布圆的圆心处建立惯性坐标系O_a-X_aY_aZ_a，上铰链分布圆的圆心为原点O_a，在下铰链分布圆的圆心处建立惯性坐标系O_b-X_bY_bZ_b，下铰链分布圆的圆心为原点O_b，即静坐标系，设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸缸筒一端的球形铰链分别与下表面连接形成的六连接点分别为B₁、B₂、B₃、B₄、B₅、B₆；

设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸活塞杆一端的球形铰链分别与上表面连接形成的六连接点分别为A₁、A₂、A₃、A₄、A₅、A₆；

X_a轴的正方向为原点O_a指向A₁与A₆的中点，即X_a轴穿过A₁与A₆的中点，Y_a轴在上表面上超前X_a轴90°，即将X_a轴的正方向在上铰链分布圆的平面上顺时针转动90°为Y_a轴的正方向，根据右手定则确定Z_a轴的正方向；

X_b轴的正方向为原点O_b指向B₁与B₆的中点，即X_b轴穿过B₁与B₆的中点，Y_b轴在下表面上超前X_b轴90°，即将X_b轴的正方向在下铰链分布圆的平面上顺时针转动90°为Y_b轴的正方向，根据右手定则确定Z_b轴的正方向；

建立三维正交坐标系，即在上铰链分布圆的圆心处建立惯性坐标系O_a-X_aY_aZ_a，上铰链分布圆的圆心为原点O_a，在下铰链分布圆的圆心处建立惯性坐标系O_b-X_bY_bZ_b，下铰链分布圆的圆心为原点O_b，即静坐标系，设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸缸筒一端的球形铰链分别与下表面连接形成的六连接点分别为B₁、B₂、B₃、B₄、B₅、B₆；

如图3所示，设定第一电动缸、第二电动缸、第三电动缸、第四电动缸、第五电动缸、第六电动缸活塞杆一端的球形铰链分别与上表面连接形成的六连接点分别为A₁、A₂、A₃、A₄、A₅、A₆；

X_a轴的正方向为原点O_a指向A₁与A₆的中点，即X_a轴穿过A₁与A₆的中点，Y_a轴在上表面上超前X_a轴90°，即将X_a轴的正方向在上铰链分布圆的平面上顺时针转动90°为Y_a轴的正方向，根据右手定则确定Z_a轴的正方向；

X_b轴的正方向为原点O_b指向B₁与B₆的中点，即X_b轴穿过B₁与B₆的中点，Y_b轴在下表面上超前X_b轴90°，即将X_b轴的正方向在下铰链分布圆的平面上顺时针转动90°为Y_b轴的正方向，根据右手定则确定Z_b轴的正方向；

视觉监测单元安装在力检测单元旁，并与六自由度并联机构上表面固定连接，由高速摄像头等设备组成，可以实现箭上推进剂加注口的无接触式检测，防止接触式测量对火箭的影响，有效减轻火箭载荷，同时保护火箭的箭体结构；同时在箭上推进剂加注口附近设置靶标，用于视觉监测单元检测使用。视觉监测单元实时获取箭上的推进剂加注口的靶标图像，根据靶标图像获得视觉监测单元和靶标图像之间的位置偏差和姿态偏差，利用位置偏差和姿态偏差可以得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置的位置和姿态，将该位置偏差和姿态偏差传送至运动控制单元，运动控制单元根据该位置偏差和姿态偏差，得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置的位置和姿态，再进一步得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置需六个电动缸的活塞杆运动的位移，将该位移传送至伺服控制器，伺服控制器根据该位移驱动六个电动缸运动，使锁紧机构和箭上的推进剂加注口对接，当力检测单元检测到锁紧机构和箭上的推进剂加注口有力产生后，锁紧机构执行锁紧动作，即将锁紧机构和箭上的推进剂加注口锁紧；

执行锁紧动作后，力检测单元检测实时检测锁紧机构和箭上的推进剂加注口对接面上各个方向的力，F_x,F_y,F_z分别表示锁紧机构和箭上的推进剂加注口对接面在X_a、Y_a、Z_a轴的受力，T_θ,T_φ为锁紧机构和箭上的推进剂加注口对接面在X_a、Y_a、Z_a轴的力矩，即扭矩；将该力信号传送至运动控制单元，运动控制单元根据该力信号，根据刚体的刚度计算原理，得出使锁紧机构和箭上的推进剂加注口对接面上各个方向的力为零时六个电动缸的活塞杆需运动的位移，将该位移传送至伺服控制器，伺服控制器根据该位移驱动六个电动缸运动，完成锁紧机构和箭上的推进剂加注口的对接。至此，与锁紧机构固定连接的加泄连接器对接完成，加泄连接器能够将地面的燃料罐中的燃料加注到箭上的推进剂加注口或将箭上的推进剂加注口内(即火箭内)的燃料泄回至地面的燃料罐中。加注或泄回过程在火箭起飞点火前完成。

自动对接完成后，控制系统持续检测对接面的力，并实时调整六自由度并联机构的位置和姿态，以减小箭上推进剂加注口的受力，起到保护火箭结构的作用。

如图1所示，为本发明的一种具体实现方案，其中，监控单元选用PC机、运动控制单元选用PXI实时控制计算机，为系统的核心单元，完成箭上推进剂加注口的位置姿态的采集、对接面受力和力矩参数的采集，并通过伺服控制算法，计算出六个电动缸的位移量，通过伺服控制器来驱动电动缸进行移动，达到自动对接的工作；视觉检测单元主要通过视觉检测的方式测量待测物体的位置和姿态，力检测单元主要在对接锁紧后实时检测对接面受力，为六自由度机构的位置姿态调整提供依据。

根据实际的六自由度并联机构结构，建模中加入上表面的质心与上铰链分布圆圆心的距离H_a、下表面的质心与下铰链分布圆中心的距离H_b这两个参数，提高系统控制精度和伺服控制性能。

图2显示了本发明中的控制算法及控制信号流程、用于指导控制算法的工程实现。对接控制流程为：在六自由度并联机构初始中位条件下，先控制上表面运动至箭上推进剂加注口晃动包络范围附近，之后进入视觉监测阶段，视觉检测单元采集到其与靶标的位置偏差和姿态偏差并传送到运动控制单元中的坐标转换单元进行计算分析，得出加注口中心与上表面中心，即锁紧机构中心的位姿偏差，并进行逆运动学闭环控制得到六个电动缸位移，通过伺服控制器驱动电动缸运动。锁紧之后，为力反馈阶段，此时，力检测单元采集到对接面得受力后，传送至力闭环控制单元，得到位姿偏差信号，并进行逆运动学闭环控制得到六个电动缸位移，通过伺服控制器驱动电动缸运动。

本发明的方案经过仿真计算，可实现加泄连接器的自动对接功能，解决了目前加泄连接器均由人工对接完成的难题，在运载火箭以1Hz，振幅100mm左右晃动的条件下，对接时间可达到15s内，可实现快速全自动化对接，为运载火箭发射前紧急关机时的推进剂泄回的无人值守问题的解决提供了具体可实施的方案。