Motion Planning Networks: Bridging the Gap Between Learning-Based and Classical Motion Planners阅读记录

Introduction

通过环境信息，如原始点云，初始位置以及目标位置。通过递归调用本身产生双向的可连接的路径。并且可以混合神经网络与传统的基于采样的规划算法，同时能够保证计算性能以及最优性。

作为一个神经网络算法，其训练策略如下：

1. 可以离线批处理数据进行学习，该过程假定所有数据可用，
2. 利用记忆进行持续学习。假定专家演示过程流形式进入，同时全局训练数据分布未知
3. active continual learning 整合了MPNet到学习过程中，并且只在需要的时候去询问专家演示

文章贡献主要有：解决了专家系统以及规划是需要流数据的，而深度学习不能用流数据，采取了情景记忆以及在必要情况下询问专家系统；提升了训练效率；有比较低的计算复杂度，平均计算时间小于1 s；可以和RRT算法结合；能够扩展到训练集外的环境（但是动态环境没有提到）；可以和其他经典算法结合；
Section Ⅱ 介绍了一些经典算法

基于采样的路径规划算法RRT及其衍生算法。

RRT是单一查询方法，而PRM是综合查询方法，更多是用单一查询，因为综合查询可以分解成多个单一查询。
RRT算法能够比较快速的找到一条可行路径，但是其找到的很难是最短路径。其衍生算法RRT*则保证了能够找到最短路径，但是RRT*随着维度的增加。计算效率会降低。在高维空间中并不是一个好的搜索方法。

在基于采样的规划算法中加入启发式。有biased sampling启发自适应式采样。如在RRT\8引入人工势场。还有IRRT*在RRT*产生的路径基础上，定义了一个椭圆区域，在此区域进行自适应采样。虽然计算性能提升，但是在寻找初始路径时还是采用的RRT*。还有BIT*算法。但是在高维表现还是不够好。(没找到这个算法)

lazy edge evaluation 方法提升了计算速度，但是依赖于底层的边缘检测器，在混乱环境表现差。Bidirectional path generation提升了在狭窄通道性能，但是还是存在基于采样本身的问题。

还介绍了强化学习，其在解决连续控制和规划问题也成为重要工具。并且可以加入神经网络，即Deep Reinforcement Learning。但是在解决weak rewards 和 long horizons问题时仍有挑战。
还有应用一些学习策略的方法，虽然和传统算法相比提升了一些性能，但是都缺乏通用性，并且在新环境下都需要hand-engineering。因此采用有效的函数近似器，如Neural Net，去嵌入规划器，或者在采样中去学习辅助函数如采样启发式。 还提到了一些其他的神经运动规划算法。 与本文最相关的算法是L2RRT算法。

MPNet

Planner

MPNet 由Enet和Pnet组成》Enet的输入是原始点云或者是点云转换成的体元素，取决于神经网络结构。输出是嵌入了给定信息的潜在空间。Pnet对当前环境，机器人状态，目标状态进行编码，输出用于规划或者生成树的样本。

机器人构型空间$C\subset R^d$ 如下：C为当前空间，$C_{obs}$|是障碍空间，$C_{free}$无障碍空间，d是维度。
机器人周围环境$\chi \subset R^m$,相关符号意义同上，m是工作空间维度。虽然机器人构造空间可能不仅限于三维，是根据自由度确定。但是工作空间最大就是三维。

σ = { c 0 , c 1 . . . c T } \sigma =\{c_0, c_1 ... c_T\} σ={c0​,c1​...cT​}是一个有序的列表，${\sigma }_i$代表第i个的状态。运动规划问题就表述为在 { c i n i t , c g o a l , C o b s } \{ c_init, c_goal, C_{obs}\} {ci​nit,cg​oal,Cobs​}中，一个连接init和goal状态并不与$C_{obs}$发生碰撞的可行路径。实际中，$C_{obs}$是未知的，采用碰撞检测器对工作空间信息，${\chi }_{obs}$,以及机器人构型决定了是否碰撞。
规划的另外一个重要问题是给定代价函数后最优路径解决。代价函数记为$J(\cdot )$。最优问题变为使代价函数最小。
假定有一个碰撞检测器可以验证用MPNet生成路径的可行性。
Enet，的输入是环境信息$x_{obs}$，以及参数${\theta }^e$。输出一个潜在空间Z。Pnet，输入为Z，机器人当前或者是初始状态$c_t\in C_{free}$，目标点机器人状态，通过增量状态产生轨迹。


整个架构如下：

Training

这一块讲了三种不同的训练策略：offline batch learning, continual learning, ACL(active continual learning)。
offline batch learning 方法假设在线运行MPNet以针对未知的或新的规划问题规划运动之前有完整的数据进行离线训练。continual learning使MPNet能够从流数据中学习而不会忘记过去的经验。ACL将MPNet整合到continual learning过程中，当给定的问题需要时，MPNet会主动请求专家演示。

offline batch learning

