技术领域
[0001] 本
发明涉及飞机装配技术与装备领域，尤其涉及一种含双C轴的六轴自动化制孔锪窝机床的运动学反解方法。
背景技术
[0002] 随着现代军用飞机设计
水平的不断提升，对飞机隐身性能不断追求，整体式机翼被广泛应用。在整体式机翼的装配中，使用椭圆头无
耳托板
螺母实现机翼壁板与机翼骨架之间的单侧固定。在使用这种特殊
紧固件前，需要在机翼骨架上加工类似椭圆形的装配孔(简称椭圆窝)。
[0003] 传统椭圆窝加工方法采用一种手工加工工具，依靠工人技术水平控制椭圆窝的窝形和窝深，不仅加工步骤繁琐、效率低，而且加工
质量的
稳定性难以保证，大大降低了整体式机翼的装配质量和可靠性。因此，设计研发了该六轴自动化制孔锪窝机床，以提高椭圆窝的加工效率和质量，保证整体式机翼的装配质量和可靠性，为军用飞机装配技术发展做出贡献。
[0004] 该六轴自动化制孔锪窝机床采用双C轴结构形式，共有X、Y、Z、C1、A、C2轴六个运动轴。通常，运动学反解方法有
解析法和数值法。其中，解析法计算速度快、
精度高，但仅适用于相互垂直、且交于一点的运动轴；而数值法适用范围广，但计算速度和精度受限。单一的方法均不适用于该六轴自动化制孔锪窝机床。同时，由于双C轴的结构形式，运动学反解中易产生相互影响，求解时易造成求解不准确的情况；且三个转动轴行程都比较大，同时求解时易出现周期性多解的情况。例如：分析六轴自动化制孔锪窝机床的
运动学模型可以发现，至少有两组转动量能满足椭圆窝加工要求，即[θ5,θ6,θ7]和[θ5±π,-θ6,θ7±π]。
[0005] 针对含双C轴的六轴自动化制孔锪窝机床，综合考虑运动学反解的计算速度、计算精度与稳定性，亟需提出一种切实有效的运动学反解
算法，用于机床的运动控制，以实现自动化椭圆窝加工。
发明内容
[0006] 本发明为克服
现有技术的不足，本发明提出一种含双C轴的六轴自动化制孔锪窝机床的运动学反解方法，可实现对六轴自动化制孔锪窝机床的运动学正反解，保证机床运动控制的准确性，以实现椭圆窝自动化加工。
[0007] 本发明的技术方案为：一种含双C轴的六轴自动化制孔锪窝机床的运动学反解方法，包括如下步骤：
[0008] (1)在六轴自动化制孔锪窝机床中，建立设备基
坐标系、各运动轴子坐标系和刀具坐标系，并绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致；
[0009] (2)对六轴自动化制孔锪窝机床作运动学分析，建立正向运动学模型；
[0010] (3)根据六轴自动化制孔锪窝机床的目标
位姿和正向运动学模型，建立关节量的方程组，并利用关节量分离求解的运动学反解策略求解各关节量，以得到其逆向运动学模型。
[0011] 所述的步骤(1)的具体步骤如下：
[0012] (1.1)定义六轴自动化制孔锪窝机床中的各坐标系：基坐标系O0，运动轴X、Y、Z、C1、A、C2所对应的子坐标系O1、O2、O3、O4、O5、O6，及刀具坐标系O7；
[0013] (1.2)绘制六轴自动化制孔锪窝机床的坐标系定义图，各子坐标系的坐标轴方向与设备基坐标系的坐标轴方向一致。
[0014] 所述的步骤(2)的具体步骤如下：
[0015] (2.1)对六轴自动化制孔锪窝机床作运动学分析，分别计算各运动轴的齐次变换矩阵：
[0016]
[0017]
[0018]
[0019] 其中，T01是从坐标系O0到坐标系O1的理想齐次变换矩阵，T12是从坐标系O1到坐标系O2的理想齐次变换矩阵，T23是从坐标系O2到坐标系O3的理想齐次变换矩阵，T34是从坐标系O3到坐标系O4的理想齐次变换矩阵，T45是从坐标系O4到坐标系O5的理想齐次变换矩阵，T56是从坐标系O,5到坐标系O6的理想齐次变换矩阵，T67是从坐标系O6到坐标系O7的理想齐次变换矩阵；d1,d2,d3,θ4,θ5,θ6分别是六轴自动化制孔锪窝机床X、Y、Z、C1、A、C2轴的关节量；x7,y7,z7是最后一个运动轴子坐标系到刀具坐标系在x,y,z方向的位移偏置；
[0020] (2.2)建立六轴自动化制孔锪窝机床的正向运动学模型：
[0021]
[0022] 其中，T07是从基坐标系O0到刀具坐标系O7的理想齐次变换矩阵，为六轴自动化制孔锪窝机床的正向运动学模型，表示六轴自动化制孔锪窝机床末端的理论位姿，R(q)为3×3的刀具坐标系
姿态矩阵，P(q)为3×1的刀具坐标系
位置矩阵。姿态矩阵中，[r11,r21,r31]T是刀具坐标系X轴的姿态向量，[r12,r22,r32]T是刀具坐标系Y轴的姿态向量，[r13,r23,r33]T是刀具坐标系Z轴的姿态向量。
[0023] 分析步骤(2.2)中的正向运动学模型可知，平动轴关节量只影响机床末端位置，转动轴关节量既影响机床末端位置又影响机床末端姿态；姿态矩阵中，刀具坐标系Z轴的姿态T向量[r13,r23,r33] 只与转动轴关节量θ4,θ5有关，与θ6无关，刀具坐标系X轴和Y轴的姿态向量[r11,r21,r31]T和[r12,r22,r32]T，与θ4,θ5,θ6均有关。
[0024] 所述的步骤(3)的具体步骤如下：
[0025] (3.1)根据
工件表面的法向及制孔位置要求，将六轴自动化制孔锪窝机床的末端目标位姿表示为：
[0026]
[0027] 其中，Td为机床刀具坐标系的目标位姿矩阵，Pd为刀具坐标系的目标位置矩阵，Rd为刀具坐标系的目标姿态矩阵，[ux,uy,uz]T是刀具坐标系X轴的目标姿态向量，[vx,vy,vz]T是刀具坐标系Y轴的目标姿态向量，[wx,wy,wz]T是刀具坐标系Z轴的目标姿态向量，[ox,oy,oz]T是目标位置向量。
[0028] (3.2)令六轴自动化制孔锪窝机床末端的理论位姿等于目标位姿状态，用公式表示为：
[0029] T07＝Td
[0030] (3.3)根据矩阵对应元素相等，建立关节量的方程组，如下：
[0031]
[0032] 其中，方程ox、oy、、oz称为机床末端位置约束方程，方程wx、wy、、wz称为刀具轴线方向约束方程，方程vx、vy、、vz称为椭圆窝长轴方向约束方程；运动学反解过程即求解上述方程组，以解得各关节量的表达式；
[0033] (3.4)利用关节量分离求解的运动学反解策略求解关节量。
[0034] 所述的关节量分离求解的运动学反解策略包括转动量与平动量的分离求解、转动量之间的分离求解对应到六轴自动化制孔锪窝机床的运动学反解过程中，表现为机床各关节量的求解顺序。具体流程为：
[0035] 首先，根据刀具轴线方向约束方程，采用L-M非线性优化法求解转动轴C1轴的关节量θ4和A轴的关节量θ5，该过程的目标函数为：
[0036] J(q)＝Jori(q)＝Ewx2+Ewy2+Ewz2
[0037] 其中，
[0038] Ewx＝sinθ4sinθ5-wx
[0039] Ewy＝-cosθ4sinθ5-wy
[0040] Ewz＝cosθ5-wz
[0041] 然后，根据求得的θ4、θ5和椭圆窝长轴方向约束方程，采用解析法求解C2轴的关节量θ6，表达式为：
[0042] θ6＝arccos(vz/sinθ5)
[0043] 最后，根据求得的θ4、θ5、θ6和机床末端位置约束方程，采用解析法求解平动轴关节量d1,d2,d3；表达式为：
[0044] d1＝ox-x7(cosθ4cosθ6-sinθ4cosθ5sinθ6)
[0045] -y7(-cosθ4sinθ6-sinθ4cosθ5cosθ6)-z7sinθ4sinθ5
[0046] d2＝oy-x7(sinθ4cosθ6+cosθ4cosθ5sinθ6)
[0047] -y7(-sinθ4sinθ6+cosθ4cosθ5cosθ6)+z7cosθ4sinθ5
[0048] d3＝oz-x7sinθ5sinθ6-y7sinθ5cosθ6-z7cosθ5
[0049] 由此，实现对含双C轴的六轴自动化制孔锪窝机床的运动学反解，得到含双C轴的六轴自动化制孔锪窝机床的逆向运动学模型。
[0050] 步骤(3.2)中，为通过运动学反解求得关节量q，需要使六轴自动化制孔锪窝机床末端的理论位姿达到目标位姿状态，即T07＝Td。
[0051] 步骤(3.3)中，由于影响制孔质量的主要因素是孔的位置和孔的轴线方向，对应为机床的末端位置和
主轴进给方向的姿态，即[ox,oy,oz]T和[wx,wy,wz]T，当有锪窝方向要求时，还应考虑与此相关的姿态向量，即[vx,vy,vz]T，因此，根据矩阵对应元素相等，建立关节量的方程组。
[0052] 步骤(3.4)中，所述的关节量分离求解的运动学反解策略是经过分析六轴自动化制孔锪窝机床正向运动学模型的特点，并综合考虑方程组求解的计算精度、计算速度和稳定性提出的。
[0053] 与现有技术相比，本发明的优点在于：
[0054] (1)针对含双C轴的六轴自动化制孔锪窝机床，提出了一种关节量分离求解的运动学反解策略，将平动轴与转动轴分离求解，转动轴之间分离求解，保证了运动学反解过程的准确性和稳定性；
[0055] (2)针对含双C轴的六轴自动化制孔锪窝机床的结构特点，采用解析法与数值法相结合的运动学反解方法，最大程度地保证了运动学反解过程计算速度和计算精度，提高了该运动学反解方法的鲁棒性；
[0056] (3)实现了六轴自动化制孔锪窝机床的运动学正反解，为机床运动控制奠定
基础，实现了椭圆窝自动化加工，提高了飞机装配自动化程度。
附图说明
[0057] 图1六轴自动化制孔锪窝机床的坐标系定义图；
[0058] 图2六轴自动化制孔锪窝机床的运动学反解
流程图。
具体实施方式
[0059] 为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。
[0060] 如图1所示，六轴自动化制孔锪窝机床采用双C轴的结构形式，定义该机床的基坐标系、各运动轴子坐标系和刀具坐标系；
[0061] 如图2所示，为六轴自动化制孔锪窝机床的运动学反解流程图，包括运动学反解过程的步骤和相应算法。
[0062] 如图1所示，本
实施例一种含双C轴的六轴自动化制孔锪窝机床的运动学反解方法的步骤如下：
[0063] 步骤1：在六轴自动化制孔锪窝机床中，建立设备基坐标系、各运动轴子坐标系和刀具坐标系；绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致。具体分为以下两步：
[0064] 步骤1.1：定义六轴自动化制孔锪窝机床中的各坐标系：基坐标系O0，运动轴X、Y、Z、C1、A、C2轴所对应的子坐标系O1、O2、O3、O4、O5、O6，及刀具坐标系O7；
[0065] 步骤1.2：绘制六轴自动化制孔锪窝机床的坐标系定义图，如图2所示，各子坐标系的坐标轴方向与设备基坐标系的坐标轴方向一致。
[0066] 步骤2：对六轴自动化制孔锪窝机床作运动学分析，建立正向运动学模型。具体分为以下两步：
[0067] 步骤2.1：对六轴自动化制孔锪窝机床作运动学分析，分别计算各运动轴的齐次变换矩阵：
[0068]
[0069]
[0070]
[0071] 其中，T01是从坐标系O0到坐标系O1的理想齐次变换矩阵，T12是从坐标系O1到坐标系O2的理想齐次变换矩阵，T23是从坐标系O2到坐标系O3的理想齐次变换矩阵，T34是从坐标系O3到坐标系O4的理想齐次变换矩阵，T45是从坐标系O4到坐标系O5的理想齐次变换矩阵，T56是从坐标系O,5到坐标系O6的理想齐次变换矩阵，T67是从坐标系O6到坐标系O7的理想齐次变换矩阵；d1,d2,d3,θ4,θ5,θ6分别是六轴自动化制孔锪窝机床X、Y、Z、C1、A、C2轴的关节量；x7,y7,z7是最后一个运动轴子坐标系到刀具坐标系在x,y,z方向的位移偏置；
[0072] 步骤2.2：建立六轴自动化制孔锪窝机床的正向运动学模型：
[0073]
[0074] 其中，T07是从基坐标系O0到刀具坐标系O7的齐次变换矩阵，即为六轴自动化制孔锪窝机床的正向运动学模型。R(q)为3×3的刀具坐标系姿态矩阵，P(q)为3×1的刀具坐标系位置矩阵。姿态矩阵中，[r11,r21,r31]T是刀具坐标系X轴的姿态向量，[r12,r22,r32]T是刀具T坐标系Y轴的姿态向量，[r13,r23,r33] 是刀具坐标系Z轴的姿态向量。
[0075] 分析该模型可知，平动轴关节量只影响机床末端位置，转动轴关节量既影响机床末端位置又影响机床末端姿态；姿态矩阵中，刀具坐标系Z轴的姿态向量[r13,r23,r33]T只与转动轴关节量θ4,θ5有关，与θ6无关，刀具坐标系X轴和Y轴的姿态向量[r11,r21,r31]T和[r12,r22,r32]T，与θ4,θ5,θ6均有关。
[0076] 步骤3：根据六轴自动化制孔锪窝机床的目标位姿和正向运动学模型，建立关节量的方程组；提出一种关节量分离求解的运动学反解策略，解析法与数值法相结合求解各关节量，以得到其逆向运动学模型。具体包括以下几步：
[0077] 步骤3.1：根据工件表面的法向及制孔位置要求等，可将六轴自动化制孔锪窝机床的末端目标位姿表示为：
[0078]
[0079] 其中，Td为机床刀具坐标系的目标位姿矩阵，Pd为刀具坐标系的目标位置矩阵，Rd为刀具坐标系的目标姿态矩阵，[ux,uy,uz]T是刀具坐标系X轴的目标姿态向量，[vx,vy,vz]T是刀具坐标系Y轴的目标姿态向量，[wx,wy,wz]T是刀具坐标系Z轴的目标姿态向量，[ox,oy,oz]T是目标位置向量。
[0080] 步骤3.2：为通过运动学反解所求得关节量q，需要使六轴自动化制孔锪窝机床末端的理论位姿达到目标位姿状态，即：
[0081] T07＝Td
[0082] 步骤3.3：由于影响制孔质量的主要因素是孔的位置和孔的轴线方向，对应为机床的末端位置和主轴进给方向的姿态，即[ox,oy,oz]T和[wx,wy,wz]T。当有锪窝方向要求时，还应考虑与此相关的姿态向量，即[vx,vy,vz]T。因此，根据矩阵对应元素相等，建立关节量的方程组，如下：
[0083]
[0084] 其中，方程ox、oy、、oz称为机床末端位置约束方程，方程wx、wy、、wz称为刀具轴线方向约束方程，方程vx、vy、、vz称为椭圆窝长轴方向约束方程；运动学反解过程即求解上述方程组，以解得各关节量的表达式；
[0085] 步骤3.4：经过分析六轴自动化制孔锪窝机床正向运动学模型的特点，并综合考虑方程组求解的计算精度、计算速度和稳定性，提出一种关节量分离求解的运动学反解策略。该策略包括两个方面：1)转动量与平动量的分离求解；2)转动量之间的分离求解。
[0086] 对应到六轴自动化制孔锪窝机床的运动学反解过程中，表现为机床各关节量的求解顺序。具体流程为：
[0087] 首先，根据刀具轴线方向约束方程，采用L-M非线性优化法求解转动轴C1轴和A轴关节
角，即θ4和θ5；该过程的目标函数为：
[0088] J(q)＝Jori(q)＝Ewx2+Ewy2+Ewz2
[0089] 其中，
[0090] Ewx＝sinθ4sinθ5-wx
[0091] Ewy＝-cosθ4sinθ5-wy
[0092] Ewz＝cosθ5-wz
[0093] 然后，根据求得的θ4、θ5和椭圆窝长轴方向约束方程，采用解析法求解C2轴关节量，即θ6；表达式为：
[0094] θ6＝arccos(vz/sinθ5)
[0095] 最后，根据求得的θ4、θ5、θ6和机床末端位置约束方程，采用解析法求解平动轴关节量，即d1,d2.d3；表达式为：
[0096] d1＝ox-x7(cosθ4cosθ6-sinθ4cosθ5sinθ6)
[0097] -y7(-cosθ4sinθ6-sinθ4cosθ5cosθ6)-z7sinθ4sinθ5
[0098] d2＝oy-x7(sinθ4cosθ6+cosθ4cosθ5sinθ6)
[0099] -y7(-sinθ4sinθ6+cosθ4cosθ5cosθ6)+z7cosθ4sinθ5
[0100] d3＝oz-x7sinθ5sinθ6-y7sinθ5cosθ6-z7cosθ5
[0101] 由此，实现对含双C轴的六轴自动化制孔锪窝机床的运动学反解。
[0102] 以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的最优选实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何
修改、补充和等同替换等，均应包含在本发明的保护范围之内。
