用于工业机器人的过辅助点姿态空间圆弧插补方法
【技术领域】
[0001] 本发明涉及一种插补方法，具体说是一种用于机器人的过辅助点姿态空间圆弧插 补方法。【背景技术】
[0002] 机器人的轨迹规划，在机器人控制中具有重要的作用，直接影响着控制的准确性 和快速性。而插补算法是整个机器人轨迹规划控制过程的精华所在，占据着举足轻重的地 位。通过示教机器人运动路径上的某些关键点，然后根据轨迹特征算出这些示教点之间必 须到达的中间位置点，通过插补进行控制，从而实现高效率高精度的运动控制。
[0003] 当曲线轨迹为圆弧时，除了示教圆弧起点和终点外，至少还应知道圆心或圆弧上 一个辅助点。显然，示教圆心是很困难的，因而工业机器人终端执行器的轨迹圆弧通常由示 教的圆弧起点、辅助点及圆弧终点决定，而这三点所决定的平面通常不一定平行于某一坐 标平面，因而需要研究空间任意三点圆弧的插补算法。
[0004] 目前普遍采用基于坐标转换的空间圆弧插补方法，即通过坐标转换将空间圆弧转 化为平面圆弧，利用平面圆弧插补算法进行计算，之后再通过坐标转换把插补点从平面结 果转换为空间结果。此方法需要进行多次坐标变换，计算过程复杂，计算工作量大。
[0005] 目前工业机器人应用的空间圆弧插补方法未提及过辅助点姿态；随着工业发展的 进步，控制指令的简便，某些特殊应用场合如焊接、涂胶等需要经过辅助点的姿态。
【发明内容】
[0006] 本发明所要解决的技术问题，在于针对以上问题，克服现有技术存在的缺陷，提出 了一种用于工业机器人的过辅助点姿态空间圆弧插补方法。该插补方法示教任意不共线的 空间三点，不需要坐标转换将空间点转换成平面点进行计算，而是直接进行空间离散点的 计算，并且对姿态进行规划使其过辅助点姿态，能根据给定的圆弧起点、辅助点及终点完成 任意空间圆弧插补。
[0007] 为实现上述发明目的，本发明采取的技术方案是：
[0008] 用于工业机器人的过辅助点姿态空间圆弧插补方法，包括以下步骤：
[0009] 步骤一：通过示教确定工业机器人的要求位姿，机器人控制器通过通信端口获取 示教器提供的示教点信息，确定工业机器人轨迹的空间圆弧起点、辅助点及终点空间坐标 及位姿，分别为：Pi (Xi, y。Z!，a!，b!，c!)、P2(x2, y2, z2, a2, b2, c2)、P3(x3, y3, z3, a3, b3, c3);机器 人轨迹上各点位姿由位置矢量（x，y，z)和RPY姿态矢量（α，β，γ)共同描述，组合成一个 6 自由度的复合矢量（X，y, ζ, α，β，γ )，即（X，y, ζ, a, b, c)。
[0010] 步骤二：求出空间圆弧的圆心、半径、法向量、圆心角和弧长：
[0011] A、空间圆弧起点、辅助点及终点为不共线的空间三点，确定了空间圆弧所在的平 面（简称圆弧平面），根据圆心到三点的距离都为半径，联立方程，可求出圆心的空间坐标 P。(X。，y。，ζ。)，进而求得半径R ;
[0012] B、根据外积公式，求得垂直于圆弧平面的单位法向量
[0013] C、根据余弦定理和三个角之间的关系，求得空间圆弧的起点到辅助点、辅助点到 终点、起点到终点的圆心角分别为：Θ i，θ 2, θ 3( θ 3= Θ θ 2);
[0014] D、根据圆弧弧长公式，求得弧长L = R Θ 3;进而求得空间圆弧插补的总位移s ;
[0015] 步骤三：姿态规划模块规划出经过空间圆弧辅助点的姿态；
[0016] 各轴姿态随圆心角变化的变化率公式为：
[0018] 分别对w( Θ )积分即是各轴姿态值，故可根据极值法设计使得各轴姿态变化率连 续，并求得相应的系数值h，k2, m2;
[0019] 步骤四：速度轨迹规划模块计算出每个插补周期的插补位移；
[0020] 速度轨迹规划模块，可以根据现有技术规划基于梯形曲线加减速控制或基于S形 曲线加减速控制或其它曲线控制；根据不同的曲线控制方式，进行速度规划处理，计算圆弧 段所需要的插补总时间和插补信息，最后计算出每个插补周期的插补位移；
[0021] 步骤五：计算得到插补点的空间位姿；
[0022] 计算空间圆弧插补点位姿Pi (Xi, yi, Zi, a;, bi, cj步骤如下：
[0023] A、根据圆弧弧长公式，每个插补周期的插补位移除以半径得到空间圆弧每个插补 周期的插补角度Θ ;
[0024] B、利用以下公式可求得插补点的空间坐标位置
[0028] C、利用以下公式可求得插补点的姿态
[0032] 步骤六：机器人控制器的中央处理器将最终得到的位姿通过通信端口提供给机器 人运动机构进行执行。
[0033] 本发明方法可以实现工业机器人的高精高效的运动控制，经过辅助点姿态，并且 运动平滑。
[0034] 本发明的优点：一是，本发明方法根据空间任意三点进行圆弧插补，避免了示教圆 心的困难和确定圆弧方向的问题；二是，本发明方法不需要进行坐标转换计算平面圆弧插 补，而是直接计算空间角度和空间离散点得到实际空间圆弧插补点坐标，方法过程简洁方 便、算法易于实现、计算效率高、插补精度高；三是，本发明方法通过姿态规划模块，使得实 际空间圆弧插补点经过辅助点姿态，满足某些特殊应用场合；四是，本发明方法实现姿态变 化率与插补角度的连续关系，使得各轴姿态变化连续，机器人运动平滑。
[0035] 本方法利用空间几何关系直接进行离散点计算可以实现多轴快速插补、控制精度 高；通过姿态规划算法经过辅助点姿态，同时实现姿态连续且姿态变化率连续的关系，可以 使其更好的应用于某些特殊场合。
【附图说明】
[0036] 图1为本发明用于工业机器人的过辅助点姿态空间圆弧插补方法的流程图。
[0037] 图2为本发明中的空间圆弧示例图。
【具体实施方式】
[0038] 下面结合附图和实施例，对本发明做进一步详细说明。
[0039] 参照图1所示，本发明用于工业机器人的过辅助点姿态空间圆弧插补算法，包括 如下步骤：
[0040] 步骤一：通过示教确定工业机器人的要求位姿，机器人控制器通过通信端口获取 示教器提供的示教点信息，确定工业机器人轨迹的空间圆弧起点、辅助点及终点空间坐标 及位姿，分别为：Pi (Xi, y。Z!，a!，b!，c!)、P2(x2, y2, z2, a2, b2, c2)、P3(x3, y