CN106292472A - 一种运动控制方法及装置 - Google Patents

Abstract

公开了一种运动控制方法和装置。所述方法包括：第一信号转换器将外部编码器采集的差分数据信号转换为单端数据信号，并将单端数据信号发送至FPGA解码器；FPGA解码器对单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器；ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将控制输出量输出至DA转换器；DA转换器对控制输出量进行数模转换，然后将其输出至电机驱动器；其中，所述ARM控制器装载有UCOSII操作系统。本发明的运动控制方法、装置的通用性强、实时性强、控制精度高。

Description

一种运动控制方法及装置

技术领域

本发明涉及伺服系统运动控制技术领域，尤其涉及一种运动控制方法及装置。

背景技术

在高精度伺服系统中，光栅传感器常用于采集待测目标的位置信息。按运动形式分，光栅传感器可分为直线型和旋转型。其中，直线型光栅传感器主要由光栅尺和读数头构成，旋转型光栅传感器主要由码盘和读数头构成。在实际使用光栅传感器时，用户只需按照固定的时钟频率向读数头发送脉冲信号，读数头就会将封装好的数据返回给用户。但是，如果用户想要得到待测目标的角度信息，则还需要对读数头的数据进行采集。

在现有技术中，运动控制器可以针对光栅进行数据采集、并进行高精度的伺服控制。但是，现有的运动控制器价格昂贵，即使便宜的也高达几万元。另外，现有的运控控制器因其复杂的结构和复杂的控制算法，基本上很难用于对实时性要求很高的系统。

针对现有的运动控制器存在的结构复杂、实时性差、成本高的缺陷，亟需一种通用性强、结构简单、成本低、实时性高，能适用于多种绝对式光栅和增量式光栅的运动控制方法和装置。

发明内容

本发明的目的在于提出一种通用性强、实时性高、精度高的运动控制方法及装置。

一方面，本发明提供了一种运动控制方法，所述方法包括：

S1、第一信号转换器将外部编码器采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA解码器；

S2、FPGA解码器对所述单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器；

S3、ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器；所述复合控制算法由PID闭环控制、前馈控制构成；

S4、DA转换器将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器；

其中，所述ARM控制器装载有UCOSII操作系统。

优选的，在步骤S3中，所述ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，具体包括：

读取上位机发送的控制参数，并判断ARM控制器中的控制参数是否改变；若控制参数改变，则对ARM控制器重新进行初始化；若控制参数未改变或者在重新初始化之后，读取FPGA解码器解码得到的数据信号、并将其转换为角度数据。

优选的，在步骤S3中，所述主定时器模块根据预设的复合控制算法获取控制输出量，具体包括：

根据所述角度数据获取位置反馈数据，并对所述位置反馈数据进行一阶滤波获取速度反馈数据；

根据电机运行模式调用相应的轨迹规划函数，计算位置期望数据；其中，所述轨迹规划函数为梯形规划函数或三角形规划函数；

根据所述位置反馈数据、位置期望数据计算位置环偏差，并将所述位置环偏差输入PID位置闭环，以获取位置环PID控制量；

对所述位置环PID控制量进行二阶滤波，获取速度期望数据；

根据所述速度反馈数据、速度期望数据计算速度环偏差，并将所述速度环偏差输入PID速度闭环，以获取速度环PID控制量。

优选的，所述一阶滤波具体为：

$\mathrm{\Δ}v=\frac{s_4+s_1+3s_3+3s_2}{8};$

式中，Δv为速度反馈数据，s₁为当前时刻的角度增量，s₂为与s₁相邻的前一时刻的角度增量、s₃为与s₂相邻的前一时刻的角度增量、s₄为与s₃相邻的前一时刻的角度增量。

优选的，所述根据所述角度数据获取位置反馈数据，具体包括：

判断当前采集的角度数据是否正确；若当前采集的角度数据正确，则将错误计数清零，并将当前采集的角度数据作为当前的位置反馈数据；若当前采集的角度数据错误，则将错误计数加1，并且，在连续出错次数小于100次时，用前一时刻的位置反馈数据作为当前的位置反馈数据。

优选的，在当前采集的角度数据错误时，所述方法还包括：若连续出错次数大于等于100次，则令ARM控制器进入紧急停车模式，以使控制输出量为0。

