基于STM32的5段式S型交流伺服电机控制方法*

夏侯命栋，张雄杰※，王 鲍，李 东，叶煜力，冯 丽

（1.东华理工大学核资源与环境国家重点实验室，南昌 330013；2.东华理工大学核技术应用教育部工程研究中心，南昌 330013）

0 引言

在实际应用中交流伺服电机主要用于精密测量台、工业机器人、人形机器人等，其主要特点在于精密的位置控制[1]。为减少电机在启停阶段的抖动和噪声、速度不稳定、定位不准确，就必须采用专门的加减速控制算法设计一款控制器，使得该控制器能够指定电机按照预设速度、预设位置进行移动。

加减速控制算法的使用已较为普遍，主要包括：指数型模式、线性模式、S 型模式，多项式加减速[2-3]。指数加减速算法实现启停时速度突变按照指数规律上升或下降，肖立娜[4]将传统的指数加减速算法进行改进，提出微小程序段间的速度平滑指数加减速算法，可实现微小程序段之间的连续加工，减少加减速控制次数，提高运行效率。方佳伟[5]提出基于Sigmoid 函数S型加减速算法，此算法通过时间和速度数组的遍历，减少了机械设备启停的抖动时间。黄艳[6]在2005 年提出一种7 段式的S型曲线加减速算法，该算法可以获得平滑的速度和加速度，减少电机在运行过程中产生不希望的振动。钟前进[7]在2019 年提出了一种新型的S 型曲线加减速算法，该方法是利用三角函数在梯形速度轨迹上拟合一条S 型速度曲线，该算法简单且能够输出连续的速度和加速度。徐川[8]提出一种多项式加减速控制方法，该多项式包含11个速度调整阶段，使得系统具有更高的加减速柔性。

7 段式和11 段式的加减速算法，虽然可以实现较好地加减速，但分段公式多，运算量较大。为降低计算复杂度，保证速度柔性[9]，本文提出5 段的S 型加减速算法。相比于指数型和线性模式，可以减小启动和停止过程中的噪声，速度也趋于平滑。采用位移和速度数组遍历查询的方式进行程序设计，可实现电机速度、加减速时间、位移可调的加速启动、匀速运行、减速停止。将该算法运用到STM32 单片机上实现对电机速度和位移进行调整和验证。

1 5段式S型加减速控制算法1.1 5段式S型控制算法原理

5段的S型加减速是根据速度、时间、加速度、加加速度和位移等物理公式所构造的分段函数。图1和图2分别为梯形加减速曲线和5段式S型加减速曲线。由于梯形加减速的加速度在运行过程中存在突变，因此会使得伺服电机产生冲击和噪声，降低了运行的平稳性[10]。而在5段式的S型加减速中，电机的加速度是连续的，不存在突变，从而可以减少冲击，提高运行平稳性。

图1 梯形加减速曲线示意图

图2 5段式S型加减速曲线示意图

如图2（a）中所示t1阶段是加加速阶段，t2阶段为减加速阶段，t3为匀速阶段，t4为加减速阶段，t5为减减速阶段，Vm为中间点速度；图2（b）中t阶段为加速过程加速度曲线。

式中：a为电机运行的加速度；k为加速运动的加加速度；t为电机运行的当前时刻。

式（1）中加加速度恒定为k，加速度是一条根据时间变化的直线。

式中：T为电机加速过程中间点时间；t为总的加速过程时间。加加速运动与减加速运动的运行时间是总加速时间的一半。

式中：vm为电机运行的中间点速度；v0为初速度，vt为末速度。中间点速度是初速度与末速度的一半。

式中：V为电机运动的瞬时速度。

式（4）对式（1）进行积分求得某一时刻的瞬时速度，因此式（4）中速度的改变是由时间所确定的。

当瞬时速度V=Vm，且设置初速度V0=0时，将式（4）进行转变得到式（5），进而得到恒定的k值。

式中：S为总的位移，通过对瞬时速度进行积分即可得到。因此运行过程中总位移S的值是由加加速度k和运行时间t共同决定。

1.2 控制算法的分段规划

在5 段式S 型加减速中加速阶段和减速阶段是对称的，加速阶段速度表顺序取值，减速阶段速度表逆序取值，匀速阶段则是以加速阶段最大速度运行，因此只需建立加速阶段的速度表。

（1）加加速阶段

