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

权利要求书

1.一种机器人末端执行器的姿态计算方法，其特征在于，包括以下步骤：

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

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

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

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

2.根据权利要求1所述的一种机器人末端执行器的姿态计算方法，其特征在于，所述步骤S2中确定末端执行器TCP主要控制轴的单位向量的方法具体为：末端执行器TCP的主要控制轴为末端执行器TCP当前需要姿态的主要控制轴，计算末端执行器TCP姿态主要控制轴的单位向量根据当前参考坐标系与末端执行器TCP间的位姿关系。

3.根据权利要求1所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述步骤S4中的旋转矩阵R具体为

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

4.根据权利要求3所述的一种机器人末端执行器的姿态计算方法，其特征在于：测量点的旋转矩阵R为时，最终该测量点的三个姿态角为

5.根据权利要求4所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述测量点的姿态角的余弦cosβ不等于0。

6.根据权利要求4所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述步骤S4中加工点位P的旋转矩阵R的表达式为：其中nx、ny、nz分别是向量的x、y、z轴坐标，ox、oy、oz分别是向量的x、y、z轴坐标，ax、ay、az分别是向量的x、y、z轴坐标。

7.根据权利要求1所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述步骤S2中，求末端执行器TCP在y轴的单位向量时，将末端执行器TCP的x轴对应的向量满足

8.根据权利要求1所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述步骤S2中末端执行器TCP在z轴的单位向量的坐标和末端执行器TCP法向轴的单位向量的坐标相同。

9.根据权利要求8所述的一种机器人末端执行器的姿态计算方法，其特征在于：所述步骤S3中最终计算出的末端执行器TCP在y轴的单位向量满足：

其中，ax、ay、az分别是向量的x、y、z轴坐标。