CN107046382B - 一种基于三角函数的迭代速度控制方法 - Google Patents

Abstract

本发明公开了一种基于三角函数的迭代速度控制方法，包括如下步骤：步骤一、构造伺服电机三角函数速度控制公式步骤二、构造三个连续速度之间的迭代控制公式v(t+2)＝2D×v(t+1)‑v(t)+2A(1‑D)，根据速度的迭代步长为Δt，初始速度v(0)＝v_s以及第一步的速度值v(1)＝A‑Bcos(CΔt)，通过迭代得到第t+2步的速度值，其中，A＝(v_e+v_s)/2，B＝(v_e‑v_s)/2，C＝π/t_a，D＝cos(CΔt)；步骤三、构造迭代速度控制电路，其内部的迭代电路可通过6个寄存器，1个移位器，1个乘法器，1个减法器，1个加法器,1个计数器，1个比较器构成，在统一的时钟控制下即可得到连续的速度控制信号。步骤四、控制器按照步骤三得到的速度控制信号控制脉冲发生器，产生脉宽调制信号。步骤五、控制器按照步骤四中脉宽调制信号对电机进行控制。

Description

一种基于三角函数的迭代速度控制方法

技术领域

本发明属于电机控制领域，特别涉及一种伺服电机的迭代速度控制方法。

背景技术

在高速数控系统中,伺服电机速度控制方法不当，容易导致冲击、失步、震荡或者超程等问题。常用的加减速算法有T形加减、指数加减速、S曲线加减速算法等。目前，国内低速数控设备大多数采用的T形加减速算法对伺服电机进行控制。T形加减速算法和指数加减速算法在启停过程中加加速度会产生阶跃变化，会对设备产生冲击和剧烈振动。这将会直接影响零件加工质量和机器的使用寿命。

在中高速数控设备中，往往采用S曲线加减速算法对伺服电机进行控制。S曲线加减速算法在加减速阶段控制加速度的线性变化，使得冲击力的变化是柔性的，从而能减少对机械部件的冲击。S曲线实现方法有典型的7段S曲线方法，多项式S曲线方法，三角函数S曲线方法等。发明专利《基于S型曲线的加减速控制方法和装置及数控机床》(授权公告号为CN103135501B)中涉及到的S型加减速算法将S曲线速度控制分为加加速、匀加速、减加速、匀速、减减速、匀减速、加减速七个过程,该方法在FPGA中实现时计算过程较为复杂，在加工路径上曲率频繁变化时很难保证其实时性。多项式S曲线方法中，多项式的次数越高,则精度越高,但是运算的效率就会降低。通过仿真比较，同样条件下的三角函数S曲线其柔性要优于多项式S曲线。发明专利《一种用于伺服压力机的伺服电机加减速控制方法》(授权公告号为CN102522944B)中涉及到一种三角函数S曲线加减速方法，但是没有提到如何用FPGA硬件实现算法。

发明内容

本发明设的目的是克服现有伺服电机加减速控制方法冲击较大的缺陷，提供了一种三角函数柔性加减速控制方法。

本发明设的另一个目的是解决现有控制方法中通过上位机计算好速度的控制数据再传递给下位机造成的耗费大量硬件资源且实时性难以保证的缺陷，通过迭代产生下一时刻脉冲控制信号。

本发明提供的技术方案为：

一种基于三角函数的迭代速度控制方法，包括如下步骤：

获取加/减速时间t_a，初始速度v_s，终止速度v_e；

按照如下三角函数速度公式变化对伺服电机速度进行控制：

优选的是，根据速度的迭代步长Δt，初始速度v(0)＝v_s以及第一步的速度值v(1)＝A-Bcos(CΔt)后，通过下式迭代得到第t+2步的速度值：

v(t+2)＝2D×v(t+1)-v(t)+2A(1-D)

其中，A＝(v_e+v_s)/2，B＝(v_e-v_s)/2，C＝π/t_a，D＝cos(CΔt)。

优选的是，还包括以下步骤：

构造基于三角函数的迭代速度控制电路，输入信号Clk提供时钟信号，输入Reset提供复位信号，输入Enable提供模块启动信号；

