用于工业机器人的过辅助点姿态空间圆弧插补方法

摘要

本发明公开用于工业机器人的过辅助点姿态空间圆弧插补方法，包括以下步骤：机器人控制器通过通信端口确定了示教器示教的空间不共线三点；根据空间几何关系直接进行空间离散点的计算，求出空间圆弧的圆心、半径、法向量、圆心角和弧长；对姿态进行规划使运动轨迹经过辅助点姿态且轨迹光滑；速度轨迹规划模块计算出每个插补周期的插补位移；利用实时插补算法计算出每个插补周期的插补点位姿；将最终满足示教要求的位姿通过通信端口提供给机器人运动机构并执行。本发明避免了示教圆心的困难和确定圆弧方向的问题；计算效率高、插补精度高，可实现机器人的快速插补、控制精度高；运行平滑地经过辅助点姿态，拓宽了机器人的应用场合。

说明书

技术领域

本发明涉及一种插补方法，具体说是一种用于机器人的过辅助点姿态空间圆弧插补方法。背景 技术

机器人的轨迹规划，在机器人控制中具有重要的作用，直接影响着控制的准确性和快速性。而 插补算法是整个机器人轨迹规划控制过程的精华所在，占据着举足轻重的地位。通过示教机器人运 动路径上的某些关键点，然后根据轨迹特征算出这些示教点之间必须到达的中间位置点，通过插补 进行控制，从而实现高效率高精度的运动控制。

当曲线轨迹为圆弧时，除了示教圆弧起点和终点外，至少还应知道圆心或圆弧上一个辅助点。 显然，示教圆心是很困难的，因而工业机器人终端执行器的轨迹圆弧通常由示教的圆弧起点、辅助 点及圆弧终点决定，而这三点所决定的平面通常不一定平行于某一坐标平面，因而需要研究空间任 意三点圆弧的插补算法。

目前普遍采用基于坐标转换的空间圆弧插补方法，即通过坐标转换将空间圆弧转化为平面圆 弧，利用平面圆弧插补算法进行计算，之后再通过坐标转换把插补点从平面结果转换为空间结果。 此方法需要进行多次坐标变换，计算过程复杂，计算工作量大。

目前工业机器人应用的空间圆弧插补方法未提及过辅助点姿态；随着工业发展的进步，控制指 令的简便，某些特殊应用场合如焊接、涂胶等需要经过辅助点的姿态。

发明内容

本发明所要解决的技术问题，在于针对以上问题，克服现有技术存在的缺陷，提出了一种用于 工业机器人的过辅助点姿态空间圆弧插补方法。该插补方法示教任意不共线的空间三点，不需要坐 标转换将空间点转换成平面点进行计算，而是直接进行空间离散点的计算，并且对姿态进行规划使 其过辅助点姿态，能根据给定的圆弧起点、辅助点及终点完成任意空间圆弧插补。

为实现上述发明目的，本发明采取的技术方案是：

用于工业机器人的过辅助点姿态空间圆弧插补方法，包括以下步骤：

步骤一：通过示教确定工业机器人的要求位姿，机器人控制器通过通信端口获取示教器提供的 示教点信息，确定工业机器人轨迹的空间圆弧起点、辅助点及终点空间坐标及位姿，分别为： P₁(x₁,y₁,z₁,a₁,b₁,c₁)、P₂(x₂,y₂,z₂,a₂,b₂,c₂)、P₃(x₃,y₃,z₃,a₃,b₃,c₃)；机器人轨迹上各点位姿由位置矢 量(x,y,z)和RPY姿态矢量(α,β,γ)共同描述，组合成一个6自由度的复合矢量(x,y,z,α,β,γ)，即 (x,y,z,a,b,c)。

步骤二：求出空间圆弧的圆心、半径、法向量、圆心角和弧长：

A、空间圆弧起点、辅助点及终点为不共线的空间三点，确定了空间圆弧所在的平面(简称圆 弧平面)，根据圆心到三点的距离都为半径，联立方程，可求出圆心的空间坐标P₀(x₀,y₀,z₀)，进 而求得半径R；

B、根据外积公式，求得垂直于圆弧平面的单位法向量

C、根据余弦定理和三个角之间的关系，求得空间圆弧的起点到辅助点、辅助点到终点、起点 到终点的圆心角分别为：θ₁，θ₂，θ₃(θ₃＝θ₁+θ₂)；

D、根据圆弧弧长公式，求得弧长L＝Rθ₃；进而求得空间圆弧插补的总位移S；

步骤三：姿态规划模块规划出经过空间圆弧辅助点的姿态；

各轴姿态随圆心角变化的变化率公式为：

