运动控制方法及装置、运动轨迹规划方法与流程

本发明涉及运动控制领域，特别是涉及一种运动控制方法及装置、运动轨迹规划方法。

背景技术：

在电机伺服系统中，一般把位置作为控制目标,利用当前位置和目标位置的差值为输入量，采用比例-积分-导数(proportion-integral-derivative，PID)的控制方式，实现电压输出脉冲宽度调制(Pulse Width Modulation，PWM)的占空比控制。由于系统参数变化和外部干扰的存在,PID控制方法往往使系统产生较大的超调量甚至不稳定。

技术实现要素：

本发明主要解决的技术问题是提供一种运动控制方法及装置、运动轨迹规划方法，能够解决现有技术中PID控制不稳定的问题。

为了解决上述技术问题，本发明采用的一个技术方案是：提供一种运动控制方法，包括：获取控制对象在当前时刻的当前位置及当前速度；根据当前位置及当前速度计算当前加速度，当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关；利用当前加速度控制驱动控制对象的电机；重复执行上述步骤直至控制对象到达目标位置。

其中，根据当前位置及当前速度计算当前加速度包括：当前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)为加速度，K为调节系数，s₀为目标位置，s(·)为位置，v(·)为速度，t为当前时刻。

其中，利用当前加速度控制驱动控制对象的电机包括：利用当前加速度及当前速度计算下一时刻速度v(t+1)＝v(t)+h*a(t)，其中t+1为下一时刻，h为迭代步长，即下一时刻与当前时刻之间的时间间隔；根据下一时刻速度控制电机。

其中，获取控制对象在当前时刻的当前位置及当前速度包括：利用位置传感器获取控制对象的当前位置，至少利用当前位置及前一时刻的位置进行微分以获取当前速度。

为了解决上述技术问题，本发明采用的另一个技术方案是：提供一种运动轨迹规划方法，包括：获取当前时刻的当前位置及当前速度；根据当前位置及当前速度计算当前加速度，其中当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关；利用当前位置及当前速度计算下一时刻位置，利用当前加速度及当前速度计算下一时刻速度，下一时刻与当前时刻的时间间隔为迭代步长；重复执行上述步骤直至到达目标位置，以获取运动轨迹。

其中，根据当前位置及当前速度计算当前加速度包括：当前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)为加速度，K为调节系数，s(·)为位置，v(·)为速度，t为当前时刻。

其中，利用当前位置及当前速度计算下一时刻位置包括：下一时刻位置s(t+1)＝s(t)+h*v(t)，其中h为迭代步长；利用当前加速度及当前速度计算下一时刻速度包括：下一时刻速度v(t+1)＝v(t)+h*a(t)。

其中，运动轨迹包括位置轨迹和速度轨迹，其中速度轨迹连续不出现跳变，且在起始位置和目标位置处速度均为0。

为了解决上述技术问题，本发明采用的另一个技术方案是：提供一种运动控制装置，包括：处理器、传感器和驱动器，处理器连接传感器及驱动器；处理器用于通过传感器获取控制对象在当前时刻的当前位置及当前速度；根据当前位置及当前速度计算当前加速度，当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关；利用当前加速度通过驱动器控制驱动控制对象的电机；重复执行上述步骤直至控制对象到达目标位置。

其中，处理器用于根据当前位置及当前速度计算当前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)为加速度，K为调节系数，s₀为目标位置，s(·)为位置，v(·)为速度，t为当前时刻；利用当前加速度及当前速度计算下一时刻速度v(t+1)＝v(t)+h*a(t)，其中t+1为下一时刻，h为迭代步长，即下一时刻与当前时刻之间的时间间隔；向驱动器输出下一时刻速度以控制电机。

本发明的有益效果是：利用当前位置及当前速度计算当前加速度，并利用当前加速度控制电机，可以实现电机的平稳加速和平稳减速，轨迹平滑，震动较小，没有超调，稳定性更好。

附图说明

图1是本发明运动控制方法第一实施例的流程图；

图2是本发明运动轨迹规划方法第一实施例的流程图；

图3是本发明运动轨迹规划方法第一实施例一个例子中位置轨迹的仿真图；

图4是本发明运动轨迹规划方法第一实施例一个例子中速度轨迹的仿真图；

图5是本发明运动控制装置第一实施例的结构示意图。

具体实施方式

如图1所示，本发明运动控制方法第一实施例包括：

S11：获取控制对象在当前时刻的当前位置及当前速度。

一般利用传感器获取当前位置及当前速度。由于速度是位置对时间的微分，加速度是速度对时间的微分，可以使用位置传感器、速度传感器、加速度传感器中的至少一种，结合必要的微分和/或积分运算来获取当前位置及当前速度。