所述控制电路的输入接口V0,V1,D,AddNum,IteraNum分别与内部寄存器v0,v1,D,AddNum,IteraNum相连，用于输入初始数据；内部寄存器v0,寄存器v1用于存放迭代的两个初始速度值，内部寄存器D用于存放D值；寄存器D与寄存器v1与乘法器相连，其中存放的数据作为乘法器的两个输入操作数；乘法器的输出与移位寄存器相连，实现左移一位；移位寄存器shift的输出和寄存器v0与减法器相连，其中的数据作为减法器的两个输入操作数；寄存器AddNum用于存入参数2A(1-D)，该寄存器和减法器的输出与加法器相连，其中存放的数据作为加法器的两个输入操作数；加法器的输出与寄存器v2相连，寄存器v2与输出FreqControl相连，其值用于控制脉冲发生器；寄存器v2与寄存器v1相连，寄存器v1与寄存器v0相连，实现迭代过程中的速度反馈；迭代进行的次数由寄存器counter记录,寄 存器IteraNum存放总的迭代次数，这两个寄存器与比较器相连，其中的值作为比较器的两个输入操作数；比较器的输出与OutputValid相连，用于控制迭代数据的有效性；

在输入时钟Clk的控制下，电路不断地产生三角函数加减速控制信号，控制脉冲发生器产生脉宽调制信号，以实现对伺服电机的柔性加减速控制。

优选的是，对加速阶段伺服电机加速度求一阶导数，得到加速阶段伺服电机加速度表达式

控制器按照上述加速度值对电机进行控制。

优选的是，对加速阶段伺服电机加速度求一阶导数，得到加加速度表达式

控制器按照上述加加速度值对电机进行控制。

优选的是，对加速阶段伺服电机加速度进行积分，得到加速段的位移表达式

控制器按照上述位移值对电机进行控制。

本发明的有益效果是：本发明提供的基于三角函数的迭代速度控制方法，根据三角函数的变换规则和当前脉冲产生的控制参数推导出下一时刻的脉冲产生控制参数。实现了一种迭代方法产生脉冲控制信号，从而避免了软件实现方式和查表实现方式的缺点。

附图说明

图1为本发明所述的加减速电路结构结构示意图。

图2为三角函数加减速速度控制过程速度曲线。

图3为加速段误差比较示意图。

图4为减速段误差比较示意图。

具体实施方式

下面结合附图对本发明做进一步的详细说明，以令本领域技术人员参照说明书文字能够据以实施。

一个完整的三角函数速度控制过程可以描述为加速段、匀速段、减速段三段。对于伺服电机来说有几个物理参数是确定的：v_max：电机的最大转速；A_max：电机的最大加速度；J_max：通常称为加加速度和减减速度，这个参数反映了力的变化情况。力的剧烈变化是产生机械冲击力的根本原因。传统的T型加减速算法在每次启动、加速、减速、停止过程中出现四次速度突变的情况。这种速度突变会产生巨大的冲击力，因而容易对设备造成损坏。

因此需要选择一种合适的速度控制曲线，使其具备渐变性的速度就可以使得冲击力柔性变化，避免产生巨大的冲击力。三角函数曲线正好具备这种特性。由于三角函数的导数依然是三角函数。因此采用三角函数控制速度的加速度、加加速度公式是一个渐变性的连续函数。

选取正弦函数y＝sin(πx-π/2)+1的半个周期作为加速曲线。选取正弦函数y＝-sin(πx-π/2)+1的半个周期作为减速曲线。因为描述运动过程的位移、速度、加速度、加加速度之间的具备导数关系，下面我们进一步对运动过程中的位移、加速度、加加速度公式进行推导。以加速段为例，设加速段或减速段时间为t_a，构造加速段的速度公式如下

上式可变换为：

上述表达式满足时间边界条件：0≤t≤t_a，t_a为加速时间；速度边界条件：初始速度v(0)＝v_s，终止速度为v(t_a)＝v_e；

对上式求一阶导数得到加速度表达式。

加速度表达式满足边界条件:当t＝0时，起始加速度a(0)＝0；t＝t_a时，加速末端加速度a(t_a)＝0。设A_max为系统的最大加速度，则有：