$w\left(\theta \right)=\{\begin{array}{cc}{k}_1\theta +m_1& (0<\theta \le {\theta }_1)\\ k_2\theta +m_2& ({\theta }_1<\theta \le {\theta }_3)\end{array}---\left(1\right)$

分别对w(θ)积分即是各轴姿态值，故可根据极值法设计使得各轴姿态变化率连续，并求得相 应的系数值k₁,m₁,k₂,m₂；

步骤四：速度轨迹规划模块计算出每个插补周期的插补位移；

速度轨迹规划模块，可以根据现有技术规划基于梯形曲线加减速控制或基于S形曲线加减速控 制或其它曲线控制；根据不同的曲线控制方式，进行速度规划处理，计算圆弧段所需要的插补总时 间和插补信息，最后计算出每个插补周期的插补位移；

步骤五：计算得到插补点的空间位姿；

计算空间圆弧插补点位姿P_i(x_i,y_i,z_i,a_i,b_i,c_i)步骤如下：

A、根据圆弧弧长公式，每个插补周期的插补位移除以半径得到空间圆弧每个插补周期的插补 角度θ；

B、利用以下公式可求得插补点的空间坐标位置

x_i＝(x₁-x₀)[n_xn_x(1-cosθ)+cosθ]+(y₁-y₀)[n_xn_y(1-cosθ)-n_zsinθ] +(z₁-z₀)[n_xn_z(1-cosθ)+n_ysinθ]+x₀

y_i＝(x₁-x₀)[n_xn_y(1-cosθ)+n_zsinθ]+(y₁-y₀)[n_yn_y(1-cosθ)+cosθ](2) +(z₁-z₀)[n_yn_z(1-cosθ)-n_xsinθ]+y₀

z_i＝(x₁-x₀)[n_xn_z(1-cosθ)-n_ysinθ]+(y₁-y₀)[n_yn_z(1-cosθ)+n_xsinθ] +(z₁-z₀)[n_zn_z(1-cosθ)+sinθ]+z₀

C、利用以下公式可求得插补点的姿态

$a_i=a_1+{\int }_0^{\theta }w\left(\theta \right)d\theta$

$b_i=b_1+{\int }_0^{\theta }w\left(\theta \right)d\theta ---\left(3\right)$

$c_i=c_1+{\int }_0^{\theta }w\left(\theta \right)d\theta$

步骤六：机器人控制器的中央处理器将最终得到的位姿通过通信端口提供给机器人运动机构进 行执行。

本发明方法可以实现工业机器人的高精高效的运动控制，经过辅助点姿态，并且运动平滑。

本发明的优点：一是，本发明方法根据空间任意三点进行圆弧插补，避免了示教圆心的困难和 确定圆弧方向的问题；二是，本发明方法不需要进行坐标转换计算平面圆弧插补，而是直接计算空 间角度和空间离散点得到实际空间圆弧插补点坐标，方法过程简洁方便、算法易于实现、计算效率 高、插补精度高；三是，本发明方法通过姿态规划模块，使得实际空间圆弧插补点经过辅助点姿态， 满足某些特殊应用场合；四是，本发明方法实现姿态变化率与插补角度的连续关系，使得各轴姿态 变化连续，机器人运动平滑。

本方法利用空间几何关系直接进行离散点计算可以实现多轴快速插补、控制精度高；通过姿态 规划算法经过辅助点姿态，同时实现姿态连续且姿态变化率连续的关系，可以使其更好的应用于某 些特殊场合。

附图说明

图1为本发明用于工业机器人的过辅助点姿态空间圆弧插补方法的流程图。

图2为本发明中的空间圆弧示例图。

具体实施方式

下面结合附图和实施例，对本发明做进一步详细说明。

参照图1所示，本发明用于工业机器人的过辅助点姿态空间圆弧插补算法，包括如下步骤：

步骤一：通过示教确定工业机器人的要求位姿，机器人控制器通过通信端口获取示教器提供的 示教点信息，确定工业机器人轨迹的空间圆弧起点、辅助点及终点空间坐标及位姿，分别为： P₁(x₁,y₁,z₁,a₁,b₁,c₁)、P₂(x₂,y₂,z₂,a₂,b₂,c₂)、P₃(x₃,y₃,z₃,a₃,b₃,c₃)；机器人轨迹上各点位姿由位置矢 量(x,y,z)和RPY姿态矢量(α,β,γ)共同描述，组合成一个6自由度的复合矢量(x,y,z,α,β,γ)，即 (x,y,z,a,b,c)。

步骤二：求出空间圆弧的圆心、半径、法向量、圆心角和弧长：

A、空间圆弧起点、辅助点及终点为不共线的空间三点，确定了空间圆弧所在的平面(简称圆 弧平面)，根据圆心到三点的距离都为半径，联立方程，可求出圆心的空间坐标P₀(x₀,y₀,z₀)，进 而求得半径R；

B、根据外积公式，求得垂直于圆弧平面的单位法向量

C、根据余弦定理和三个角之间的关系，求得空间圆弧的起点到辅助点、辅助点到终点、起点 到终点的圆心角分别为：θ₁，θ₂，θ₃(θ₃＝θ₁+θ₂)；

D、根据圆弧弧长公式，求得弧长L＝Rθ₃；进而求得空间圆弧插补的总位移S；

步骤三：姿态规划模块规划出经过空间圆弧辅助点的姿态；

各轴姿态随圆心角变化的变化率公式为：

$w\left(\theta \right)=\{\begin{array}{cc}{k}_1\theta +m_1& (0<\theta \le {\theta }_1)\\ k_2\theta +m_2& ({\theta }_1<\theta \le {\theta }_3)\end{array}---\left(1\right)$

分别对w(θ)积分即是各轴姿态值，故可根据极值法设计使得各轴姿态变化率连续，并求得相 应的系数值k₁,m₁,k₂,m₂；

起点到辅助点的各轴姿态线性变化率为w₁，辅助点到终点的各轴姿态线性变化率为w₂；当 w₂＝2w₁时，可设计系数分别为m₁＝0,k₂＝0,m₂＝w₂，公式1可实例为 $w\left(\theta \right)=\left(\begin{array}{cc}\frac{2w_1}{{\theta }_1}\theta & (0<\theta \le {\theta }_1)\\ w_2& ({\theta }_1<\theta \le {\theta }_3)\end{array}\right)$进行求解各轴姿态变化率；

步骤四：轨迹速度规划模块计算出每个插补周期的插补位移s(iT)；

轨迹速度规划模块，可以根据现有技术规划基于梯形曲线加减速控制或基于S形曲线加减速控 制或其它曲线控制；以下以梯形曲线加减速控制为例，介绍计算每个插补周期插补位移的具体步骤 如下：

A、基于梯形曲线加减速控制的空间圆弧插补的加减速时间计算公式为：

$\{\begin{array}{c}{t}_1=\frac{v}{a}\\ t_2=\frac{1}{v}(S-\frac{1}{2}{{\mathrm{at}}_1}^2-\frac{v^2}{2a})\\ t_3=\frac{v}{a}\end{array}---\left(4\right)$

式中，t₁为空间圆弧插补的加速过程时间；t₂为空间圆弧插补的匀速过程时间；t₃为空间圆弧 插补的减速过程时间；v为空间圆弧插补线速度，空间圆弧起点和终点的速度都设为0；a为空间 圆弧插补加速度和减速度；

B、第i个插补周期的加速度计算公式为：

$a\left(iT\right)=\left(\begin{array}{cc}a& (0\le i\le N_1)\\ 0& (N_1<i\le N_2)\\ -a& (N_2<i\le N_3)\end{array}\right)---\left(5\right)$

式中，T为空间圆弧插补周期，N₁为需要的插补周期总个数，N₂为需要的插补周期 总个数，N₃为需要的插补周期总个数，i＝0,1,2,…,N₃；

C、第i个插补周期的线速度计算公式为：

$v\left(iT\right)=\underset{i=0}{\overset{N_3}{\Sigma }}a\left(iT\right)·T---\left(6\right)$

D、第i个插补周期的插补位移计算公式为：

$s\left(iT\right)=\underset{i=0}{\overset{N_3}{\Sigma }}v\left(iT\right)·T---\left(7\right)$

步骤五：计算得到插补点的空间位姿；

计算空间圆弧插补点位姿P_i(x_i,y_i,z_i,a_i,b_i,c_i)步骤如下：

A、计算空间圆弧每个插补周期的插补角度

B、利用以下公式可求得插补点的空间坐标位置

x_i＝(x₁-x₀)[n_xn_x(1-cosθ)+cosθ]+(y₁-y₀)[n_xn_y(1-cosθ)-n_zsinθ] +(z₁-z₀)[n_xn_z(1-cosθ)+n_ysinθ]+x₀

y_i＝(x₁-x₀)[n_xn_y(1-cosθ)+n_zsinθ]+(y₁-y₀)[n_yn_y(1-cosθ)+cosθ](2) +(z₁-z₀)[n_yn_z(1-cosθ)-n_xsinθ]+y₀

z_i＝(x₁-x₀)[n_xn_z(1-cosθ)-n_ysinθ]+(y₁-y₀)[n_yn_z(1-cosθ)+n_xsinθ] +(z₁-z₀)[n_zn_z(1-cosθ)+sinθ]+z₀

C、利用以下公式可求得插补点的姿态

$a_i=a_1+{\int }_0^{\theta }w\left(\theta \right)d\theta$

$b_i=b_1+{\int }_0^{\theta }w\left(\theta \right)d\theta ---\left(3\right)$

$c_i=c_1+{\int }_0^{\theta }w\left(\theta \right)d\theta$

步骤六：机器人控制器的中央处理器将最终得到的位姿通过通信端口提供给机器人运动机构进 行执行。