[发明专利]用于工业机器人的过辅助点姿态空间圆弧插补方法

权利要求书

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、根据余弦定理和三个角之间的关系，求得空间圆弧的起点到辅助点、辅助点到终点、起点到终点的圆心角分别为：θ₁，θ₂，θ_3′；θ₃＝θ₁+θ₂；

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

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

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

w(θ)=k1θ+m1(0<θ≤θ1)k2θ+m2(θ1<θ≤θ3)]]>

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

轨迹速度规划模块，可以根据现有技术规划基于梯形曲线加减速控制或基于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、利用以下公式可求得插补点的姿态

ai=a1+∫0θw(θ)dθbi=b1+∫0θw(θ)dθci=c1+∫0θw(θ)dθ;]]>

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