添加了原始和修改DMP模型

v1是消除镜像、目标不变无f等
v2是添加速度跟踪
v3是加速点收敛
usc是最初的DMP