DMP (Dynamic Movement Primitives) 动态运动基元

来源知乎文章Dynamic Movement Primitives介绍及Python实现与UR5机械臂仿真

1、DMP用处

DMP是一种用于轨迹模仿学习的方法，以其高度的非线性特性和高实时性，被应用到机器人的各个领域。

• 在机器人的规划控制中，我们需要事先规划参考轨迹，例如关节角度曲线，通过编程来规划参考轨迹就比较复杂，示教是一种比较简单直观的方法。
• DMP使用少量的参数来建模示教的轨迹，通过这些参数能够快速地复现示教轨迹。
• 在复现示教轨迹的时候能够增加一些任务参数来泛化和改变原始轨迹，例如改变关节角度曲线的幅值和频率。

2、DMP公式

到达目标点：具有自稳定性的二阶动态系统，最常用的二阶系统就是弹簧阻尼系统（PD控制器）
$$\ddot{y}={\alpha }_y({\beta }_y(g-y)-\dot{y})\text{\hspace{1em}(1)}$$

• $y$:系统状态（例如关节角度 [$\theta$] ）
• $g$:目标状态
• ${\alpha }_y$ 、${\beta }_y$ 是常数，相当于PD控制器中的P参数和D参数。

弹簧阻尼系统虽然能够让系统收敛到目标状态 ，却无法控制收敛的过程量，比如轨迹的形状。
DMP作者在这个PD控制器上叠加一个非线性项来控制收敛过程量。在直观上，可以把DMP看作是一个PD控制器与一个轨迹形状学习器的叠加：
$$\ddot{y}={\alpha }_y({\beta }_y(g-y)-\dot{y})+f\text{\hspace{1em}(2)}$$

• 右边的第一项就是PD控制器
• 第二项$f$就是轨迹形状学习器，是一个非线性函数

1）可以通过改变目标状态 [$g$] 和非线性项 [$f$] 来调整我们的轨迹终点和轨迹形状,这可以称之为空间上的改变。
2）如果还需要改变轨迹的速度，从而获得不同收敛速度的轨迹，这个可以通过在速度曲线 [$\dot{y}$] 上增加一个放缩项 [$\tau$] 来实现：
$${\tau }^2\ddot{y}={\alpha }_y({\beta }_y(g-y)-\tau \dot{y})+f\text{\hspace{1em}(3)}$$
上式即为DMP的基本公式。这个公式表明在这个动态的控制过程中，系统最后会收敛到状态 [$g$] ，并且非线性项 [$f$] 的参与会直接影响 [$\dot{y}$] ，也就是直接影响到收敛的过程。通过给定不同的 [$\tau$] 和 [$g$] ，我们就可以在示教曲线的基础上得到不同目标状态、不同速度的轨迹了。

• 叠加 [$f$] 的目的就是为了改变轨迹的形状为我们期望的形状
• [$\tau$]改变轨迹的速度

3、非线性函数$f$的选择

什么样的非线性函数可以很容易的拟合各种形状的轨迹，而且方便进行参数调整呢？

• 大佬作者的考虑，通过多个非线性基函数的归一化线性叠加来实现
  $$f(t)=\frac{{\sum }_{i=1}^N{\Psi }_i(t){\omega }_i}{{\sum }_{i=1}^N{\Psi }_i(t)}\text{\hspace{1em}(4)}$$
• 在原论文中，作者把 [$f$] 称之为forcing term，意为给PD控制系统增加的一个“外力”（为什么是力？因为 [$f$] 直接影响到轨迹的加速度项 [$\ddot{y}$] ，相当于是一个外力的作用），用这个力去改变轨迹的形状。
• [${\Psi }_i$] 基函数，一般使用的就是高斯基函数(径向基函数)
• [${\omega }_i$] 每个基函数对应的权重
• [$N$] 基函数的个数
• 通过给定不同的基函数和对应权重就可加权得到复杂的轨迹了。

为了保证系统收敛到目标状态 [$g$] 的时候， [$f$] 也收敛到 [$0$] ，从而不再对系统的控制过程产生影响， [$f$] 需要被重新定义。

4、$f$的叠加问题

非线性项 [$f$] 与时间 [$t$] 是高度相关的，无法与其他的动态系统直接叠加，也无法同时建模多个自由度的轨迹并让它们在时间上与控制系统保持同步。

• 离散型DMP（Discrete DMP）是解决空间轨迹规划的问题的
• 节律型DMP（Rhythmic DMP）是解决连续周期的节律型轨迹规划问题的
• 离散型DMP，论文使用了一个与时间无关的量 [$x$] 来替代时间 [$t$]
• 节律型DMP，论文使用了一个与时间无关的相位量 [$\varphi$] 来替代时间 [$t$]

5、离散型DMP

离散型DMP（Discrete DMP）是解决空间轨迹规划的问题的，主要的目的是为了解决笛卡尔空间的轨迹规划问题。
离散型DMP，论文使用了一个与时间无关的量 [$x$] 来替代时间 [$t$]，[$x$] 来自一个一阶系统(正则系统)，满足以下关系：
$$\tau \dot{x}=-{\alpha }_{x}x\text{\hspace{1em}(6)}$$

• [${\alpha }_x$]是一个常数
• [$\tau$] 是一个常数与DMP公式的 [$\tau$] 保持一致
• 在此系统中，不论给定的初值 [$x_0$] 是多少，该系统最终（当时间区域无穷的时候）都会单调地收敛到 [$x=0$] 状态。因此 [$x$] 可以看成一个相位变量，例如，当 [$x=1$] 的时候表示系统处于初始状态，当 [$x=0$] 的时候表示系统收敛到了目标状态。
• 实际编写程序的时候，常常将常数 [$\tau$] 移动到右侧来实现，以方便直接表示和计算 [$\dot{x}$] ，也与DMP公式中的 [$\tau$] 起同样的速度调制作用，即
  $$\tau \dot{x}=-{\alpha }_{x}x$$变成
  $$\dot{x}=-\tau {\alpha }_{x}x\text{\hspace{1em}(7)}$$