例如，可以只利用位置传感器直接获取当前位置，由于速度是位置对时间的微分，当迭代步长，即两次测量之间的时间间隔足够小时，对速度公式进行离散化，则当前速度可以表示为当前位置与前一时刻位置之差除以迭代步长。也可以只利用加速度传感器获取加速度，对加速度进行积分得到当前速度，对速度进行积分得到当前位置。以此类推，可以得到其他情况。

S12：根据当前位置及当前速度计算当前加速度。

当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关，使得控制对象的运动速度连续不出现跳变，在整个运动过程的初期逐渐加速，后期逐渐减速，符合正常运动习惯。当前位置与目标位置之差可以是指两者之间的直线距离，也可以是指两者之间的运动轨迹长度。

在本发明一个实施例中，当前加速度表示为：

a(t)＝K²(s₀-s(t))-2Kv(t) (1)

其中a(·)为加速度，K为调节系数，s₀为目标位置，s(·)为位置，v(·)为速度，t为当前时刻，且控制对象在起始位置和目标位置处速度均为0。调整K的大小可以改变加速度的大小，从而改变加减速的快慢进而改变运动时间。

S13：利用当前加速度控制驱动控制对象的电机。

可以直接根据当前加速度来控制电机，即将当前加速度输出至电机的驱动器。

也可以利用当前加速度计算及当前速度计算得到下一时刻速度：

v(t+1)＝v(t)+h*a(t) (2)

其中v(·)为速度，a(·)为加速度，t为当前时刻，t+1为下一时刻，h为迭代步长，将下一时刻速度输出至电机的驱动器以控制电机。电机的驱动器可以根据输入量来调节电机的电压大小、频率等参数，从而调节电机的转速。调节h的大小也可以改变速度变化的剧烈程度，h越大，速度变化越剧烈。

S14：判断控制对象是否到达目标位置。

一般将控制对象的当前位置与目前位置进行比较，若两者完全相同或者误差小于预设阈值，则判定控制对象到达目标位置。

若是，则结束流程；若否，则跳转至步骤S11以重复执行上述步骤。

通过上述实施例的实施，利用当前位置及当前速度计算当前加速度，并利用当前加速度控制电机，可以实现电机的平稳加速和平稳减速，轨迹平滑，震动较小，没有超调，稳定性更好。

如图2所示，本发明运动轨迹规划方法第一实施例包括：

S21：获取当前时刻的当前位置及当前速度。

S22：根据当前位置及当前速度计算当前加速度。

当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关，使得控制对象的运动速度连续不出现跳变，在整个运动过程的初期逐渐加速，后期逐渐减速，符合正常运动习惯。当前位置与目标位置之差可以是指两者之间的直线距离，也可以是指两者之间的运动轨迹长度。

在本实施例中，当前加速度表示为：

a(t)＝K²(s₀-s(t))-2Kv(t) (1)

其中a(·)为加速度，K为调节系数，s₀为目标位置，s(·)为位置，v(·)为速度，t为当前时刻，且控制对象在起始位置和目标位置处速度均为0。调整K的大小可以改变加速度的大小，从而改变加减速的快慢进而改变运动时间。在其他实施例中，加速度也可以用其他公式表示。

S23：利用当前位置及当前速度计算下一时刻位置，利用当前加速度及当前速度计算下一时刻速度。

速度为位置对时间的微分，进行离散化之后代入当前位置和当前速度当前加速度可得下一时刻位置：

s(t+1)＝s(t)+h*v(t) (3)

其中s(·)为位置，t为当前时刻，t+1为下一时刻，h为迭代步长，即下一时刻与当前时刻的时间间隔。

加速度为速度对时间的微分，进行离散化之后代入当前速度和当前加速度可得下一时刻速度：

v(t+1)＝v(t)+h*a(t)＝v(t)+h*(K²(s₀-s(t))-2Kv(t)) (4)

其中v(·)为速度，a(·)为加速度，t为当前时刻，t+1为下一时刻，h为迭代步长，即下一时刻与当前时刻的时间间隔。

本步骤计算得到的下一时刻速度和下一时刻位置可以作为下一循环中步骤S21获取的速度和位置。

S24：判断是否到达目标位置。

若是，则获取运动轨迹，结束流程；若否，则跳转至步骤S21以重复执行上述步骤。运动轨迹包括位置轨迹和速度轨迹，其中速度轨迹连续不出现跳变，且在起始位置和目标位置处速度均为0。

