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

摘要

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

主权项

用于工业机器人的过辅助点姿态空间圆弧插补方法，包括以下步骤：步骤一：通过示教确定工业机器人的要求位姿，机器人控制器通过通信端口获取示教器提供的示教点信息，确定工业机器人轨迹的空间圆弧起点、辅助点及终点空间坐标及位姿，分别为：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、根据余弦定理和三个角之间的关系，求得空间圆弧的起点到辅助点、辅助点到终点、起点到终点的圆心角分别为：θ₁，θ₂，θ_3′；θ₃＝θ₁+θ₂；D、根据圆弧弧长公式，求得弧长L＝Rθ₃；进而求得空间圆弧插补的总位移S；步骤三：姿态规划模块规划出经过空间圆弧辅助点的姿态；各轴姿态随圆心角变化的变化率公式为：$w\left(\theta \right)=\left\{\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}\right.$步骤四：轨迹速度规划模块计算出每个插补周期的插补位移；轨迹速度规划模块，可以根据现有技术规划基于梯形曲线加减速控制或基于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θ]；+(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、利用以下公式可求得插补点的姿态$\begin{array}{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 \\ c_i=c_1+{\int }_0^{\theta }w\left(\theta \right)d\theta \end{array};$步骤六：机器人控制器的中央处理器将最终得到的位姿通过通信端口提供给机器人运动机构进行执行。