CN106843151A - 一种进给轴随ab轴摆动的五轴数控制孔机床正反解方法 - Google Patents

Abstract

本发明公开了一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法，包括如下步骤：(1)在异型五轴数控制孔机床中，建立设备基坐标系、各运动轴子坐标系和刀具坐标系；绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致。(2)对异型五轴数控制孔机床作运动学分析，建立正向运动学模型。(3)根据异型五轴数控制孔机床的目标位姿和正向运动学模型，建立关节量的方程组，推导其逆向运动学模型。该方法实现了对进给轴随AB轴摆动的异型五轴数控制孔机床的运动学正反解，为机床运动控制奠定基础。

Description

一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法

技术领域

本发明涉及飞机装配技术与装备领域，尤其涉及一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法。

背景技术

飞机装配很大程度上决定了飞机的最终质量、制造成本和交货周期，是飞机制造过程中的关键和核心技术。其中，预连接是飞机装配中的重要工艺，是保证后续制孔加工精度和飞机装配质量的重要环节。实现自动化预连接是全面实现飞机装配自动化进程中不可缺少的一部分。

该异型五轴数控制孔机床是飞机装配中重要的自动化制孔与预连接设备，能够自动完成制孔、插钉、旋钉等操作。该机床依次有X、Y、B、A、Z五个运动轴，采用平动轴与转动轴交叉分布的结构形式，与通用五轴机床中先平动轴后转动轴的结构形式不同，导致西门子五轴联动包不能直接用于该异型五轴数控制孔机床。

针对异型五轴数控制孔机床中进给轴随AB轴摆动的特殊结构，亟需提出一种可行的运动学正反解算法，用于机床的运动控制，以实现自动化预连接操作，从而提高飞机装配自动化程度。

发明内容

本发明为克服现有技术的不足，本发明提出一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法，可实现对异型五轴数控制孔机床的运动学正反解，以实现自动化预连接操作，从而提高飞机装配自动化程度。

本发明的技术方案为：一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法，包括如下步骤：

(1)在异型五轴数控制孔机床中，建立设备基坐标系、各运动轴子坐标系和刀具坐标系，并绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致；

(2)对异型五轴数控制孔机床作运动学分析，建立正向运动学模型；

(3)根据异型五轴数控制孔机床的目标位姿和正向运动学模型，建立关节量的方程组，推导其逆向运动学模型。

所述的步骤(1)的具体步骤如下：

(1.1)定义异型五轴数控制孔机床中的各坐标系：基坐标系O_base，各运动轴子坐标系O_X、O_Y、O_B、O_A、O_Z，刀具坐标系O_T；

(1.2)绘制异型五轴数控制孔机床的坐标系定义图，各子坐标系的坐标轴方向与设备基坐标系的坐标轴方向一致。

所述的步骤(2)的具体步骤如下：

(2.1)对异型五轴数控制孔机床作运动学分析，分别计算各运动轴的齐次变换矩阵：

其中，是从坐标系O_base到坐标系O_X的理想齐次变换矩阵，从坐标系O_X到坐标系O_Y的理想齐次变换矩阵、从坐标系O_Y到坐标系O_B的理想齐次变换矩阵、从坐标系O_B到坐标系O_A的理想齐次变换矩阵、从坐标系O_A到坐标系O_Z的理想齐次变换矩阵、从坐标系O_Z到坐标系O_T的理想齐次变换矩阵，d₁,d₂,θ₃,θ₄,d₅分别是异型五轴数控制孔机床X、Y、B、A、Z轴的关节量；x₆,y₆,z₆是最后一个运动轴子坐标系到刀具坐标系在x,y,z方向的位移偏置；

(2.2)建立异型五轴数控制孔机床的正向运动学模型：

其中，是理想情况下从基坐标系O_base到刀具坐标系O_T的齐次变换矩阵，即为异型五轴数控制孔机床的正向运动学模型，表示异型五轴数控制孔机床末端的理论位姿；R(q)为3×3的刀具坐标系的姿态矩阵，P(q)为3×1的刀具坐标系的位置矩阵，姿态矩阵中，[r₁₁,r₂₁,r₃₁]^T是刀具坐标系X轴的姿态向量，[r₁₂,r₂₂,r₃₂]^T是刀具坐标系Y轴的姿态向量，[r₁₃,r₂₃,r₃₃]^T是刀具坐标系Z轴的姿态向量。

所述的步骤(3)的具体步骤如下：

(3.1)根据工件表面的法向及制孔位置要求，将异型五轴数控制孔机床的末端目标位姿表示为：

