CN105278462A - 机器人控制系统轨迹加减速插补算法 - Google Patents

Abstract

本发明涉及一种机器人控制系统轨迹加减速插补算法，S1.光顺曲线：插补轨迹先用三次样条spline插值使曲线光滑，计算出各个节点处的函数值和两个边界处的导数信息；S2.在S1已经计算好的三次样条曲线上插入等距点，通过D-H建模后的运动逆运算计算各个转动轴的运动距离。本发明的机器人运动控制方法加减速采用S形+直线加减速，加速度连续变化，加速时间快，运动精度高，速度平滑性好。插补轨迹采用多次样条插补作为粗插补，再采用数字积分法以及等时间插补对轨迹做精插补，V速度高，不会存在插补精度变差。

Description

机器人控制系统轨迹加减速插补算法

技术领域

本发明涉及机器人控制领域，具体涉及一种机器人控制系统轨迹加减速插补算法。

背景技术

运动控制加减速方式有：

1、直线加减速。缺点：加速度有突变，速度曲线不平滑。

2、三角函数加减速。缺点：计算复杂。

3、指数型加减速。缺点：在启动和停止2点对机床有冲击。

4、S形加减速。在任何一点的加速度都是连续变化的，从而避免速度的柔性冲击，速度的平滑性好，运动精度高。缺点：当加速度设定跟直线加速度设定相同时，加速时间是直线加速时间的2倍。

传统插补算法：

1、直线插补：根据2点求出直线长度L，由要求的运动速度V及采样周期T,得步长ΔL和总步数S:ΔL＝VT，S＝INT(L/ΔL)+1，于是可得插补点坐标(S+1个点)。

2、圆弧插补:根据三点求出圆心坐标、半径及圆心角Φ。根据速度求步长ΔL，再求总步数S,S＝INT(Φ/ΔL)+1，最后据ΔΦ＝Φ/s及圆弧曲线求插补点坐标。

以上两种方法都会存在当插补周期一定，速度越快，ΔL越长，插补精度越差。

发明内容

有鉴于此，本发明的目的在于提供一种机器人控制系统轨迹加减速插补算法。

为达到上述目的，本发明提供如下技术方案：一种机器人控制系统轨迹加减速插补算法，

S1.光顺曲线：插补轨迹先用三次样条spline插值使曲线光滑，计算出各个节点(节点是指什么意思)处的函数值和两个边界处的导数信息；

S2.在S1已经计算好的三次样条曲线上插入等距点，通过D-H建模后的运动逆运算计算各个转动轴的运动距离。

进一步，所述曲线包括：加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，对每个加速度积分得到各段对应速度；

