S形加减速算法因其曲线呈S形而得名，其特点是柔性好。如图1所示，加速度曲线呈梯形形状，速度曲线是光滑的，可以保证运动控制系统的平稳性。运动曲线有3个速度区（加速、恒速、减速），按运行次序细分为7段，依次为加加速段${T_1}$、匀加速段${T_2}$、减加速段${T_3}$、恒速段${T_4}$、加减速段${T_5}$、匀减速段${T_6}$、减减速段${T_7}$。其运动学模型表示为

图  1  S形加减速运动曲线

Figure 1.  Motion profiles of S-shaped ACC/DEC

通常加速区与减速区具有相同的数值，即S形速度曲线呈对称性，同时伺服电机从0加速到最大加速度${a_{\rm{m}}}$的时间与${a_{\rm{m}}}$减速到0的时间相同，这个值称为电机的特征时间常数${t_{\rm{m}}}$^[14]。

${t_{\rm{m}}}$的值越大，表示电机的柔性越大，速度曲线趋于平滑；反之，${t_{\rm{m}}}$的值越小，则冲击越大，速度曲线趋向梯形速度曲线，如图2所示。其中${S_*}$表示各段速度曲线下的面积，亦表示各阶段位移，可由式(1)积分获得。根据对称性条件，有

图  2  柔性S形速度曲线

Figure 2.  Flexible S-shaped velocity profile

为突出速度曲线柔性特征，本文采用的接口参数与传统参数有所区别，传统参数通常为起始速度${v_0}$、最大允许速度${v_{\rm{m}}}$、最大加速度${a_{\rm{m}}}$和加加速度${J_{\rm{m}}}$^[3-4,7,15]，而本文将${a_{\rm{m}}}$和${J_{\rm{m}}}$替换为总加速时间${t_{\rm{r}}}$与${t_{\rm{m}}}$，这样在建模过程中可有效避免三次多项式^[15]的求解，从而为运动控制器减轻计算负担。

在实际应用中S形加减速算法不可能完整地包含图2中的7段曲线^[16]。速度曲线将依据参数以及运动路径L的设置相应地发生变化，故S形加减速算法不是固定的7个阶段。

在分析前，应对已知参数实施约束。本文设${v_0}$、${t_{\rm{r}}}$和${t_{\rm{m}}}$为不变量，最大允许速度${v_{\rm{m}}}$因运动路径L的大小而变化。在此约定下，规划后的S形曲线主要存在以下几种情形：

(1) 存在恒速段${T_4}$，即$L > {L_{\text{α}} }$。${L_{\text{α}} }$为恒速段临界位移，表示系统正好从初始速度运行到达最大允许速度时的情况，定义为

根据${t_{\rm{m}}}$的变化可分为

1当${t_{\rm{m}}} = 0$时，S形曲线退化为直线，为标准的梯形速度曲线。由式(1)、式（3）、式（4）获得下列参数：

2 若${t_{\rm{m}}} \in \left( {0,{t_{\rm{r}}}/2} \right)$，则为完整的7段S形速度曲线。式(1)中的核心参数及各阶段运行时间计算如下：

③ ${t_{\rm{m}}} = {t_{\rm{r}}}/2$时，速度曲线无匀加速段和匀减速段，曲线呈纯S形，称其为5段S形速度曲线。参数仍可通过式(6)获得，此时${T_2} = {T_6} = 0$。

(2) 无恒速段，要求$L \in \left( {{L_{\text{α}}},{L_{\text{β}} }} \right]$。${L_{\text{β}} }$为加速段临界位移，表示系统正好在设定时间以${v_0}$完成运行的情况，定义为

在该情形下，最大速度随运动路径而变化，表示为

① 当${t_{\rm{m}}} = 0$时，仅存在匀加速段和匀减速段，称为三角形速度曲线。由式(5)简化为

