[发明专利]一种运载火箭加泄连接器自动对接控制系统

权利要求书

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

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

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

设定上表面上的六个电动缸活塞杆一端的球形铰链与上表面的连接点所形成的圆为上铰链分布圆，上铰链分布圆的半径为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)执行锁紧动作后，力检测单元实时检测锁紧机构和箭上的推进剂加注口对接面上各个方向的力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个电动缸的位移,最终得到全部六个电动缸的位移。

2.根据权利要求1所述的一种运载火箭加泄连接器自动对接控制系统，其特征在于：所述运动控制单元根据该位置偏差和姿态偏差，得到六自由度并联机构上的锁紧机构的当前位置到箭上的推进剂加注口位置需六个电动缸的活塞杆运动的位移，步骤如下：

(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坐标系中的三维坐标为当上铰链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个电动缸的位移，最终得到全部六个电动缸的位移。