机械臂示教轨迹参数化方法 DMP, Dynamic Movement Primitive (一)

很早之前就想写一栏关于DMP的博客笔记了，拖到现在一来是对这个东西了解得不够，二来是没有找到合适的时间静下心来写。最近看到了一篇2021发表，总结了DMP这项技术的综述( Dynamic Movement Primitives in Robotics: A Tutorial Survey, https://arxiv.org/abs/2102.03861)，内容详尽，脉络清晰，于是在其中挑了两三篇内容，写一个简单的博客。

1. 关于DMP

DMP展开翻译过来就是动态运动原语的意思，以人类伸手去抓取杯子喝水为例子，这里的原语primitives的意思是运动的单元，把一项任务的运动轨迹切割成多条子运动轨迹，如伸手到达杯子的位置，抓取杯子，再到把杯子放到口中，倒水，放回杯子。这些子的运动轨迹可以再继续划分分成更细的运动单元。而DMP要解决的问题就是，人工示教某项任务之后，如何将获取的示教轨迹（任务空间中的一系列离散点）参数化，使得这条示教轨迹能够用少量的参数表示，同时具有一定的泛化能力（即，能够随意调整轨迹的目标点，自动生成新的轨迹）以及抗干扰能力（能够稳定渐进地收敛到目标点且速度和加速度不会出现不连续现象）。

2. 香草味的DMP

初代DMP，我自己给它起了个名字，叫Vanilla DMP， Vanilla是香草的意思，也有原味的意思，用它来修饰DMP有。DMP的雏形出现在这篇文章中 Dynamic Movement Primitives–A Framework for Motor Control in Humans and Humanoid Robotics， 后来逐渐发展成下面的形式
τ 2 y ¨ = α y ( β y ( g − y ) − τ y ˙ ) + f ( x , g ) τ x ˙ = − α x x ,       x ( 0 ) = 1 f ( x , g ) = ∑ i = 0 N ψ i ( x ) ω i ∑ i = 0 N ψ i ( x ) x ( g − y 0 )

τ2y¨​τx˙f(x,g)​=αy​(βy​(g−y)−τy˙​)+f(x,g)=−αx​x,     x(0)=1=∑i=0N​ψi​(x)∑i=0N​ψi​(x)ωi​​x(g−y0​)​
其中$f$被称为forcing term，也是DMP模型的灵魂所在。如果我们令$e:=g-y,\tau =1$，且$g$为常数，展开第一条公式，可得
e ¨ = − α y ( β y e + e ˙ ) − f ( x , g ) = − α y β y e − α y e ˙ − f ( x , g )

e¨=−αy(βye+e˙)−f(x,g)=−αyβye−αye˙−f(x,g)" role="presentation" style="position: relative;">e¨=−αy(βye+e˙)−f(x,g)=−αyβye−αye˙−f(x,g)$$\begin{array}{rl}\ddot{e}& =-{\alpha }_y({\beta }_{y}e+\dot{e})-f(x,g)\\ & =-{\alpha }_y{\beta }_{y}e-{\alpha }_y\dot{e}-f(x,g)\end{array}$$

e¨​=−αy​(βy​e+e˙)−f(x,g)=−αy​βy​e−αy​e˙−f(x,g)​
因此整个DMP动态方程最终其实就是一个质量为1，刚度为${\alpha }_y{\beta }_y$, 阻尼系数为${\alpha }_y$的质量弹簧阻尼系统，随着时间的推移，x将会趋向0，$f$项也随之趋于零，最终$y$向目标值收敛。
$$\ddot{e}+{\alpha }_y\dot{e}+{\alpha }_y{\beta }_{y}e=-f(x,g)$$
其中forcing term 的存在就是为了改变系统收敛过程中的轨迹的状态的，这条轨迹的形状将拟合示教时的轨迹，达到轨迹复现的目的。

这个模型有很大的缺点，在主要集中在forcing term中，如果初始点与目标点重合，难么forcing term将十始终为0，无法学习到给定的示教轨迹。即使手动设置一定的偏差，改变该模型的目标后，其泛化出来的轨迹与给定的示教轨迹在形状上有很大的偏差。因此需要对forcing term进行修改。

3. 改进型DMP

Biologically-inspired dynamical systems for movement generation: Automatic real-time
goal adaptation and obstacle avoidance 2009年，提出了新的改性后的DMP模型，基于观察青蛙的运动轨迹特征。抽象化后总结出运动是在运动过程中按顺序激活力场，受力场牵引的运动。这里的力场由使用加速度场来模拟，
a i ( y , v ) = K ( ω i − y ) − D v a ′ ( y , v ) = ∑ i = 0 N ψ i ( x ) a i ( y , v ) ∑ i = 0 N ψ i ( x ) = ∑ i = 0 N ψ i ( x ) ( K ( ω i − y ) − D v ) ∑ i = 0 N ψ i ( x ) = K ( ∑ i = 0 N ψ i ( x ) ω i ∑ i = 0 N ψ i ( x ) − y ) − D v

ai​(y,v)a′(y,v)​=K(ωi​−y)−Dv=∑i=0N​ψi​(x)∑i=0N​ψi​(x)ai​(y,v)​=∑i=0N​ψi​(x)∑i=0N​ψi​(x)(K(ωi​−y)−Dv)​=K(∑i=0N​ψi​(x)∑i=0N​ψi​(x)ωi​​−y)−Dv​
接着我们再设置一个目标力场，期望运动轨迹最终收敛到目标点中
$$a_g(y,v)=K(g-y)-Dv$$
最终的加速度场为待学习的力场与目标力场的线性叠加，且令初始时刻，系统加速度在初始位置时变化较少，在待学习力场中插入了初始位置作为偏置项
a ( y , v ) = x a ′ ( y , v ) + ( 1 − x ) a g ( y , v ) = x K ( ∑ i = 0 N ψ i ( x ) ω i ∑ i = 0 N ψ i ( x ) + y 0 − y ) + ( 1 − x ) K ( g − y ) − D v

a(y,v)=xa′(y,v)+(1−x)ag(y,v)=xK(∑i=0Nψi(x)ωi∑i=0Nψi(x)+y0−y)+(1−x)K(g−y)−Dv" role="presentation" style="position: relative;">a(y,v)=xa′(y,v)+(1−x)ag(y,v)=xK(∑Ni=0ψi(x)ωi∑Ni=0ψi(x)+y0−y)+(1−x)K(g−y)−Dv$$\begin{array}{rl}a(y,v)& =x{a}^{\mathrm{\prime }}(y,v)+(1-x)a_g(y,v)\\ & =xK\left(\frac{\sum _{i=0}^N{\psi }_i(x){\omega }_i}{\sum _{i=0}^N{\psi }_i(x)}+y_0-y\right)+(1-x)K(g-y)-Dv\end{array}$$

a(y,v)​=xa′(y,v)+(1−x)ag​(y,v)=xK(∑i=0N​ψi​(x)∑i=0N​ψi​(x)ωi​​+y0​−y)+(1−x)K(g−y)−Dv​
整理后可得
$$\ddot{y}=K(g-y)-Dv-K(g-y_0)x+Kf(x)$$
其中x为时钟信号，即Canonical system的信号。从左到右可以分别理解为与目标值相关刚度项，与速度相关的阻尼项，与目标值和初始值相关的偏置项，以及最后需要学习的与示教轨迹相关的forcing term。这种改进后的模型具有的一个优势是，当目标位置与初始位置重合的时候，并不会影响轨迹的参数化，改进后的DMP模型依旧能够生成示教的轨迹。而关于权值的计算，则是使用了高斯混合模型中的LWR，local weighted regression 方法。

4. 考虑Orientation的DMP

早期的DMP大多都是拟合笛卡尔空间中的position，而比较少考虑orientation的问题，比如机械臂在执行倒水动作的时候就需要考虑末端的方向问题。对示教轨迹中的方向信号进行拟合与位置信号的参数化是不一样的，因为方向的表示本身就有很多种形式，如欧拉角，旋转矩阵，轴角，单位四元数等形式，这些形式各有各的优缺点，除表示参数量最少的欧拉角外，其他表示方法都有约束条件，如旋转矩阵的各行相互垂直，各列相互垂直，单位四元数的范数为1等，如何保证参数化后的表达式能够满足这些约束条件呢？

总结

一般的DMP是按照单个维度进行拟合的，如果涉及到位姿表示的方向问题，我们需要查看更多的文章，拟合的时候也需要多个维度同时拟合以满足方向的表达满足的约束条件，如四元数的范数为1。其他研究内容还有：

• 多条DMP轨迹拼接
• DMP架构的小改进，DMP的时间信号，Canonical System的设计，forcing term的建模方式和高斯混合模型中核函数的选择等
• DMP的速度曲线的拟合和调整
• 多条示教轨迹情况下怎么处理等

以上都是DMP已经研究过和正在研究的，仅作为一个引子引起大家的兴趣，如果阅读量足够多，是大家比较关注的内容的话，会更新DMP的教程。

祝周末愉快！

2022年8月27日
Dianye Huang