经典DMP的问题及其解决办法
1. 经典DMP的问题
本文是上一篇文章[Dynamic Movement Primitives与UR5机械臂仿真]的续文，目的在于介绍经典DMP的问题及其解决办法。同样地，所有的源代码均开源，详见Github：
https://github.com/chauby/PyDMPs_Chauby
首先，我们还是从这个基本的DMP公式出发：
其中，非线性项
为：
其中，
是基函数，对于离散型的DMP来说是高斯基函数，对于节律型的DMP来说是冯米塞斯函数。
是对应基函数的权重。
来自于一个一阶系统：
其中，
是一个常数。
给定示教轨迹
，则我们可以求得需要拟合的非线性函数为：
构造损失函数：
求解损失函数可得到非线性项
的权重值。损失函数的求解过程，这里不再细讲，读者可以参考我们的上一篇文章[Dynamic Movement Primitives与UR5机械臂仿真]。
对于上面这个最原始的DMP来说，是有一些问题的，我们以离散型DMP为例，主要有以下2个问题：
- 从第一个DMP的公式们可以看到，如果示教轨迹的起始点
和目标位置
非常接近，那么
就趋于零，则
这一项也趋于零，
项本身是什么样已经不重要了，因此失去意义；
- 从
的计算公式可以看到，当
趋于零的时候，分母趋于零，我们无法计算
，也就会导致示教轨迹的形状无法被
建模。就算我们通过在分母上添加一个极小的值
来解决除数为零的情况，则一旦新的目标位置与示教的目标位置偏差一点就会让幅值产生极大的变化，甚至是放缩的符号相反导致轨迹形状复现出镜像效果，无法得到正确的放缩轨迹。
这里有一个简单的例子，对于一个周期的正弦曲线，其轨迹的起点和终点相同，DMP的建模结果如下图所示：
DMP_problem
图中，设定的目标位置为1e-15，与原始示教轨迹的目标位置
相比只变动了一点点，但是结果导致了复现的轨迹“镜像”且幅值放缩巨大。
这个问题最直接的后果就是经典的DMP无法拟合起点终点非常接近的曲线，对于多维度的曲线拟合来说，其中任何一个维度存在这个问题就会导致整条曲线拟合失真，轨迹无法被复现。例如，我们拟合一个机械臂从桌面拿起东西再放回桌面，那么示教轨迹在
轴方向的分量就是一个起点和终点非常接近的情况。
2. 改进型DMP
那么，有什么办法可解决这个问题呢？Stefan Schaal等人在其2008年的一篇论文（见文末的参考文献）中，给出了一种解决办法。既然在DMP的公式中，
无法直接使用了，那我们就想办法转变一下来避免这个问题。我们既要通过
来计算曲线的放缩“幅值”，便于适应不同的任务场合，又要使用
来建模曲线的“形状信息”，那么我们就想办法把两者分开，让他们分别独立起作用就好了。因此，在改进型的DMP中，我们把经典的DMP公式改写为：
其中，
是一个常数（这里为了与系统的参数保持一致，我们取
），
依旧使用原来的表达方式，
来自一阶系统。我们可以分开来理解，上面这个公式右边的第一项是PD控制器；第二项是轨迹形状拟合器，当
趋近于目标状态
的时候（
趋于零）这一项趋于零；第三项是幅值放缩器，通过
来计算幅值的放缩量，同样地，当
趋于零的时候这一项趋于零，趋于稳定。
这样，给定示教轨迹的时候，
就重新定义为：
其中，DMP在学习示教轨迹的时候不需要进行时间放缩，因此
，使用与经典DMP同样的方式来构造损失函数，求解过程与之前保持一致。
现在，我们来看看实验结果：
Modified_DMP
图中的两条示教曲线都是起点和终点重合的轨迹，改进型的DMP可以完美的学习得到轨迹的参数，并且对于给定的新的目标位置也能精确地复现轨迹去达到要求。
参考文献
- 2008-Movement reproduction and obstacle avoidance with dynamic movement primitives and potential fields | IEEE Conference Publication | IEEE Xplore
上述参考文献也已经放进了源代码的paper路径下。如果看完了觉得对你有帮助，欢迎点赞和转发分享。