优选的，在获取速度反馈数据之后，且在计算位置期望数据之前，所述方法还包括：根据所述速度反馈数据判断电机是否超速；若电机未超速，则将超速计数清零；若电机超速，则将超速计数加1；并且，在连续超速次数大于100次时，令ARM控制器进入紧急停车模式，以使控制输出量为0。

优选的，在根据预设的复合控制算法获取控制输出量之后，且在将所述控制输出量输出至DA转换器之前，所述方法还包括：

判断所述控制输出量是否超出预设的输出极限值；若所述控制输出量超出所述输出极限值，则将所述输出极限值输出至DA转换器；若所述控制输出量未超出所述输出极限值，则将控制输出量输出至DA转换器。

优选的，所述方法还包括：所述ARM控制器通过网络通信模块接收上位机发送的控制参数，以及向上位机返回ARM控制器的状态信息。

另一方面，本发明还提供了一种运动控制装置，所述装置包括：第一信号转换器、FGPA解码器、装载有UCOSII操作系统的ARM控制器、DA转换器；所述ARM控制器包括：主定时器模块、网络通信模块；

第一信号转换器用于将外部编码器采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA解码器；

FPGA解码器用于对所述单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器；

ARM控制器用于通过所述网络通信模块接收上位机发送的控制参数，以及向上位机返回ARM控制器的状态信息；ARM控制器还用于通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器；所述复合控制算法由PID闭环控制、前馈控制构成；

DA转换器用于将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器；

其中，所述ARM控制器为STM32芯片。

从以上技术方案可以看出，本发明的运动控制方法主要包括以下步骤：第一信号转换器将外部编码器采集的差分数据信号转换为单端数据信号，并将单端数据信号发送至FPGA解码器；FPGA解码器对单端数据信号进行解码，并将解码得到的数据信号发送至装载UCOSII操作系统的ARM控制器；ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将控制输出量输出至DA转换器；DA转换器对控制输出量进行数模转换，然后将其输出至电机驱动器。在本发明的运动控制方法中，通过采用差分输入方式采集编码器信号、并通过第一信号转换器将差分数据信号转换为单端数据信号，提高了控制方法的通用性；通过FPGA解码器进行数据解码、并通过载有实时操作系统的ARM控制器进行闭环控制，提高了控制方法的实时性。进一步的，本发明通过采用由PID闭环控制、前馈控制构成的复合控制算法，提高了控制方法的精度和动态响应性。

附图说明

通过以下参照附图而提供的具体实施方式部分，本发明的特征和优点将变得更加容易理解，在附图中：

图1是示出的本发明实施例的运动控制方法的流程示意图；

图2是图1中读取解码得到的数据信号以及控制参数的优选方式；

图3是图1中根据复合控制算法获取控制输出量的优选方式；

图4是图3中根据角度数据获取位置反馈数据的优选方式；

图5是根据图3中的速度反馈数据判断电机是否超速的步骤示意图；

图6是示出的本发明实施例的运动控制装置的结构组成示意图；

图7是图6所示的ARM控制器的结构组成示意图；

1、外部编码器；2、第一信号转换器；3、FPGA解码器；4、ARM控制器；5、DA转换器；6、上位机；7、电机驱动器；8、IO输入输出；9、LCD显示器；10、第二信号转换器；11、总线；401、网络通信模块；402、主定时器模块；403、初始化模块。

具体实施方式

下面参照附图对本发明的示例性实施方式进行详细描述。对示例性实施方式的描述仅仅是出于示范目的，而绝不是对本发明及其应用或用法的限制。

现有的运动控制器普遍存在以下三个问题：一是由于技术垄断、生产成本高，导致运动控制器的购买价格昂贵；二是由于控制器的结构设计、控制算法复杂，用户很难进行二次开发、修改；三是由于结构、控制算法复杂，导致控制器很难用于对实时性要求很高的系统，即不能对控制器的高精度与高实时性进行兼顾。

针对现有的运动控制器的缺陷，本发明提供了一种运动控制方法与装置，以提高运动控制方法与运动控制装置的通用性、实时性、控制精度，同时降低运动控制的成本。

下面结合附图1-5对本发明实施例中的运动控制方法进行详细说明。图1示出了本发明实施例的运动控制方法的流程示意图。从图1可见，该运动控制方法主要包括步骤S1～S4：

步骤S1、第一信号转换器将外部编码器采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA(现场可编程门阵列)解码器。

在该步骤中，通过设置第一信号转换器进行信号变换，便于采用差分输入方式进行数据采集，极大地提高了运动控制方法的通用性。

