CN110823170B - 基于双目视觉测量的运载火箭大部段调姿对接方法 - Google Patents

Abstract

本发明公开了一种基于双目视觉测量的运载火箭大部段调姿对接方法，包括如下步骤：步骤一、建立全局坐标系；步骤二、建立两个对接部段的局部坐标系；步骤三、建立装配坐标系，并分别确定两个局部坐标系与装配坐标系间的转换关系矩阵；步骤四、建立虚拟调姿坐标系；步骤五、确定各调姿控制点在装配坐标系下的控制量。本发明在双目视觉引导自动化装配的基础上提出一系列坐标系标定转化方法，并将视觉测量获得的控制量分配到各运动轴，实现部段对接视觉测量***的快速标定，从而达到测量及运动控制目的。

Description

基于双目视觉测量的运载火箭大部段调姿对接方法

技术领域

本发明属于测量控制领域，具体涉及一种基于双目视觉测量的运载火箭大部段调姿对接运动解算方法，应用于运载火箭部段装配的自动化对接。

背景技术

航天运载火箭部段装配对接最初采用人工手动装配的方法，这种装配方式工作环境差，人工劳动强度大，装配效率低，装配工作中受工作人员制约因素较多，不确定性大，因此该装配方式被自动化装配所取代已经成为必然的趋势；现在较成熟的自动化装配方式是采用激光跟踪仪引导实现自动化装配对接，但该装配方式设备投入大，每次装配都需要详细标定，对操作人员素质要求较高。

发明内容

为了克服现有技术的上述缺点，本发明提出了一种基于双目视觉测量的运载火箭大部段调姿对接方法，以期解决如下技术问题：

本发明是为了在视觉测量引导下实现运载火箭部段的快速自动化对接，提出双目视觉测量技术在部段对接中坐标系体系的确定、快速标定、转化以及自动对接时控制量的解析问题；

装配部段对接平面局部坐标系的选择、标定，局部坐标系与全局坐标系之间转换关系的计算方法；

装配坐标系的选择、标定，并选择装配坐标系作为部段对接的目标坐标系，原点作为对接目标点，降低位姿偏转量计算难度；

确定装配坐标系与部段对接局部坐标系间转换关系的计算方法，其转换矩阵所求量即为部段对接时所需要的运动量；

选取调姿平台局部坐标系，对调姿平台托架进行标定，确定调姿平台局部坐标系与全局坐标系间的转换关系；

根据调姿平台局部坐标系提出建立调姿平台虚拟坐标系的计算方法，并求解出调姿平台坐标系与全局坐标系间的转换关系；

根据全局坐标系与装配坐标系间的转换关系、全局坐标系与调姿坐标系间的转换关系，求取装配坐标系与调姿坐标系间的转换关系；

选择调姿控制点，建立部段位姿调整逆解模型，获得调姿坐标系下，各控制轴的运动量情况，实现自动化调姿对接。

本发明解决其技术问题所采用的技术方案是：一种基于双目视觉测量的运载火箭大部段调姿对接方法，包括如下步骤：

步骤一、建立全局坐标系O_q-X_qY_qZ_q；

步骤二、建立两个对接部段的局部坐标系O₁-X₁Y₁Z₁、O₂-X₂Y₂Z₂；

步骤三、建立装配坐标系O-XYZ，并分别确定两个局部坐标系O₁-X₁Y₁Z₁和O₂-X₂Y₂Z₂与装配坐标系O-XYZ间的转换关系矩阵；

步骤四、建立虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1；

步骤五、确定各调姿控制点在装配坐标系下的控制量。

与现有技术相比，本发明的积极效果是：

本发明在双目视觉引导自动化装配的基础上提出一系列坐标系标定转化方法，并将视觉测量获得的控制量分配到各运动轴，实现部段对接视觉测量***的快速标定，从而达到测量及运动控制目的。在本发明中坐标系的标定、建立操纵简单快捷，对人员培训压力小，同时标定速度也大大提升；运动控制***采用“PC+运动控制卡”的开放式控制方式，视觉检测***与控制***共用一台PC，实现视觉测量与运动控制的一体化，可直接对测量所得值进行运动学逆解，获得各轴运动控制量。具体优点包括：