其中，T_d为刀具坐标系的目标位姿矩阵，P_d为刀具坐标系的目标位置矩阵，R_d为刀具坐标系的目标姿态矩阵。[u_x,u_y,u_z]^T是刀具坐标系X轴的目标姿态向量，[v_x,v_y,v_z]^T是刀具坐标系Y轴的目标姿态向量，[w_x,w_y,w_z]^T是刀具坐标系Z轴的目标姿态向量，[o_x,o_y,o_z]^T是目标位置向量；

(3.2)令异型五轴数控制孔机床末端的理论位姿等于目标位姿状态，用公式表示为：

(3.3)根据矩阵对应元素相等，建立关节量的方程组：

o_x＝p_x＝d₁+d₅sinθ₃cosθ₄+x₆cosθ₃+y₆sinθ₃sinθ₄+z₆sinθ₃cosθ₄

o_y＝p_y＝d₂-d₅sinθ₄+y₆cosθ₄-z₆sinθ₄

o_z＝p_z＝d₅cosθ₃cosθ₄-x₆sinθ₃+y₆cosθ₃sinθ₄+z₆cosθ₃cosθ₄

w_x＝r₁₃＝sinθ₃cosθ₄

w_y＝r₂₃＝-sinθ₄

w_z＝r₃₃＝cosθ₃cosθ₄

(3.4)对步骤(3.3)中的关节量的方程组进行推导，得到异型五轴数控制孔机床的逆向运动学模型：

θ₃＝-atan2(w_x,w_z)

d₅＝(o_z+x₆sinθ₃-y6cosθ₃sinθ₄-z₆cosθ₃cosθ₄)/(cosθ₃cosθ₄)

d₁＝o_x-d₅sinθ₃cosθ₄-x₆cosθ₃-y₆sinθ₃sinθ₄-z₆sinθ₃cosθ₄

d₂＝o_y+d₅sinθ₄-y₆cosθ₄+z₆sinθ₄

其中，cosθ₃cosθ₄≠0其中，且θ₃∈[-8°,8°],θ₄∈[-12.5°,12.5°]。

步骤(3.2)中，为通过运动学反解求得关节量q，需要使异型五轴数控制孔机床末端的理论位姿达到目标位姿状态，即

步骤(3.3)中，由于影响制孔质量的主要因素是孔的位置和孔的轴线方向，对应为机床的末端位置和主轴进给方向的姿态，即[o_x,o_y,o_z]^T和[w_x,w_y,w_z]^T，根据矩阵对应元素相等，建立关节量的方程组。

将以上获得逆向运动学模型输入到异型五轴数控制孔机床中，并输入异型五轴数控制孔机床的末端目标位姿，该机床即根据逆向运动学模型和输入的末端目标位姿计算得到刀具的进给量及进给路径。

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

(1)实现了对进给轴随AB轴摆动的异型五轴数控制孔机床的运动学正反解，为机床运动控制奠定基础；

(2)该算法针对平动轴与转动轴交叉部分的结构形式，弥补了西门子五轴联动包的不足；

(3)该异型五轴数控制孔机床可实现自动化预连接工艺，大大提高了飞机装配自动化程度。

附图说明

图1是本发明异型五轴数控制孔机床BAZ轴部件的正视图；

图2是本发明异型五轴数控制孔机床BAZ轴部件的俯视图；

图3是本发明异型五轴数控制孔机床BAZ轴部件的左视图；

图4是本发明进给轴随AB轴摆动的五轴数控制孔机床正反解方法的流程框图；

图5是本发明异型五轴数控制孔机床的坐标系定义图。

具体实施方式

为了更为具体地描述本发明，下面结合附图及具体实施方式对本发明的技术方案进行详细说明。

图1～图3所示的是异型五轴数控制孔机床BAZ轴部件是进给轴随AB轴摆动的结构形式，本实施例所提出的进给轴随AB轴摆动的五轴数控制孔机床正反解方法应用于该异型五轴数控制孔机床。

如图4所示，本实施例一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法的步骤如下：

(1)在异型五轴数控制孔机床中，建立设备基坐标系、各运动轴子坐标系和刀具坐标系；绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致。具体分为以下两步：

1.1：定义异型五轴数控制孔机床中的各坐标系：基坐标系O_base，各运动轴子坐标系O_X、O_Y、O_B、O_A、O_Z，刀具坐标系O_T；

1.2：绘制异型五轴数控制孔机床的坐标系定义图，各子坐标系的坐标轴方向与设备基坐标系的坐标轴方向一致，如图5所示。

(2)对异型五轴数控制孔机床作运动学分析，建立正向运动学模型。具体分为以下两步：