步骤S2、FPGA解码器对所述单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器。

在一个优选实施例中，FPGA解码器在对单端数据信号进行解码之后，将解码后的数据信号分为两路：一路数据信号发送至ARM控制器，另一路数据信号经第二信号转换器转变为差分信号形式后输出至外部设备。这样一来，既满足了运动控制的需求，又满足了将光栅信息传到外部设备的需求，极大提高了运动控制方法的通用性。

步骤S3、ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器。其中，所述ARM控制器装载有UCOSII操作系统。

在步骤S3中，所述ARM控制器读取所述解码得到的数据信号以及上位机发送的控制参数存在多种实施方式。图2示出了一种读取解码得到的数据信号以及控制参数的优选方式。由图2可见，该优选方式具体包括：首先，读取上位机发送的控制参数；然后，判断ARM控制器中的控制参数是否改变；若控制参数改变，则对ARM控制器重新进行初始化；若控制参数未改变或者在重新初始化之后，读取FPGA解码器解码得到的数据信号、并将其转换为角度数据。在具体实施时，ARM控制器可通过网络通信模块接收上位机发送的控制参数，以及通过网络通信模块向上位机返回ARM控制器的状态信息。

在图2所示的优选方式中，通过在进行运动控制之前判断ARM控制器中的控制参数是否改变，极大提高了运动控制方法的可靠性。

另外，在步骤S3中，所述主定时器模块根据预设的复合控制算法获取控制输出量也存在多种实施方式。图3示出了一种根据复合控制算法获取控制输出量的优选方式。从图3可见，该优选方式具体包括：步骤S321～步骤S325：

S321、根据所述角度数据获取位置反馈数据，并对所述位置反馈数据进行一阶滤波获取速度反馈数据。

在步骤S21中，根据所述角度数据获取位置反馈数据存在多种实施方式。图4示出了一种根据角度数据获取位置反馈数据的优选方式。从图4可见，该优选方式具体包括：判断当前采集的角度数据是否正确；若当前采集的角度数据正确，则将错误计数清零，并将当前采集的角度数据作为当前的位置反馈数据；若当前采集的角度数据错误，则将错误计数加1，并且，在连续出错次数小于100次时，用前一时刻的位置反馈数据作为当前的位置反馈数据。进一步的，若连续出错次数大于等于100次，则令ARM控制器进入紧急停车模式，以使控制输出量为0。在图4所示的优选方式中，通过对当前采集的角度数据进行判断，并在连续出错多次时使ARM控制器进入紧急停车模式，极大提高了运动控制的可靠性。

进一步的，在一个优选实施例中，所述一阶滤波具体为：

$\mathrm{\Δ}v=\frac{s_4+s_1+3s_3+3s_2}{8};$

式中，Δv为速度反馈数据，s₁为当前时刻的角度增量，s₂为与s₁相邻的前一时刻的角度增量、s₃为与s₂相邻的前一时刻的角度增量、s₄为与s₃相邻的前一时刻的角度增量。与现有的一阶滤波方法相比，该一阶滤波方法不仅简单高效、而且极大提高了运动控制方法的精度。

S322、根据电机运行模式调用相应的轨迹规划函数，计算位置期望数据，其中，所述轨迹规划函数为梯形规划函数或三角形规划函数。

在一个优选实施例中，在步骤S321获取速度反馈数据之后，且在步骤S322计算位置期望数据之前，所述方法还包括图5所示的步骤。由图5可见，该步骤具体为：根据所述速度反馈数据判断电机是否超速；若电机未超速，则将超速计数清零；若电机超速，则将超速计数加1；并且，在连续超速次数大于100次时，令ARM控制器进入紧急停车模式，以使控制输出量为0。在图5所示的步骤中，通过判断电机是否超速、并在连续超速多次时使ARM控制器进入紧急停车模式，极大提高了运动控制的可靠性、安全性。

S323、根据所述位置反馈数据、位置期望数据计算位置环偏差，并将所述位置环偏差输入PID位置闭环，以获取位置环PID控制量。

S324、对所述位置环PID控制量进行二阶滤波，获取速度期望数据。

S325、根据所述速度反馈数据、速度期望数据计算速度环偏差，并将所述速度环偏差、前馈控制输出值输入PID速度闭环，以获取速度环PID控制量。

