CN104298151A - 新型速度控制算法及新型脉冲控制算法 - Google Patents

Abstract

本发明公开了新型速度控制算法及新型脉冲控制算法，在进行轨迹处理中，包括用户设定的速度参数在上位机进行解析步骤；将轨迹划分成微线段步骤；基于微线段的速度前瞻步骤和逐线段输出步骤；在所述基于微线段的速度前瞻步骤与逐线段输出步骤之间的处理过程中，在对每一条微线段处理时，采用分周期控制速度，实时改变当前速度，本发明解决现有技术到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整所存在的技术不足之处，而提供新型速度控制算法及新型脉冲控制算法，在处理中，运动曲线不是预先定好的，而是可随时调速的，基于微线段的速度前瞻和逐线段输出之间的控制方式上，可随时调整速度，以适应多种变形轨迹场合。

Description

新型速度控制算法及新型脉冲控制算法

技术领域

本发明涉及自动控制技术领域，具体的说，是新型速度控制算法及新型脉冲控制算法。

背景技术

现有的轨迹运动中，在进行轨迹处理中，包括用户设定的速度参数在上位机进行解析步骤；将轨迹划分成微线段步骤；基于微线段的速度前瞻步骤和逐线段输出步骤；而在基于微线段的速度前瞻步骤和逐线段输出步骤的处理上，基于微线段的速度前瞻之后到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整。

如图1所示，轨迹由三条线段组成，以往的算法是先进行速度前瞻，目的是：

（1）根据初始A点速度，结束C点速度，和限制的最高速度，以及脉冲数，计算出B点C点的速度，这牵涉到B点C点方向及矢量角，由前瞻算法决定；

（2）根据计算出的A点速度B点速度以及每一段的脉冲数，计算出当前段的降速点，这样保证每一段的速度曲线为T形或S形。

因为算法在程序运行前期就将所有的速度及减速点设置好，之后板卡将按照既定的参数运行至轨迹终止。如果想要在运动过程中改变当前速度或最大速度，将是很困难的事情。

在原有的算法中，速度前瞻决定了整条曲线的所有拐点及每条微线段的最高速和减速点，因此当数据设置好之后，不能再修改，只能按此参数运行完之后才能开始下一条曲线，比较机械，更适用于比较专用的场合。

目前已有的实现脉冲的技术是由DSP+FPGA实现，DSP计算轨迹前瞻，对FPGA进行预置参数，FPGA根据参数完成整条轨迹的输出，中途调整参数不易。

发明内容

本发明的目的在于解决现有技术到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整所存在的技术不足之处，而提供新型速度控制算法及新型脉冲控制算法，在处理中，运动曲线不是预先定好的，而是可随时调速的，基于微线段的速度前瞻和逐线段输出之间的控制方式上，可随时调整速度，以适应多种变形轨迹场合。

本发明通过下述技术方案实现：新型速度控制算法，在进行轨迹处理中，包括用户设定的速度参数在上位机进行解析步骤；将轨迹划分成微线段步骤；基于微线段的速度前瞻步骤和逐线段输出步骤；在所述基于微线段的速度前瞻步骤与逐线段输出步骤之间的处理过程中，在对每一条微线段处理时，采用分周期控制速度，实时改变当前速度。

进一步的，为更好的实现本发明，所述在对每一条微线段处理时的具体步骤为：前瞻设定一条微线段的起点速度和终点速度，依据终点速度为基准值，在此条微线段上运动过程中，通过微线段剩余距离的返算量，实时调节当前速度，使当前速度最终到达终点处与基准值相同。

进一步的，为更好的实现本发明，在所述采用分周期控制速度处理中，采用等周期控制速度，所述微线段处理时采用1ms周期控制。

新型脉冲控制算法，用于完成新型速度控制算法的脉冲控制算法，具体分为下述步骤：

（1）设定单位时间内运行速度以及在该速度下走的脉冲数；

（2）按照设定发出第一个脉冲串，并开始计时；

（3）在第一个控制周期结束时刻，DSP写入新的脉冲串参数命令字，同时FPGA读取此参数；

（4）上一个单位时间内最后一个脉冲结束时，立即使用新的脉冲串，此时进入下一个单位计时阶段；

（5）反复运行步骤（4），直至一条轨迹运行结束。

进一步的，为更好的实现本发明，所述分周期控制的控制周期为0.5~2ms。

进一步的，为更好的实现本发明，所述分周期控制的的脉冲占空比为50%。

进一步的，为更好的实现本发明，所述分周期控制的控制周期为1ms。

本发明与现有技术相比，具有以下优点及有益效果：

（1）本发明解决现有技术到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整所存在的技术不足之处，而提供新型速度控制算法及新型脉冲控制算法，在处理中，运动曲线不是预先定好的，而是可随时调速的，基于微线段的速度前瞻和逐线段输出之间的控制方式上，可随时调整速度，以适应多种变形轨迹场合。