• 系统的两个参数[${\alpha }_x$] 和 [$\tau$] 将会影响到系统的收敛速度

为了保证系统收敛到目标状态 [$g$] 的时候， [$f$] 也收敛到 [$0$] ，从而不再对系统的控制过程产生影响， [$f$] 被重新定义:
$$f(t)=\frac{{\sum }_{i=1}^N{\Psi }_i(t){\omega }_i}{{\sum }_{i=1}^N{\Psi }_i(t)}x(g-y_0)\text{\hspace{1em}(8)}$$

• $y_0$表示起始状态 $y_0=y(t=0)$
• $x$可以保证$f$会随着$x$的收敛而消失
• $g-y_0$决定了$f$的“幅值”，用于后续对轨迹的形状进行“放缩”。
• 实际放缩的大小为$\frac{g_{new}-y_0}{g_0-y_0}$

基函数${\Psi }_i(x)$为径向基函数
$${\Psi }_i(x)=exp(-h_i(x-c_i{)}^2)=exp(-\frac{1}{2{\sigma }_i^2}(x-c_i{)}^2)\text{\hspace{1em}(9)}$$

• ${\sigma }_i$ 基函数${\Psi }_i$的宽度
• $c_i$ 基函数${\Psi }_i$的中心位置
• 这些参数都需要在模型的学习阶段通过示教轨迹得到的。
  
• 这些高斯基函数是随着正则系统的$x$分布的，如上图的第一张图所示，假设正则系统是线性收敛的，则 $x$的递减速度为恒定速度，那么高斯基函数的分布要覆盖从$x=1$开始到$x=0$的全部位置，其中心值应该尽可能覆盖均匀一点，这样可以在尽量广的空间上都能有足够的基函数用于拟合复杂的曲线。
• 第二张图中展示了给不同的高斯基函数分配的权值${\omega }_i$，这些权值与基函数${\Psi }_i$ 的乘积之和就是我们要的非线性项。而由于正则系统最终都能收敛到$0$，从而保证了随着时间的流逝，非线性项起的作用越来越小，系统最终会趋于稳定，而不会发散失控。
• 但实际上，正则系统并不是线性收敛的给定不同的${\alpha }_x$和$\tau$，可以得到不同收敛速度的正则系统，而且收敛速度都是由快到慢，在初始位置附近收敛速度非常快，而在目标位置附近收敛速度逐渐趋于平缓。如果我们仅仅是按照正则系统$x$的分布来均匀布置高斯基函数，那么从时间的维度上来看，我们就会得到类似如下的基函数分布：
  
• 在时间的维度上，大部分的高斯基函数都集中在初始时刻附近，而随着时间的进行，基函数所起的作用越来越小，这样会导致在曲线拟合的初始阶段效果很好，而在趋于目标位置的末段曲线的拟合效果变差，甚至是无法拟合。因此，我们需要改变一下策略，使得高斯基函数能够在时间的维度上也尽量分布均匀，保证整条曲线的拟合效果。
• 正则系统的收敛过程
  
• 如果要在时间上分布均匀，例如按照图中所示的红点分布，那么我们就应该选择这些时刻的 $x$ 值作为高斯基函数的中心位置 $c_i$，从而保证在时间的维度上高斯基函数是均匀分布的。
• 中心值确定了以后，接下来就是基函数的宽度（方差 ${\sigma }_i$ ）了，正则系统$x$的收敛速度是越来越慢的，那么如果我们把每个基函数的宽度在$x$的维度上设定为一样的，就会导致早期的基函数持续时间短，而后期的基函数持续时间长。因为时间越往后，经过同样宽度的$x$所需的时间越多，从而导致在时间的维度上出现基函数的宽度不一致的问题，时间约靠后的宽度越大。因此，基函数的宽度应该随着时间的递增而递减，从而保证在时间的维度上，每个基函数的宽度也是均匀的。
  
  $$h_i=\frac{B{F}_s}{c_i}\text{\hspace{1em}(10)}$$

其中， [$B{F}_s$] 表示基函数的数量， [$c_i$] 表示那个基函数的中心值。

注意：这里其实是有点问题的，例如对于轨迹的目标位置与起始位置都在同一个位置的情况来说$g-y_0=0$，那$f$就没有存在的意义了，因此，对于示教轨迹的起始点和目标点一致的情况这里不再适用，注意，对于三维空间中的轨迹来说，不是每个维度都要一致才叫一致，只要其中有任何一个维度是一致的，那么这个维度的 就会失效，我们就无法学习得到轨迹的形状，在使用的时候要注意这一点。经典DMP的问题及其解决办法

6、节律型DMP

• 节律型DMP（Rhythmic DMP）是解决连续周期的节律型轨迹规划问题的。例如关节空间的周期性关节角度规划问题。周期性曲线我们需要进行改变和泛化的往往是曲线的幅值与频率，从而获取不同的曲线来满足不同的任务需求。
• 节律型DMP的计算过程与周期型DMP一致。

6.1 不同点

• 所采用的正则系统（CS）不一样
• 所使用的基函数构造方法不一样
• 非线性项 $f$的构造方法稍有不同
  节律型DMP的$x(\varphi )$来自另一个正则系统，该系统收敛于一个极限环（Limit cycle），公式定义为
  KaTeX parse error: Undefined control sequence: \t at position 1: \̲t̲