|a(t)|≤A_max

对加速度求导数得到加加速度表达式。

设J_max为系统的最大加速度，则有：

|J(τ)|≤J_max

为了得到更好的柔性加减速，有：

对速度积分，可得到加速段的位移表达式：

位移表达式满足边界条件：s(0)＝0；t＝t_a时有：

通过以上推导，我们构建了三角函数速度表达式及其相关物理量的关系如下：

三角函数加减速常用的方式是通过上位机计算好速度的控制数据，然后传递给下位机或者脉冲控制芯片产生伺服电机需要的驱动频率进而控制伺服电机的加减速。这种方法需要耗费大量硬件资源而且实时性难以保证。也有方法通过存储三角函数值，芯片在产生控制脉冲时通过查表方式计算控制参数，这种方式能保证一定的实时性，但是需要耗费额外的存储资源，而且查表的时间会造成瓶颈，不利于产生高速驱动频率。本专利根据三角函数的变换规则和当前脉冲产生的控制参数推导出下一时刻的脉冲产生控制参数。实现了一种迭代方法产生脉冲控制信号，从而避免了软件实现方式和查表实现方式的缺点。

加减速电路迭代的原理为：

对于基本的三角函数有如下公式：

利用三角函数的和差性质，我们对速度公式进行分析，提出一种迭代算法的电路实现对三角函数加减速控制信号值进行计算。考察速度控制公式，给定v_e,v_s,a_max,j_max,则(v_e+v_s)/2,(v_e-v_s)/2,π/t_a可通过上位机计算好，作为常数传递给下位机，这里分别设为A,B,C.，速度公式可以表述如下：

对于上需要重点考虑某一时刻cos(Ct)的计算，假设某时刻t的速度为vt 经过Δt和2Δt后的速度v_t+1和，v_t+2，对这三个速度中的三角函数关系做如下简化并进行推导。

由上式可知：

v_t+2＝cos(Ct+CΔt+CΔt)

＝cos(Ct+CΔt)×cos(CΔt)-sin(Ct+CΔt)×sin(CΔt)

＝cos(Ct+CΔt)×cos(CΔt)-(sinCt×cos(CΔt)+cosCt×sin(CΔt))×sin(CΔt)

＝cos(Ct+Δt)×cos(CΔt)-sinCt×cos(CΔt)×sin(CΔt)-cosCt×sin²(CΔt)

上式可知：

-sinCt×sin(CΔt)＝v_t+1-cosCt×cos(CΔt)

因此上式可化为：

＝v_t+1×cos(CΔt)+cos(CΔt)×(v_t+1-cosCt×cos(CΔt))-cosCt×sin²(CΔt)

＝2v_t+1×cos(CΔt)-cosCt×cos²(CΔt))-cosCt×sin²(CΔt)

＝2v_t+1×cos(CΔt)-cosCt×(cos²(CΔt)+sin²(CΔt))

＝2v_t+1×cos(CΔt)-cosCt

＝2v_t+1×cos(CΔt)-v_t

对于特定某次加减速，其时间间隔Δt是不变的，因此公式中的cos(CΔt)也可在上位机计算好，作为常数参数传递给下位机，这里设为D，则上述公式可表示为：

v_t+2＝2D×v_t+1-v_t

综合以上公式有如下公式

v(t+2)＝A-B×(2D×v_t+1-v_t)

＝A-2DB×v_t+1+Bv_t

＝A+2D(v(t+1)-A)+A-v(t)

＝2D×v(t+1)-v(t)+2A(1-D)

由此构成了三个连续速度三角函数之间的递推关系,其中A＝(ve+vs)/2，B＝(ve-vs)/2，C＝π/ta,D＝cos(CΔt)。特别地，当t＝0时v(0)＝vs,即加减速的初始速度。经过Δt后，由公式可知：

v(1)＝A-Bcos(CΔt)

＝A-BD

由以上推导，可做如下结论，上位机需要将四个参数v(0)，v(1),D,2A(1-D)， 迭代的次数传递给三角加减速控制模块，该模块就能构建一个迭代的三角函数加减速控制电路，在时钟控制下，不断地产生三角函数加减速控制信号，控制脉冲发生器产生伺服电机驱动脉冲，以实现柔性加减速控制。