（2）本发明在新型分周期控制中，采用等周期控制，微线段之间的衔接速度也是由速度前瞻计算好的，在具体每条微线段中，分周期控制速度，可以实时改变速度，即运动曲线不是预先定好的，而是可随时调速的，更符合一般运动控制卡的概念。

（3）本发明基于等周期的控制更通用，更灵活，适于在更复杂的控制领域。

（4）本发明每个周期内不须计算所有剩余轨迹的参数，而只需预先计算随后一小段轨迹，就能保证轨迹的匀滑。

（5）本发明采用控制周期为1ms，能有效提高控制精度。

附图说明

图1为现有技术处理的一条轨迹示意图。

图2为本发明进行处理时的一条轨迹进行微线段的示意图。

图3为本发明处理一条微线段时的表现图。

图4为本发明的脉冲运行图。

具体实施方式

本申请人自认为技术领域内技术员结合现有公知技术，并根据本申请文件所公开的内容即可实现本发明。

下面结合实施例对本发明作进一步地详细说明，但本发明的实施方式不限于此。

实施例1：

新型速度控制算法，在进行轨迹处理中，包括用户设定的速度参数在上位机进行解析步骤；将轨迹划分成微线段步骤；基于微线段的速度前瞻步骤和逐线段输出步骤；在所述基于微线段的速度前瞻步骤与逐线段输出步骤之间的处理过程中，在对每一条微线段处理时，采用分周期控制速度，实时改变当前速度，解决现有技术到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整所存在的技术不足之处，而提供新型速度控制算法及新型脉冲控制算法，在处理中，运动曲线不是预先定好的，而是可随时调速的，基于微线段的速度前瞻和逐线段输出之间的控制方式上，可随时调整速度，以适应多种变形轨迹场合。

实施例2：

本实施例是在上述实施例的基础上进一步优化，进一步的，为更好的实现本发明，所述在对每一条微线段处理时的具体步骤为：前瞻设定一条微线段的起点速度和终点速度，依据终点速度为基准值，在此条微线段上运动过程中，通过微线段剩余距离的返算量，实时调节当前速度，使当前速度最终到达终点处与基准值相同；在新型分周期控制中，采用等周期控制，微线段之间的衔接速度也是由速度前瞻计算好的，在具体每条微线段中，分周期控制速度，可以实时改变速度，即运动曲线不是预先定好的，而是可随时调速的，更符合一般运动控制卡的概念。

实施例3：

本实施例在实施例1的基础上进一步优化，进一步的，为更好的实现本发明，在所述采用分周期控制速度处理中，采用等周期控制速度，所述微线段处理时采用1ms周期控制；基于等周期的控制更通用，更灵活，适于在更复杂的控制领域。

实施例4：

新型脉冲控制算法，用于完成新型速度控制算法的脉冲控制算法，具体分为下述步骤：

（1）设定单位时间内运行速度以及在该速度下走的脉冲数；

（2）按照设定发出第一个脉冲串，并开始计时；

（3）在第一个控制周期结束时刻，DSP写入新的脉冲串参数命令字，同时FPGA读取此参数；

（4）上一个单位时间内最后一个脉冲结束时，立即使用新的脉冲串，此时进入下一个单位计时阶段；

（5）反复运行步骤（4），直至一条轨迹运行结束。

实施例5：

本实施例是在上述实施例的基础上进一步优化，进一步的，为更好的实现本发明，所述分周期控制的控制周期为0.5~2ms。

实施例6：

本实施例在实施例4的基础上进一步优化，进一步的，为更好的实现本发明，所述分周期控制的的脉冲占空比为50%。

实施例7：

本实施例在实施例4的基础上进一步优化，进一步的，为更好的实现本发明，所述分周期控制的控制周期为1ms。

实施例8：

本实施例综合上述所有实施例而给出的一种优化的实施方式，如图2、图3所示，新型等周期控制中，微线段之间的衔接速度（B点、C点）也是由速度前瞻计算好的，但在具体每条微线段中，分周期控制速度，可以实时改变速度。

这样，在每一周期返算时需判断在该微线段终点速度能不能达到预先前瞻好的节点速度（如图2所示，此时的节点为B点和C点），每个微线段中间的速度可以调整，主要是要保证到达B点和C点的速度，因为速度前瞻的目的就是根据轨迹拐点的矢量角度计算好B点和C点的速度，保证在运动轨迹的拐点方向改变时速度能够平滑，不至于产生太大冲击，而起始速度A点速度和终点速度D点速度是预先用户设定的，而在微线段内如线段AB，起点速度指A点速度，终点速度指B点速度，以往的算法已经预算好了AB线段上以及以后每条线段的每一点的速速，硬件输出就行，改变起来不便，新型算法在AB线段上进行周期（L）等分，每一点的速度可以调整，在调整当前速度的时候要去返算调整后能不能在B点达到前瞻好的速度，其他线段也一样，因为是1毫秒改变一次速度，再返算一次，控制更灵活。