由式（6）可知，若已知当前加加速段的位移S，即可求得当前加速的时刻值：

式中：i为当前加速次数对应位移；Ti为当前位移下的时刻。

将Ti代入到式（4）则可求得速度的变化量dV：

存入数组的速度值即为：Speed[n]=V0+dV，0≤n＜加加速阶段次数。

（2）减加速阶段

在减加速度阶段中，电机运动的实际加加速度值为-k。因此相比于加加速阶段，该速度表的生成类似于以初速度为Vt、加加速度为k的类减速运动。求取速度时本文将减加速阶段的速度-时间进行等时离散化。

式中：Tn为类减速运动的当前加速次数时刻；temp为当前加速次数的位移；TimeDec为运行一小段位移的平均时间。

将时间Tn代入式（4）则有速度变化量dV：

存入数组的速度值即为：Speed[n]=Vt-dV，加加速阶段次数＜n＜最大加速次数。将加加速阶段和减加速阶段构成的速度表从小到大依次排列和组合，因此可以得到整个加速阶段速度表为Speed[n]。

2 S型加减速的程序设计

传统的伺服控制器多以DSP 为主，对于一些成本低和体积小的就不在适用于DSP[11-12]。DSP 对电机控制有很大的优化，但不利于伺服系统功能扩展[13]。且DSP 多以现场进行速度位移计算。本文的主控制芯片为STM32F103ZET6，通过发送有限脉冲和控制脉冲频率以改变伺服电机的位移和速度。由于发送脉冲过程中的间隔时间短，为减少期间的计算，因此需要提前创建好速度表，同时减少定时精度造成的控制精度[14]。且由于单片机的内存有限、若每个脉冲值都有对应的速度值，就易出现单片机内存不足或速度表数量庞大。为减少扫描时间和计算时间，本文提出以空间换速度的方式，设定加速次数以增大速度表取值时脉冲间隔，减少扫描次数，求取当前加速次数的速度值来减少速度表的容量，提高运行效率。整体实现框图如图3所示。

图3 整体实现框图

2.1 速度表的生成

交流伺服电机包括位置模式、速度模式和转矩模式[15]。本文选用的是位置控制模式，该模式中速度是由单位时间内发送的脉冲个数即脉冲频率决定的，脉冲总量是电机位移量。为便于程序设计，控制器设置并写入单圈脉冲数为6 400 pulse/rev。

根据5 段S 型算法原理和分段规划，设置外部参数:初末速度、加速时间、加速次数计算出加速段总脉冲数、加速脉冲时间间隔、单次加速脉冲个数。即可计算出速度表Speed[n]。

速度表的生成流程如图4所示，通过图4计算不同参数下的速度表，使得控制器可以扫描和取值进行速度的切换和状态的改变。

图4 速度表的生成流程

2.2 速度曲线设置及速度表扫描

伺服电机在运动控制系统中伺服驱动器接收到控制器发送的一个脉冲就会转动一定的角度，位移和速度均由脉冲信号确定。STM32F103ZET6 通过定时器发脉冲主要包括两种方式，一种是PWM模式，另一种是输出比较模式。PWM 模式输出的脉冲信号频率固定，占空比可变[16]。在实际控制伺服电机脉冲信号的频率和占空比是连续变化的。PWM 模式对于控制伺服电机来说存在很大的困难和不便利，而输出比较模式满足频率和占空比连续可调，便于控制伺服电机高效运转。

在速度设置和扫描函数中，满足速度的指标要求，首先必须保证速度表值是正确的，其次为保证运行过程中误动作需要扫描电机当前的状态，如若不是停止状态则设置无效。电机的运转方向可通过设置总脉冲的正负号来改变。为实现加速启动、匀速、减速停止3个过程，则需要将电机速度切换点分成3 段，且加速启动脉冲数与减速停止脉冲数相等且对称。电机完整运行图如图5所示。

图5 电机完整运行图

电机从加速启动到减速停止的过程中，当前脉冲值是逐一递减的，为便于选取正确的运行状态，减少中断过程的计算量，应提前设定好速度切换点位置，比较当前脉冲值与速度切换点，从而判别当前所处的运行状态。且由于运行过程中加速阶段与减速阶段是对称的，则有：

图5 和式（11）（12）中：TPulse为行程总脉冲；ATpulse为加速段总脉冲；UVpoint为匀速切换点；DPoint为减速切换点。

控制流程如图6所示。

