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

从示例中学习

从示例中学习，即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,\dots ,x_{N^m}\}$。最终我们将找到一系列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,\dots ,X_M]$，代表所有的采样点

LBG部分

LBG是一种k-近邻方法，使用投票的方式对数据点在空间中进行分类，类的数目由人来拟定。该步输入预处理后的轨迹数组 $U$，对所有轨迹的所有数据点统一分类，输出每个数据点 $\mathbf{a}$所属的类，及每条轨迹初步的key-points： $K_m^{LBG}=\{{\kappa }_1^{LBG},{\kappa }_2^{LBG},\dots \}$。
对于某条轨迹，简图如下所示：

每一个新类的起点作为 ${\kappa }^{LBG}$。对于上图，为点1和4.

HMM部分

HMM将已知序列建模为对一系列隐变量状态的观测，观测序列的变化由隐变量的变化所驱动，观测量可理解为隐变量加入了噪声，如下图所示：

定义一个HMM需要
1.隐变量数目(不需确定隐变量具体值是什么，只需确定隐变量集合的大小)
2.观测量的取值集合，即codebook: $\{q_1,q_2,\dots ,q_Q\}$
3.序列长度
待辨识参数有三个
1. $A$: $s$的转移概率矩阵
2. $B$:已知状态 $P(o\mid s)$的概率矩阵
3. $\pi$:第一个状态 $s$的概率分布，是一个列向量
由于HMM的学习结果与初值有很大的关系，因此之前LBG的结果就是为了给HMM提供一些合理的初始化。利用HMM最终我们可以根据学习到的模型参数找到每条轨迹最有可能的key-points序列。
下面对HMM部分的操作步骤进行介绍：
1.参考序列的选取。目的是为了得到隐变量数目。设在LBG中，共分出 $Q$类，每类的均值为 ${\mu }_i$。找到一条序列 $X_{\delta }$，使 $\frac{{\sum }_{n=1}^{N_{\delta }}({\mathbf{a}}_{n,\delta }-\mu (a_{n,\delta }){)}^2}{N_{\delta }}$最小。
2.确定隐变量数目。数目为 $X_{\delta }$中 $K_{\delta }^{LBG}$的数目加1。每个隐变量 $s_i$代表 ${\kappa }_i$和 ${\kappa }_{i+1}$之间的某个位姿。
3.codebook的确定。为LBG输出的 $Q$个类关联上一个标签，可以是一串数字或一串字母。可以得到 $\{q_1,q_2,\dots ,q_Q\}$
4.观测序列的表示。对轨迹任意 $X_m$，采样点 ${\mathbf{a}}_{n,m}$属于哪个类 $i$，就将其表示 $o_{n,m}=q_i$。
5.转移矩阵 $A$的初始化。形式上满足如下形式。note：仅仅举个例子，并不代表状态只有5个 $$\left[\begin{array}{ccccc}{a}_{11}& a_{12}& a_{13}& 0& 0\\ 0& a_{22}& a_{23}& a_{24}& 0\\ 0& 0& a_{33}& a_{34}& a_{35}\\ ⋮& ⋮& ⋮& \ddots & \cdots \\ 0& 0& 0& 0& a_{55}\end{array}\right]$$
$a_{i,j}$初始值的设定，与隐变量 $s_i$的持续时间 ${\tau }_i$有关， ${\tau }_i$由 $s_i$两侧key-points: ${\kappa }_i^{LBG}$ ${\kappa }_{i+1}^{LBG}$时间差算得。如此初始化相当于加入了一定主观经验，即时空上离得越近的2个点，同时发生的可能性就越高。
6.前五步初始化操作结束后，学习模型参数 $\lambda =(A,B,\pi )$。现已得到 $M$条观测序列 $O_1,O_2,\dots ,O_M$，及初始化后的模型参数，利用Baum-Welch算法可以算得最终的模型参数 $\lambda$。至于如何利用多条轨迹学得一个参数，原文并没有提，但应该不难想到。这只是个trick的问题，解决方法不唯一。得到 $\lambda$后，对每个 $O_m$，使用Viterbi算法找到相应的可能性最大的隐变量序列 $s_1,s_2,\dots ,s_{n^m}$。利用隐变量序列，找到key-points序列 $K_m^{HMM}$。类似于 $K_m^{LBG}$的寻找，每当与 $o_{n,m}$相关联的 $s_n$变化时，就把当前这个 $o_{n,m}$所对应的 ${\mathbf{a}}_{n,m}$设为 ${\kappa }_{n,m}^{HMM}$

DTW部分

由于每条轨迹的key-points： $K_m^{HMM}$数目不同，总时间长度也不同，难以进行差值拟合，因此利用DTW将它们在时间上进行对应，输出在时间轴上标定好的 $K_m^{DTW}$。

关于插值：
当经过前述过程，得到最终key-points序列后，就基本大功告成了。后面只需要对key-points进行插值，生成一条连续的，可以送入执行器中的理想(去噪)轨迹。由于有多条key-points序列，原文中使用了一种“加权”插值方法，比较好理解，这里不详细讨论。另外关于插值次数，有很多文献表明，若要生成一条轨迹模拟人手的位姿变化，用五次或六次插值会比较理想。