1)基于双目视觉测量技术的标定***，使标定的步骤大大简化，实现了快速标定；

2)确定了自动装配对接过程所涉及的各坐标系，同时通过提出算法建立了坐标系体系的转换关系；

3)通过坐标系转换关系，快速计算实现对接所需运动量，建立对接平台运动解算模型，获得实现对接的各轴运动量。

4)采用开放式运动控制***，可在一台PC上编写界面软件，完成部段装配测量、运动控制的整体工作。

附图说明

本发明将通过例子并参照附图的方式说明，其中：

图1为视觉测量引导运载火箭部段对接示意图；

图2为运载火箭部段对接局部坐标系示意图，其中：

(a)为在运载火箭对接部段1、部段2对接平面上建立的局部对接参考系；

(b)为部段对接平面特征点示意图；

图3为运载火箭部段对接装配坐标系示意图；

图4为调姿平台虚拟调姿坐标系示意图；

图5为坐标系旋转矩阵变换示意图；

图6为调姿机构运动求解投影示意图。

具体实施方式

一种基于双目视觉测量的运载火箭大部段调姿对接方法，包括：A.建立全局坐标系O_q-X_qY_qZ_q，确定全局坐标系与相机坐标系的相对位置关系；B.要进行对接的运载火箭大部段有各自的局部坐标系；C.建立装配坐标系O-XYZ，作为部段装配的目标坐标系；D.调整运载火箭部段位姿保持一致，从而实现对接需要调姿平台的调姿坐标系；E.在运载火箭部段对接区域内布置测量靶标，建立全局坐标系；F.在运载火箭大部段端面布置测量靶标点，确定部段端面中心点及部段端面局部坐标系；G.在调姿平台加持机构布置靶标点，建立调姿坐标系；H.计算全局坐标系与局部坐标系间的转化关系；I.计算全局坐标系与调姿坐标系间的转化关系。最终将部段调姿量分解到调姿平台各运动轴，实现测量***内各坐标系位置关系的相对统一，为大部段的精确自动化对接提供数据支持。

1.本发明用于运载火箭大部段各级间的自动化对接，所述方法基于双目视觉测量技术，在该双目视觉测量***中，用到了四台相机，每两台为一组，分为两组双目视觉***，分别位于对接部段的前后两侧；由于需要检测的部段太大，可能会对检测视场造成阻碍，采用两组双目视觉***的测量分布方式可对此缺陷预先规避。

在此测量***中将全局坐标系作为部段的装配坐标系O_q-X_qY_qZ_q，全局坐标系Y_q轴正方向垂直于地面向下，X_q轴与对接平台轨道平行，Z_q轴方向由右手法则确定，坐标原点O_q位于标定板上任一点。其与相机坐标系位置关系通过线性标定法获得，该方法为成熟的相机内外参数标定方法，在此不做讨论。

2.前后部段分别建立局部坐标系O₁-X₁Y₁Z₁、O₂-X₂Y₂Z₂。O₁、O₂点分别位于前后部段对接平面的圆心，Y₁、Y₂指向第Ⅰ象限，X₁、X₂分别沿各自部段的中轴线指向运载火箭航向，Z₁、Z₂方向根据右手法则确定。坐标系建立及标定步骤如下所示：

部段对接平面特征点为圆周上的定位销孔，分别位于测量点Ⅰ、测量点Ⅱ、测量点Ⅲ、测量点Ⅳ，销孔对接方式分为对接与插接两种，在标定时需要通过转接工装在测点处安装靶标，靶标位置坐标依托定位销轴位置精度获得。以部段1对接平面为例，在全局坐标系O_q-X_qY_qZ_q下，四个靶标测量点坐标值记为C_i(i＝1，2，3，4)，C_i＝(X_qi，Y_qi，Z_qi)；

从中任取三点，由式(X_qi-X_Oq1)²+(Y_qi-Y_Oq1)²+(Z_qi-Z_Oq1)²＝R²(R为部段已知半径)，可得O_q1＝(X_Oq1，Y_Oq1，Z_Oq1)。由排列组合可知圆心点O_q1存在C₄ ³＝4个值，根据式