$v\left(\mathrm{\τ}\right)=\left\{\begin{array}{c}{v}_1+0.5{\mathrm{J\τ}}_1^2,0\≤t\≤t_1,({\mathrm{\τ}}_1=t),A=Jt,v_1=v_0+0.5{\mathrm{Jt}}^2\\ v_2+{\mathrm{A\τ}}_2,t_1\≤t\≤t_2,({\mathrm{\τ}}_2=t-t_1),A=A_1,v_2=v_1+At\\ v_3+{\mathrm{A\τ}}_3-0.5{\mathrm{J\τ}}_3^2,t_2\≤t\≤t_3,({\mathrm{\τ}}_3=t-t_2),A=A_1-Jt,v_3=v_2+At-0.5{\mathrm{Jt}}^2\\ v_4,t_3\≤t\≤t_4,({\mathrm{\τ}}_4=t-t_3),A=0,v_4=v_{\max }\\ v_5-0.5{\mathrm{J\τ}}_5^2,t_4\≤t\≤t_5,({\mathrm{\τ}}_5=t-t_4),A=-Jt,v_5=v_4-0.5{\mathrm{Jt}}^2\\ v_6-{\mathrm{A\τ}}_6,t_5\≤t\≤t_6,({\mathrm{\τ}}_6=t-t_5),A=A_2,v_6=v_5-At\\ v_7-{\mathrm{A\τ}}_7+0.5{\mathrm{J\τ}}_7^2,t_6\≤t\≤t_7,({\mathrm{\τ}}_7=t-t_6),A=A_2-Jt,v_7=v_6-At+0.5{\mathrm{Jt}}^2\end{array}\right.$

A为最大加速度，J为加加速度，v₀为起始速度，v_max为匀加速阶段速度。

本发明的有益效果在于：

本发明的机器人运动控制方法加减速采用S形+直线加减速，加速度连续变化，加速时间快，运动精度高，速度平滑性好。插补轨迹采用多次样条插补作为粗插补，再采用数字积分法以及等时间插补对轨迹做精插补，V速度高，不会存在插补精度变差。

附图说明

为了使本发明的目的、技术方案和优点更加清楚，下面将结合附图对本发明作进一步的详细描述，其中：

图1为本发明的方法流程图；

图2为三次样条插值曲线图；

图3为分段线性插值曲线图；

图4为距离与时间图，速度与时间图，加速度与时间图，加加速与时间图。

具体实施方式

下面将结合附图，对本发明的优选实施例进行详细的描述。

一种机器人控制系统轨迹加减速插补算法，包括以下步骤：

1、光顺曲线。插补轨迹先用三次样条spline插值使曲线光滑，给出各个节点(spline插值点)处的函数值和两个边界处的导数信息，便可得到不同光滑性的曲线，曲线越光滑，速度越快。这里采用的三次样条是标准的插值方式，不论在区间内或其边界上，其一阶导数dy/dx平滑，二阶导数d²y/dx连续。我们这里不采用分段线性插值的原因是其各个节点用折线连接，拐点的折线夹角越小，运行速度就越低，降低了机器的整体运行速度。三次样条插值和分段线性插值的区别如图1所示：

2、机器人坐标逆运算升降频。在第一步已经计算好的三次样条曲线上插入等距点，通过D-H建模(4X4的奇次变换矩阵描述相邻关节空间坐标系的关系)后的运动逆运算计算各个转动轴的运动距离。加速过程中的加加速和减加速中间插入一段等加速度A₁，在减速过程中的加加速和减加速过程中也插入一段匀减加速度A₂，使速度快速升到最高速，匀速后从最高速快速降到最低速，且曲线平滑。

所述曲线包括：加加速段I、匀加速段II、减加速段III、匀速段IV、加减速段V、匀减速段VI、减减速段VII，对每个加速度积分得到各段对应速度；

$v\left(\mathrm{\τ}\right)=\left\{\begin{array}{c}{v}_1+0.5{\mathrm{J\τ}}_1^2,0\≤t\≤t_1,({\mathrm{\τ}}_1=t),A=Jt,v_1=v_0+0.5{\mathrm{Jt}}^2\\ v_2+{\mathrm{A\τ}}_2,t_1\≤t\≤t_2,({\mathrm{\τ}}_2=t-t_1),A=A_1,v_2=v_1+At\\ v_3+{\mathrm{A\τ}}_3-0.5{\mathrm{J\τ}}_3^2,t_2\≤t\≤t_3,({\mathrm{\τ}}_3=t-t_2),A=A_1-Jt,v_3=v_2+At-0.5{\mathrm{Jt}}^2\\ v_4,t_3\≤t\≤t_4,({\mathrm{\τ}}_4=t-t_3),A=0,v_4=v_{\max }\\ v_5-0.5{\mathrm{J\τ}}_5^2,t_4\≤t\≤t_5,({\mathrm{\τ}}_5=t-t_4),A=-Jt,v_5=v_4-0.5{\mathrm{Jt}}^2\\ v_6-{\mathrm{A\τ}}_6,t_5\≤t\≤t_6,({\mathrm{\τ}}_6=t-t_5),A=A_2,v_6=v_5-At\\ v_7-{\mathrm{A\τ}}_7+0.5{\mathrm{J\τ}}_7^2,t_6\≤t\≤t_7,({\mathrm{\τ}}_7=t-t_6),A=A_2-Jt,v_7=v_6-At+0.5{\mathrm{Jt}}^2\end{array}\right.$

A为最大加速度，J为加加速度，v₀为起始速度，v_max为匀加速阶段速度。

在时间0～t₁段，加速度按线性方式增长，加速度曲线的I，速度按抛物线方式增长从V₀到V₁。J设定越大，加速度升到A₁越快。

时间t₁～t₂段，加速度不变，加速度曲线的II，速度按直线方式增长从V₁到V₂。

在时间t₂～t₃段，加速度按线性方式减小到0，加速度曲线的III，速度按反抛物线方式增长从V₂到V₃。

在时间t₃～t₄段，加速度为0，加速度曲线的IV，速度匀速V₃到V₄。

在时间t₄～t₅段，减加速度按线性方式增加，加速度曲线V，速度按反抛物线方式减小从V₄到V₅。

在时间t₅～t₆段，加速度不变，加速度曲线的VI，速度按线性方式减小从V₅到V₆。

在时间t₄～t₅段，减加速度按线性方式减小，加速度曲线V，速度按反抛物线方式减小从V₆到V₇。

最后说明的是，以上优选实施例仅用以说明本发明的技术方案而非限制，尽管通过上述优选实施例已经对本发明进行了详细的描述，但本领域技术人员应当理解，可以在形式上和细节上对其作出各种各样的改变，而不偏离本发明权利要求书所限定的范围。

Claims (2)

1.一种机器人控制系统轨迹加减速插补算法，其特征在于：

S1.光顺曲线：插补轨迹先用三次样条spline插值使曲线光滑，计算出各个节点处的函数值和两个边界处的导数信息；

S2.在S1已经计算好的三次样条曲线上插入等距点，通过D-H建模后的运动逆运算计算各个转动轴的运动距离。

2.根据权利要求1所述的机器人控制系统轨迹加减速插补算法，其特征在于：所述曲线包括：加加速段、匀加速段、减加速段、匀速段、加减速段、匀减速段、减减速段，对每个加速度积分得到各段对应速度；

$v\left(\mathrm{\τ}\right)=\left\{\begin{array}{c}{v}_1+0.5{\mathrm{J\τ}}_1^2,0\≤t\≤t_1,({\mathrm{\τ}}_1=t),A=Jt,v_1=v_0+0.5{\mathrm{Jt}}^2\\ v_2+{\mathrm{A\τ}}_2,t_1\≤t\≤t_2,({\mathrm{\τ}}_2=t-t_1),A=A_1,v_2=v_1=At\\ v_3+{\mathrm{A\τ}}_3-0.5{\mathrm{J\τ}}_3^2,t_2\≤t\≤t_3,({\mathrm{\τ}}_3=t-t_2),A=A_1-Jt,v_3=v_2+At-0.5{\mathrm{Jt}}^2\\ v_4,t_3\≤t\≤t_4,({\mathrm{\τ}}_4=t-t_3),A=0,v_4=v_{\max }\\ v_5-0.5{\mathrm{J\τ}}_5^2,t_4\≤t\≤t_5,({\mathrm{\τ}}_5=t-t_4),A=-Jt,v_5=v_4-0.5{\mathrm{Jt}}^2\\ v_6-{\mathrm{A\τ}}_6,t_5\≤t\≤t_6,({\mathrm{\τ}}_6=t-t_5),A=A_2,v_6=v_5-At\\ v_7-{\mathrm{A\τ}}_7+0.5{\mathrm{J\τ}}_7^2,t_6\≤t\≤t_7,({\mathrm{\τ}}_7=t-t_6),A=A_2-Jt,v_7=v_6-At+0.5{\mathrm{Jt}}^2\end{array}\right.$

A1为最大加速度，A2是最大减速度，J为加加速度，v₀为起始速度，v_max为匀加速阶段速度。