图6 电机状态和速度表扫描流程

2.3 电机运行状态切换

在状态切换过程中，电机包含4 种状态：加速、匀速、减速、停止，每一个状态都对应一个脉冲值区间，判断当前脉冲值所处的脉冲值区间，选择不同的运行状态。状态之间的切换是由定时器中断完成的，且中断时间短，不宜加入过多的数学运算。因此需要事先将所有的速度表的值转换成定时器计数值，并以数组的形式存放。

STM32F103ZET6单片机主频为72 MHz，定时器采用高级定时器TIM1用于脉冲的产生，其模式为输出比较下的翻转模式，当计数值达到比较值时对应通道引脚会翻转电平，且半周期翻转一次。电机方向引脚选用PB13。为得到实际时钟频率2 MHz，预分频系数为35，为方便计算将实际的时钟频率值扩大10 倍。电机转速单位为0.1 r/s，为实现实际转速与定时器计数值匹配，则需将电机转速与单圈脉冲数相乘。根据时钟频率与速度表的关系即可求得定时器的计数值。

式中：Ft为单片机的时钟频率；V为速度表内的速度值，pulse/s；C为半周期计数值。

因此C的值最后都被按顺序存放入数组中，此数组的值即为CCR 寄存器的值。通过获取CNT 的值和重新设置CCR 寄存器的值，来改变占空比和频率。状态切换流程如图7所示。

图7 状态切换流程

3 实验及结果分析

为减少矿石在分选平台加速启动和减速停止时的撞击，以不同速度较平稳地进入匀速区域进行矿石成分的分析，本文将设计的控制器与矿石分选平台上的伺服电机驱动器连接，发送电机运行参数进行速度调整，以验证5段S型算法和控制器调速的可行性，以及加减速阶段和匀速阶段速度平稳性的验证。

3.1 实验平台

矿石分选平台采用皮带式线性导轨滑台，全长4 m，电机旋转一圈为12.5 cm，将滑台与两侧的直线导轨固定，形成平面带动负载。伺服电机安装在导轨的尾部，平台通过多个三角式的支架，使得平台可以在稳固的前提下，节省占地空间。

3.2 速度表验证

为验证速度表的正确性，将串口打印的速度值进行整理，并将速度点进行拟合得出相应的速度表。与设定值进行误差分析。图8 所示为部分速度表的速度图像。图中横坐标为加速次数，纵坐标为速度，初速度均为0，加速总次数为250 次。由于5段式的S型加减速度是将加速过程分成两段，时间与加速次数转换过程中存在细小的时间差、因此存在较小不平滑的速度现象。经过计算图（a）（b）（c）（d）速度表内最大速度分别为99.97×0.1 r/s，199.99×0.1 r/s，249.99×0.1 r/s，299.99×0.1 r/s，与实际设置的速度，误差在0.1%以下。因此速度表构建成立。

图8 速度表拟合图像

3.3 实际测试

在该平台上进行控制器的实际测试，采集次数为1 000 次，单次采集周期为10 ms。图像横坐标单位为ms，纵坐标单位为r/min，速度反馈的速度值为正时，电机正转，速度值为负时，电机反转。

图9 为两种不同速度下的采集图像。经计算图中实际运行速度下的速度波动在15 r/min 上下范围内，小范围内波动平稳，因此噪声和抖动较小。加减速时间较短，速度反馈的图像近似于梯形，设定位移为30 rad，由梯形的面积公式求得总位移，整个运行过程定位误差在0～2 rad 左右的范围内。匀速区域是矿石分选平台的探测区域，因此只需在探测区域前达到指定速度，且加速时间极短，使得定位误差几乎可以忽略。因而该控制方法适用于矿石分选平台的控制。

图9 速度实测图

4 结束语

本文设计了一款5段式S型加减速的交流伺服电机控制器。将5 段式S 型加减速算法改进后运用到STM32 上，通过设置加速次数的方式代替速度表内呈现每一个脉冲值的速度。减少单片机的扫描时间、速度表计算的时间、中断过程处理的时间。采用位移与速度遍历查询的方式进行程序设计，并将该控制器运用到矿石分选平台上进行实际控制。

实验结果表明：本文改进后的S 型加减速控制算法，对基于STM32F103 这种低成本、小型化、易扩展而设计的控制器具有适用性，为以后嵌入其他算法而设计的单片机-伺服控制器提供了参考依据。