2.1：对异型五轴数控制孔机床作运动学分析，分别计算各运动轴的齐次变换矩阵：

其中，是从坐标系O_base到坐标系O_X的理想齐次变换矩阵，从坐标系O_X到坐标系O_Y的理想齐次变换矩阵、从坐标系O_Y到坐标系O_B的理想齐次变换矩阵、从坐标系O_B到坐标系O_A的理想齐次变换矩阵、从坐标系O_A到坐标系O_Z的理想齐次变换矩阵、从坐标系O_Z到坐标系O_T的理想齐次变换矩阵，d₁,d₂,θ₃,θ₄,d₅分别是异型五轴数控制孔机床X、Y、B、A、Z轴的关节量；x₆,y₆,z₆是最后一个运动轴子坐标系到刀具坐标系在x,y,z方向的位移偏置。

2.2：建立异型五轴数控制孔机床的正向运动学模型：

其中，是理想情况下从基坐标系O_base到刀具坐标系O_T的齐次变换矩阵，即为异型五轴数控制孔机床的正向运动学模型，表示异型五轴数控制孔机床末端的理论位姿；R(q)为3×3的刀具坐标系的姿态矩阵，P(q)为3×1的刀具坐标系的位置矩阵，姿态矩阵中，[r₁₁,r₂₁,r₃₁]^T是刀具坐标系X轴的姿态向量，[r₁₂,r₂₂,r₃₂]^T是刀具坐标系Y轴的姿态向量，[r₁₃,r₂₃,r₃₃]^T是刀具坐标系Z轴的姿态向量。

(3)根据异型五轴数控制孔机床的目标位姿和正向运动学模型，建立关节量的方程组，推导其逆向运动学模型。具体分为以下两步：

3.1：根据工件表面的法向及制孔位置要求，将异型五轴数控制孔机床的末端目标位姿表示为：

其中，T_d为刀具坐标系的目标位姿矩阵，P_d为刀具坐标系的目标位置矩阵，R_d为刀具坐标系的目标姿态矩阵。[u_x,u_y,u_z]^T是刀具坐标系X轴的目标姿态向量，[v_x,v_y,v_z]^T是刀具坐标系Y轴的目标姿态向量，[w_x,w_y,w_z]^T是刀具坐标系Z轴的目标姿态向量，[o_x,o_y,o_z]^T是目标位置向量。

3.2：运动学反解所求得的关节量q，要使得异型五轴数控制孔机床末端的理论位姿达到目标位姿状态，即：

3.3：由于影响制孔质量的主要因素是孔的位置和孔的轴线方向，对应为机床的末端位置和主轴进给方向的姿态，即[o_x,o_y,o_z]^T和[w_x,w_y,w_z]^T。根据矩阵对应元素相等，建立关节量的方程组:

o_x＝p_x＝d₁+d₅sinθ₃cosθ₄+x₆cosθ₃+y₆sinθ₃sinθ₄+z₆sinθ₃cosθ₄

o_y＝p_y＝d₂-d₅sinθ₄+y₆cosθ₄-z₆sinθ₄

o_z＝p_z＝d₅cosθ₃cosθ₄-x₆sinθ₃+y₆cosθ₃sinθ₄+z₆cosθ₃cosθ₄

w_x＝r₁₃＝sinθ₃cosθ₄

w_y＝r₂₃＝-sinθ₄

w_z＝r₃₃＝cosθ₃cosθ₄

3.4：对步骤(3.3)中的关节量的方程组进行推导，得到异型五轴数控制孔机床的逆向运动学模型：

θ₃＝-atan2(w_x,w_z)

d₅＝(o_z+x₆sinθ₃-y₆cosθ₃sinθ₄-z₆cosθ₃cosθ₄)/(cosθ₃cosθ₄)

d₁＝o_x-d₅sinθ₃cosθ₄-x₆cosθ₃-y₆sinθ₃sinθ₄-z₆sinθ₃cosθ₄

d₂＝o_y+d₅sinθ₄-y₆cosθ₄+z₆sinθ₄

其中，cosθ₃cosθ₄≠0其中，且θ₃∈[-8°,8°],θ₄∈[-12.5°,12.5°]。

将以上获得逆向运动学模型输入到异型五轴数控制孔机床中，并输入异型五轴数控制孔机床的末端目标位姿，该机床即根据逆向运动学模型和输入的末端目标位姿计算得到刀具的进给量及进给路径。

