CANopen伺服系统中速度前馈和转矩前馈的处理方法
【专利摘要】CANopen伺服系统中速度前馈和转矩前馈的处理方法：轨迹规划器接收目标控制命令并计算位置指令曲线类型和加、减速转折点位置；判断当前是否启动伺服控制：若否，速度前馈曲线、转矩前馈曲线及位置指令曲线均为0；若是，将截至前一伺服控制周期已规划的位置累计值与加、减速转折点位置、目标位置进行比较，根据比较结果执行加速规划、减速规划、匀速规划或不执行规划，以得到当前伺服控制周期的速度前馈值和转矩前馈值；计算当前伺服控制周期的位置增量值和截至当前伺服控制周期已规划的位置累计值；多个位置增量值形成位置增量指令曲线，多个位置累计值形成位置累计值指令曲线，多个速度前馈值形成速度前馈曲线，多个转矩前馈值形成转矩前馈曲线。
【专利说明】
CANopen伺服系统中速度前馈和转矩前馈的处理方法
技术领域
[0001] 本发明设及伺服运动控制技术领域，尤其设及CANopen伺服系统中速度前馈和转 矩前馈的处理方法。
【背景技术】
[0002] 在伺服运动控制领域，传统的脉冲型伺服系统已无法满足市场要求，总线型伺服 系统已然成为市场主流，例如基于PROFI脚S、CANopen、EtherCAT等的总线型伺服系统。
[0003] 其中，CANopen伺服系统具有接线简单且连线少、通讯干扰小、运动性能有所提高、 同数目伺服驱动器所需主控装置更少等优点。如图1所示，CANopen伺服系统主要由CANopen 主控装置、CANopen伺服驱动器和伺服电机S大部分组成。CANopen主控装置一般为国内主 流的工控机控制卡或国外主流的控制器，由控制卡或控制器产生目标控制命令输入给伺服 驱动器内部的轨迹规划器，由轨迹规划器得到位置指令，位置指令分别经过反馈控制器和 前馈控制器进行控制。反馈控制器使得反馈位置与位置指令之间的误差减少，从而实现跟 随控制;前馈控制器根据位置指令来使得跟随的动态性能更好。
[0004] 目前，CANopen伺服系统中，获得速度前馈和转矩前馈是通过传统微分方法，具体 如下：
[000引参考图1，由CANopen主控装置产生目标控制命令11 (包括目标位置、目标速度、目 标加/减速度、控制模式和控制字)输入至轨迹规划器，得到位置指令12;位置指令12微分后 得到速度前馈13;再对速度前馈13进行微分得到转矩前馈14。然而，此传统微分方法得到的 速度前馈曲线和转矩前馈曲线的平滑度对反馈位置检测装置的精度有很大的影响性，一般 需要经过一定的滤波后分别加到速度指令和电流指令上W及分别与反馈速度信号15和反 馈电流信号16相加输入到速度调节器和电流调节器上进行控制。采用微分方法得到速度前 馈和转矩前馈的曲线示意图如图2所示，其中tl~t7为采样时间点，可W看出，由位置指令 微分得到的速度前馈在相位上比位置指令延迟了半个采样周期;再由得到速度前馈微分得 到的转矩前馈在相位上又比速度前馈延迟了半个采样周期。即：由传统微分方法得到的速 度前馈曲线比位置指令曲线（位置指令曲线包括位置增量指令曲线和位置累计值指令曲 线，运两条曲线本身是同步的，图2中仅绘示出位置累计值指令曲线)有延时，而由速度前馈 曲线微分得到转矩前馈曲线与速度前馈曲线、位置指令曲线相比均存在延时。
【发明内容】
[0006] 本发明的主要目的在于针对上述现有技术存在的不足，提出一种CANopen伺服系 统中速度前馈和转矩前馈的处理方法，该处理方法所规划出的速度前馈曲线和转矩前馈曲 线是与位置指令曲线严格同步的，W解决现有的CANopen伺服系统采用传统微分方法得到 的速度前馈曲线和转矩前馈曲线所存在的延时问题，消除该延时问题对动态跟随性能的影 响，使CANopen伺服系统的动态跟随性能更优。
[0007] 本发明为解决上述技术问题所提出的技术方案如下：
[000引CANopen伺服系统中速度前馈和转矩前馈的处理方法，该处理方法是通过位于伺 服驱动器内的轨迹规划器执行W下步骤：
[0009] 步骤S1、接收来自主控装置的目标控制命令；
[0010] 步骤S2、依据所述目标控制命令，计算位置指令曲线的曲线类型、加速转折点位置 和减速转折点位置;其中位置指令曲线包括两条:位置增量指令曲线和位置累计值指令曲 线；
[0011] 步骤S3、从所述目标控制命令中判断当前是否需要启动伺服控制:若否，则速度前 馈曲线、转矩前馈曲线W及位置指令曲线均为0;若是，则执行步骤S4;
[0012] 步骤S4、将截至前一伺服控制周期已规划的位置累计值与加速转折点位置、减速 转折点位置W及目标位置进行比较，根据比较结果选择执行加速规划、减速规划、匀速规划 或不执行规划，W得到当前伺服控制周期的速度前馈值和转矩前馈值；
[0013] 步骤S5、计算当前伺服控制周期的位置增量值；
[0014] 步骤S6、将当前伺服控制周期的位置增量值与截至前一伺服控制周期已规划的位 置累计值相加，得到截至当前伺服控制周期已规划的位置累计值；
[0015] 每一伺服控制周期重复上述步骤Sl~S6,得到多个位置增量值W形成位置增量指 令曲线，得到多个位置累计值W形成位置累计值指令曲线，得到多个速度前馈值W形成速 度前馈曲线，W及得到多个转矩前馈值W形成转矩前馈曲线。
[0016] 本发明提供的前述处理方法，在每一伺服控制周期内，由轨迹规划器同时得到位 置增量值、位置累计值、速度前馈值和转矩前馈值，而位置增量指令曲线、位置累计值指令 曲线、速度前馈曲线和转矩前馈曲线均分别是由多个连续控制周期所对应的多个点值构 成，因此，随着伺服系统的运行，轨迹规划器周期性连续不断地产生位置增量值、位置累计 值、速度前馈值和转矩前馈值，从而可W得到位置增量指令曲线、位置累计值指令曲线、速 度前馈曲线和转矩前馈曲线，由于每一伺服控制周期所对应的位置增量值、位置累计值、速 度前馈值和转矩前馈值都是同一时刻由轨迹规划器产生，因此四条曲线:位置增量指令曲 线、位置累计值指令曲线、速度前馈曲线和转矩前馈曲线是严格同步的，四者之间并无延 时。运对于伺服驱动器来说，充分发挥了前馈控制对动态跟随性能的改善作用，使得整个 CANopen伺服系统的动态跟随性能更优。
[0017] 在更优选的技术方案中，所述目标控制命令至少包括目标位置、目标速度、目标加 速度、目标减速度W及控制字。
[0018] 在更优选的技术方案中，步骤S2具体为：依据目标位置、目标速度、目标加速度和 目标减速度，计算位置指令曲线的曲线类型W及加速转折点位置、减速转折点位置。
[0019] 在更优选的技术方案中，步骤S3中，判断当前是否需要启动伺服控制，具体是判断 目标控制命令中的控制字是否为启动。
[0020] 在更优选的技术方案中，加速规划为：当前伺服控制周期的速度前馈值等于前一 伺服控制周期的速度前馈值与目标加速度之和，当前伺服控制周期的转矩前馈值等于目标 加速度； 减速规划为：当前伺服控制周期的速度前馈值等于前一伺服控制周期的速度前馈值减 去目标减速度，当前伺服控制周期的转矩前馈值等于目标减速度； 匀速规划为：当前伺服控制周期的速度前馈值等于目标速度，当前伺服控制周期的转 矩前馈值等于O; 不执行规划时，当前伺服控制周期的位置增量值为0,截至前一伺服控制周期已规划的 位置累计值清零，且当前伺服控制周期的速度前馈值、转矩前馈值均为0。
[0021] 在更优选的技术方案中，步骤S5中，根据伺服控制周期值、编码器分辨率W及步骤 S4得到的当前伺服控制周期的速度前馈值，计算当前伺服控制周期的位置增量值。
[0022] 在更优选的技术方案中，基于步骤S2得到的位置指令曲线的曲线类型，所述步骤 S4具体为： al、若曲线类型为=角形类型且满足条件:P《P0，则执行所述加速规划； a2、若曲线类型为=角形类型且满足条件:P>P0,则执行所述减速规划； a3、若曲线类型为S角形类型且满足条件:P =化，则不执行规划； bl、若曲线类型为梯形类型且满足条件:P>P1，则执行所述减速规划； b2、若曲线类型为梯形类型且满足条件:P《P2，则执行所述加速规划； b3、若曲线类型为梯形类型且满足条件:P2<P<P1，则执行所述匀速规划； b4、若曲线类型为梯形类型且满足条件:P =化，则不执行规划； 其中，P表示截至前一伺服控制周期已规划的位置累计值，PO表示曲线类型为=角形类 型时的加速转折点位置与减速转折点位置，两者相同；Pl和P2分别表示曲线类型为梯形类 型时的减速转折点位置、加速转折点位置;Pn为目标位置。
【附图说明】
[0023] 图1是现有的CANopen伺服系统通过传统微分方法处理速度前馈和转矩前馈的整 体结构示意图；
[0024] 图2是图1中采用传统微分方法所得到的速度前馈曲线和转矩前馈曲线的示意图；
[0025] 图3是本发明提供的CANopen伺服系统中速度前馈和转矩前馈的处理方法流程图；
[0026] 图4-1是通过本发明如图3所示的处理方法所得到的速度前馈曲线和转矩前馈曲 线的示意图；
[0027] 图4-2是通过本发明如图3所示的处理方法所得到的速度前馈曲线和转矩前馈曲 线的示意图。
【具体实施方式】
[0028] 下面结合附图和具体的实施方式对本发明作进一步说明。
[0029] 本发明的【具体实施方式】提供了一种CANopen伺服系统中速度前馈和转矩前馈的处 理方法，所述CANopen伺服系统包括CANopen主控装置、CANopen伺服驱动器和伺服电机，在 伺服驱动器内具有轨迹规划器、位置调节器、速度调节器和电流调节器。
[0030] 参考图3,该处理方法是通过所述轨迹规划器执行W下步骤：
[0031 ]步骤Sl、接收来自主控装置的目标控制命令。其中所述目标控制命令包括目标位 置、目标速度、目标加速度、目标减速度、控制模式W及控制字。
[0032]步骤S2、依据接收到的目标控制命令，具体是依据其中的目标位置、目标速度、目 标加速度和目标减速度，计算位置指令曲线的曲线类型W及加速转折点位置、减速转折点 位置。其中位置指令曲线包括两条:位置增量指令曲线和位置累计值指令曲线，二者是曲线 类型是一致的。
[0033] 步骤S3、判断目标控制命令中的控制字是否为启动:若否，即表示当前不需要进行 伺服控制，则速度前馈曲线、转矩前馈曲线W及位置指令曲线均为0;若是，则执行步骤S4， 即启动伺服控制，开始进行运动曲线的规划。
[0034] 步骤S4、将截至前一伺服控制周期已规划的位置累计值与加速转折点位置、减速 转折点位置W及目标位置进行比较，并根据比较结果执行加速规划、减速规划、匀速规划或 不执行规划，W得到当前伺服控制周期的速度前馈值和转矩前馈值。
[0035] 具体地，加速规划为：当前伺服控制周期的速度前馈值等于前一伺服控制周期的 速度前馈值与目标加速度之和，当前伺服控制周期的转矩前馈值等于目标加速度;减速规 划为：当前伺服控制周期的速度前馈值等于前一伺服控制周期的速度前馈值减去目标减速 度，当前伺服控制周期的转矩前馈值等于目标减速度;匀速规划为：当前伺服控制周期的速 度前馈值等于目标速度，当前伺服控制周期的转矩前馈值等于0。而不执行规划时：当前伺 服控制周期的位置增量值为0,截至前一伺服控制周期已规划的位置累计值清零，且当前伺 服控制周期的速度前馈值、转矩前馈值均为0。
[0036] 步骤S5、根据伺服控制周期值、编码器分辨率W及步骤S4得到的速度前馈值，计算 当前伺服控制周期的位置增量值。
[0037] 步骤S6、将当前伺服控制周期的位置增量值与截至前一伺服控制周期已规划的位 置累计值相加，得到截至当前伺服控制周期已规划的位置累计值。
[0038] 根据位置指令曲线的曲线类型的不同，步骤S4具体包括W下几种情况： 情况al:若曲线类型为=角形类型且满足条件:P《P0,则执行所述加速规划； 情况曰2、若曲线类型为=角形类型且满足条件:P>P0,则执行所述减速规划； 情况曰3、若曲线类型为S角形类型且满足条件:P =化，则不执行规划； 情况bl、若曲线类型为梯形类型且满足条件:P>P1，则执行所述减速规划； 情况b2、若曲线类型为梯形类型且满足条件:P《P2,则执行所述加速规划； 情况b3、若曲线类型为梯形类型且满足条件:P2<P<P1，则执行所述匀速规划； 情况b4、若曲线类型为梯形类型且满足条件:P =化，则不执行规划； 其中，P表示截至前一伺服控制周期已规划的位置累计值，PO表示曲线类型为=角形类 型时的加速转折点位置与减速转折点位置，两者相同；Pl和P2分别表示曲线类型为梯形类 型时的减速转折点位置、加速转折点位置;Pn为目标位置。
[0039] 需要说明，曲线类型并非等同于曲线的具体形状。例如角形类型并非表示曲线 形状是=角形，而是指曲线上无匀速段，由加速段直接进入减速段，而后再到达目标位置， 加速段和减速段之间的衔接点为加速转折点，也即减速转折点；梯形类型并非表示曲线形 状是梯形，而是指曲线上由加速段经加速转折点进入匀速段，再由匀速段经减速转折点进 入减速段，最终到达目标位置。
[0040] W图4-1为例来说明本发明所提供的速度前馈和转矩前馈的处理方法:如图4-1所 示，该图示例，轨迹规划器所计算出的位置指令曲线类型为梯形类型，同时还计算出位置累 计值指令曲线上加速转折点位置和减速转折点位置，但曲线的具体轨迹是未知的。
[0041] 在时间节点Tl判断出控制字为"启动"，轨迹规划器即开始规划曲线轨迹，伺服控 制进入第一个控制周期。根据图3的流程，此时应当判断截至前一伺服控制周期已规划的位 置累计值P的大小，由于是第一个伺服控制周期，此时的位置累计值p=0,可见肯定小于加 速转折点位置P2,应当进入加速规划：当前伺服控制周期的速度前馈值等于前一伺服控制 周期的速度前馈值与目标加速度之和，当前伺服控制周期的转矩前馈值等于目标加速度， 从而得到速度前馈曲线和转矩前馈曲线的各自一个点值；
[0042] 接着，根据伺服控制周期值、编码器分辨莖H术浩面I的巧度前馈值，计算当前伺 服控制周期的位置增量值，具体的计算为公知技术
其中y为当前伺服控制周 期的位置增量值，V为前述计算出的当前伺服控制周期的速度前馈值，m为编码器分辨率，T 为一个伺服控制周期的大小(单位为微秒）。从而也得到位置增量指令曲线和位置累计值指 令曲线的各自的一个点。
[0043] 接上述，将当前伺服控制周期的位置增量值y与截至前一伺服控制周期已规划的 位置累计值P相加，得到截至当前伺服控制周期已规划的位置累计值，即位置累计值指令曲 线上对应每一伺服控制周期的点值都是一个累加值，而不仅仅是该个周期内的位置增量 值。例如，初始位置为0,第一个周期的位置增量值为y = l，则位置累计值指令曲线上对应第 一个周期的点值即位置累计值P = 〇+y = 〇+l = l;接着第二个周期的位置增量值y = 2,则位 置累计值指令曲线上对应第二个周期的点值即更新的位置累计值P = l+2, W此类推。
[0044] 按照上述的方法每循环一个周期，位置累计值P都会得到更新，直至P = P2,规划出 位置累计值指令曲线在Tl~T2之间的轨迹，对应地，在此期间，也逐个地分别得到其余=条 曲线上的多个点值，分别形成运立条曲线在Tl~T2期间的轨迹。当P2<P<P1时，即截至前 一伺服控制周期已规划的位置累计值位于加速转折点位置与减速转折点位置之间，则进行 所述匀速规划，得到T2~T3之间的曲线。位置累计值P增大到P^Pl，则开始进行减速规划， 得到T3~T4之间的曲线。进行减速规划直至P =化时，则不执行规划。从而可W得到位置增 量指令曲线、位置累计值指令曲线、速度前馈曲线和转矩前馈曲线。
[0045] 从上面的例子可W看到，如图4-1所示，按照本发明的前述处理方法所规划出的四 个曲线在时间节点T1、T2、T3 J4处是严格同步的，无任何的时间延迟，运就避免了传统微分 处理得到速度前馈曲线和转矩前馈曲线存在延时的问题，对于精度要求高的工业运动控制 领域而言，无疑是一大进步。
[0046] 通过本发明如图3所示的处理方法所得到的速度前馈曲线和转矩前馈曲线的另一 种例子如图4-2所示，与图4-1中例子不同的是，由于本例中的位置指令曲线的曲线类型为 =角形类型，因此本例不进行匀速规划。同样，本例中得到的四条曲线在时间上严格同步。
[0047] W上内容是结合具体的优选实施方式对本发明所作的进一步详细说明，不能认定 本发明的具体实施只局限于运些说明。对于本发明所属技术领域的技术人员来说，在不脱 离本发明构思的前提下，还可W做出若干等同替代或明显变型，而且性能或用途相同，都应 当视为属于本发明的保护范围。
【主权项】
1. CANopen伺服系统中速度前馈和转矩前馈的处理方法，其特征在于:通过位于伺服驱 动器内的轨迹规划器执行以下步骤： 步骤Sl、接收来自主控装置的目标控制命令； 步骤S2、依据所述目标控制命令，计算位置指令曲线的曲线类型、加速转折点位置和减 速转折点位置;其中位置指令曲线包括两条:位置增量指令曲线和位置累计值指令曲线； 步骤S3、从所述目标控制命令中判断当前是否需要启动伺服控制:若否，则速度前馈曲 线、转矩前馈曲线以及位置指令曲线均为〇;若是，则执行步骤S4; 步骤S4、将截至前一伺服控制周期已规划的位置累计值与加速转折点位置、减速转折 点位置以及目标位置进行比较，根据比较结果选择执行加速规划、减速规划、匀速规划或不 执行规划，以得到当前伺服控制周期的速度前馈值和转矩前馈值； 步骤S5、计算当前伺服控制周期的位置增量值； 步骤S6、将当前伺服控制周期的位置增量值与截至前一伺服控制周期已规划的位置累 计值相加，得到截至当前伺服控制周期已规划的位置累计值； 每一伺服控制周期重复上述步骤Sl~S6,得到多个位置增量值以形成位置增量指令曲 线，得到多个位置累计值以形成位置累计值指令曲线，得到多个速度前馈值以形成速度前 馈曲线，以及得到多个转矩前馈值以形成转矩前馈曲线。2. 如权利要求1所述的处理方法，其特征在于:所述目标控制命令至少包括目标位置、 目标速度、目标加速度、目标减速度以及控制字。3. 如权利要求2所述的处理方法，其特征在于：步骤S2具体为：依据目标位置、目标速 度、目标加速度和目标减速度，计算位置指令曲线的曲线类型以及加速转折点位置、减速转 折点位置。4. 如权利要求2所述的处理方法，其特征在于：步骤S3中，判断当前是否需要启动伺服 控制，具体是判断目标控制命令中的控制字是否为启动。5. 如权利要求2所述的处理方法，其特征在于： 加速规划为：当前伺服控制周期的速度前馈值等于前一伺服控制周期的速度前馈值与 目标加速度之和，当前伺服控制周期的转矩前馈值等于目标加速度； 减速规划为：当前伺服控制周期的速度前馈值等于前一伺服控制周期的速度前馈值减 去目标减速度，当前伺服控制周期的转矩前馈值等于目标减速度； 匀速规划为：当前伺服控制周期的速度前馈值等于目标速度，当前伺服控制周期的转 矩前馈值等于〇; 不执行规划时，当前伺服控制周期的位置增量值为〇,截至前一伺服控制周期已规划的 位置累计值清零，且当前伺服控制周期的速度前馈值、转矩前馈值均为0。6. 如权利要求5所述的处理方法，其特征在于:步骤S5中，根据伺服控制周期值、编码器 分辨率以及步骤S4得到的当前伺服控制周期的速度前馈值，计算当前伺服控制周期的位置 增量值。7. 如权利要求5所述的处理方法，其特征在于:基于步骤S2得到的位置指令曲线的曲线 类型，所述步骤S4具体为： al、若曲线类型为三角形类型且满足条件:PSPO，则执行所述加速规划； a2、若曲线类型为三角形类型且满足条件:Ρ>Ρ0，则执行所述减速规划； a3、若曲线类型为三角形类型且满足条件:P = Pn，则不执行规划； bl、若曲线类型为梯形类型且满足条件:P多Pl，则执行所述减速规划； b2、若曲线类型为梯形类型且满足条件:P2，则执行所述加速规划； b3、若曲线类型为梯形类型且满足条件:P2<P<P1，则执行所述匀速规划； b4、若曲线类型为梯形类型且满足条件:P = Pn，则不执行规划； 其中，P表示截至前一伺服控制周期已规划的位置累计值，PO表示曲线类型为三角形类 型时的加速转折点位置与减速转折点位置，两者相同；Pl和P2分别表示曲线类型为梯形类 型时的减速转折点位置、加速转折点位置;Pn为目标位置。
【文档编号】G05B19/414GK105955203SQ201610310921
【公开日】2016年9月21日
【申请日】2016年5月11日
【发明人】覃海涛, 龙世鹏, 吴立, 田天胜, 李卫平
【申请人】深圳市雷赛软件技术有限公司, 深圳市雷赛智能控制股份有限公司