进一步的，在一个优选实施例中，在根据图3所示的优选方式获取控制输出量以后，且在将所述控制输出量输出至DA转换器之前，该运动控制方法还包括：判断所述控制输出量是否超出预设的输出极限值；若所述控制输出量超出所述输出极限值，则将所述输出极限值输出至DA转换器；若所述控制输出量未超出所述输出极限值，则将控制输出量输出至DA转换器。通过以上步骤，能够有效防止由于控制输出量过大引起电机损坏的故障出现。

步骤S4、DA转换器将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器。

在本发明实施例的运动控制方法中，通过采用PID位置闭环、PID速度闭环，并采用路径规划算法，极大提高了运动控制的精度。进一步的，通过在速度环的输入中加入前馈控制，极大地提高了系统的动态性能。进一步的，通过在ARM控制器中装载实时操作系统UCOSII，极大提高了运动控制的实时性。另外，通过设置第一信号转换器，便于以差分方式进行数据采集，极大提高了运动控制的通用性。

下面结合图6和图7对本发明实施例的运动控制装置进行详细说明。图6示出了本发明实施例的运动控制装置的结构示意图。图7示出了本发明实施例中的ARM控制器的结构示意图。从图6可见，该运动控制装置主要包括：第一信号转换器2、FGPA解码器3、装载有UCOSII操作系统的ARM控制器4、DA转换器5。其中，ARM控制器4主要包括：主定时器模块402、网络通信模块401。另外，从图6可见，该运动控制装置还包括：IO输入输出8、LCD显示器9、总线11。其中，IO输入输出8包括：IO输入接口、IO输出接口、以及用于隔离IO输入接口与IO输出接口的光耦。LCD显示器9与ARM控制器4相连，用于显示ARM控制器的状态信息。总线11用于运动控制装置中多个部件的连接，比如ARM控制器与DA转换器，ARM控制器与IO输入输出8，等等。

第一信号转换器2用于将外部编码器1采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA解码器3。在具体实施时，第一信号转换器可优先选用DS26LS31芯片。在本发明实施例中，通过设置第一信号转换器，便于以差分方式对外部编码器的数据进行采集，极大提高了运动控制系统的通用性。

FPGA解码器3用于对所述单端数据信号进行解码，并将解码得到的数据信号通过并口发送至ARM控制器4。在具体实施时，FPGA解码器可优先选用EP2C5T144C8芯片，ARM控制器优先选取STM32芯片。

ARM控制器4用于通过网络通信模块401接收上位机发送的控制参数，以及向上位机返回ARM控制器的状态信息。ARM控制器4用于通过主定时器模块402读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器5。其中，所述复合控制算法由PID闭环控制、前馈控制构成。另外，从图6可见，ARM控制器4还包括：初始化模块403。初始化模块403用于对运动控制装置进行初始化，如对实时操作系统UCOSII进行初始化，对FPGA解码器进行初始化等等。在本发明实施例中，通过在ARM控制器中装载UCOSII实时操作系统，并在此基础上直接利用ARM控制器的32位的高精度定时器，以完成控制算法的周期性调用，从而极大提高了运动控制的实时性。

DA转换器5用于将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器7。

在一个优选实施例中，运动控制装置还包括：第二信号转换器10。在该优选实施例中，FPGA解码器3在对单端数据信号进行解码之后，将解码后的数据信号分为两路：一路数据信号发送至ARM控制器4，另一路数据信号经第二信号转换器10转变为差分信号形式后输出至外部设备。这样一来，既满足了运动控制的需求，又满足了将光栅信息传到外部设备的需求，极大提高了运动控制装置的通用性。

在本发明实施例的运动控制装置中，通过设置第一信号转换器，便于以差分方式进行数据采集，极大提高了运动控制的通用性。通过在ARM控制器中装载实时操作系统UCOSII，极大提高了运动控制的实时性。另外，通过将本发明的运动控制方法应用到运动控制装置中，极大提高了运动控制的精度以及系统的动态性能。

虽然参照示例性实施方式对本发明进行了描述，但是应当理解，本发明并不局限于文中详细描述和示出的具体实施方式，在不偏离权利要求书所限定的范围的情况下，本领域技术人员可以对所述示例性实施方式做出各种改变。

Claims (10)

1.一种运动控制方法，其特征在于，所述方法包括：

S1、第一信号转换器将外部编码器采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA解码器；

S2、FPGA解码器对所述单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器；

S3、ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器；所述复合控制算法由PID闭环控制、前馈控制构成；