以上所述的具体实施方式对本发明的技术方案和有益效果进行了详细说明，应理解的是以上所述仅为本发明的最优选实施例，并不用于限制本发明，凡在本发明的原则范围内所做的任何修改、补充和等同替换等，均应包含在本发明的保护范围之内。

Claims (4)

1.一种进给轴随AB轴摆动的五轴数控制孔机床正反解方法，包括如下步骤：

(1)在异型五轴数控制孔机床中，建立设备基坐标系、各运动轴子坐标系和刀具坐标系，并绘制坐标系定义图，使各子坐标系与设备基坐标系的坐标轴方向保持一致；

(2)对异型五轴数控制孔机床作运动学分析，建立正向运动学模型；

(3)根据异型五轴数控制孔机床的目标位姿和正向运动学模型，建立关节量的方程组，推导其逆向运动学模型。

2.根据权利要求1所述的进给轴随AB轴摆动的五轴数控制孔机床正反解方法，其特征在于，所述的步骤(1)的具体步骤如下：

(1.1)定义异型五轴数控制孔机床中的各坐标系：基坐标系O_base，各运动轴子坐标系O_X、O_Y、O_B、O_A、O_Z，刀具坐标系O_T；

(1.2)绘制异型五轴数控制孔机床的坐标系定义图，各子坐标系的坐标轴方向与设备基坐标系的坐标轴方向一致。

3.根据权利要求2所述的进给轴随AB轴摆动的五轴数控制孔机床正反解方法，其特征在于，所述的步骤(2)的具体步骤如下：

(2.1)对异型五轴数控制孔机床作运动学分析，分别计算各运动轴的齐次变换矩阵：

$\begin{array}{ccc}T_X^O=\left[\begin{array}{cccc}1& 0& 0& d_1\\ 0& 1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& T_Y^X=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& d_2\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]& T_B^Y=\left[\begin{array}{cccc}{\mathrm{cos\θ}}_3& 0& {\mathrm{sin\θ}}_3& 0\\ 0& 1& 0& 0\\ -{\mathrm{sin\θ}}_3& 0& {\mathrm{cos\θ}}_3& 0\\ 0& 0& 0& 1\end{array}\right]\end{array}$

$\begin{array}{ccc}T_A^B=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& {\mathrm{cos\θ}}_4& -{\mathrm{sin\θ}}_4& 0\\ 0& {\mathrm{sin\θ}}_4& {\mathrm{cos\θ}}_4& 0\\ 0& 0& 0& 1\end{array}\right]& T_Z^A=\left[\begin{array}{cccc}1& 0& 0& 0\\ 0& 1& 0& 0\\ 0& 0& 1& d_5\\ 0& 0& 0& 1\end{array}\right]& T_T^Z=\left[\begin{array}{cccc}1& 0& 0& x_6\\ 0& 1& 0& y_6\\ 0& 0& 1& z_6\\ 0& 0& 0& 1\end{array}\right]\end{array}$

其中，是从坐标系O_base到坐标系O_X的理想齐次变换矩阵，从坐标系O_X到坐标系O_Y的理想齐次变换矩阵、从坐标系O_Y到坐标系O_B的理想齐次变换矩阵、从坐标系O_B到坐标系O_A的理想齐次变换矩阵、从坐标系O_A到坐标系O_Z的理想齐次变换矩阵、从坐标系O_Z到坐标系O_T的理想齐次变换矩阵，d₁,d₂,θ₃,θ₄,d₅分别是异型五轴数控制孔机床X、Y、B、A、Z轴的关节量；x₆,y₆,z₆是最后一个运动轴子坐标系到刀具坐标系在x,y,z方向的位移偏置；

(2.2)建立异型五轴数控制孔机床的正向运动学模型：

