Dynamic Movement Primitives与UR5机械臂仿真

Dynamic Movement Primitives （DMP），中文译名为动态运动基元、动态运动原语等，最初是由南加州大学的Stefan Schaal教授团队在2002年提出来的，是一种用于轨迹模仿学习的方法，以其高度的非线性特性和高实时性，被应用到机器人的各个领域。时间已经过去了20年，DMP在机器人的规划控制领域也得到了长足的发展和应用，其优点得到了大家的肯定，其缺点也在逐渐被大家发现和解决。

我当年在学习DMP的过程中看论文也是看得一头雾水，后面结合论文看了加拿大滑铁卢大学的博士生Travis DeWolf（现在已经是博后了）的博文以后才算是比较理解DMP，因此在这里把所看的东西结合个人的理解做一个总结，文章有点长，其中夹杂了个人的理解和思考过程。

本文旨在从应用的角度出发，给初学者一种更加直观的方式学习DMP，尽量避免使用复杂的术语来阐述这个方法，也不会从理论上去严格证明该方法，需要了解的读者可以看原论文，在文末有参考文献列表。本文中介绍的DMP会使用Python代码实现，并在CoppeliaSim（VREP）中使用UR5机械臂来完成部分应用的仿真，所有的代码开源在Github上，地址为：https://github.com/chauby/PyDMPs_Chauby

---

1. DMP的基本概念

在机器人的规划控制中，我们需要事先规划参考轨迹，例如关节角度曲线、机械臂末端轨迹等，如果任务参数比较复杂多变，那么通过编程来规划参考轨迹就比较复杂了，而示教是一种比较简单直观的方法，我们可以让有经验的人带着机器人先完成一次任务，然后让机器人自动学习其中的过程，从而省去编程的复杂过程。我们希望能有一种方法，能够使用少量的参数来建模示教的轨迹，通过这些参数能够快速地复现示教轨迹，同时，还希望在复现示教轨迹的时候能够增加一些任务参数来泛化和改变原始轨迹，例如改变关节角度曲线的幅值和频率，改变机械臂末端轨迹的起始和目标位置等等，这样才能更加灵活的使用示教方法来完成实际任务。

论文作者最基本的出发点是想利用一个具有自稳定性的二阶动态系统来构造一个“吸引点”模型（attractor model），使得可以通过改变这个“吸引点”来改变系统的最终状态，从而达到修改轨迹目标位置的目的，而最简单最常用的二阶系统就是弹簧阻尼系统（PD控制器）：

其中， 表示系统状态（例如关节角度 ）， 和 分别表示 的一阶导数和二阶导数。 表示目标状态，最后系统会收敛到这个状态上。 和 是两个常数，相当于PD控制器中的P参数和D参数。

这里我们来看一下PD控制器的收敛过程，参数合适的话大致会得到如下的收敛过程：

PID

使用弹簧阻尼系统虽然能够让系统收敛到目标状态 ，却无法控制收敛的过程量，比如轨迹的形状。那么作者就想在这个PD控制器上叠加一个非线性项来控制收敛过程量，因此，在直观上，我们可以把DMP看作是一个PD控制器与一个轨迹形状学习器的叠加：

其中，上式右边的第一项就是PD控制器，而第二项 就是轨迹形状学习器，是一个非线性函数。这样，我们就可以通过改变目标状态 和非线性项 来调整我们的轨迹终点和轨迹形状了，这可以称之为空间上的改变。但还不够，我们还需要改变轨迹的速度，从而获得不同收敛速度的轨迹，这个可以通过在速度曲线 上增加一个放缩项 来实现：

上式即为DMP的基本公式了（由于 ，因此，上式左边的 的系数是 ），在后面的核心过程都是基于这个DMP公式。从直观上来看，我们可以知道，上面这个公式表明在这个动态的控制过程中，系统最后会收敛到状态 ，并且非线性项 的参与会直接影响 ，也就是直接影响到收敛的过程。通过给定不同的 和 ，我们就可以在示教曲线的基础上得到不同目标状态以及不同速度的轨迹了。

那么，这个非线性项 具体怎么构造呢？要知道，我们叠加 的目的就是为了改变轨迹的形状为我们期望的形状，那么，什么样的非线性函数可以很容易的拟合各种形状的轨迹，而且方便我们进行参数的调整呢？原大佬作者是这样考虑的，通过多个非线性基函数的归一化线性叠加来实现，因此，非线性函数