[发明专利]一种机器人末端执行器的姿态计算方法

说明书

一种机器人末端执行器的姿态计算方法，包括以下步骤：在当前参考坐标系中，根据加工点位P(x₀，y₀，z₀)的法向确定末端执行器TCP的主要控制轴在法向轴的单位向量；确定末端执行器TCP的主要控制轴在x轴的单位向量和在z轴的单位向量根据得出的单位向量和再利用末端执行器TCP三轴两两正交的关系式计算出末端执行器TCP在y轴的单位向量根据末端执行器TCP三轴的单位向量组成的旋转矩阵计算得出姿态角α、β、γ。本方案可以粗略地、初步地计算末端执行器的朝向和姿态，便于优化实际的机器人加工姿态，降低出现干涉碰撞情况的风险。

技术领域

本发明涉及控制方法领域，特别涉及一种机器人末端执行器的姿态计算方法。

背景技术

随着工业机器人技术的发展，机器人自动制孔系统在飞机零部件自动化装配领域的应用已经越来越广泛。由于飞机零部件产品大多曲率较大，且形状不规则，为避免末端执行器与产品出现碰撞干涉的情况，机器人制孔系统在加工某些点位时，末端执行器需要按照某种特定姿态进行加工，但在仅给定制孔法向的情况下，末端执行器的加工姿态并不能完全确定。因此，本文提出了一种工业机器人末端执行器加工姿态的初步控制算法，控制末端执行器制孔加工时的大致朝向，优化机器人加工姿态，同时降低出现干涉碰撞情况的风险。

发明内容

本发明的目的在于：提供了一种机器人末端执行器的姿态计算方法，可以粗略地、初步地计算末端执行器的朝向和姿态，便于优化实际的机器人加工姿态，降低出现干涉碰撞情况的风险，解决了上述问题。

本发明采用的技术方案如下：

一种机器人末端执行器的姿态计算方法，包括以下步骤：

步骤S1：在当前参考坐标系中，根据加工点位P(x₀，y₀，z₀)的法向确定末端执行器TCP的主要控制轴在法向轴的单位向量；

步骤S2：确定末端执行器TCP的主要控制轴在x轴的单位向量和在z轴的单位向量

步骤S3：根据得出的单位向量和再利用末端执行器TCP三轴两两正交的关系式计算出末端执行器TCP在y轴的单位向量

步骤S4：根据末端执行器TCP三轴的单位向量组成的旋转矩阵计算得出姿态角α、β、γ。

为了更好地实现本方案，进一步地，所述步骤S2中确定末端执行器TCP主要控制轴的单位向量的方法具体为：末端执行器TCP的主要控制轴为末端执行器TCP当前需要姿态的主要控制轴，计算末端执行器TCP姿态主要控制轴的单位向量根据当前参考坐标系与末端执行器TCP间的位姿关系。

为了更好地实现本方案，进一步地，所述步骤S4中的旋转矩阵R具体为

其中，α、β、γ为姿态角。

为了更好地实现本方案，进一步地，测量点的旋转矩阵R为时，最终该测量点的三个姿态角为

为了更好地实现本方案，进一步地，所述测量点的姿态角的余弦cosβ不等于0。

为了更好地实现本方案，进一步地，所述步骤S4中加工点位P的旋转矩阵R的表达式为：其中nx、ny、nz分别是向量的x、y、z轴坐标，ox、oy、oz分别是向量的x、y、z轴坐标，ax、ay、az分别是向量的x、y、z轴坐标。

为了更好地实现本方案，进一步地，所述步骤S2中，求末端执行器TCP在y轴的单位向量时，将末端执行器TCP的x轴对应的向量满足

为了更好地实现本方案，进一步地，所述步骤S2中末端执行器TCP在z轴的单位向量的坐标和末端执行器TCP法向轴的单位向量的坐标相同。