通过加减速电路实现上述控制方法。

电路模块的外部接口信号包括输入和输出信号，如图1所示。其具体标识和含义如下。

输入接口信号包括：

clk信号:时钟信号，为电路模块的工作提供统一的时钟信号。

v0信号：用来输入初始速度v0的值。

v1信号：用来输入第一次变化后的速度值。

D信号：用来输入D值。

AddNum信号：用来输入AddNum值,其值为2A(1-D)。

Reset信号：复位信号。

Enable信号：模块工作使能信号，其输入信号为“1”表示使能，电路开始迭代运算，否则迭代电路不工作。

IteraNum信号：用来输入迭代次数值。

输出接口信号包括：

FreqControl信号:脉冲频率控制信号，用来控制伺服电机PWM的产生。

OutputValid信号:输出信号的有效性控制，其输出为“1”表示FreqControl的输出信号是有效的，否则表示无效。

其内部的迭代电路可通过6个通用寄存器，1个移位寄存器，1个乘法器，1个减法器，1个加法器,1个计数器，1个比较器构成，在统一的时钟控制下即可得到连续的控制信号。由此构成硬件电路的基本原理,其结构如图1所示。各部件功能如下：

1.通用寄存器1，存放参数v0。

2.通用寄存器2，存放参数v1。

3.通用寄存器3，存放参数IteraNum，其值为迭代的终止次数。

4.通用寄存器4，存放迭代过程中的累加数AddNum,其值为2A(1-D)。

5.通用寄存器5，存放参数D。

6.通用寄存器6，存放参数v2。

7.移位寄存器，实现2*D*v1。

8.计数器，用于记录迭代次数。

9.乘法器，实现D*v1。

10.减法器，实现2*D*v1-v0。

11.加法器，实现2*D*v1-v0+2A(1-D)。

12.比较器，比较迭代次数和迭代的终止次数，其结果控制输出结果有效性。

如果位移足够长，一个完整三角函数加减速为加速，匀速，减速三个过程。当然，根据位移，初始速度，末速度的情况，有可能只有加速，减速，加减速过程。对于不同细节情况而言，仅仅是参数值的区别，而电路是相同的，因此这些不同细节情况本专利不做进一步讨论。本专利重点在于加减速电路的研究，因此仅以一个完整的速度控制过程为例进行阐述，经过如下步骤，即可在FPGA芯片中实现三角函数加减速控制电路。

步骤1：在上位机中，根据位移的要求和系统工作参数a_max,j_max,v_e,v_s,时间控制精度Δt，结合计算加速段时间t_a，减速段的时间t_d，匀速段时间t_c。

步骤2：计算迭代的次数IteraNum。

步骤3：在上位机中，根据初始速度v_s，v_e，分别计算参数A＝(v_e+v_s)/2,B＝(v_e-v_s)/2。

步骤4：在上位机中，根据t_a，计算C＝π/t_a。

步骤5：在上位机中，根据Δt，计算D＝cos(CΔt)。

步骤6：在上位机中，根据v_s的值设置v0＝v_s。

步骤7：在上位机中，根据A,D的值计算AddNum，其值为2A(1-D)。

步骤8：在上位机中，根据A,B,D的值计算v1＝A-BD。

步骤9：上位机将参数v0,v1,D,1-DIteraNum，AddNum传递给下位机的FPGA芯片中。

步骤10：使用硬件描述语言，在FPGA开发平台中中实现图1所示的电路。

步骤11：在硬件开发平台中编译，仿真，逻辑综合后将所实现的目标程 序下载到FPGA芯片中。

步骤12：实现了该迭代算法的FPGA芯片在时钟clk控制下计算v₂。

步骤13：根据输出的v₂值控制脉冲发生器，产生伺服电机驱动脉冲。

步骤14：脉冲发生器产生的PWM脉冲输出到伺服电机驱动器实现三角函数柔性加减速运动。

实施实例一：算法的FPGA实现

本实例说明了如何在FPGA芯片上实现三角函数柔性加减速控制电路的实现过程和步骤。

