从示例中学习(三)机械臂运动规划问题

从示例中学习

从示例中学习，即learn from demonstration(LFD)，是为了能让机器人处理未知环境或任务而出现的一类方法。对于很多机器人尤其是服务机器人，在实际中根据周围环境的不同，可能需要完成很各种各样的任务，而要将这些任务一一进行提前编程是不太现实的，因此人们希望可以开发一些方法或算法，让机器人出厂之后可以从人类或其他机器人的示范中学习学习各种各样的技能，实现处理复杂问题的能力。更宏观的背景可以参见这里。

LFD在机械臂运动规划中的应用

LFD在机械臂的运动规划中主要用来解决的是关节空间的约束。这之中往往会涉及到key-points\key-frame的概念。二者的motivation相同，即对于一条连续的轨迹，找到一些可以表征轨迹的特征点，这些特征点是综合考虑了执行器的位姿变化，速度变化与加速度的变化，通过一些分类算法得到的。

LFD的核心问题是：如何确定每个示例中的key-points，并综合使用多个示例中的key-points生成一条理想轨迹。解决这个问题一般没有理论上的方法，大多是一些trick，而且不同文章使用的trick不同。本文以《Trajectory Learning for Robot Programming by demonstration Using HMM and DTW》为例对其做个简要的介绍。
方法流程图：

要让机械臂学习操作一个工件时工件的位姿变化(因为得到工件的位姿后，可以用设定好的执行器与工件的坐标关系及逆运动学得到机械臂的姿态)。由人用手进行示范。设有$M$条示例轨迹，每条轨迹采样序列为 X m = { x 1 , x 2 , … , x N m } X_m=\{x_1,x_2,\dots,x_{N^m}\} Xm​={ x1​,x2​,…,xNm​}。最终我们将找到一系列key-points，这些key-points代表了在位姿、广义速度有较大变化的采样点。为了找到这些key-points，需要四个步骤：

1. 数据预处理
2. LBG部分
3. HMM部分
4. DTW部分

LBG对数据进行预先分类；HMM对数据进行进一步分类，得到每条轨迹的key-points：${\kappa }^{HMM}$，及每条轨迹的生成概率；DTW将多个示例的key-points进行时间上的标定，使它们具有统一的长度。利用DTW标定好的key-points我们可以进行进行带权插值，得到无噪音的轨迹。

数据预处理

对每一条轨迹
1.对 $x_i$做 $p$步平滑，生成速度 $v$(前 $p-1$个采样点速度设为和第 $p$个一样)
2 $x_i,v_i$组合成列向量 ${\mathbf{a}}_i=\left[\begin{array}{c}{x}_i\\ v_i\end{array}\right]$
3 坐标归一化，使单位统一。每个维度减去改维度均值除以标准差(注意是标准差不是方差)
4 归一化后的列向量 $a$在水平方向拼接成表征整个轨迹的数组 $X_m=[a_1,a_2,a_3,\dots ,a_{N^m}]$
5 将 $M$个示例的数组拼接成一个更长的数组 U = [ X 1 , X 2 , … , X M ] U=[X_1,X_2,\dots,X_M]