② 当${t_{\rm{m}}} \in \left( {0,{t_{\rm{r}}}/2} \right)$时，呈6段S形速度曲线。有

③ 若${t_{\rm{m}}} = {t_{\rm{r}}}/2$，曲线简化成4段S形速度曲线。代入式(10)得${T_2} = {T_4} = {T_6} = 0$。

（3）无加速段，运动路径很短，$L \in \left( {0,{L_{\text{β}} }} \right]$。该情况较为特殊，若${v_0}$为0，根据加速段临界位移${L_{\text{β}} }$定义，将不存在无加速段情形；若同时满足${v_0} > 0$且$L \in \left( {0,{L_{\text{β}}}} \right]$，此时存在无加速段，要求运动部件以${v_0}$匀速运行至目标点。运行时间为

当前速度曲线仅剩恒速段，有

由于其特殊性，除初始速度${v_0}$外，其余参数均无效。

对不同条件下的柔性S形加减速算法分类，分类结果如表1所示。

Classification	Name	Condition	Feature
Group 1	7-segment S-shaped speed curve	$L > {L_{\text{α}} }$ and ${t_{\rm{m}}} \in \left( {0,{t_{\rm{r}}}/2} \right)$	/
	6-segment S-shaped speed curve	$L \in \left( {{L_{\text{β}} },{L_{\text{α}} }} \right]$ and ${t_{\rm{m}}} \in \left( {0,{t_{\rm{r}}}/2} \right)$	Without ${T_4}$
	5-segment S-shaped speed curve	$L > {L_{\text{α}} }$ and ${t_{\rm{m}}} = {t_{\rm{r}}}/2$	Without ${T_2},{T_6}$
	4-segment S-shaped speed curve	$L \in \left( {{L_{\text{β}} },{L_{\text{α}} }} \right]$ and ${t_{\rm{m}}} = {t_{\rm{r}}}/2$	Without ${T_2},{T_4},{T_6}$
Group 2	Trapezoidal speed curve (3-segment)	$L > {L_{\text{α}} }$ and ${t_{\rm{m}}} = 0$	Only ${T_2},{T_4},{T_6}$
	Triangle speed curve (2-segment)	$L \in \left( {{L_{\text{β}} },{L_{\text{α}}}} \right]$ and ${t_{\rm{m}}} = 0$	Only ${T_2},{T_6}$
Group 3	Speed curve without acceleration (1-segment)	$L \in \left( {0,{L_{\text{β}}}} \right]$ and ${v_0} > 0$	Only ${T_4}$

表 1  不同条件下的柔性S形加减速算法

Table 1.  S-shaped ACC/DEC algorithms under different conditions

基于先前的分析，采用MATLAB 2020a对不同条件下的S形加减速算法进行仿真。当仿真数据为${v_0} = 100\;{\rm{mm/s}}$，${v_{\rm{m}}} = 600\;{\rm{mm/s}}$，${t_{\rm{r}}} = 0.6\;{\rm{s}}$，${t_{\rm{m}}} =$(0,0.15,0.3 s)时，分析不同运动路径下S形速度曲线的变化情况，仿真结果如图3所示。

图  3  不同条件下S形加减速算法的仿真结果

Figure 3.  Simulation results of S-shaped ACC/DEC algorithm under different conditions

当$L = 800\;{\rm{mm}}$时，不同特征时间常数下的速度曲线均存在匀速段，体现为梯形速度曲线、7段S形速度曲线和5段S形速度曲线。