步骤1:在上位机中，根据位移的要求和系统工作参数a_max,j_max,v_e,v_s,时间控制精度Δt，计算加速段时间t_a，匀速段时间t_c，减速段的时间t_d。

对于给定位移s＝360mm，伺服电机启动速度v_s＝120mm/s,终止速度v_e＝120mm/s,给定的伺服电机参数a_max＝15000mm/s²；j_max＝500000mm/s³；v_max＝1500mm/s；设定控制精度Δt＝0.01ms。根据公式(9)计算加速时间t_a＝0.14ms，加速段位移为110.339mm。根据对称性减速时间为t_d＝0.14ms，减速段位移为110.339mm。以最大速度v_max匀速运行的时间t_c＝0.9ms,匀速段位移为139.322，合计运行时间0.37ms。对于实际加减速中产生的位移误差，本专利中的处理方式是由匀速段加以补偿。

步骤2：设置计算迭代的次数IteraNum，时间间隔。在0-0.14ms内，以时间精度0.01ms设置离散控制点，共15个控制点，在上位机已知v_s，v1，还需要迭代次数13次。

步骤3:在上位机中，根据初始速度v_s，v_e，分别计算参数A＝(v_e+v_s)/2,B＝(v_e-v_s)/2。在加速段，初始速度值v_s＝120mm/s，末速度值v_e＝1500mm/s，计算出参数A＝810，B＝690。

步骤4:在上位机中，根据t_a，计算C＝π/t_a。在加速段，初始速度值v_s＝120mm/s，末速度值v_e＝1500mm/s。计算出参数C＝21.75。

步骤5:在上位机中，根据Δt，计算D＝cos(CΔt)。计算出参数D＝0.9764。

步骤6：在上位机中，根据v_s的值设置v0＝v_s。计算出参数V0＝120。

步骤7：在上位机中，根据A,D的值计算参数AddNum＝38.168。

步骤8：在上位机中，根据A,B,D的值计算参数v1＝136.257。

步骤9：上位机将参数v0,v1,D,IteraNum，AddNum传递给下位机的FPGA芯片中。

步骤10:使用硬件描述语言，在FPGA开发平台中中实现图1所示的电路。

实现迭代部分的verilog关键代码如下：

步骤11:在硬件开发平台中编译，仿真，逻辑综合后将所实现的目标程序下载到FPGA芯片中。系统的开发平台为：Quartus II 12.0，仿真平台为：modelsim10.0。所用FPGA芯片型号为：Cyclone IV EP4CE10F17C8。该FPGA芯片的工作频率clk为50MHZ。

步骤12:实现了该迭代算法的FPGA芯片在时钟clk控制下计算控制参数v₂。表1给出了37个参考离散点的数据。与软件实现方法相比，其加减速阶段的误差比较如图2、图3、图4所示，图2中显示了三角函数加减速控制实现的速度控制过程。由图3、图4可知，迭代过程中，其误差会产生累计效应，但是其最大值为1.14X10^-12,对于控制频率数量级为MHZ(10^-6)来说，是完全可用的。

表1：三角函数速度控制离散点

步骤13：根据输出的控制参数v₂的值控制脉冲发生器，产生伺 服电机驱动脉冲。其驱动脉冲范围为1～5MPPS。

步骤14：脉冲发生器产生的PWM脉冲(如图3所示)输出到伺服电机驱动器实现三角函数柔性加减速运动。图3中a图显示的是Malab平台下算法的仿真结果。图b显示的是在FPGA开发平台Quartus和Modelsim平台下的仿真结果，其中drive_pul信号即可用来实现对伺服电机的速度柔性控制。

实施实例二：算法实现对伺服电机的驱动

本实例说明了具备三角函数柔性加减速控制电路如何控制伺服电机的运行过程和步骤。本专利实现对伺服电机的控制结构如图4所示。其工作流程如下：

步骤1：上位机的功能一般由PC机实现，主要实现对运动轨迹的规划，不同伺服电机运动轨迹的插补等功能，具体到某一个伺服电机的某一次运动，需要根据运动的位移，初始速度，末速度，加速度等计算下位机工作需要的各种参数，形成各种运动控制命令。

