基于Machinekit的S型曲线前瞻规划算法
2018-08-29马殿光唐厚君张英隆
段 炼, 马殿光, 唐厚君，张英隆
(上海交通大学 a.电子信息与电气工程学院；b.基建处，上海 200240)
0 引言
随着数控行业的不断发展，CNC系统的运动控制不断朝着高速度，高精度的方向发展。目前大部分国产CNC系统仍然采用简单但加速度突变较大的梯形加减速运动控制算法[1]，而国外高端数控系统已经普遍应用了S型加减速、三次样条插补[2]等运动控制算法。加速度突变会造成机床在高速运动过程中受到超出机床承受力的冲击，引起机床抖动和速度过冲，无法达到运动控制的高精度要求，同时也会对机床的机械部分造成损耗。
Machinekit是在LinuxCNC的基础上衍变而来的数控系统开源软件[3]，架构清晰，易于二次开发，有非常广阔的市场前景。其已具备常规的梯形加减速运动控制算法，对于更加平滑的运动规划算法，如S型速度规划尚未支持。 对于常规S型算法的研究，Kim-Doang等在文献[8] 中提出了7段S型速度运动过程的分类以及加速度，加加速度的计算公式，史步海等在文献[1] 中提出了S型曲线在LinuxCNC上的开环控制应用。
Machinekit闭环控制的特点决定着研究计算简单可靠，实时性高的闭环规划算法是很有必要的。本文在分析Machinekit的实时运动规划架构的基础上，提出了一种基于周期离散化和前瞻算法的S型速度规划算法，通过设定S型加减速过程中不同的状态量和有效的前瞻算法捕捉状态量切换时间点，从而有效地通过单个轨迹规划周期的计算，实现实时规划和闭环运动控制。
1 Machinekit的运动控制分析
1.1 实时轨迹规划架构
Machinekit的主要功能模块是运动控制器(EMCMOT), 输入输出模块(EMCIO)，图形用户接口(GUI)，任务控制器(EMCTASK)[4]。Machinekit通过GUI界面接收到用户输入的命令，通过EMCTASK进行解析，再分别传递给EMCMOT和EMCIO进行执行[5]。其中关于运动规划是在运动控制器EMCMOT中执行的，包括正解、逆解运动计算，直线圆弧轨迹规划，样条插补等。
通过EMCMOT模块创建EmcmotController实时线程，该线程是以伺服周期运行的主线程，所有的状态逻辑转换与轨迹运算均在此实时线程中完成。
Machinekit支持不同的轴运动模式，如单轴运动的自由模式和多轴的联动模式等，并根据不同的运动状态来进行不同的运动轨迹规划，主要检测的运动状态和运动状态间的转换如图1所示。
图1 运动状态检测框架
1.2 实时轨迹规划流程
Machinekit通过轨迹规划模块(TP/TC)和三次样条插补来完成规划和插补任务[6], 其自带的前瞻计算功能，可以区分当前位置到目标位置所需要的时间是否能达到规划周期的最小整数，实时微调规划周期和末点速度，然后进行每个规划周期内位置、速度以及加速度的规划。如果实际位置等于目标位置时，轨迹规划队列中的当前任务完成，规划结束，该任务从队列中清除，然后进行下一条运动轨迹任务的规划。
每完成一次完整的路径规划后，均会通过逆运动学计算得到每一个轴的进给位置，即将笛卡尔世界坐标系坐标转换成关节坐标系位置，并且将各个轴的计算位置放在三次样条曲线的第四个点，最后通过小于规划周期的精插补周期进行三次样条插补，即其余三个点通过三次样条插补计算得到[7]。
2 S型加减速前瞻规划
基于Machinekit的周期性轨迹规划的特点，本文提出了一种基于当前位移和目标位置距离的前瞻算法，可以快速的计算出当前位移所对应的7段S型运动过程[8]。
vcur,acur,jcur分别表示当前规划周期机床的实时速度，加速度，加加速度，vmax,amax,jmax分别表示数控机床实际运行过程中所能达到的最大速度，最大加速度，最大加加速度，其中一般为了保证机床加速性能利用率最大化，设定jcur=jmax。tc为轨迹规划周期，vreq,P,T分别为进给速度，当前规划周期机床的实际位移和目标位置三个参数[9]。由此可得基于位置差值的前瞻算法步骤如图2所示。
图2 前瞻算法流程图
步骤1：判断当前速度是否为0，若是，则运动已经停止。
步骤2：由公式：
(1)
anext=acur+jcur·tc
(2)
在假设jcur不变的情况下，可计算出下一个周期的预测速度与加速度。根据下一个周期的加速度和速度值来进行前瞻预测，可以有效地避免规划周期的1ms延迟导致的参数溢出和拐点滞后的问题。
步骤3：假设从anext开始，立刻减速运动，计算所需要最短的刹车距离[10]。如果存在减加速段，即anext>0，则：
tdec=ceil(anext/jcurtc)·tc
(3)
(4)
(5)
anext= 0
(6)
如果不存在步骤3，则减加速段距离d2=0。此时，不同于常规的S型算法[11]，可以认为通过公式(3)～公式(6)的状态更新，减加速段已经结束，机床处于加速与减速阶段的临界拐点。如果在另外的情况下，即使步骤3直接从加减速段甚至减减速段开始时，可以将已经开始存在的anext<0的减速状态回溯至此拐点。至此，所有的运动情况的减速过程均保持一致，计算公式唯一，将大大减少运算量。统一所有的加速度状态至加速段与减速段的临界拐点公式如下：
tf= -anext/jcur
(7)
(8)
(9)
当从减加速段过渡到临界拐点时，公式(7)～公式(9)均为0，即跳过计算，而减加速段和减减速阶段通过上述公式可以统一回溯至减速前临界点，方便后续计算。
步骤4：进行减速段前瞻运算，此时anext<0, 速度从规划的最大速度减小到0。
t5= -(vnext-2vjerk)/amax
(10)
(11)
剩余的加减速和减减速过程由于对称，可以直接求出：
(12)
d6=vnext·t4
(13)
步骤5：获得了前瞻规划的刹车分段距离，根据所获得的d2,d4,d5,d6值，可以快速确定当前的规划状态：
如果d2+d4+d5+d6>T-P，表明必须立即刹车，设定jcur=-jmax，否则会造成过冲，由于提前了一个周期规划，所以不会造成拐点滞后导致的目标点溢出的问题。如果∑d
步骤6：在求出jcur的值后，根据公式(1)、公式(2)更新当前速度，加速度状态，将更新后的速度与加速度值与amax,vmax进行比较，如果达到这些限定值，则分别进入匀加(减)速和匀速运动状态，同时更新规划位置值：
(14)
通过以上基于位置差值的前瞻算法，可以避免复杂的常规S型曲线计算，大大减少了计算量，同时符合Machinekit基于规划周期的实时规划特点，在每个周期通过简单的刹车距离前瞻规划，可以得到下一个周期的加速状态。
3 离散化S型曲线前瞻规划
3.1 离散化计算原理
(15)
(16)
(17)
(18)
(19)
(20)
由上述公式可知，连续时间转换成离散化计算，只需将相应的速度，加速度，加加速度在规划前通过公式一次性转换，即可在实时规划中去掉tc的计算分子。
3.2 Machinekit的离散转换方法
通过对Machinekit的实时规划架构分析可知，更新后的前瞻算法具体代码位于EMCMOT模块，除了将前瞻算法的连续时间公式更改为离散化公式以外，还需要将与机床本身参数有关的最大加速度，最大速度等变量根据式(16)、式(17)转换成以tc为基本时间单位的离散量。
Machinekit的离散转换可以通过在INI参数文件中添加最大最小速度，加速度离散转换系数，将其转换成离散量，直接在规划中使用。也可以通过在计算公式处添加离散转换系数，将最大加速度，最大速度等参数转换。为了避免每次修改参数需要重新编译源代码，本文采用第一种离散转换方式。
4 测试结果
Machinekit提供了强大的Halcmd功能来观察实时规划过程中速度，加速度等各种参数的变化，可以清晰地观察出与管脚绑定的各种参数的具体值。HalScope是Machinekit开发的一款示波器显示UI，可以实时跟踪各个管脚或者信号量的变化情况，支持边沿触发模式，自动捕获模式，同时支持高达16个管脚的检测和200000的采集量的保存。
为了验证高速环境下S型前瞻算法的效果，设定最大加速度5000mm/s2，最大速度1000mm/s，最大加加速度5000mm/s3[12]，在Machinekit实验平台上测试效果如下：
图3 位移为500mm时的速度曲线
图4 位移为5000mm时的速度曲线
图5 联动模式下，默认梯形规划速度曲线
图6 联动模式下，改进后S型速度曲线
通过HalScope的显示结果，可以清晰地看到，前瞻性的S型曲线规划可以很好的适应不同的规划距离、速度，并且在联动模式下能够提前规划速度的变化量，使得机床速度变化更加平滑，大大减小了速度、加速度的突变对机床的冲击。
通过测试不同距离下，离散化与连续化分别需要的规划时间，通过对比试验，得到结果如表1所示。
表1 连续与离散化计算时间对照表
由上面结果可知，将S型曲线规划算法离散化后，能够带来毫秒级的优化效果，这对于在运行条件苛刻或者实时性较差的硬件平台上进行前瞻规划具备较大的优势。同样，通过试验结果可以发现规划距离越短，优化率越高，说明离散化前瞻算法在多条连续小线段规划中能够起到更好的作用。
5 结束语
通过对Machinekit的运动控制架构进行分析，针对已有的梯形规划算法的弊端，结合Machinekit闭环和小周期运动规划的特点，本文提出了基于位置前瞻的S型曲线规划算法及其离散化优化方法，可以有效地解决常规Machinekit规划算法的加速度突变和机床抖动问题，本算法计算简单，易于实现，支持多轴联动的连续轨迹规划。通过实际测试证明，该算法的规划误差在实际工业生产的允许范围内，同时使得机床运动更加平滑，满足在高速度，频繁加减速情况下的工业生产应用。
