Dynamic Movement Primitives for matlab

by chauby Zou

2018.04.09

1. 简介

本代码主要是 Dynamic Movement Primitives(DMPs) 的matlab代码实现, DMPs又称为动态运动基元，基本思想是将运动过程分解为一系列的运动基元，通过运动基元的加权叠加来得到运动轨迹。

2. Dynamic System (动力系统、动态系统)

2.1 一阶动力系统

微分方程表示为 $$ \dot{x} = -\alpha x $$ 给定目标位置的微分方程表示为 $$ \begin{align} \dot{x} &= -\alpha (x-x_g)\\ &= \alpha (x_g - x) \end{align} $$ 其中，$\alpha$ 为衰减率（时间常数）, $x_g$ 为目标状态。
一阶动态系统的时间响应示意图如下

2.2 二阶动力系统

二阶动力系统其实就是弹簧-阻尼系统，示意图如下所示：

其微分方程表示为 $$ m\ddot{x} = -kx-c\dot{x} $$ 其中，$m$ 为模块的质量，$k$ 为弹簧系数，$c$ 为阻尼系数。

给定目标位置的微分方程推导过程为 $$ \begin{align} m\ddot{y} &= -ky-c\dot{y} \\ m\ddot{y} &= c(-\frac{k}{c}y-\dot{y}) \\ \tau\ddot{y} &= \alpha(-\beta y -\dot{y}) \\ \tau\ddot{y} &= \alpha(-\beta (y - y^g) -\dot{y}) \\ \tau\ddot{y} &= \alpha(\beta (y^g - y) -\dot{y}) \\ \end{align} $$

其中，$\alpha=c, \beta=\frac{k}{c}, m=\tau$, $y^g$ 为设定的目标状态。二阶动态系统的时间响应示意图如下所示：

其中，$\tau=1, \alpha=1,\beta=\frac{\alpha}{4}=0.25$, $y, dy, ddy$ 分别是曲线的位置、速度和加速度信息。

其中，$\tau=1$, $\alpha$ 和 $\beta$ 的值如图中所示。

3. Dynamic Movement Primitives

动态运动基元(Dynamic Movement Primitives, DMPs)是基于动态系统的一种轨迹调制方法，通过将动作分解为一系列的基元，并使用运动基元加权的方式来实现轨迹的拟合。DMP分为离散DMP(discrete DMP)和节律DMP（rhythmicity DMP）。

3.1 离散DMP

其微分方程可以表示为

$$ \tau \ddot{y} = \alpha(\beta(g-y) - \dot{y}) + f $$

其中 $f$ 是一个非线性函数，表示外界对于动态系统的一个非线性“力”， $f$ 可以表示为

$$ f(x, g) = \frac{\sum^{N}{i=1}\psi_i w_i}{\sum^{N}{i=1}\psi_i}x(g-y_0) $$

其中，$x$ 是一个典型动态系统(canonical dynamical system), 其微分方程可以表示为 $$ \dot{x} = -\alpha_x x $$

$y_0$ 表示系统的初始值。 $w_i$ 表示权值， $\psi_i$ 表示第 $i$ 个中心值在 $c_i$的高斯函数： $$ \psi_i=exp(-h_i(x-c_i)^2) $$

4. Application