基于模型设计方法的机器人柔性运动控制算法的实现

唐志祺， 李 钰， 包怿杰， 肖家麟

（华东理工大学信息科学与工程学院，上海 200237）

工业4.0 和中国制造2025 计划推动了国内工业机器人产业的快速增长[1]。运动控制算法作为机器人控制器的核心技术，对机器人运动性能有重要影响。伺服系统的速度控制是运动控制器的重要工作，柔性控制的目的是实现速度的平稳变化，增加系统的稳定性，同时尽可能降低系统的效率损失[2]。在运动控制系统中，主要有直线（含梯形）、三角函数、指数、抛物线、S 形等加减速算法[3]，其中，S 形加减速算法因柔性最佳而受到广泛关注，但是该算法中涉及三阶多项式运算，制约了其在低端嵌入式运动控制器中的应用。

有许多国内外学者为实现柔性控制算法应用于嵌入式系统进行了深入的研究。Huang 等[4]采用数字卷积技术通过在FPGA 中嵌入软核处理器与硬件配合实现多种加减速控制算法，但该算法无法配置初始速度，并且应用软核对硬件资源损耗过大，不利于系统多轴扩展。刘鹏等[5]采用FPGA 实现了梯形加减速控制，在此基础上引入滑动滤波器思想实现了等效的S 形加减速效果，解决了加速度跳变问题，但未考虑滤波器引起的延时对系统实时性和连续性造成的不良影响。文献[6]采用查表法存储按时间间隔的转速值，实现过程简单、快速，然而查表法受采样密度影响导致容量受限，限制了S 形曲线随参数变化而变化的灵活性。蔡锦达等[7]在Cortex-M3 开发板上实现加减速柔性控制，提出了通过指针函数和动态数组进行程序设计的方法，但是该设计思路与查表法如出一辙，而且内核中定时器资源有限，不易功能扩展。上述算法的实现通常以基于代码的形式开发，并以实体形式进行测试，开发过程单一且较容易出错[8]，尤其是通用性差，对象一旦变化就要推倒重来。

本文针对机器人柔性S 形加减速运动控制算法嵌入复杂的问题，提出了一种有效的软硬件协同实现策略，旨在充分利用软件和硬件的优势实现算法功能，同时兼顾系统开销、时序等方面的要求。与此同时，将可视化的基于模型设计（Model-Based Design，MBD）方法引入到软硬件协同设计中，进一步提高算法通用性和开发效率。基于模型设计方法因其图形化设计优势，在汽车、航空、能源等领域得到广泛应用[9-11]，国内外许多大型项目中均采用这种高效的开发方法。Airbus 公司使用基于模型设计的方法对空客A380 的燃油系统进行建模和控制模型设计，大大加速了各部门的协调开发，在没有增加员工的情况下提前数月完成了燃油控制管理系统的设计[12]。汽车行业非常重视项目的需求验证，采用基于模型设计方法可为其最大程度地缩短开发效率，降低成本并提高生产率，因为大多数代码都是通过模型自动生成的[13]。

基于模型设计方法在机器人柔性运动控制设计方面的应用鲜见报道。本文引入这一先进的设计思想，以机器人柔性S 形加减速运动控制算法为研究对象，将其应用于基于SoC 的运动控制系统，进一步拓展其在机器人运动控制领域中的应用，为个性化机器人运动控制系统的快速设计提供思路。首先对柔性S 形加减速算法建模，建立一组易于解算的接口参数列表，分析不同参数约束下其速度曲线的变化规律；在此基础上，引入基于模型设计方法在Simulink 中为算法设计软硬件模型并通过仿真测试，进而利用MathWorks 工具箱为模型自动生成代码并将其部署到以Zynq-7000 为核心的运动控制器；最后，通过搭建实验平台以验证基于模型设计方法的可行性与可靠性。

1 柔性运动控制算法建模与仿真

1.1 S 形加减速算法的运动学建模

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

图1 S 形加减速运动曲线Fig. 1 Motion profiles of S-shaped ACC/DEC

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

图2 柔性S 形速度曲线Fig. 2 Flexible S-shaped velocity profile

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

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

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

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

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

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

表1 不同条件下的柔性S 形加减速算法Table 1 S-shaped ACC/DEC algorithms under different conditions

1.2 S 形加减速算法仿真

基于先前的分析，采用MATLAB 2020a 对不同条件下的S 形加减速算法进行仿真。当仿真数据为v0=100 mm/s ，vm=600 mm/s ，tr=0.6 s ，tm分别为0、0.15、0.3 s 时，分析不同运动路径下S 形速度曲线的变化情况，仿真结果如图3 所示。

图3 不同条件下S 形加减速算法的仿真结果Fig. 3 Simulation results of S-shaped ACC/DEC algorithm under different conditions

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

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

2 基于模型的柔性运动控制算法设计

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

图4 基于模型设计工作流程Fig. 4 Workflow for model-based design

2.1 SoC 系统结构设计

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

图5 柔性控制算法的实现结构Fig. 5 Implementation structure of flexible control algorithm

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

2.2 软件模型设计

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

图6 参数计算单元的程序设计流程Fig. 6 Program design flowchart of parameter calculation unit

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

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

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

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

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

图7 软件设计模型Fig. 7 Software design model

2.3 硬件模型设计

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

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

图8 脉冲发生器模块原理图Fig. 8 Schematic diagram of pulse generator module

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

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

图9 速度控制状态机Fig. 9 Speed state machine model

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

图10 速度计算模块核心计算单元Fig. 10 Core unit of speed calculation module

2.4 模型仿真测试

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

图11 柔性运动控制算法仿真模型Fig. 11 Simulation model of flexible motion control algorithm

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

图12 模型仿真结果Fig. 12 Model simulation results

3 平台搭建与实验验证

3.1 模型代码生成及SoC 实现

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

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

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

图13 模型部署工作流程Fig. 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。

3.2 实验验证

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

图14 实验环境搭建Fig. 14 Experimental setup

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

表2 可编程逻辑硬件资源使用情况Table 2 Programmable logic hardware resource usage

图15 仿真、实验、测量结果比较Fig. 15 Comparison of simulation, experiment and measurement results

图16 示波器测量波形Fig. 16 Waveform measured by oscilloscope

4 结 论

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