求取四个值的中心作为圆心点O_q1，以此减小误差。

在局部坐标系O₁-X₁Y₁Z₁下，四个靶标点依托销轴尺寸位置值获得，记为S_i(i＝1，2，3，4)，S_i＝(0，Y_1i，Z_1i)；

从中任取三点，由式(Y_1i-Y_O11)²+(Z_1i-Z_O11)²＝R²(R为部段已知半径)，可得O₁₁＝(0，Y_O11，Z_O11)。由排列组合可知圆心点O₁₁存在C₄ ³＝4个值，根据式

求取四个值的中心作为圆心点O₁₁，以此减小误差。

同理可获得部段2对接平面全局坐标系下的圆心坐标O_q2、局部坐标系下的圆心坐标O₂₂。

3.建立装配坐标系O-XYZ，装配坐标系各轴方向与全局坐标系相同，坐标原点O位于O_q1、O_q2连线的中点。坐标系建立及转换关系求取步骤如下所示：

在全局坐标系下O_q＝(O_q1+O_q2)/2。因此装配坐标系与全局坐标系存在平移转换关系^OT_Oq：

根据转换关系^OT_Oq，可求得部段对接平面靶标点、圆心点在装配坐标系下的坐标值C_Oi、O_O1。

坐标点C_Oi与S_i、O_O1与O₁₁为不同坐标系O-XYZ与O₁-X₁Y₁Z₁下的相同空间点，且坐标值已知。局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ存在如下式转换关系：

为求得局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ间的偏转关系矩阵，先求取已知点的中心：

通过式C_mi＝C_Oi-m，O_mO1＝O_O1-m，S_mi＝Si-n，O_m11＝O₁₁-n，将两坐标系平移到中心点处，采用牛顿迭代法可求出旋转变换矩阵^OR_O1，最终可求出^OT_O1。

同理可求解出局部坐标系O₂-X₂Y₂Z₂与装配坐标系O-XYZ间的转换关系^OR_O2，^OT_O2。

4.建立前托架局部坐标系O_J1-X_J1Y_J1Z_J1，后托架局部坐标系O_J2-X_J2Y_J2Z_J2，两坐标系各坐标轴方向与全局坐标系方向一致，坐标原点O_J1、O_J2分别位于托架圆心。坐标系建立及转换关系求取如下所示：

在前后托架上分别布置四个靶标点，每三个靶点之间不共线；调姿平台前后托架间距离为恒定值，在车架侧面，平行于地面轨道方向上贴靶标1与靶标2，两靶标距离为L；在全局坐标系下前托架上四个靶标点为J_i1(i＝1，2，3，4)，后托架上四个靶标点为J_i2(i＝1，2，3，4)，前后托架圆心坐标J₅₁、J₅₂，由此确定两局部托架坐标系与全局坐标系转换关系为^OqT_OJ1，^OqT_OJ2。

建立虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1，坐标原点O_t1为托架圆心坐标J₅₁、J₅₂连线的中点，X_t1轴沿圆心连线指向部段航向，平面O_t1-X_t1Z_t1恒定过托架局部坐标系O_J1-X_J1Y_J1Z_J1的Z_J1轴，即恒定过坐标系O_J1-X_J1Y_J1Z_J1下d点(0，0，R)，则Y_t1轴可由向量

获得，Z_t1轴由右手定则获得。最终求得虚拟调姿坐标系与装配坐标系的转换关系：旋转变换矩阵^OR_t1，偏移量^OT_t1。

5.选取托架中心原点为调姿控制点A、B，在装配坐标系下A_O＝(X_ao，Y_ao，Z_ao)，B_O＝(X_bo，Y_bo，Z_bo)，在调姿坐标系下A_t1＝(X_at1，Y_at1，Z_at1)，B_t1＝(X_bt1，Y_bt1，Z_bt1)。坐标系建立及转换关系求取如下所示：

以部段1为例，部段1夹持在托架上，相当于部段对接面局部坐标系与调姿坐标系刚体互联，在对接过程中，局部坐标系绕装配坐标系旋转的角度，即为调姿坐标系绕装配坐标系旋转的角度。

