多轴步进电机的加速度，速度原理分析和如何实现

对于多轴步进电机联动的加减速，通过网络搜索，许多介绍也是比较单一讲解，像几条轴联动时如何计算各轴的加速度，减速度，空间矢量速度等如何计算都比较少有提及，通过GRBL源码，MACH3的插件SDK代码（可以通过编写插件实现外部运动，比如USB卡，即不使用打印端口），linuxcnc源码等学习研究，作一个记录和分享，希望也有对有这方面的同仁有所帮助，首先声明，本人只有初中的学历，所有的一切知识都是通过网络自学而来，所以在表达上面并不像哪些专科生哪样各种专业词汇，错漏之处也请提出指正

如何计算空间矢量速度和加速度

一般使用的CNC系统，可以有多条轴，XYZABC等多轴控制，笛卡尔坐标中，XYZ三条线性轴之间都是正交坐标，哪么像x0y0移动到x1y1这个矢量距离，实际上等于直角三角形的斜边,通过直角三角形的勾股定理即可求出空间矢量长度，对应的各轴实际的最大速度和加速度，空间矢量就是实际的刀路路径了，即刀路路径移动的速度，加速度，减速度

c*c=a*a+b*b

c=sqrt(a*a+b*b)

空间矢量的长度 Vector_length =sqrt(x*x+y*y+z*z),x=x1-x0,y=y1-y0,z=z1-z0

使用过CNC系统的都应该知道对应每条轴都有好几个参数，分别是轴的<最大速度>,<加速度>,<每毫米步数>，每条轴都可以不同值的参数，先说说<最大速度>,<加速度>

比如x_max_rate=2100mm/分钟,x_acceleration=120mm/秒平方

y_max_rate=2100mm/分钟,y_acceleration=120mm/秒平方

z_max_rate=1800mm/分钟,z_acceleration=100mm/秒平方

在执行G代码时，G0和G1的运动是有区别的，G0是以轴设置的最大速度以最快的速度进行移动的，并不用在切削加中，G1用在切削加工，带有F进给值，即切削速度，比如G1x10y20F1000,空间矢量的移动速度就是每分钟1000毫米，先按上面假设的这些参数看如何计算求得各轴的加速度和最大速度，比如G0x60y50z10代码

首先要从各轴的设置中以最小的加速度作为acceleration=min(min(120,120),100),即加速度为

acceleration=100