举例说明，取初始位置为0，目标位置为5米，初始速度为0，在2.5kHz频率控制系统下,取迭代步长＝0.0004秒,调节系数K取2，通过仿真得到位置轨迹如图3所示，速度轨迹如图4所示。从图中可以看出，位置轨迹和速度轨迹均比较平滑，效率高,耗能少。

如图5所示，本发明运动控制装置第一实施例包括：处理器110、传感器120和驱动器130，处理器110分别连接传感器120及驱动器130。

处理器110控制运动控制装置的操作，处理器110还可以称为CPU(Central Processing Unit，中央处理单元)。处理器110可能是一种集成电路芯片，具有信号的处理能力。处理器110还可以是通用处理器、数字信号处理器(DSP)、专用集成电路(ASIC)、现成可编程门阵列(FPGA)或者其他可编程逻辑器件、分立门或者晶体管逻辑器件、分立硬件组件。通用处理器可以是微处理器或者该处理器也可以是任何常规的处理器等。

传感器120用于检测控制对象和/或驱动控制对象的电机的参数。

驱动器130用于控制电机，可以根据输入量来调节电机的电压大小、频率等参数，从而调节电机的转速。

运动控制装置可以进一步包括存储器(图中未画出)，存储器用于存储处理器110工作所必需的指令及数据。

在某些实施例中，处理器110和驱动器130可以集成在一起。

处理器110用于通过传感器120获取控制对象在当前时刻的当前位置及当前速度；根据当前位置及当前速度计算当前加速度，当前加速度与当前位置与目标位置之差正相关，且与当前速度负相关；利用当前加速度通过驱动器130控制驱动控制对象的电机；重复执行上述步骤直至控制对象到达目标位置。

可选的，处理器用于根据当前位置及当前速度计算当前加速度a(t)＝K²(s₀-s(t))-2Kv(t)，其中a(·)为加速度，K为调节系数，s₀为目标位置，s(·)为位置，v(·)为速度，t为当前时刻；利用当前加速度及当前速度计算下一时刻速度v(t+1)＝v(t)+h*a(t)，其中t+1为下一时刻，h为迭代步长，即下一时刻与当前时刻之间的时间间隔；向驱动器输出下一时刻速度以控制电机。调节系数和目标位置可以是存储在存储器中的，也可以是通过输入模块(未画出)，例如键盘、触摸屏，麦克风等获取的，当然也可以是通过通信电路从其他装置获取的。

本发明运动控制装置各实施例中各部分的具体功能可参考本发明运动控制方法实施例中的对应描述，在此不再重复。

在本发明所提供的几个实施例中，应该理解到，所揭露的运动控制装置和方法，可以通过其它的方式实现。例如，以上所描述的运动控制装置实施方式仅仅是示意性的，例如，所述模块或单元的划分，仅仅为一种逻辑功能划分，实际实现时可以有另外的划分方式，例如多个单元或组件可以结合或者可以集成到另一个系统，或一些特征可以忽略，或不执行。另一点，所显示或讨论的相互之间的耦合或直接耦合或通信连接可以是通过一些接口，装置或单元的间接耦合或通信连接，可以是电性，机械或其它的形式。

所述作为分离部件说明的单元可以是或者也可以不是物理上分开的，作为单元显示的部件可以是或者也可以不是物理单元，即可以位于一个地方，或者也可以分布到多个网络单元上。可以根据实际的需要选择其中的部分或者全部单元来实现本实施方式方案的目的。

另外，在本发明各个实施例中的各功能单元可以集成在一个处理单元中，也可以是各个单元单独物理存在，也可以两个或两个以上单元集成在一个单元中。上述集成的单元既可以采用硬件的形式实现，也可以采用软件功能单元的形式实现。

所述集成的单元如果以软件功能单元的形式实现并作为独立的产品销售或使用时，可以存储在一个计算机可读取存储介质中。基于这样的理解，本发明的技术方案本质上或者说对现有技术做出贡献的部分或者该技术方案的全部或部分可以以软件产品的形式体现出来，该计算机软件产品存储在一个存储介质中，包括若干指令用以使得一台计算机设备(可以是个人计算机，服务器，或者网络设备等)或处理器(processor)执行本发明各个实施方式所述方法的全部或部分步骤。而前述的存储介质包括：U盘、移动硬盘、只读存储器(ROM，Read-Only Memory)、随机存取存储器(RAM，Random Access Memory)、磁碟或者光盘等各种可以存储程序代码的介质。

以上所述仅为本发明的实施方式，并非因此限制本发明的专利范围，凡是利用本发明说明书及附图内容所作的等效结构或等效流程变换，或直接或间接运用在其他相关的技术领域，均同理包括在本发明的专利保护范围内。