S4、DA转换器将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器；

其中，所述ARM控制器装载有UCOSII操作系统。

2.如权利要求1所述的方法，其中，在步骤S3中，所述ARM控制器通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，具体包括：

读取上位机发送的控制参数，并判断ARM控制器中的控制参数是否改变；若控制参数改变，则对ARM控制器重新进行初始化；若控制参数未改变或者在重新初始化之后，读取FPGA解码器解码得到的数据信号、并将其转换为角度数据。

3.如权利要求2所述的方法，其中，在步骤S3中，所述主定时器模块根据预设的复合控制算法获取控制输出量，具体包括：

根据所述角度数据获取位置反馈数据，并对所述位置反馈数据进行一阶滤波获取速度反馈数据；

根据电机运行模式调用相应的轨迹规划函数，计算位置期望数据；其中，所述轨迹规划函数为梯形规划函数或三角形规划函数；

根据所述位置反馈数据、位置期望数据计算位置环偏差，并将所述位置环偏差输入PID位置闭环，以获取位置环PID控制量；

对所述位置环PID控制量进行二阶滤波，获取速度期望数据；

根据所述速度反馈数据、速度期望数据计算速度环偏差，并将所述速度环偏差输入PID速度闭环，以获取速度环PID控制量。

4.如权利要求3所述的方法，其中，所述一阶滤波具体为：

$\mathrm{\Δ}v=\frac{s_4+s_1+3s_3+3s_2}{8};$

式中，Δv为速度反馈数据，s₁为当前时刻的角度增量，s₂为与s₁相邻的前一时刻的角度增量、s₃为与s₂相邻的前一时刻的角度增量、s₄为与s₃相邻的前一时刻的角度增量。

5.如权利要求3所述的方法，其中，所述根据所述角度数据获取位置反馈数据，具体包括：

判断当前采集的角度数据是否正确；若当前采集的角度数据正确，则将错误计数清零，并将当前采集的角度数据作为当前的位置反馈数据；若当前采集的角度数据错误，则将错误计数加1，并且，在连续出错次数小于100次时，用前一时刻的位置反馈数据作为当前的位置反馈数据。

6.如权利要求5所述的方法，其中，在当前采集的角度数据错误时，所述方法还包括：

若连续出错次数大于等于100次，则令ARM控制器进入紧急停车模式，以使控制输出量为0。

7.如权利要求3所述的方法，其中，在获取速度反馈数据之后，且在计算位置期望数据之前，所述方法还包括：

根据所述速度反馈数据判断电机是否超速；若电机未超速，则将超速计数清零；若电机超速，则将超速计数加1；并且，在连续超速次数大于100次时，令ARM控制器进入紧急停车模式，以使控制输出量为0。

8.如权利要求3所述的方法，其中，在根据预设的复合控制算法获取控制输出量之后，且在将所述控制输出量输出至DA转换器之前，所述方法还包括：

判断所述控制输出量是否超出预设的输出极限值；若所述控制输出量超出所述输出极限值，则将所述输出极限值输出至DA转换器；若所述控制输出量未超出所述输出极限值，则将控制输出量输出至DA转换器。

9.如权利要求1-8任一所述的方法，其中，所述方法还包括：所述ARM控制器通过网络通信模块接收上位机发送的控制参数，以及向上位机返回ARM控制器的状态信息。

10.一种运动控制装置，其特征在于，所述装置包括：第一信号转换器、FGPA解码器、装载有UCOSII操作系统的ARM控制器、DA转换器；所述ARM控制器包括：主定时器模块、网络通信模块；

第一信号转换器用于将外部编码器采集的差分数据信号转换为单端数据信号，并将所述单端数据信号发送至FPGA解码器；

FPGA解码器用于对所述单端数据信号进行解码，并将解码得到的数据信号发送至ARM控制器；

ARM控制器用于通过所述网络通信模块接收上位机发送的控制参数，以及向上位机返回ARM控制器的状态信息；ARM控制器还用于通过主定时器模块读取所述解码得到的数据信号以及上位机发送的控制参数，并根据预设的复合控制算法获取控制输出量，并将所述控制输出量输出至DA转换器；所述复合控制算法由PID闭环控制、前馈控制构成；

DA转换器用于将所述控制输出量由数字量形式转变为模拟量形式，然后将其输出至电机驱动器；

其中，所述ARM控制器为STM32芯片。