当$L = 300\;{\rm{mm}}$时，该路径小于恒速段临界位移$({L_{\text{α}} } = 420\;{\rm{mm}})$，故速度曲线均表现为无匀速段，此时最大速度更新为${v'_{\rm{m}}} = 400\;{\rm{mm/s}}$。

当$L = 100\;{\rm{mm}}$时，计算可知加速段临界位移${L_{\text{β}} } = 120\;{\rm{mm}}$，因此S形加减速算法表现为匀速运动。根据式(11)得到运行时间${t_{\text{β}} } = 1\;{\rm{s}}$，仿真结果与理论结果一致。

基于模型设计的工作流程如图4所示，其优势在于从需求获取到设计、实现和集成，模型始终占据开发流程的核心。其中，测试和验证贯穿整个开发周期，能够方便地实现对算法的早期验证，降低后期维护成本；同时，工作流程中大量费时及易出错的步骤均可实现自动化。本文采用基于模型设计为柔性运动控制算法从模型创建到集成实现，设计完整的工作流程。

图  4  基于模型设计工作流程

Figure 4.  Workflow for model-based design

本文针对柔性S形加减速运动控制算法的实现，提出了SoC软硬件协同设计方法，实现结构如图5所示。该系统结构的软硬件划分主要依据：对于系统中实时性要求严格、并行处理的功能模块采用可编程逻辑实现；对于灵活度较高、计算相对复杂的功能模块采用嵌入式软件实现。

图  5  柔性控制算法的实现结构

Figure 5.  Implementation structure of flexible control algorithm

脉冲发生器模块是运动控制器伺服系统控制的基本接口模块，具有可重用性和高实时性的特点，故采用高速率的可编程逻辑实现；迭代速度控制模块负责为脉冲发生器提供可控的脉冲频率，同样置于可编程逻辑中，但由于该模块受布线后关键路径影响，将其运行于1 MHz；参数计算单元用于处理灵活多变的接口参数，其内部涉及多次多项式乘除运算，由嵌入式软件实现最佳。

参数计算单元的作用是根据接口参数列表辨识S形速度曲线类型并计算核心参数以及各阶段运动位移，进而将参数以总线形式传输给硬件部分，程序设计流程如图6所示。可以看出，该设计流程的支路数与表1的分类相对应。

图  6  参数计算单元的程序设计流程

Figure 6.  Program design flowchart of parameter calculation unit

在模型设计中，软件模型的设计步骤如下：

（1）在M脚本文件中以函数形式编写参数计算单元代码；

（2）使用MATLAB Coder工具箱将脚本代码转变为C语言函数；

（3）在Simulink中采用Stateflow图形状态机搭建命令控制模型并在状态转换中调用生成的C语言函数。

图7示出了完整的软件设计模型。左侧各端口表示软件模型的输入信号，包括命令控制、接口参数以及状态反馈信号，均直接传递给Stateflow模块。从细节图可以看出，Stateflow中包含多个状态转移，其中黑色箭头所指位置即为生成的函数调用，表示由外部命令控制以条件触发方式从等待状态迁移至加减速状态执行的动作，该过程中函数仅执行一次即可获得计算参数，继而将得到的参数值传递给输出接口并伴随加减速状态的启动信号。

图  7  软件设计模型

Figure 7.  Software design model

由图5可知，硬件模型由脉冲发生器和迭代速度控制模块组成，均具有可重用性的特点，但两者并非运行于同一时钟下。脉冲发生器用于产生脉冲信号以驱动伺服系统，由于脉冲频率对应于电机转速，因此脉冲频率的精度至关重要，即系统时钟频率越高，脉冲频率越精确。迭代速度控制模块运行于系统时钟频率经过采样后的时钟下，其运行频率可表示为脉冲发生器输出的最高动态变化频率，但该频率受可编程逻辑布局布线后关键路径的影响，两者互相制约，需设置合理的过采样系数。

脉冲发生器的设计采用直接数字频率合成器（Direct digital Synthesizer, DDS）技术思想，DDS具有频率切换速度快、频率分辨率高、可靠性高等优点^[17]。图8所示为脉冲发生器模块原理图，其内部含累加寄存器、比较器、计数器等组件，实现脉冲生成与计数功能。输入信号In是迭代速度控制模块提供的速度值；输入时钟频率Clk对应系统工作频率，即100 MHz；计数信号Cnt作为迭代控制模块的反馈信号；脉冲信号Pul分配至控制器IO引脚与伺服驱动器相连以驱动电机旋转。

图  8  脉冲发生器模块原理图

Figure 8.  Schematic diagram of pulse generator module

在硬件模型的设计中，定点化设计是必不可少的步骤，通过引入定点化设计能够进一步提高输出脉冲频率的精度。若设定输入信号采用32位位宽4位小数数据长度的定点数据类型，其输出脉冲频率范围为0~67 MHz，频率分辨率可达0.0625 Hz，足以满足任何伺服驱动器控制需求。

迭代速度控制是算法模型的核心部分，内含状态机与速度计算两个子系统。状态机的作用是依据各段位移及当前位置切换速度运行的阶段，以状态变量形式控制速度计算模块。在模型设计中，速度状态机由Stateflow实现，如图9所示。状态机设计为Mealy型，其中的7个子状态对应于S形速度曲线中的7个阶段。在实际应用中，运行的状态需根据各阶段的位移参数确认状态是否被激活。图9中，状态检测采取自上而下的方式，当满足[ ]内的迁移条件时，目标状态被相应激活；未满足迁移条件时，执行优先级为2的迁移支路，并且状态的退出同样取决于是否满足迁移条件，该条件中的变量是由脉冲发生器提供的计数信号。

图  9  速度控制状态机

Figure 9.  Speed state machine model

速度计算模块虽然运行于过采样后的时钟频率下，但唯有在脉冲发送完成后速度才会迭代更新。图10所示为速度计算模块中的核心计算单元，体现为式(2)的模型表达形式，其中该模块的输出取决于状态机提供的状态变量。可以发现，核心计算单元中存在多个乘加运算模块，这些模块是导致关键路径延长的主要因素。因此为避免系统时延及资源滥用，需要将各运算模块映射到可编程逻辑的DSP硬件资源，进一步提高计算效率。

图  10  速度计算模块核心计算单元

Figure 10.  Core unit of speed calculation module

完成算法软硬件模型设计后，执行系统级互连和仿真验证算法模型的可行性和准确性。图11所示为级联后的仿真模型，控制器算法模块中包含了除脉冲发生器外的软硬件模型，每部分模型的执行速率可通过Rate Transition模块实现划分。仿真设置脉冲发生器模块运行于100 MHz，迭代速度控制模块为1 MHz，软件模型为1 kHz。

图  11  柔性运动控制算法仿真模型

Figure 11.  Simulation model of flexible motion control algorithm

假设被控对象脉冲当量为$10\;{\rm{pulse}}/{\rm{mm}}$，当${v_{\rm{s}}} = 15\;{\rm{mm/s}}$、${v_{\rm{m}}} = 200\;{\rm{mm/s}}$、${t_{\rm{r}}} = 0.4\;{\rm{s}}$、$\;{t_{\rm{m}}} = 0.1\;{\rm{s}}$、$L = 100\;{\rm{mm}}$时，仿真结果如图12所示，其中图12（a）为位移曲线、图12（b）为速度曲线、图12（c）为速度状态、图12（d）为脉冲输出波形。仿真结果表明，设计模型实现了S形加减速算法，准确地控制了步进速度，具有良好的柔性特征。由此可知，MBD方便地进行了算法的开发，减少了调试以及修改的难度。

图  12  模型仿真结果

Figure 12.  Model simulation results

自动化代码生成是MBD的关键优势。一旦完成模型的设计及验证，便可利用Simulink平台提供的工具箱为目标平台生成相应的代码。Embedded Coder工具箱为软件模型生成C语言代码，将其部署在嵌入式处理器上；HDL Coder工具箱对硬件模型生成HDL代码或IP核，部署在可编程逻辑中。

本文采用的目标平台是以Zynq-7000为核心的运动控制器，优势在于ARM处理器和FPGA可编程逻辑器件集成在一个芯片上，同时具备了ARM处理器的灵活性以及FPGA的配置能力，而且AXI标准接口为ARM与FPGA实现了高带宽、低延时的连接。

从仿真算法模型部署目标硬件平台的工作流程如图13所示。具体步骤如下：

图  13  模型部署工作流程

Figure 13.  Workflow of model deployment

（1）模型兼容性检查：确保模型符合代码生成规则；

（2）使用HDL Coder为脉冲发生器模型生成IP核；

（3）在Vivado开发环境中为系统创建参考设计工程，并在块设计(block design)调用脉冲发生器IP核，系统最大工作频率为100 MHz；

（4）为迭代速度控制模型生成IP核并将其添加到参考设计工程中，此处设置过采样系数为100；

（5）在HDL Workflow Advisor中为参考设计生成工程并创建比特流文件，然后部署至FPGA中；

（6）使用Embedded Coder为软件模型生成嵌入式C代码，迭代步长为1 ms；

（7）Simulink设置为外部模式，代码将自动嵌入到Linux参考架构中，随后再编译至ARM。

实验环境如图14所示。运动控制器采用基于Xilinx Zynq-7000为核心的低成本Zedboard开发板，板载PMOD接口连接外部自制信号调理板以控制被控对象，被控对象由步进驱动器和57式步进电机组成。该步进电机末端内含1000线增量式编码器，在实验测试中可通过读取编码器脉冲进而获得测量速度。测量设备为Tektronix MSO3032混合信号示波器，示波器探头连接脉冲控制引脚测量输出脉冲。同时，利用Simulink与Linux系统之间的以太网链路可实时捕捉AXI总线中的接口信号。

图  14  实验环境搭建

Figure 14.  Experimental setup

为验证MBD的可行性以及测试软硬件协同设计算法的有效性，在Simulink中实时捕捉FPGA中迭代的速度计算值，并与模型仿真结果进行比较。输入列表参数为${v}_{\rm{s}}=30\;{\rm{mm/s}},{v}_{\rm{m}}=300\;{\rm{mm/s}},{t}_{\rm{r}}=0.4\;{\rm{s}}$，如图15所示，仿真与实验设置启动时间差为$0.02{\rm{s}}$以示区分。实验期间，步进电机以相应速度旋转，旋转过程中通过编码器反馈信号获得实时的测量速度。从4次仿真、实验与测量结果对比分析，仿真与实验的运行曲线基本一致，测量速度符合期望速度变化规律，由此表明基于模型设计方法是可行且可靠的。图16示出了示波器测量脉冲控制引脚获得的实际脉冲波形，其脉冲频率随期望速度的变化而变化。表2示出了FPGA布局布线后的硬件资源使用情况，可以看出，模型生成的IP核占用了较少的硬件资源，因此也证明了软硬件协同设计算法的有效性。

图  15  仿真、实验、测量结果比较

Figure 15.  Comparison of simulation, experiment and measurement results

图  16  示波器测量波形

Figure 16.  Waveform measured by oscilloscope

本文针对嵌入复杂的柔性S形加减速运动控制算法提出了软硬件协同实现策略，并引入基于模型设计方法加快算法的实现与验证，提高算法的通用性。利用Simulink图形化设计优势分别为软件与硬件设计模型；通过自动代码生成工具为软硬件模型生成代码；搭配Vivado和MathWorks工具箱在以Zynq-7000为核心的运动控制器上实现完整的MBD部署。实验结果表明：提出的柔性S形加减速算法的软硬件协同实现策略是有效的，实现的算法同时具备参数变化的灵活性以及硬件实时性并且占用了较少的硬件资源；采用MBD方法实现柔性运动控制算法的软硬件协同设计是可行且可靠的，该方法大大缩短从需求到实现的设计时间，为个性化机器人柔性运动控制系统的实现提供了高效的设计途径。