每个周期内不须计算所有剩余轨迹的参数，而只需预先计算随后一小段轨迹，只要保证在B点、C点到达前瞻的速度，就能保证轨迹的匀滑。例如在AB线段上运行，当前改变速度时，假如速度增加，如果无限增加的话，在临近B点时减速，以预定加速度减速是怎么也减不到前瞻的B点速度的，所以增加速度的时候要返算能不能降速至前瞻好的B点速度。

如图3所示，A 点为一条微线段的起始点，B点为微线段的结束点，两点速度都是前瞻算好的，运动就是要保证在这些点的速度符合前瞻速度，而微线段之内速度可以按T形变化，升速-匀速-减速，也可以按S形变化。以T形为例，以往的算法规定了AB线段内的最大速度和加速度，从A点以加速度上升，升至最大速度时匀速运动，达到减速点时再以加速度下降，这样可以保证两点的速度，如图3中实线所示，这条运动曲线对于以往的算法是固定的，不能调整，新型算法中如虚线所示，在M点改变速度，则计算剩余的位移能不能满足达到B点速度要求，比如升速，速度最大升至N，因为再升的话加速度下降时速度不能降至B点设定的速度，这就是改变-返算的过程，每1毫秒进行一次。

当实时改变速度后，数据返算量较小，这样在运动过程中调速会很方便；更易于速度控制，也更符合一般运动控制卡的概念，即运动曲线不是预先定好的，而是可随时调速的，在分周期的控制速度中，基于等周期的控制更通用，更灵活，适于在更复杂的控制领域，比如机器人，而以往的更专用，适用固定轨迹的场合；比如在机器人控制中，即便能前瞻算好线段两端点的速度，也不能固定这条线段的轨迹，要随时去检测，去调整，这样只有在周期性的控制中才能做到。现有的技术实时性更好更好，但效率没有旧有的高，因为旧有方式算好后就由硬件固定输出，而现在要每个周期返算剩余路径速度；每个周期内不须计算所有剩余轨迹的参数，而只需预先计算随后一小段轨迹，就能保证轨迹的匀滑。

实施例9：

本实施例是在上述9个实施例的基础上，当分周期控制的控制周期为1ms时的具体实施方式，微线段的起点速度和终点速度已经由速度前瞻算好，而升降速的过程是变化的，如果速度和加速度一样，可以实现跟以往算法一样的效果，但在每一周期会返算当前微线段剩余路径，从而对当前速度进行微调，且用户可以随时调整，调整时只需要返算当前微线段剩余路径即可，在实施例9已有表述，在此不再赘述。

如图4所示，新型脉冲控制算法：

1）、设定单位时间（Ti）内运行速度以及在该速度下走的脉冲数：用户要设定运行速度，加速度，总脉冲数个数，控制周期为1ms，运行速度的单位为每分钟多少个脉冲；那么在匀速运动时，根据速度（用户设定）和时间（1毫秒）相乘即得到在当前周期我要运行多少个脉冲；由于单位时间内的脉冲数就是运行速度，在这单位时间内的脉冲速度是一样的，即脉冲的频率是一定的；但每个单位时间不是刚好运动整数个脉冲，那就需要在下一个单位时间内多退少补，所以每个单位时间内的速度和脉冲数有可能是变化的；匀速运动时，根据速度V和我们固定的周期即可得到单位时间的脉冲数，而变速运动时结合加速度dV来判断，加速度的单位为每分钟变化多少个脉冲，通过与毫秒之间的换算，即可得到下一周期该多发或少发多少个脉冲。设定脉冲指设定脉冲的个数：由个数和时间（1ms），即可得到速度为脉冲数/分钟，也就是脉冲频率，跟用户设定速度单位一致；由于脉冲为数字信号，占空比为50%，幅度值由外围电路决定。

2）、按照设定发出脉冲串1，并开始计时；

3）、在设定的单位时间结束时，第一个控制周期结束时刻，即在A时刻，DSP写入新的脉冲串（脉冲串2）参数命令字，同时FPGA读取此参数，A点即1毫秒定时点，DSP根据FPGA返回的位置值计算下一个1毫秒的速度，在此点并写入FPGA，FPGA只有在以上一个周期的速度值运行完当前的完整脉冲后，才取用这个速度值，进行当前周期的输出；