此种方法训练需要提前将所有的训练数据提供给MPNet。Enet和Pnet既可以一起用Pnet的损失函数进行端到端的方式训练，也可以各自分开用自己的损失函数进行训练。
对Enet单独进行训练，要有足够数量的环境点云数据可供无监督学习，我们就使用一个编码器-解码器架构。文章用的是 contractive autoencoders （CAE）。
其重构损失以及正则化编码器参数如：

其中${\theta }^e$ ${\theta }^d$为编码器和解码器参数，$\hat{x}$为重构的点云，$\lambda$为正则化系数，$D_{obs}$就是训练集障碍点云(障碍物才存在点云)，$N_{obs}\in N$为点云来自的工作空间。（这里不明白不同的工作空间是啥意思）

Pnet是一个随机的前馈神经网络，给定专家演示序列点为 σ = { c 0 , c 1 , . . . c T } \sigma = \{c_0, c_1,...c_T\} σ={c0​,c1​,...cT​}。在无障碍的连接起点和终点构型。为了进行单独的Pnet训练或者是联合Enet一起训练，考虑了one-step look ahead prediction strategy。Pnet将Z，current state $c_t$,目标状态$c_T$，为输入，输出下一个朝向目标区域的状态${\hat{c}}_{t+1}$，损失函数为预测的下一步状态和目标下一步状态的均方差。如下：

其中$T_j$是第j条轨迹序列的长度，$\hat{N}$为多少条轨迹序列。$N_P$为这些序列的平均长度。除了采用均方差损失函数，还可采用对抗性损失函数。在刚体运动规划如SE(3)中，会涉及到旋转用四元数表示的。则采用如下的损失函数：

Continual Learning

在continual learning设置下，MPNet的两个模块(Enet和Pnet)都是端到端的训练方式，因为这两个神经模型都需要适应传入的数据[图2(a)]。我们考虑成一个监督学习问题。因此，数据是有目标的，即：
$$(s_1,y_1...s_i,y_i,...s_N,y_N)$$
其中$s=(c_t,c_T,x_{obs})$为输入，目标y是在专家演示中的下一个状态$c_{t+1}$。利用神经网络进行持续学习通常由于每一步的梯度会产生灾难性的遗忘问题。然后文章解释了一下为啥。是由于当新一次准确率比上一次低的时候，会产生负向的迁移导致模型退化，从而引起遗忘。
灾难性遗忘问题
为了解决这个问题，文章使用了Gradient episodic memory方法。（2017年提出的一个解决神经网络中的灾难性遗忘的方法）对GEM简单的一个介绍
大概意思就是会看一部分之前训练的数据。对于MPNet，使用以下优化目标函数将GEM用于回归问题:

其中$l(\cdot )$如下：

也就是在 t 时刻训练的模型。此外，如果在t时刻的梯度(g)与在训练集M中的梯度之间的夹角是正的，即（g, g(M)）≥0，则不需要保持旧的函数参数$f_{\theta }^t$

$\hat{E}$为过去经验平均。
在大多数情况下，都不满足（g, g(M)）≥0约束，更新g会造成之前数据损失的增加。因此将g投影到最近的梯度 g’ 使得满足大于0。这块不明白，前边说的是大于0就不用更新了，在这却是更新？？

基于对偶原理的二次规划(QP)可以有效地求解梯度 g 到 g’ 的投影问题。

采样策略对episodic memory很重要，也有很多种方法，文章采取的是global distribution matching也被称为水塘抽样。
除了episodic memory,还加入了replay/rehearsal memory机制$B^{\ast }$。表现更好了。重播周期为$r\in N_{\ge 0}$
continual learning 框架如下：

Active continual Learning （ACL）

这个方法是实用的，并且只有在有需要的时候才调用专业演示，它建立在上文continual learning 框架基础之上。
ACL引入了两个级别的样本选择策略。首先，ACL通过主动询问有关MPNet未能找到路径的问题的演示来收集训练数据。其次，它采用了一个样本选择策略，进一步删减专家演示以填充情景记忆，使其接近流数据的分布。ACL中的两级数据选择提高了MPNet广义神经模型学习的训练样本效率。

框架如上图。在调用MPNet之前，先用专家演示进行学习。如果MPNet调用失败，则利用专家规划器进行规划。然后把规划出的轨迹放到重播缓冲区$B^{\ast }$以及情景记忆$M$中。再进行训练

Online Path Planning

在Pnet中，几乎每一层都用了Dropout。从而产生了随机性，这种随机性有助于路径规划以及产生SMP所需的样本。并且迭代用Pnet，使得路径最终到达目标点。

path planning with MPnet

MPNet的路径规划使用了Enet和Pnet以及我们的迭代、递归和双向规划算法。我们的规划策略有以下突出特点。
Forward–backward Bidirectional planning：算法是双向的，既从起始点到目标点，也后反向从目标到起始，直到相遇。使用了贪婪RRT-Connect
Recursive divide-and-conquer planning：首先进行全局规划，如图三(a)。产生关键的无碰撞状态，如果全局中有不能连接的点，如图三(b)和©,则把其当做起始和目标点再次规划(递归思想？)。因此，MPNet将给定的问题分解成子问题，并在这些子问题上递归地执行，最终找到路径解决方案。