步骤2：上位机通过某一种通信方式将控制命令和参数发送给下位机，本例中是将上位机和下位机通过路由器的网络接口连接在一起，上位机和下位机之间通过TCP/IP协议进行通信。

步骤3：下位机中实现了三角函数加减速控制电路的FPGA芯片，能根据参数不断地产生伺服电机驱动脉冲。

步骤4：下位机和伺服电机的驱动器连接，其主要连接信号有pul和dir信号，pul信号用来控制伺服电机的转速，dir信号用来控制伺服电机的转动方向。

步骤5：伺服电机驱动器与伺服电机相连，驱动电机按照控制信号转动。

尽管本发明的实施方案已公开如上，但其并不仅仅限于说明书和实施方式中所列运用，它完全可以被适用于各种适合本发明的领域，对于熟悉本领域的人员而言，可容易地实现另外的修改，因此在不背离权利要求及等同范围所限定的一般概念下，本发明并不限于特定的细节和这里示出与描述的图例。

Claims (5)

1.一种基于三角函数的迭代速度控制方法，其特征在于，包括如下步骤：

获取加/减速时间t_a，初始速度v_s，终止速度v_e；

按照如下三角函数速度变化公式对伺服电机速度进行控制：

根据速度的迭代步长Δt，初始速度v(0)＝v_s以及第一步的速度值v(1)＝A-Bcos(CΔt)后，通过下式迭代得到第t+2步的速度值：

v(t+2)＝2D×v(t+1)-v(t)+2A(1-D)

其中，A＝(v_e+v_s)/2，B＝(v_e-v_s)/2，C＝π/t_a，D＝cos(CΔt)。

2.根据权利要求1所述的基于三角函数的迭代速度控制方法，其特征在于，还包括以下步骤：

构造基于三角函数的迭代速度控制电路，输入信号Clk提供时钟信号，输入Reset提供复位信号，输入Enable提供模块启动信号；

所述控制电路的输入接口分别与内部寄存器v0,v1,D,AddNum,IteraNum相连，用于输入初始数据；内部寄存器v0,内部寄存器v1用于存放迭代的两个初始速度值，内部寄存器D用于存放D值；内部寄存器D与内部寄存器v1与乘法器相连，其中存放的数据作为乘法器的两个输入操作数；乘法器的输出与移位内部寄存器shift相连，实现左移一位；移位内部寄存器shift的输出和内部寄存器v0与减法器相连，其中的数据作为减法器的两个输入操作数；内部寄存器AddNum用于存入参数2A(1-D)，该内部寄存器和减法器的输出与加法器相连，其中存放的数据作为加法器的两个输入操作数；加法器的输出与内部寄存器v2相连，内部寄存器v2与输出端口FreqControl相连，其值用于控制脉冲发生器；内部寄存器v2与内部寄存器v1相连，内部寄存器v1与内部寄存器v0相连，实现迭代过程中的速度反馈；迭代进行的次数由内部寄存器counter记录,内部寄存器IteraNum存放总的迭代次数，内部寄存器counter和内部寄存器IteraNum与比较器相连，其中的值作为比较器的两个输入操作数；比较器与输出端口OutputValid相连，用于控制迭代数据的有效性；

在输入时钟Clk的控制下，电路不断地产生三角函数加减速控制信号，控制脉冲发生器产生脉宽调制信号，以实现对伺服电机的柔性加减速控制。

3.根据权利要求1所述的基于三角函数的迭代速度控制方法，其特征在于，对加速阶段伺服电机速度求一阶导数，得到加速阶段伺服电机加速度表达式

控制器按照上述加速度对电机进行控制。

4.根据权利要求3所述的基于三角函数的迭代速度控制方法，其特征在于，对加速阶段伺服电机加速度求一阶导数，得到加加速度表达式

控制器按照上述加加速度对电机进行控制。

5.根据权利要求4所述的基于三角函数的迭代速度控制方法，其特征在于，对加速阶段伺服电机加速度进行积分，得到加速段的位移表达式

控制器按照位移表达式得到的位移值对电机进行控制。