$\begin{array}{c}T_T^O=T_X^O\·T_Y^X\·T_B^Y\·T_A^B\·T_Z^A\·T_T^Z\\ =\left[\begin{array}{cccc}{\mathrm{cos\θ}}_3& {\mathrm{sin\θ}}_3{\mathrm{sin\θ}}_4& {\mathrm{sin\θ}}_3{\mathrm{cos\θ}}_4& d_1+d_5{\mathrm{sin\θ}}_3{\mathrm{cos\θ}}_4+x_6{\mathrm{cos\θ}}_3+y_6{\mathrm{sin\θ}}_3{\mathrm{sin\θ}}_4+z_6{\mathrm{sin\θ}}_3{\mathrm{cos\θ}}_4\\ 0& {\mathrm{cos\θ}}_4& -{\mathrm{sin\θ}}_4& d_2-d_5{\mathrm{sin\θ}}_4+y_6{\mathrm{cos\θ}}_4-z_6{\mathrm{sin\θ}}_4\\ -{\mathrm{sin\θ}}_3& {\mathrm{cos\θ}}_3{\mathrm{sin\θ}}_4& {\mathrm{cos\θ}}_3{\mathrm{cos\θ}}_4& d_5{\mathrm{cos\θ}}_3{\mathrm{cos\θ}}_4-x_6{\mathrm{sin\θ}}_3+y_6{\mathrm{sin\θ}}_3+y_6{\mathrm{cos\θ}}_3{\mathrm{sin\θ}}_4+z_6{\mathrm{cos\θ}}_3{\mathrm{cos\θ}}_4\\ 0& 0& 0& 1\end{array}\right]\\ =\left[\begin{array}{cccc}{r}_{11}& r_{12}& r_{13}& p_x\\ r_{21}& r_{22}& r_{23}& p_y\\ r_{31}& r_{32}& r_{33}& p_z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}R\left(q\right)& P\left(q\right)\\ 0& 1\end{array}\right]\end{array}$

其中，是理想情况下从基坐标系O_base到刀具坐标系O_T的齐次变换矩阵，即为异型五轴数控制孔机床的正向运动学模型，表示异型五轴数控制孔机床末端的理论位姿；R(q)为3×3的刀具坐标系的姿态矩阵，P(q)为3×1的刀具坐标系的位置矩阵，姿态矩阵中，[r₁₁,r₂₁,r₃₁]^T是刀具坐标系X轴的姿态向量，[r₁₂,r₂₂,r₃₂]^T是刀具坐标系Y轴的姿态向量，[r₁₃,r₂₃,r₃₃]^T是刀具坐标系Z轴的姿态向量。

4.根据权利要求3所述的进给轴随AB轴摆动的五轴数控制孔机床正反解方法，其特征在于，所述的步骤(3)的具体步骤如下：

(3.1)根据工件表面的法向及制孔位置要求，将异型五轴数控制孔机床的末端目标位姿表示为：

$T_d=\left[\begin{array}{cccc}{u}_x& v_x& w_x& o_x\\ u_y& v_y& w_y& o_y\\ u_z& v_z& w_z& o_z\\ 0& 0& 0& 1\end{array}\right]=\left[\begin{array}{cc}{R}_d& P_d\\ 0& 1\end{array}\right]$

其中，T_d为刀具坐标系的目标位姿矩阵，P_d为刀具坐标系的目标位置矩阵，R_d为刀具坐标系的目标姿态矩阵。[u_x,u_y,u_z]^T是刀具坐标系X轴的目标姿态向量，[v_x,v_y,v_z]^T是刀具坐标系Y轴的目标姿态向量，[w_x,w_y,w_z]^T是刀具坐标系Z轴的目标姿态向量，[o_x,o_y,o_z]^T是目标位置向量；

(3.2)令异型五轴数控制孔机床末端的理论位姿等于目标位姿状态，用公式表示为：

$T_T^O=T_d$

(3.3)根据矩阵对应元素相等，建立关节量的方程组：

o_x＝p_x＝d₁+d₅sinθ₃cosθ₄+x₆cosθ₃+y₆sinθ₃sinθ₄+z₆sinθ₃cosθ₄

o_y＝p_y＝d₂-d₅sinθ₄+y₆cosθ₄-z₆sinθ₄

o_z＝p_z＝d₅cosθ₃cosθ₄-x₆sinθ₃+y₆cosθ₃sinθ₄+z₆cosθ₃cosθ₄

w_x＝r₁₃＝sinθ₃cosθ₄

w_y＝r₂₃＝-sinθ₄

w_z＝r₃₃＝cosθ₃cosθ₄

(3.4)对步骤(3.3)中的关节量的方程组进行推导，得到异型五轴数控制孔机床的逆向运动学模型：

θ₃＝-atan2(w_x,w_z)

${\mathrm{\θ}}_4=-\arcsin \left(w_y\right)=-atan2(w_y,\sqrt{w_x^2+w_z^2})$

d₅＝(o_z+x₆sinθ₃-y₆cosθ₃sinθ₄-z₆cosθ₃cosθ₄)/(cosθ₃cosθ₄)

d₁＝o_x-d₅sinθ₃cosθ₄-x₆cosθ₃-y₆sinθ₃sinθ₄-z₆sinθ₃cosθ₄

d₂＝o_y+d₅sinθ₄-y₆cosθ₄+z₆sinθ₄

其中，cosθ₃cosθ₄≠0其中，且θ₃∈[-8°,8°],θ₄∈[-12.5°,12.5°]。