一些基本组件有：Bidirectional Neural Planner (BNP)；Replanning；Lazy States Contraction (LSC)；Steering (SteerTo)；isFeasible；

BNP

这个算法的框架如图：

输入和Pnet的输入一样，为环境信息，起始点，目标点构型。双向路径即为：start到goal的${\sigma }_a$以及从goal到start的${\sigma }_b$。在第一行对${\sigma }_a$和${\sigma }_b$初进行始化。分别为$c_{init}$和$c_{goal}$.

我们以交替的方式扩展路径，即，如果在任意迭代i中，一条路径被${\sigma }_a$扩展，那么在下一次迭代中，一条路径${\sigma }_b$将被扩展，这是通过在每次迭代结束时交换${\sigma }_a$和${\sigma }_b$的角色来实现的(第10行)。此外，在每一个展开步骤之后，规划者尝试用一条直线连接两条路径，如果可能的话。我们使用steerTo来执行直线连接，这使得我们的连接变得启发式贪婪。因此，与RRT-Connect[57]一样，我们的方法在每次路径展开后都尽量将两条路径连接起来。如果两条路径${\sigma }_a$和${\sigma }_b$都是可连接的，BNP返回一个连接的路径σ，它由${\sigma }_a$和${\sigma }_b$的状态组成(第8行和第9行)

replanning

重规划针对的是第一次产生的$\sigma$连接不可通的问题，也就是图三(a)中的问题。在两个角点处直接连接会碰到障碍。如果可以在obstacle-free中连接，就不用replanning。重规划算法流程如下：

考虑有BNP给出的轨迹 σ = { c 0 , c 1 , . . . c T } \sigma = \{ c_0, c_1, ... c_T\} σ={c0​,c1​,...cT​}。然后用steerTo函数，迭代每个${\sigma }_i$和 ${\sigma }_1$的连接状态，是否存在无碰撞的轨迹。如果不存在无碰撞轨迹，则需要重规划。重规划时则用无法连接的两个节点作为起始点和目标点。重规划方法文章提出了两种。
Neural replanning(NP)：利用BNP进行有限次数尝试规划以找的两点之间的路径。
Hybrid replanning：结合了NP和oracle planner(没找到这个方法)，如果NP在尝试次数内没有找到解决方案，则利用oracle planner去寻找。

Lazy States Contraction (LSC)

也就是去除一些冗余的点，增加平滑性。把LSC看做一个递归的函数，应用在 σ = { c 0 , c 1 , . . . c T } \sigma = \{ c_0, c_1, ... c_T\} σ={c0​,c1​,...cT​}中，把能够连接的非连续状态${\sigma }_i$和${\sigma }_{>i+1}$进行连接。如图三(d)所示：

这种处理提高了计算效率。在规划中会处理更少的点。

steerTo

检测两个点连接之间是否会有碰撞，通过离散两点之间的直线完成。$\sigma (\delta )=(1-\delta )c_1+\delta c_2$ 。

is Feasible

利用steerTo函数检测规划出的路径$\sigma$是否都无碰撞。

MPNetPath的执行流程图如下：

Informed Sampling with MPNet

这块是把PNet当做经典算法基于采样的算法中的样本生成器。由于Pnet中每一层都引入了dropout，因此其本身具有随机性。融合了MPNet采样以及经典SMP算法的算法流程如下：

先利用BNP迭代$N_{smp}$次数产生样本，然后切换到随机采样，最后利用底层的SMP根据样本建立tree,由于起始采样是有目标点的采样，因此树刚开始生长的方向也偏向于有解的子空间。在一定迭代次数后，树开始均匀的展开。一旦找到目标点，就返回或者是继续进行有代价函数的优化。
并且能够为双向SMP方法生成样本，因为Pnet就是在双向的产生样本。如果这样的话，算法6则修改为：第2行初始化两个随机变量$c_{randS}$和$c_{randG}$，分别具有起始状态和目标状态。然后，使用$c_{randG}$而不是$c_{goal}$，并在每次迭代结束时交换$c_{randS}$和$c_{randG}$的角色。这种对狭窄环境的解决比较好。

Results

文章后面就是结果演示，和RRT*, IRRT*, BIT*三种算法的对比。以及本身采取不同的训练方式：Offline batch learning，continual learning, active continual learning对比。还有在replanning上选择NP还是HP方法对比。实验环境包括了二维，三维以及7自由度机械臂规划问题。
还有讨论了一些组件选择，讨论MPNet的完整性保证

总结

总体来看，这篇文章主要在于采用深度学习，并且能够结合传统算法，但是传统算法仅限于基于采样的规划算法，本文的MPNet作为样本生成器，然后结合采样算法中生成树等方法。也可以仅用深度学习。类似于是一个启发式，还有就是相对来说具有通用性，可以解决从二维到三维的规划，以及机械臂的规划。而且算作是一个全局路径规划，这样的话，和自己想的在动态环境中规划效果应该不会太好。规划时间平均小于1s。

其模式也是端到端的训练方法，采集数据包括环境信息，机器人当前状态等。