最大速度是按各轴的设置最大的速度max_rate=max(max(2100,2100)，1800）即

max_rate=2100 由于最大速度设置的是以分钟为单位的，转换为秒单位

max_rate=2100/60=35 转换为秒单位

如果是G1x60y50z10F1800，哪么max_rate=1800/60=30 转换为秒单位

根据G0x60y50z10代码，各轴移动距离为x=60,y=50,z=10,求矢量长度

Vector_length =sqrt(60*60+50*50+10*10）=78.74007874

求出各轴矢量

x_Vector=60/78.74007874=0.762000762

y_Vector=50/78.74007874=0.635000635

z_Vector=10/78.74007874=0.127000127

求各轴加速度

x_acc=acceleration*x_Vector=100*0.762000762=76.2000762

y_acc=acceleration*y_Vector=100*0.635000635=63.5000635

z_acc=acceleration*z_Vector=100*0.127000127=12.7000127

GRBL的源码中acc=acceleration/Vector的，得到比轴设定的加速度大许多，从GRBL源码的plan_buffer_line函数可以看到，MACH3中是acc=acceleration*Vector，通过分析，确定是acc=acceleration*Vector才是正确的，因为空间矢量的加速度可以反求回来，包括空间矢量速度也是如此，因为空间矢量的移动速度是三条轴速度合成后的速度

acceleration=sqrt(x_acc*x_acc+y_acc*y_acc+y_acc*y_acc)

各轴最大速度

对于各轴的最大速度和空间矢量的线速度，GRBL的源码中全部使用 max_rate=max_rate/Vector,没有区分G0和G1的区别，MACH3中，计算空间矢量的线速度和各轴速度是区分开来的，各轴通过max_rate=max_rate/Vector后min出最小的哪根轴的速度作为最大速度max_rate，因为按各轴所设置的最大速度所合成后的速度才是空间矢量的线速度，sqrt(x_max_rate^2+y_max_rate^2+z_max_rate^) ,对于G1指令的F进给值，是给定了空间矢量的线速度了，所以按这个速度求各轴的最大速度，实际上G0只是快速移动，不进行切削的指令，所以区分开来计算，其它切削的指令G1，G2,G3等都带F进给值设置的，都属于切削一类的指令，空间矢量的线速度和各轴的实际速度是同一计算方式

对于G0

x_max_rate=max_rate/x_Vector=35/0.762000762=45.9317126

y_max_rate=max_rate/x_Vector=35/0.635000635=55.11805512

z_max_rate=max_rate/x_Vector=35/0.127000127=275.5903

求得实际的空间矢量的线速度：

max_rete=min(min(x_max_rate,y_max_rate),z_max_rate)=45.9317126

求得实际的各轴速度：

x_max_rate=max_rate*x_Vector=45.9317126*0.762000762=35

y_max_rate=max_rate*x_Vector=45.9317126*0.635000635=29.166666667

z_max_rate=max_rate*x_Vector=45.9317126*0.127000127=4.445004

验证空间矢量的线速度：

max_rate=sqrt(35*35+29.166666667*29.166666667+4.445004*4.445004)=45.9317126

对于G1,G2,G3的F进给值，直接如下计算：

指定的空间线速度为F35

max_rate=F=35  为空间矢量的线速度

对应各轴的速度：

x_max_rate=max_rate*x_Vector=35*0.762000762=26.67002667

y_max_rate=max_rate*x_Vector=35*0.635000635=22.22502223

z_max_rate=max_rate*x_Vector=35*0.127000127=4.445004445

关于acc,max_rate,轴移动长度，路径移动长度，根据加速度原理

t= Vector_length/max_rate 得到空间矢量即路径按这个速度移动完成所要的时间

t=78.74007874/35=2.249716535428571秒

哪么把各轴移动的长度除这个时间就同样得到上面一样的各轴的最大轴速度了

x_max_rate=60/t=60/2.249716535428571=26.67002667

y_max_rate=50/t=50/2.249716535428571=22.22502223

z_max_rate=10/t=10//2.249716535428571=4.445004445

由于x_Vector=x_max_rate/max_rate的关系,同样可以反求各轴的Vector

x_Vector=26.67002667/35=0.762000762

t=max_rate/acc 求得加速到最大速度的时间

acc=max_rate/t

t=35/100=0.35

然后通过这个时间又可以求得各轴的加速度值来

x_acc=x_max_rate/t=26.67002667/0.35=76.2000762

y_acc=22.22502223/0.35=63.5000635

z_acc=4.445004445/0.35=12.7000127

同样也是可以反求出x_Vector

x_Vector=x_acc/acceleration=76.2000762/100=0.762000762

加速度，速度，矢量长度和时间的关系，得到了以上各种关系式

为什么把各轴的加速度和最大速度都计算出来呢？GRBL中只求出空间矢量的加速度和最大速度，因为GRBL直接以空间矢量长度作为加减速度计算，然后各轴的脉冲插补输出，这里计算出来各轴加速度和最大速度，是为了分析和理解实际的原理，在MACH3中是直接计算求出各轴插补脉冲数的，放在一个缓冲区，然后取得PC底层权限控制的定时器，向打印端口输出脉冲，但WIN系统并非实时系统，所以还是存在不稳定性，要是运动部分使用支持双精度浮点的单片机或者DSP+FPGA，MCU内部有精确和稳定的定时器，支持双精度浮点硬件运算，可以高效果运算，加减速速度计算做得更实时，可以做到非常高脉冲频率，不过没接触过FPGA，对这完全不懂得呀！（：

GRBL，MAHC3都是以取一时间片为作加减速度和最大速度对应的脉冲数存放于缓存中，然后定时器把一个个时间片对应的脉冲数从缓存中取出控制各轴IO插补输出，哪么先看一下加速度与速度之间的关系

v=a*t   

v速度，a加速度，t时间

t=v/a    根据加速度a要多长时间t加速到最大速度v

s=0.5*v*v/a=t*t*a*0.5 面积公式求得加速到最大速度v所要的步数s

有了这些就可以计算出各轴加速到最大速度所要的步数了，比如各轴的每毫米步数为

x_mmsteps=y_mmsteps=z_mmsteps=640步/毫米

x_max_rate=x_max_rate*x_mmsteps 转换为步单位

x_acc=x_acc*x_mmsteps 转换为步单位

x_s=0.5*x_max_rate*x_max_rate/x_acc 根据公式s=0.5*v*v/a=t*t*a*0.5

y_s=z_s=x_s 其它轴一样算法

y轴，z轴也是这样计算，不再列出了

按时梯型加减速，按时间片，分成一段段统一时间长度的脉冲数，比如每段时间长度为4毫秒0.004

通过公式s=0.5*v*v/a=t*t*a*0.5，s=t*t*a*0.5,GRBL和MACH3也是此种方法实现加减速

 浮点数型 x_pulse[100],y_pulse[100],z_pulse[100]

浮点数型 x_pulsepos,y_pulsepos,z_pulsepos

轴当前位置 浮点型 x_current_pos=y_current_pos=z_current_pos=0,初始化时即机器当前位置

轴浮点步处理 浮点型 x_Fractions=y_Fractions=z_Fractions=0,temp=0;机器初始化时全为0

加速部分：

浮点型 tt=x_max_rate/x_acc

for(n=0;n<tt/0.004;n++){

x_pulsepos=((n+1)*t)^2*a*0.5=((n+1)*0.004)^2*x_acc*0.5 得到每递增的时间片的面积位置

temp=x_pulsepos-x_current_pos；

x_current_pos=x_pulsepos[n];

x_pulse[n]=temp+x_Fractions; 补偿浮点步，即分数补回去

if(x_pulse[n]>0.9999)x_pulse[n]=1;  是为了有的移动距离为浮点步，即0.x步

if(x_pulse[n]<-0.9999)x_pulse[n]=-1; 

x_Fractions=x_pulse[n]-(int)x_pulse[n]; 取出浮点步，即0.x步，分数

x_pulse[n]=(int)x_pulse[n]; 取为整数步

然后其它轴的脉冲数计算，不再列出了

y轴脉冲数计算。。。。。。

z轴脉冲数计算。。。。。。

}

这里要注意，上面的计算要判断不要超过加速所要的步数x_s，上面演示伪代码没有写出。

各轴中max(x_pulse[n]，y_pulse[n],z_pulse[n]) 看哪轴步数最大，求出定时器时间，比如

x_pulse[n]为最大的步数，x_pulse[n]/0.004=频率，频率倒数就是时间，根据每次不同的步数进行设置定时器的时间即可，即不同的脉冲数都对应着不同的定时器频率来进行脉冲插补输出 ，怎么插补请看GRBL定时器里头的代码

关于恒速度段以最大速度x_max_rate*0.004即公式v=a*t取得每个时间片的最大速度步数，有时候并非是整数步，所以也是得浮点步补偿回去才行，像GRBL实际上也时有浮点步的解决方式，一方面利用舍入函数，一方法把浮点时间加入到定时器里处理

比如1800mm/min的速度，即30*640=19200

=19200*0.004=76.8步

循环处理里：

{

temp=19200*0.004=76.8 ；

x_pulse[n]=temp+Fractions；补分数步

x_pulse[n]=(int)x_pulse[n];

Fractions=temp-(int)temp; 取分数步

}

一直以这个循环，根据移动步数和加速度步数计算判断是否进入减速段，减速段实际就是加速段的反过程，如果移动的距离较短，就只有加速和减速段，没有恒速度段，根据面积公式求得移动距离对应的加速，恒速，减速对应的步数，进行判断即可

 以上为直接计算出各轴对应的时间片脉冲数的多轴加减速实现原理，再把每组的脉冲数在定时器里插补输出，对于时间片的时间大小，当然时时间越短，加速斜坡会越平滑，阶梯越小了，但要根据芯片的运算速度能力来定义，还有缓冲的大小，当然像GRBL哪样，只按空间矢量计算一次，然后再插补，这样会省许多运算量，这里只是把原理详细的表达出来，当然像MACH3这样，是用电脑运行的，电脑的CPU运算能力强，直接使用双精度浮点都完全没压力，所以MACH3和linuxcnc都是直接计算全部轴的脉冲数出来的另外MACH3看插件SDK的部分代码与linuxcnc是极为相似的，估计前身也就是linuxcnc

还可以把梯型加减速增加S型加减速，如何实现自行找相关资料，像Marlin-2也实现了S型

另外再说说像G1的F进给值，当F值非常小时，比如F6,即6/60=0.1mm/s的最大速度时，这是无法计算出加速到最大速度所要的步数的，这时就得要把加速度降下来，GRBL没有这个算法，是直接把定时器插补次数乘以2和3，以增加积分次数，即多步来实现

MAHC3是直接计算出来，因为是直接计算出脉冲数的方式，具体是这样

比如G1x10F6执行这样的G命令

比如加速度为6400步，进给为每分钟6mm，转换为秒单位为vf=6/60=0.1mm/s，转换为步单位0.1*640(640为每毫米步数)=64步每秒的速度

加速度acc=64000步,vol=64步,求出加速所要的时间64/64000=0.001秒，如果我们把时间分成段来产生脉冲，比如按4毫秒一段，已经固定下来，不能再改变的情况下，按照这样的速度和加速度加速所要的时间为1毫秒，哪么4毫秒一个时间段已经远超出1毫秒的加速度时间了，余下的3毫秒也无法加速，也无法进行处理均速，所以此时就得把加速度降低下来，即衰减到适合4毫秒一段，因为速度太低了

vf=6/60*640=64

a_t=64/64000=0.001   计算加速所要的时间，显然这个加速的时间比4毫秒要小得太多了

4毫秒为一个时间段的话，哪么得到一个衰减加速度的比率

ratio=0.001/0.004=0.25

其实也可以这样算，0.004*64000=256，到这里大概明白了，按64000加速度，4毫秒一个时间段，一个时间段的加速增量256HZ，64HZ放到这个时间段时，已经一个时间段就达到256HZ的加速度，远超64HZ，所以根本无法让移动低于256HZ了，0.004*0.004*64000/2=0.512步的移动位置，可以反算实际输出频率0.512/0.004=128HZ，第二个时间段0.008*0.008*64000/2=2.048步，2.048/0.004=512HZ，好明显，已经远超64HZ了，这个加速度的加速增量，远超64HZ，2.048-0.512=1.536+0.512=2.048步，整数为输出脉冲，在第二个时间段2步脉冲，实际脉冲频率2/0.004=500HZ，在第一个时间段已经实际输出的频率已经达到500HZ，远超64HZ，所以必须衰减加速度才能实现64HZ，即进给以6mm每分钟移动

64/256=0.25

比如进给值为20mm每分钟，转换为秒步即20/60*640=213.33333，也是比256要小，

213.3333333/256=0.8333333333的比率

比率只要大于或者等于1都满足4毫秒，所以不用衰减，这里就做一个比较，如果小于1，证明这个进给值已经不能用原来的加速度以更低的速度移动了，必须降低加速度后重新计算实际的加速时间来计算输出脉冲

if(ratio<1){ acc=64000*ratio=64000*0.25=16000}

这样6mm每分钟进给的速度下使用衰减后的加速度和减速度都以acc=16000来重新计算加速所要的时间

a_t=64/16000=0.004  可以在一个时间分段内完成这个加速

s1=0.004*0.004*16000/2=0.128步，第一个时间段移动到步数的位置(注意这是位置值，并不是实际的输出步数）

s2=0.008*0.008*16000/2=0.512步  第二个时间段移动到步数的位置

s3=0.012*0.012*16000/2=1.152步 .........

s4=0.016*0.016*16000/2=2.048

每移动一个位置步，就是每个时间段的增量变化

按这三步移动的位置，计算出实际要移动的步，因为只能整数步

哪么

第一步0.128-0步 余0.128步，实际输出脉冲0步

第二步0.512-0.128步 余0.384+0.128=0.512不足一步，余0.512(即前面的分数步），实际输出脉冲0步

第二步1.152-0.512步 余0.64+0.512=1.152步，足1步，实际输出脉冲1步，余下0.152步(即前面的分数步）

第四步=2.048-1.152 余下0.896+0.152=1.048，足1步，实际输出脉冲1步，余下0.048步(即前面的分数步）

往后步数同样如此求得.........

再说一下关于前瞻，这里并不是说前瞻的原理，是关于G61和G64，对于G61在MACH3来说为精确停止模式，何为精确停止呢？实际上等于没有前瞻，循环加工时，MAHC3的精确停止模式，即G61，每一条路径从起点加速到恒速度再减速到停止再从下一条路径加速度到恒速度再减速到停止，如此重复，这样加工的时间会特别的长，GRBL在源码里说是只实现了G61模式，GRBL里G61为精确路径模式，根据每一段的路径的之间的夹角计算出转弯速度，即不同的夹角大小对应的过弯速度都不一样，夹角过大与精确停止一样，这样能大大减少加工的时间，对于G64模式呢，在MAHC4的使用手册是指连续路径模式，以更高的速度过弯，方式是根据路径与路径之间的夹角计算出相应的过弯速度，把尖角改变为圆弧过弯，即不同的夹角大小，会插入相应的不同直径大小的圆弧，这样能大大提高过弯的速度，即本身G代码编程为尖角的，会变成了圆弧插值，直接修改插入原来的尖角改为圆弧，MACH3手册说这样对于某些材料的加工表面会更光滑，但会改变尖角为圆弧，实际百度搜得G61，G61.1，G64的说明分别为G61，精确路径模式，G61.1为精确停止模式，G64为连续路径模式，G64的解释与MACH3的差不多，G61也不知道有没有前瞻，G61.1应该就是精确停止，不会有前瞻，即每一段都是加减速到停止再下一路径同样方式，MACH3没有G61.1模式

在3轴以上，即XYZ以上比如加入ABC三个轴，由于ABC通常用于转轴，并非线性轴，这样无法与XYZ以90度正交，求矢量得分开来算，XYZ为主的，如果XYZ上没有任何移动时ABC上有移动才根据ABC来计算轴的加速度和速度，具体可以分析linuxcnc源代码，linuxcnc的代码相对较专业，看着比较头痛不好理解，有些部分还用上的四元素，基于初中文化程度，有些数学还是不好理解，也希望懂得数学公式多的把linuxcnc源代码核心原理分析得适切的大佬能写个分析文章分享一下，也可以更深入的学习学习！

能力有限就先写到这里了

谢谢观看！希望能对你有所帮助！