树莓派步进电机S型加减速控制算法

S型曲线分为加速阶段、匀速阶段和减速阶段。加速阶段又分为加加速阶段和减加速阶段，减速阶断与加速阶段对称。

实现原理：

假设给定条件：初速度，加速段的末速度也就是匀速阶段的速度，加速时间，行进的总路程。

以此来计算控制步进电机的每一步速度。
加速段由加加速段(IncAccel)和减加速段(DeAccel)组成。
因为T1=T2，T4=T5，为了简化算法初速度V0和最终速度Ve都为0，使T1=T2=T4=T5=T，那么T和T3确定了加加速度，加速度，速度，位移之间的关系就确定了。
加加速段是加速度关于时间的斜率为正的直线，减加速段则是加速度关于时间的斜率为负的直线。
加速度从0开始变化，到了一定值开始减小，最后为0，由于加速度的斜率是相同的，所以加加速度段和减加速度段，所用的时间是一样。这两段曲线是关于加速度最大值的对应的速度中心对称的。

针对加加速段分析：

此时

且初速度V0=0：
当加速度从0变化到最大值，有

J为加加速度（Jerk），对加速度积分面积就是速度，
 

 

速度的变化量：

，那么

 

只要知道J即可得到加加速段的速度变化，当加速度变化到最大值时，此时
速度（V）=中点速度(VM)，可得：

其中：

vt为末速度，是减加速段最终的速度值，而这一段的距离可以求得：
 

针对减加速段分析：

此时

，且此阶段初速度为上个阶段的末速度v，此阶段末速度为vt。
当加速度从最大值变化到0，有

J为加加速度（Jerk），对加速度积分面积就是速度，
 

 

速度的变化量：

那么

，整理成此步是为了方便建立速度表。

而这一段的距离可以求得（平均速度乘时间减去加加速段位移）：
 

 

整理一下可得：
 

对于匀速阶段：

已知了加速阶段的距离，而减速阶段与加速阶段对称，用总路程减去2倍的加速阶段的距离就是匀速阶段需要形式的距离

对于减速阶段：

与加速阶段对称，只需要将加速阶段的倒序即可得到减速阶段的速度。

算法实现：

在实现算法时，我们需要将速度单位转化为step/s，这样用速度*时间，就得到了在一定时间内需要行驶的步长。

对于速度而言，只要t变化足够小，即dt小，那么速度变化就越平滑，得到的速度点拟合出来的曲线就越接近目标曲线。

首先，需要根据已知的条件计算中值速度，加速度斜率，加加速的距离，加减速的距离

    #加加速的时间为整体加速的一半
    t1 = tIncAcc/2
    #中值速度
    Vm = (Vaverage - V0)/2
    #求得加加速阶段加速度的斜率
    Jerk = (2*(Vm-V0))/(t1*t1)
    #加加速阶段的距离
    S1 = ((V0*t1) + ((Jerk*pow(t1,3))/6))
    #减加速阶段的距离
    S2 = ((Vm*tAcc) - S1)

根据给定的时间分割系数获得deltaT:

    deltaT = tIncAcc/(partition)

对于整体加速过程，分为加加速段和减加速段，且两段的时间相等，则可以根据公式求得连个阶段的速度表：

    #整体加速阶段的速度表
    for i in range(0,(partition)):
        sumT += deltaT
            
        #求得加加速阶段的速度表
        if (i < ((partition)/2)):
            DeltaV = 0.5*Jerk*pow(sumT,2)
            speedArray[i] = V0 + DeltaV
         
        #求得减加速阶段的速度表
        else:
            DeltaV = 0.5*Jerk*pow((tAcc - sumT),2)
            speedArray[i] = Vaverage - DeltaV
        

对于匀速阶段的速度则为加速阶段最后的速度

减速阶段的速度为加速阶段速度的倒序，在这里就不贴代码了。

消除误差：

因为在计算过程中很产生小数，但步进电机的步数不能是小数，所以需要将小数部分的误差消除，具体方法为，将速度表转化为路程表，即每段时间内需要行驶的步长，遍历路程表，将所有的小数都累加起来，最后将累加起来的小数部分放到匀速阶段的路程中去，这样总的路程就不会减少。

    #用于记录速度表的余数
    remainder = 0.0
    #每个时间窗口内需要走的行程，s=vt
    disArray = np.linspace(0, 0, 2*partition+1)
    for i in range(2*partition+1):
        distmp = speedArray[i]*timeArray[i]
        disArray[i] = int(distmp)
        remainder += (distmp - disArray[i])
        
        #因为时间窗口为200us，所以让每一个时间窗口内的行程都为偶数，消除不能整除误差
        if (disArray[i]%2 != 0):
            disArray[i] -= 1
            remainder = remainder+1
        
    print('remainder:',remainder)
    #将所有的累积余数放到匀速阶段
    disArray[partition] += int(remainder)

代码链接为点这里

参考文章：步进电机驱动算法——S形加减速算法原理 | 码农家园