[发明专利]基于空间解析几何求解工业机器人中圆弧轨迹的方法

摘要

本发明提出了一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，包括：示教得到工业机器人的圆弧轨迹的目标点的位置；根据目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；根据目标点的位置，采用矢量算法计算圆弧轨迹的圆心空间坐标O；根据圆心空间坐标和目标点的位置，计算圆弧轨迹的半径，并计算圆弧坐标系与基坐标系的齐次变换矩阵，以根据基坐标系和齐次变换矩阵计算圆弧坐标系；分别计算向量和，计算点积值，进而求解得到圆心角，根据圆弧长和圆心角的关系计算圆心角对应的圆弧长。本发明采用解析几何矢量法求取空间三点圆心过程简单易懂，并且计算复杂度更低，求解更快速简便。

主权项

1.一种基于空间解析几何求解工业机器人中圆弧轨迹的方法，其特征在于，包括如下步骤：步骤S1，示教得到工业机器人的圆弧轨迹的目标点的位置，其中，所述目标点包括：起点A、中间点B和终点C；步骤S2，根据所述目标点的位置判断是否能够确定唯一的圆弧轨迹，如果是则执行步骤S3，否则结束求解；步骤S3，根据所述目标点的位置，采用矢量算法计算所述圆弧轨迹的圆心空间坐标O，其中，首先，计算中间参数t，然后，根据中间参数t，计算所述圆弧轨迹的圆心相对于基坐标系的空间坐标：其中，O为圆心点坐标、P为所述工业机器人的基坐标系的原点坐标、D为线段AB的中点坐标、E为线段AC的中点坐标、根据圆心在基坐标系的不同位置，分别对t的求解进行说明：(1)圆心坐标在z轴投影值为正值，圆心在Y轴正向与Z轴正向构成象限内，与矢量方向相反，所以t＝‑|t|，可以得到以下关系式：根据式(1)～(5)得到以下关系式：将(6)、(7)、(8)公式代入可以得到：经过化简得到：由得到：当E点和圆心0重合，不构成三角形，由于与不共线，对应的两条中垂线不共线，所以求t的公式，分母不可能为零；(2)圆心坐标在z轴投影值为负值，圆心在Y轴正向与Z轴负向构成象限内，与矢量方向一致，所以t＝|t|，参考上述z轴投影值为正值推导过程，唯一变化的就是最后可以推导得到本类情况所求的计算结果(3)圆心坐标在z轴投影值为零，当点C在Z轴上，即线段AC与Z轴共线，cosθ＝0，此时计算t公式同样适用，同理当C点在Z轴负半轴时，圆心点O在Y轴正向，Z轴负向构成象限中，求t同样适用；(4)E点与圆心O点重合，当线段AC的中点E与所求圆心O点重合时，过三点的圆的圆心坐标根据中间参数t，计算过空间三点的圆的圆心相对于基坐标系的空间坐标：步骤S4，根据所述圆心空间坐标和所述目标点的位置，计算所述圆弧轨迹的半径R，并计算圆弧坐标系与基坐标系的齐次变换矩阵，以根据所述基坐标系和所述齐次变换矩阵计算所述圆弧坐标系；步骤S5，分别计算向量和计算点积值根据点积值得正负号判断和是否同向，进而求解得到圆心角θ，根据圆弧长和圆心角的关系计算所述圆心角对应的圆弧长，其中，