在旋转调姿过程中，先绕装配坐标系Y轴旋转，其旋转角度为α；再绕装配坐标系Z轴旋转，其旋转角度为β；最后绕装配坐标系X轴旋转，其旋转角度为γ。角度α、β、γ可由旋转变换矩阵^OR_O1获得。

装配坐标系下，为获得该角度转动量，控制点A、B坐标发生变化。当绕Y轴旋转时A_t1`＝(X<sub>at1</sub>，Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>`＝(X_bt1，Y_bt1，-Ltanα/2+Z_bt1)；当绕Z轴旋转时A_t1``＝(X<sub>at1</sub>，Ltanβ/2+Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>``＝(X_bt1，-Ltanβ/2+Y_bt1，-Ltanα/2+Z_bt1)；最后绕X轴做滚转运动，旋转γ角。

旋转调姿结束后，根据^OT_O1确定平移量，此时调姿点A、B同时同步运动该平移量。

运载火箭对接采用销孔定位的方式，其对接对象为大尺寸圆形筒段结构，由于自重筒段可能会发生变形；此时需要对变形量进行预测，若变形量在容差范围内，则继续执行对接任务；若变形量超出容差范围，则要补偿变形偏差，完成对接。

以上运动量(α，β，γ，x_T，y_T，z_T)即为控制***所需调节量。

以下结合附图对本发明方法详细说明如下：

具体实施方式一：结合图1、图2和图3说明本实施方式，本发明针对于在视觉测量引导下的运载火箭部段装配坐标系建立及控制量求解，其坐标系体系建立由参考靶标作为基点。

步骤1.视觉测量引导下的自动化装配对接***布局如图1所示，在运载火箭对接方位内放置标定板，选取标定板内一点为全局坐标系的原点O_q，垂直于地面向下为Y_q轴，对接平台轨道平行指向航向为X_q轴，Z_q轴方向由右手法则确定。

步骤2.根据直接线性法(DLT)公式：

(X_pi，Y_pi，Z_pi，1)为标定板上已知坐标点i，(u_i，v_i，1)为相机图像已知坐标点i，求取坐标点转换矩阵M。

步骤3.由线性摄像机模型可知，在实施方式一的步骤2方程内矩阵M包含了相机内外参数共11个未知量，需要6个以上已知点才能求解，而标定板上已知数目远远大于该数量，采用最小二乘法降低求解误差。

步骤4.相机内外参数标定完成后，去除标定板，放置标定板阻碍后续部段的自动对接。

步骤5.在运载火箭对接部段1、部段2对接平面上建立部段的局部对接参考系O₁-X₁Y₁Z₁、O₂-X₂Y₂Z₂；如图2所示，参考系原点位于各自对接平面的圆心点，X轴方向沿各自轴线指向航向，Y轴方向指向Ⅰ象限，Z轴方向由右手定则确定。

步骤6.部段对接平面特征点为圆周上的定位销孔，如图2所示，分别位于测量点Ⅰ、测量点Ⅱ、测量点Ⅲ、测量点Ⅳ，在标定时需要通过转接工装在测点处安装靶标，靶标位置坐标依托定位销轴位置精度获得。

步骤7.以部段1对接平面为例，在全局坐标系O_q-X_qY_qZ_q下，四个靶标测量点在全局坐标系下的坐标值在相机内外参数标定后可直接获得，记为C_i(i＝1，2，3，4)，C_i＝(X_qi，Y_qi，Z_qi)。

步骤8.从测量点中任取三点，部段对接平面半径R为已知值，将三点值分别代入式(X_qi-X_Oq1)²+(Y_qi-Y_Oq1)²+(Z_qi-Z_Oq1)²＝R²，可得O_q1＝(X_Oq1，Y_Oq1，Z_Oq1)。

步骤9.从测点中任取三点C₄ ³＝4种情况，因此求出对接平面圆心可能存在四个存在微小偏差的值，根据式

求取四个值的中心作为圆心点O_q1，以此减小误差。

步骤10.在局部坐标系O₁-X₁Y₁Z₁下，四个靶标点依托销轴尺寸位置值获得，记为S_i(i＝1，2，3，4)，S_i＝(0，Y_1i，Z_1i)。

步骤11.从中任取三点，由式(Y_1i-Y_O11)²+(Z_1i-Z_O11)²＝R²，可得O₁₁＝(0，Y_O11，Z_O11)。同理由排列组合可知圆心点O₁₁可能有C₄ ³＝4种值，根据式

求取四值的中心作为圆心点O₁₁，以此减小误差。

步骤12.同理可获得部段2对接平面全局坐标系下的圆心坐标O_q2、局部坐标系下的圆心坐标O₂₂。

步骤13.建立部段对接的装配坐标系O-XYZ，坐标原点O位于O_q1、O_q2的中点，装配坐标系各坐标轴方向与全局坐标系一致。

步骤14.此时部段对接平面局部坐标系原点在全局坐标下坐标已知为O_q1、O_q2，可得全局坐标系下装配坐标系原点为O_q＝(O_q1+O_q2)/2。

步骤15.因此装配坐标系与全局坐标系存在平移转换关系^OT_Oq：

步骤16.根据转换关系^OT_Oq，可求得部段对接平面靶标点、圆心点在装配坐标系下的坐标值C_Oi、O_O1。

步骤17.坐标点C_Oi与S_i、O_O1与O₁₁为不同坐标系O-XYZ与O₁-X₁Y₁Z₁下的相同空间点，且坐标值已知。局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ存在如下式转换关系：

具体实施方式二：结合图1与图5说明本实施方式，本发明针对于在视觉测量引导下的运载火箭部段局部坐标系与装配坐标系的转化求解关系。

步骤1.局部坐标系与装配坐标系转化关系可化为下式：

步骤2.为求得局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ间的偏转关系矩阵，先求取已知点的中心：

步骤3.通过式C_mi＝C_Oi-m，O_mO1＝O_O1-m，S_mi＝S_i-n，O_m11＝O₁₁-n，将两坐标系平移到中心点处，此时^OT_O1被去除，需求解量为^OR_O1。

步骤4.旋转矩阵^OR_O1按照图5方式确定可得式：

步骤5.将C_mi＝C_Oi-m，O_mO1＝O_O1-m，S_mi＝S_i-n，O_m11＝O₁₁-n代入式：

步骤6.展开获得方程组：

步骤7.将下式代入上式：

步骤8.设tan(α/2)＝X₁、tan(β/2)＝X₂、tan(γ/2)＝X₃；最终由实施方式二的步骤4可获得求未知数X₁、X₂、X₃的非线性方程：

步骤9.设X＝(X₁,X₂,X₃),F＝(f₁,f₂,f₃),求解非线性方程组。

X^(k+1)＝X^(k)-F(X^(k))^-1F(X^(k))

X^(k+1)＝X^(k)+ΔX^(k)

步骤10.非线性方程组雅克比矩阵可由下式求得：

步骤11.采用牛顿迭代法可求出旋转变换矩阵^OR_O1，采用最小二乘法求得最优解。将^OR_O1代入局部坐标系与装配坐标系的转换关系式，最终可求得^OT_O1。

步骤12.同理可求得部段2局部坐标系O₂-X₂Y₂Z₂与装配坐标系O-XYZ间的旋转矩阵^OR_O2及平移矩阵^OT_O2。

具体实施方式三：结合图4说明本实施方式，本发明针对于在视觉测量引导下，运载火箭部段自动对接平台虚拟调姿坐标系的标定建立提出了具体的实施方法。

步骤1.建立前托架局部坐标系O_J1-X_J1Y_J1Z_J1，后托架局部坐标系O_J2-X_J2Y_J2Z_J2，两坐标系各坐标轴方向与全局坐标系方向一致，坐标原点O_J1、O_J2分别位于托架圆心。

步骤2.在架车前后托架上分别布置四个靶标点，每三个靶点之间不共线。

步骤3.调姿平台前后托架间最短直线距离为恒定值，在车架侧面，平行于地面轨道方向上贴靶标1与靶标2，两靶标距离为L。

步骤4.由实施方式一步骤2中可获得全局坐标系下前托架上四个靶标点坐标值为J_i1(i＝1，2，3，4)，后托架上四个靶标点坐标值为J_i2(i＝1，2，3，4)。

步骤5.以前托架为例从测量点中任取三点，架车托架半径r为已知值，将三点值分别代入式(X_Ji-X_OJ1)²+(Y_Ji-Y_OJ1)²+(Z_Ji-Z_OJ1)²＝r²，可得O_J1＝(X_OJ1，Y_OJ1，Z_OJ1)。

步骤6.从测点中任取三点C₄ ³＝4种情况，因此求出对接平面圆心肯能存在四个存在微小偏差的值，根据式

求取四个值的中心作为圆心点O_J1，以此减小误差。

步骤7.将求取的前后托架圆心点坐标记为J₅₁、J₅₂，由此确定两局部托架坐标系与全局坐标系转换关系为^OqT_OJ1，^OqT_OJ2。

8.建立虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1，坐标原点O_t1为托架圆心坐标J₅₁、J₅₂连线的中点，即O_t1＝(J₅₁+J₅₂)/2。

步骤9.X_t1轴沿圆心连线指向部段航向，由式

求得该轴在全局坐标系中的单位方向向量。

步骤10.平面O_t1-X_t1Z_t1恒定过托架局部坐标系O_J1-X_J1Y_J1Z_J1的Z_J1轴，即恒定过坐标系O_J1-X_J1Y_J1Z_J1下d点(0，0，R)，则

步骤11.Y_t1轴可由向量

获得，Z_t1轴由右手定则

获得。

步骤12.虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1与全局坐标系O_q-X_qY_qZ_q转换关系可由实施方式二的步骤1中公式求得。

步骤13.由具体实施方式一已获得了全局坐标系与装配坐标系的转换关系，因此最终可获得虚拟调姿坐标系与装配坐标系的旋转变换矩阵^OR_t1，偏移量^OT_t1。

具体实施方式四：结合图4、图5、图6说明本实施方式，本发明针对于在视觉测量引导下，运载火箭部段自动化装配对接平台为完成部段的对接，各调姿控制点在装配坐标系下的控制量。

步骤1.以部段1装配对接平台为例，选取托架中心原点为调姿控制点A、B，在装配坐标系下A_O＝(X_ao，Y_ao，Z_ao)，B_O＝(X_bo，Y_bo，Z_bo)，在调姿坐标系下A_t1＝(X_at1，Y_at1，Z_at1)，B_t1＝(X_bt1，Y_bt1，Z_bt1)。

步骤2.部段1对接平面目标坐标系为装配坐标系，其对接面局部坐标与目标坐标系重合时所需运动转角及平移量可由^OR_O1、^OT_O1获得,其中

^OT_O1即为平移量。

步骤3.部段1夹持在架车托架上，通过夹持部段对接面局部坐标系与调姿坐标系刚体互连；介于刚体上各点的连续性，虚拟调姿坐标系绕装配坐标系旋转的角度，即为局部坐标系绕装配坐标系旋转的角度。

步骤4.由图5可知在旋转调姿过程中，先绕装配坐标系Y轴旋转，其旋转角度为α；再绕装配坐标系Z轴旋转，其旋转角度为β；最后绕装配坐标系X轴旋转，其旋转角度为γ。

步骤5.在装配坐标系下，为获得该角度转动量，控制点A、B坐标发生变化。当绕Y轴旋转时A_t1`＝(X<sub>at1</sub>，Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>`＝(X_bt1，Y_bt1，-Ltanα/2+Z_bt1)；当绕Z轴旋转时A_t1``＝(X<sub>at1</sub>，Ltanβ/2+Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>``＝(X_bt1，-Ltanβ/2+Y_bt1，-Ltanα/2+Z_bt1)；最后绕X轴做滚转运动，旋转γ角。

步骤6.旋转调姿结束后，根据^OT_O1确定平移量，此时调姿点A、B同时同步运动实现平移运动。

步骤7.为实现对接，部段2局部坐标系也以装配坐标系为目标坐标系，同理通过上述步骤实现调姿。

步骤8.部段2完成旋转调姿与Y轴、Z轴的平移运动，其X轴方向运动处于暂停状态，此时需要采用蒙特卡罗模拟法对大部段装配误差做预测。

步骤9.通过VSA建立装配部段CAD模型，将部段、销孔的公差标注在模型上。

步骤10.确定部段1、部段2向目标坐标系旋转、平移变换的顺序。

步骤11.确定部段质量，部段间装配间隙，及对应定位销孔的配合间隙。

步骤12.对装配过程进行仿真，对销孔处变形量进行记录输出。其偏差量在公差允许范围内则，执行X轴运动，完成装配。

步骤13.其偏差量不在公差范围内，将偏差量按照具体实施方式四步骤5分配到各运动轴，进行误差补偿，执行X轴运动，完成装配。

步骤14.在执行X轴运动完成装配前，要将靶标及转接工装取下，防止阻碍部段对接装配，最后实现X轴平移运动，完成对接。

Claims (7)

1.一种基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：包括如下步骤：

步骤一、建立全局坐标系O_q-X_qY_qZ_q；

步骤二、建立两个对接部段的局部坐标系O₁-X₁Y₁Z₁、O₂-X₂Y₂Z₂，其中：所述两个局部坐标系O₁-X₁Y₁Z₁、O₂-X₂Y₂Z₂的O₁、O₂点分别位于前后部段对接平面的圆心，Y₁、Y₂指向第Ⅰ象限，X₁、X₂分别沿各自部段的中轴线指向运载火箭航向，Z₁、Z₂方向根据右手法则确定；部段对接平面特征点为圆周上的定位销孔，分别位于测量点Ⅰ、测量点Ⅱ、测量点Ⅲ、测量点Ⅳ，在标定时通过转接工装在四个测量点处安装靶标，两个部段的对接平面在全局坐标系下的圆心坐标分别为O_q1和O_q2、在局部坐标系下的圆心坐标分别为O₁₁和O₂₂，其中：

所述O_q1和O₁₁的计算方法为：

(1)在全局坐标系O_q-X_qY_qZ_q下，四个靶标测量点的坐标值记为C_i，C_i＝(X_qi，Y_qi，Z_qi)，其中i＝(1，2，3，4)，从中任取三点，分别代入式(X_qi-X_Oq1)²+(Y_qi-Y_Oq1)²+(Z_qi-Z_Oq1)²＝R²，其中R为部段对接平面半径，计算得到O_q1＝(X_Oq1，Y_Oq1，Z_Oq1)；穷尽C₄ ³种组合后取均值得到O_q1；

(2)在局部坐标系O₁-X₁Y₁Z₁下，四个靶标测量点的坐标值记为S_i，S_i＝(0，Y_1i，Z_1i)，其中i＝(1，2，3，4)，从中任取三点，分别代入式(Y_1i-Y_O11)²+(Z_1i-Z_O11)²＝R²，计算得到O₁₁＝(0，Y_O11，Z_O11)；穷尽C₄ ³种组合后取均值得到O₁₁；

步骤三、建立装配坐标系O-XYZ，并分别确定两个局部坐标系O₁-X₁Y₁Z₁和O₂-X₂Y₂Z₂与装配坐标系O-XYZ间的转换关系矩阵；

步骤四、建立虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1；

步骤五、确定各调姿控制点在装配坐标系下的控制量。

2.根据权利要求1所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：步骤一所述全局坐标系O_q-X_qY_qZ_q的的Y_q轴正方向垂直于地面向下，X_q轴与对接平台轨道平行，Z_q轴方向由右手法则确定，坐标原点O_q位于标定板上任一点，全局坐标系与相机坐标系的位置关系通过线性标定法获得。

3.根据权利要求1所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：步骤三所述装配坐标系O-XYZ各轴方向与全局坐标系相同，坐标原点O位于O_q1、O_q2连线的中点，装配坐标系与全局坐标系存在如下平移转换关系：

4.根据权利要求3所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：所述局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ间的转换关系矩阵按如下方法确定：

(1)根据^OT_Oq计算得到部段对接平面靶标点、圆心点在装配坐标系下的坐标值C_Oi、O_O1；

(2)建立局部坐标系O₁-X₁Y₁Z₁与装配坐标系O-XYZ的转换关系式：

其中：^OR_O1为旋转变换矩阵，^OT_O1为偏转关系矩阵；

(3)按下式计算中心点(m，n)：

(4)通过式C_mi＝C_Oi-m，O_mO1＝O_O1-m；S_mi＝S_i-n，O_m11＝O₁₁-n，将两坐标系平移到中心点处，然后采用牛顿迭代法求出^OR_O1，进而最终求出^OT_O1。

5.根据权利要求4所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：步骤四所述虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1的建立方法为：

(1)建立前托架局部坐标系O_J1-X_J1Y_J1Z_J1和后托架局部坐标系O_J2-X_J2Y_J2Z_J2，两坐标系各坐标轴方向与全局坐标系方向一致，坐标原点O_J1、O_J2分别位于托架圆心；

(2)在前、后托架上分别布置四个靶标点，每三个靶点之间不共线；调姿平台前后托架间最短直线距离为恒定值，在车架侧面平行于地面轨道方向上贴第一靶标和第二靶标，两靶标距离为L；在全局坐标系下前托架上四个靶标点为J_i1，后托架上四个靶标点为J_i2，i＝(1，2，3，4)，求得前后托架圆心坐标J₅₁和J₅₂，进而确定两个局部托架坐标系与全局坐标系转换关系^OqT_OJ1和^OqT_OJ2；

(3)建立虚拟调姿坐标系O_t1-X_t1Y_t1Z_t1，坐标原点O_t1为前后托架圆心坐标J₅₁、J₅₂连线的中点，X_t1轴沿圆心连线指向部段航向，Y_t1轴由向量

获得，其中：

Z_t1轴由右手定则获得；

(4)利用全局坐标系与装配坐标系的转换关系求得虚拟调姿坐标系与装配坐标系转换关系中的旋转变换矩阵^OR_t1和偏移量^OT_t1。

6.根据权利要求5所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：步骤五所述确定各调姿控制点在装配坐标系下的控制量包括如下步骤：

(1)选取第一部段前后托架中心原点作为调姿控制点A和B，在装配坐标系下A_O＝(X_ao，Y_ao，Z_ao)，B_O＝(X_bo，Y_bo，Z_bo)，在调姿坐标系下A_t1＝(X_at1，Y_at1，Z_at1)，B_t1＝(X_bt1，Y_bt1，Z_bt1)；

(2)在旋转调姿过程中，先绕装配坐标系Y轴旋转，其旋转角度为α；再绕装配坐标系Z轴旋转，其旋转角度为β；最后绕装配坐标系X轴旋转，其旋转角度为γ，其中α、β、γ由旋转变换矩阵^OR_O1获得；当绕Y轴旋转时，控制点A、B坐标变化为：A_t1`＝(X<sub>at1</sub>，Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>`＝(X_bt1，Y_bt1，-Ltanα/2+Z_bt1)；当绕Z轴旋转时，控制点A、B坐标变化为：A_t1``＝(X<sub>at1</sub>，Ltanβ/2+Y<sub>at1</sub>，Ltanα/2+Z<sub>at1</sub>)，B<sub>t1</sub>``＝(X_bt1，-Ltanβ/2+Y_bt1，-Ltanα/2+Z_bt1)；

(3)旋转调姿结束后，根据^OT_O1确定平移量(x_T，y_T，z_T)，此时调姿点A、B同时同步运动该平移量；

(4)第二部段按同样的方式完成旋转调姿与Y轴、Z轴的平移运动，其X轴方向运动处于暂停状态，采用蒙特卡罗模拟法对变形量进行预测，若变形量在容差范围内，则继续执行对接任务；若变形量超出容差范围，则补偿变形偏差后完成对接。

7.根据权利要求6所述的基于双目视觉测量的运载火箭大部段调姿对接方法，其特征在于：采用蒙特卡罗模拟法对变形量进行预测的方法为：

(1)通过VSA建立装配部段CAD模型，将部段、销孔的公差标注在模型上；

(2)确定第一部段和第二部段向目标坐标系旋转、平移变换的顺序；

(3)确定部段质量，部段间装配间隙，及对应定位销孔的配合间隙；

(4)对装配过程进行仿真，对销孔处变形量进行记录输出；

(5)判断销孔处变形量是否在公差允许范围内：若是，则，执行X轴运动，完成装配；若否，则将变形量分配到各运动轴进行误差补偿，然后执行X轴运动，完成装配。

Images