4）、上一个单位时间内最后一个脉冲结束时（B时刻），立即使用脉冲串2，此时进入Ti+1时刻，在Ti+1时刻的起点即为B，A时刻为控制周期端点，在此刻就更新下一周期的数据，但此时脉冲串1尚未走完，需要等到这个脉冲串结束后，才用新数据参与运算，也就是如此才会导致有的周期多运行一个脉冲，有的周期少运行一个脉冲。

5）、以此类推直到运动结束。

DSP与FPGA之间进行速度或脉冲控制及返算原理如下：

在轨迹确定的运动中：

匀速运动时，根据用户设定速度V，设定距离（即脉冲数）S，则每个控制周期（1毫秒）内的运动应该是固定的，比如为M。在运动开始时，DSP发速度参数V给FPGA，FPGA以速度V从硬件输出速度波形，以后每个控制周期（1毫秒），DSP会检测FPGA当前发的脉冲个数，即已走距离。由于脉冲个数不可能是小数，则在每1毫秒的检测点上，运动的距离不一定为整数，假设第一个1毫秒后，DSP检测到FPGA实际运动的距离为M-1，则当前下发的速度参数为（M+1）/1ms，即在下一个1毫秒内将少走的一个脉冲补上；这样，每个1毫秒内的速度可能是变化的，但最终的速度、时间和距离不变；

在变速运动时，DSP根据加速度计算得出当前速度值，以及在当前点以后1毫秒内的平局速度，以及在1毫秒内需要发送的脉冲总数给FPGA，FPGA即以此速度和脉冲数从硬件发出相关波形，1毫秒后DSP再检测FPGA实际下发的距离，再根据此时的平均速度及应发的脉冲数（多退少补，即若检测到实际发出脉冲数比上一条指令的少，则在当前指令上加上差值，反之类似）。

若在运动中速度进行调整，以T形运动为例，每次1毫秒检测当前速度及剩余距离S1，根据当前速度V，终点速度Vs和加速度A计算平均速度和时间，平均速度Va=（当前速度V+终点速度Vs）/2，时间T=（当前速度-终点速度）/A。若Va*T<S1,表示当前速度可继续升速或者已经进入设定最高速保持匀速运动，直到Va*T>=S1时进入减速状态，开始减速。

在匀速运动过程中每个单位时间内的速度和脉冲数有可能是变化的，而变速过程中每个单位时间内的速度和脉冲数是变化的。

本发明解决现有技术到逐线段输出，采用固定的利用FPGA硬件进行输出，不能进行实时调整所存在的技术不足之处，而提供新型速度控制算法及新型脉冲控制算法，在处理中，运动曲线不是预先定好的，而是可随时调速的，基于微线段的速度前瞻和逐线段输出之间的控制方式上，可随时调整速度，以适应多种变形轨迹场合。

以上所述，仅是本发明的较佳实施例，并非对本发明做任何形式上的限制，凡是依据本发明的技术实质对以上实施例所作的任何简单修改、等同变化，均落入本发明的保护范围之内。

Claims (7)

1.新型速度控制算法，其特征在于：在进行轨迹处理中，包括用户设定的速度参数在上位机进行解析步骤；将轨迹划分成微线段步骤；基于微线段的速度前瞻步骤和逐线段输出步骤；在所述基于微线段的速度前瞻步骤与逐线段输出步骤之间的处理过程中，在对每一条微线段处理时，采用分周期控制速度，实时改变当前速度。

2.根据权利要求1所述的新型速度控制算法，其特征在于：所述在对每一条微线段处理时的具体步骤为：前瞻设定一条微线段的起点速度和终点速度，依据终点速度为基准值，在此条微线段上运动过程中，通过微线段剩余距离的返算量，实时调节当前速度，使当前速度最终到达终点处与基准值相同。

3.根据权利要求1所述的新型速度控制算法，其特征在于：在所述采用分周期控制速度处理中，采用等周期控制速度，所述微线段处理时采用1ms周期控制。

4.新型脉冲控制算法，其特征在于：用于完成新型速度控制算法的脉冲控制算法，具体分为下述步骤：

设定单位时间内运行速度以及在该速度下走的脉冲数；

按照设定发出第一个脉冲串，并开始计时；

在第一个控制周期结束时刻，DSP写入新的脉冲串参数命令字，同时FPGA读取此参数；

上一个单位时间内最后一个脉冲结束时，立即使用新的脉冲串，此时进入下一个单位计时阶段；

反复运行步骤（4），直至一条轨迹运行结束。

5.根据权利要求4所述的新型脉冲控制算法，其特征在于：所述分周期控制的控制周期为0.5~2ms。

6.根据权利要求4所述的新型脉冲控制算法，其特征在于：所述分周期控制的的脉冲占空比为50%。

7.根据权利要求4所述的新型脉冲控制算法，其特征在于：所述分周期控制的控制周期为1ms。