[发明专利]五轴运动控制方法、装置及点胶设备

权利要求书

1.一种五轴运动控制方法，其特征在于，包括：

采集参考点的坐标数据；

根据所述坐标数据采用标定算法计算X轴、Y轴、Z轴、U轴及R轴的中心坐标及法向量，其中，X轴、Y轴及Z轴为平移轴，U轴和R轴为旋转轴；

根据所述中心坐标及法向量计算点位坐标；

根据所述点位坐标控制五轴运动。

2.根据权利要求1所述的五轴运动控制方法，其特征在于，所述根据所述中心坐标及法向量计算点位坐标包括：

将旋转轴平移至XYZ坐标系的原点，得到第一平移矩阵；

将旋转轴旋转至YOZ平面，得到第一X轴旋转矩阵；

将旋转轴旋转至与Z轴重合，得到第一Y轴旋转矩阵；

将旋转轴平移至XYZ坐标系的原点，得到第二平移矩阵；

将旋转轴绕Z轴旋转，得到Z轴旋转矩阵；

执行“将旋转轴旋转至与Z轴重合”的逆过程，得到第二Y轴旋转矩阵；

执行“将旋转轴旋转至YOZ平面”的逆过程，得到第二X轴旋转矩阵；

执行“将旋转轴平移至XYZ坐标系的原点”，得到第三平移矩阵；

根据所述第一X轴旋转矩阵、第一Y轴旋转矩阵、Z轴旋转矩阵、第二Y轴旋转矩阵及第二X轴旋转矩阵得到绕任意轴旋转的结果矩阵；

根据所述结果矩阵即可计算点位坐标。

3.根据权利要求2所述的五轴运动控制方法，其特征在于，所述将旋转轴平移至XYZ坐标系的原点，得到第一平移矩阵包括：

将旋转轴V1V2平移至XYZ坐标系的原点，得到旋转轴OP，其中，V1点的坐标为(a1,b1,c1)，V2点的坐标为(a2,b2,c2)，O点的坐标为(0,0,0)，P点的坐标为(a,b,c)；

则得到第一平移矩阵T(x,y,z)：

4.根据权利要求3所述的五轴运动控制方法，其特征在于，所述将旋转轴旋转至YOZ平面，得到第一X轴旋转矩阵包括：

作p点在平面YOZ上的投影q点，再过q点作Z轴垂线，则得到r点是p点绕X轴旋转α角所得，r点的坐标为

则得到第一X轴旋转矩阵R_X(α)：

5.根据权利要求4所述的五轴运动控制方法，其特征在于，所述将旋转轴旋转至与Z轴重合，得到第一Y轴旋转矩阵包括：

将旋转轴or绕Y轴旋转β角至与Z轴重合，则有：

则得到第一Y轴旋转矩阵R_y(-β)：

6.根据权利要求4所述的五轴运动控制方法，其特征在于，所述将旋转轴绕Z轴旋转，得到Z轴旋转矩阵包括：

将旋转轴or绕Z轴旋转θ角后，则得到Z轴旋转矩阵R_z(θ)：

7.根据权利要求4所述的五轴运动控制方法，其特征在于，所述根据所述第一X轴旋转矩阵、第一Y轴旋转矩阵、Z轴旋转矩阵、第二Y轴旋转矩阵及第二X轴旋转矩阵得到绕任意轴旋转的结果矩阵包括：

将第一X轴旋转矩阵、第一Y轴旋转矩阵、Z轴旋转矩阵、第二Y轴旋转矩阵及第二X轴旋转矩阵相乘得到结果矩阵M：

8.一种五轴运动控制装置，其特征在于，包括：

采集单元，用于采集参考点的坐标数据；

标定单元，用于根据所述坐标数据采用标定算法计算X轴、Y轴、Z轴、U轴及R轴的中心坐标及法向量，其中，X轴、Y轴及Z轴为平移轴，U轴和R轴为旋转轴；

计算单元，用于根据所述中心坐标及法向量计算点位坐标；

控制单元，用于根据所述点位坐标控制五轴运动。