三角函数S型加减速算法原理及实现

1、算法原理：

如上图在使用步进或伺服电机实现物体X由A运动到B的过程中，通常需要电机在行程S内有加减速的过程。需要加减速的原因：对于步进电机而言有空载启动频率，如果脉冲频率大于该值则步进电机不能正常启动，发生丢步或堵转情况。同时，加减速的存在可以是的运动平滑，减小惯性冲击。

S型加减速分为5个阶段：加加速阶段，减加速阶段，匀速阶段，加减速阶段，减减速阶段。定时器PWM波输出说明：
定时器计数频率：Freq = CK_PSC / (PSC + 1)；

PWM频率：Freq = CK_PSC / (PSC + 1) / (ARR + 1)2、算法实现2.1、速度规划

首先需要根据用户的输入参数对整个行程范围内的速度曲线进行规划，对于S形加减速曲线而言一般分为加加速阶段、减加速阶段、匀速阶段、加减速阶段、减减速阶段，但如果输入参数不合理会导致在行程范围内无法达到设定的目标速度，此时需要重新规划。

由于在加速阶段加速度是从0增大最后又为0，所以，T1=T2=Ta/2,同理，T4=T5=Tb/2。
对于三角函数形式的S型加减速曲线，其加加速度Jerk曲线为正弦曲线，如下所示：

则：

为了避免在MCU中计算三角函数，采用切比雪夫多项式对三角函数进行逼近：

由正弦函数曲线可知，其在T/2处取得最大值则：

综上：可得各阶段的加加速度、加速度、速度、位置表达式为：

Jerk曲线方程：

加速度曲线方程：

速度曲线方程：

位置曲线方程：

由上述公式可以求得加加速、减加速、加减速、减减速阶段的运动距离，从而可以求出匀速段的运动距离：

s3=s-s1-s2-s4-s5，此时存在如下三种情况：

①、如果s3>0则说明能达到指令速度并且存在匀速段

T3 = s3/vm；

②、如果s3=0则说明能达到指令速度并且不存在匀速段

T3 =0；

③、如果s3<0则说明不能达到最大速度不存在匀速段，需要从新规划假设最大速度为vx则令s=s1+s2+s4+s5

T3 =0；

得：

将其代入上式中重新规划

2.2、插补

在MCU中利用定时器进行时间插补，插补周期detT，利用积分原理实现对速度的插补得到期望的位置。
在前面速度规划中得到了三个阶段的时间t1、t2、t3、t4、t5。
假设当前插补次数为n，则运行时间为n*detT

假设整个运动时间为T，插补周期为△t，则插补次数n=T/△t。则需要得到每个插补周期的运动距离△s。
这里得到的距离还是用户单位，输出时需要将其转换为脉冲数。

转换关系式为：

插补脉冲数=导程插补距离?一圈脉冲数

在MCU中需要在定时器中统计总共执行的脉冲数，在T形加减速算法执行完毕后与总距离的脉冲数进行对比，如果输出的脉冲数少了则需要以终止速度继续输出剩余脉冲数，保证精度足够准确。
3、MATLAB 验证

4、MATLAB和C源码

见源码章节部分

三角函数S型加减速算法原理及实现

http://bbs.plcjs.com/forum.php?mod=viewthread&tid=498567&fromuid=17

(出处: PLC论坛-全力打造可编程控制器专业技术论坛)

#加减速#