Motion Planning Networks: Bridging the Gap Between Learning-based and Classical Motion Planners

TL;DR

This paper introduces MPNet, a neural network-based motion planner that efficiently generates near-optimal paths in complex environments, combining learning and classical methods for improved performance and theoretical guarantees.

Contribution

The paper presents MPNet, a novel neural network-based motion planning approach with active learning, and demonstrates its integration with classical planners for theoretical guarantees.

Findings

MPNet outperforms state-of-the-art planners in various environments.

Active continual learning reduces training data requirements.

Hybrid approach provides theoretical guarantees while maintaining efficiency.

Abstract

This paper describes Motion Planning Networks (MPNet), a computationally efficient, learning-based neural planner for solving motion planning problems. MPNet uses neural networks to learn general near-optimal heuristics for path planning in seen and unseen environments. It takes environment information such as raw point-cloud from depth sensors, as well as a robot's initial and desired goal configurations and recursively calls itself to bidirectionally generate connectable paths. In addition to finding directly connectable and near-optimal paths in a single pass, we show that worst-case theoretical guarantees can be proven if we merge this neural network strategy with classical sample-based planners in a hybrid approach while still retaining significant computational and optimality improvements. To train the MPNet models, we present an active continual learning approach that enables MPNet to learn from streaming data and actively ask for expert demonstrations when needed, drastically reducing data for training. We validate MPNet against gold-standard and state-of-the-art planning methods in a variety of problems from 2D to 7D robot configuration spaces in challenging and cluttered environments, with results showing significant and consistently stronger performance metrics, and motivating neural planning in general as a modern strategy for solving motion planning problems efficiently.

Figures

Tables

Table 1. TABLE I : Mean computation times with standard deviations are presented for MPNet (all variations), Informed-RRT* and BIT* on two test datasets, i.e., seen and unseen (shown inside brackets), in four different environments. In all cases, MPNet path planners, MPnetPath:NP (with neural replanning) and MPnetPath:HP (with hybrid replanning), and sampler MPNetSMP with underlying RRT*, trained with continual learning (C), active continual learning (AC) and offline batch learning (B), performed significantly better than classical planners such as Informed-RRT* and BIT* by an order of magnitude.

Methods	Environments
	Simple 2D	Complex 2D	Complex 3D	Rigid-body-SE2
Informed-RRT*	$1.06\pm 0.33$ $\left(1.10\pm 0.09\right)$	$1.60\pm 0.47$ $\left(1.49\pm 0.16\right)$	$2.99\pm 0.82$ $\left(2.76\pm 0.20\right)$	$15.58\pm 2.85$ $\left(14.80\pm 2.83\right)$
BIT*	$0.59\pm 0.28$ $\left(0.65\pm 0.30\right)$	$1.79\pm 1.35$ $\left(1.61\pm 0.53\right)$	$0.19\pm 0.12$ $\left(0.20\pm 0.04\right)$	$7.16\pm 1.95$ $\left(6.52\pm 1.65\right)$
MPNetSMP (B)	$0.13\pm 0.01$ $\left(0.13\pm 0.00\right)$	$0.29\pm 0.05$ $\left(0.23\pm 0.07\right)$	$0.25\pm 0.05$ $\left(0.23\pm 0.06\right)$	$0.49\pm 0.05$ $\left(0.39\pm 0.04\right)$
MPNetPath:NP (C)	$0.02\pm 0.00$ $\left(0.02\pm 0.00\right)$	$0.05\pm 0.01$ $\left(0.05\pm 0.01\right)$	$0.07\pm 0.01$ $\left(0.08\pm 0.01\right)$	$0.41\pm 0.08$ $\left(0.39\pm 0.07\right)$
MPNetPath:NP (AC)	$0.03\pm 0.01$ $\left(0.03\pm 0.01\right)$	$0.06\pm 0.01$ $\left(0.06\pm 0.01\right)$	$0.08\pm 0.01$ $\left(0.08\pm 0.01\right)$	$0.53\pm 0.12$ $\left(0.42\pm 0.08\right)$
MPNetPath:NP (B)	$0.02\pm 0.00$ $\left(0.02\pm 0.00\right)$	$0.04\pm 0.00$ $\left(0.04\pm 0.01\right)$	$0.06\pm 0.01$ $\left(0.07\pm 0.01\right)$	$0.38\pm 0.04$ $\left(0.37\pm 0.02\right)$
MPNetPath:HP (B)	$0.04\pm 0.03$ $\left(0.07\pm 0.04\right)$	$0.13\pm 0.07$ $\left(0.14\pm 0.09\right)$	$0.09\pm 0.03$ $\left(0.12\pm 0.04\right)$	$0.42\pm 0.30$ $\left(0.41\pm 0.40\right)$

Table 2. TABLE II : Success rates of all MPNet variants in the four environments on both test datasets, seen and unseen (shown inside brackets).

Methods	Environments
	Simple 2D	Complex 2D	Complex 3D	Rigid-body-SE2
MPNetPath:NP (C)	$93.33$ $\left(93.18\right)$	$83.44$ $\left(83.78\right)$	$89.88$ $\left(90.86\right)$	$83.770$ $\left(86.18\right)$
MPNetPath:NP (AC)	$96.70$ $\left(97.83\right)$	$84.36$ $\left(84.08\right)$	$96.60$ $\left(95.28\right)$	$87.08$ $\left(87.64\right)$
MPNetPath:NP (B)	$99.30$ $\left(98.30\right)$	$99.71$ $\left(98.80\right)$	$99.11$ $\left(97.76\right)$	$94.21$ $\left(95.18\right)$
MPNetPath:HP (B)	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$
MPNetSMP	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$	$100.0$ $\left(100.0\right)$

Table 3. TABLE III : Computation time, path cost/length, and success rate comparison of MPNetPath (with neural replanning) trained with offline batch (B) and continual (C) learning against BIT* on Baxter test dataset. In case of BIT*, the times for finding the first path and further optimizing it to a path cost within 40% range of MPNet’s path cost are reported. Overall, MPNet exhibits superior performance than BIT* in terms of path costs, mean computation times, and success rates.

Methods	Baxter
	Time	Path cost	Success rate$(\%)$
BIT* (Initial Path)	$0.94\pm 0.20$	$13.91\pm 0.60$	$83.0$
BIT* ($\pm 40\%$ MPNet cost)	$9.20\pm 7.61$	$10.78\pm 0.31$	$56.0$
MPNetPath (C)	$0.81\pm 0.08$	$6.98\pm 0.18$	$78.6$
MPNetPath (B)	$0.59\pm 0.08$	$7.86\pm 0.20$	$87.8$

Equations

$$Z\leftarrow \mathrm{Enet}(x_{\mathrm{obs}};{\theta }^e)$$

$${\hat{c}}_{t+1}\leftarrow \mathrm{Pnet}(Z,c_t,c_{\mathrm{goal}};{\theta }^p)$$

$$\vspace*{-0.1in}l_{\mathrm{AE}}\big{(}\theta^{e},\theta^{d}\big{)}=\cfrac{1}{N_{\mathrm{obs}}}\sum_{x\in D_{\mathrm{obs}}}||x-\hat{x}||^{2}+\lambda\sum_{ij}(\theta^{e}_{ij})^{2}$$

$$\text{vspace}\ast -0.1in{l}_{\mathrm{Pnet}}(\mathbf{\theta })=\frac{1}{N_p}\sum _j^{\hat{N}}\sum _{i=0}^{T_j-1}||{\hat{c}}_{j,i+1}-c_{j,i+1}|{|}^2,$$

$$\text{vspace}\ast -0.1in{l}_{\mathrm{Pnet}}(\mathbf{\theta })=l_p+\beta l_q,$$

$$\text{vspace}\ast -0.1in{l}_{\mathrm{q}}=\frac{1}{N_p}\sum _j^{\hat{N}}\sum _{i=0}^{T-1}{\Vert \frac{{\hat{q}}_{j,i+1}}{||{\hat{q}}_{j,i+1}||}-q_{j,i+1}\Vert }^2$$

$$(s_1,y_1,\cdots ,s_i,y_i,\cdots ,s_N,y_N)$$

$$\begin{gathered} \underset{\theta }{\min }l(f_{\theta }^t(s),y)\text{ s.t } \\ {\hat{\mathbb{E}}}_{(s,y)\sim \mathcal{M}}[l(f_{\theta }^t(s),y)]\le {\hat{\mathbb{E}}}_{(s,y)\sim \mathcal{M}}[l(f_{\theta }^{t-1}(s),y)] \end{gathered}$$

$$\langle g,g_{\mathcal{M}}\rangle:=\bigg{\langle}\bigtriangledown_{\theta}l\big{(}f_{\theta}(s),y\big{)},\hat{\mathbb{E}}_{(s,y)\sim\mathcal{M}}\bigtriangledown_{\theta}l\big{(}f_{\theta}(s),y\big{)}\bigg{\rangle}$$

$$\underset{g^{\prime }}{\min }\frac{1}{2}\Vert g-g^{\prime }{\Vert }_2^2\text{ s.t }⟨g^{\prime },g_{\mathcal{M}}⟩\ge 0$$

$$\underset{n\to \infty }{\lim }\mathbb{P}({\mathcal{T}}_n^{RR{T}^{\ast }}\cap c_{\mathrm{goal}}\ne \varnothing )=1$$

$$\underset{n\to \infty }{\lim }\mathbb{P}({\mathcal{T}}_n^{\mathrm{MPNetSMP}}\cap c_{\mathrm{goal}}\ne \varnothing )=1$$

Full text

I Introduction

Motion planning is among the core research problems in robotics and artificial intelligence. It aims to find a collision-free, low-cost path connecting a start and goal configuration for an agent [4] [5]. An ideal motion planning algorithm for solving real-world problems should offer following key features: (i) completeness and optimality guarantees — implying that a solution will be found if one exists and that the solution will be globally optimal, (ii) computational efficiency — finding a solution in either real-time or in sub-second times or better while being memory efficient, and (iii) insensitivity to environment complexity — the algorithm is effective and efficient in finding solutions regardless of the constraints of the environment. Decades of research have produced many significant milestones for motion planning include resolution-complete planners such artificial potential fields [6], sample-based motion planners such as Rapidly-Exploring Random Trees (RRT) [4] and its optimal variant RRT* [1], heuristically biased solvers such as [3] and lazy search methods [7]. However, each planner and their variants have tradeoffs amongst the ideal features of motion planners. Thus, no single motion planner has emerged above all others to solve a broad range of problems.

A recent research wave has led to the cross-fertilization of motion planning and machine learning to solve planning problems. Motion planning and machine learning for control are both well established and active research areas where huge progress has been made. In our initial work in motion planning with neural networks [8] [9], we highlighted that merging both fields holds a great potential to build motion planning methods with all key features of an ideal planner ranging from theoretical guarantees to computational efficiency. In this paper, we formally present Motion Planning Networks, or MPNet, its features corresponding to an ideal planner and its merits in solving complex robotic motion planning problems.

MPNet is a deep neural network-based bidirectional iterative planning algorithm that comprises two modules, an encoder network and planning network. The encoder network takes the environment information, such as the raw or voxelized output of a depth camera or LIDAR, and embeds them into a latent space. The planning network takes the environment encoding, robot’s current and goal state, and outputs a next state of the robot that would lead it closer to the goal region. MPNet can very effectively generate steps from start to goal that are likely to be part of the optimal solution with minimal-to-no branching required. Being a neural network approach, we also propose three learning strategies to train MPNet: i) offline batch learning which assumes the availability of all training data, ii) continual learning with episodic memory which assumes that the expert demonstrations come in streams and the global training data distribution is unknown, and iii) active continual learning that incorporates MPNet into the learning process and asks for expert demonstrations only when needed. The following are the major contributions of MPNet:

• •

MPNet can learn from streaming data which is crucial for real-world scenarios in which the expert demonstrations usually come in streams, such as in semi self-driving cars. However, as MPNet uses deep neural networks, it can suffer from catastrophic forgetting when given the data in streams. To retain MPNet prior knowledge, we use a continual learning approach based on episodic memory and constraint optimization.

• •

The active continual learning approach that asks for demonstrations only when needed, hence improving the overall training data efficiency. This strategy is in response to practical and data-efficient learning where planning problems come in streams, and MPNet attempts to plan a motion for them. In case MPNet fails to find a path for a given problem, only then an Oracle Planner is called to provide an expert demonstration for learning.

• •

MPNet plans paths with a low computational complexity and exhibits a mean computation time of less than 1 second in all presented experiments.

• •

MPNet can generate informed samples, thanks to its stochastic planning network, for sampling-based motion planners such as RRT* without incurring any additional computational load. The MPNet informed sampling based RRT* exhibits mean computation time of less than a second while ensuring asymptotic optimality and completeness guarantees.

• •

A hybrid planning approach that combines MPNet with classical planners to provide worst-case guarantees of our approach. MPNet plans motion through divide-and-conquer since it first outputs a set of critical states and recursively finds paths between them. Therefore, it is straightforward to outsource a segment of a planning problem to a classical planner, if needed, while retaining the computational benefits of MPNet.

• •

MPNet generalizes to similar but unseen environments that were not in the training examples.

We organize the remaining paper as follows. Section II provides a thorough literature review of classical and learning-based planning methods. Section III describes notations required to outline MPNet, and our propositions highlighting the key features of our method. Section IV presents approaches to train the neural models whereas Section V outlines our novel neural-network-based iterative planning algorithms. Section VI presents results followed by Section VII that provides discussion and theoretical analysis concerning worst-cases guarantees for MPNet. Section VIII concludes the paper with consideration of future avenues of development.

II Related Work

The quest for solving the motion planning problem originated with the development of complete algorithms which suffer from computational inefficiency, and thus led to more efficient methods with resolution- and probabilistic- completeness [4]. The algorithms with full completeness guarantees [10] [11] find a path solution, if one exists, in a finite-time. However, these methods are computationally intractable for most practical applications as they require the complete environment information such as obstacle geometry which is not usually available in real-world scenarios [12]. The algorithms with resolution-completeness [6] [13] also find a path, if one exists, in a finite-time, but require tedious fine-tuning of resolution parameters for every given planning problem. To overcome the limitations of complete and resolution-complete algorithms, the probabilistically complete methods, also known as Sampling-based Motion Planners (SMPs), were introduced [4]. These methods rely on sampling techniques to generate rapidly-exploring trees or roadmaps in robot’s obstacle-free state-space. The feasible path is determined by querying the generated graph through shortest path-finding methods such as Dijkstra’s method. These methods are probabilistically complete, since the probability of finding a path solution, if one exists, approaches one as the number of samples in the graph approaches infinity [4].

The prominent and widely used SMPs include single-query rapidly-exploring random trees (RRT) [14] and multi-query probabilistic roadmaps (PRM) [15]. In practice, single-query methods are preferred since most multi-query problems can be solved as a series of single-query problems [14] [1]. Besides, PRM based methods require pre-computation of a roadmap which is usually expensive to determine in online planning problems [1]. Therefore, RRT and its variants have now emerged as a promising tools for motion planning that finds a path irrespective of obstacles geometry. Although the RRT algorithm rapidly finds a path solution, if one exists, they fail to find the shortest path solution [1]. An optimal variant of RRT called RRT* asymptotically guarantees to find the shortest path, if one exists [1]. However, RRT* becomes computationally inefficient as the dimensionality of the planning problem increases. Furthermore, studies show that to determine a $\epsilon$-near optimal path in $d\in\mathbb{N}$ dimensions, nearly $O(1/\epsilon^{d})$ samples are required. Thus, RRT* is no better than grid search methods in higher dimensional spaces [16]. Several methods have been proposed to mitigate limitations in current asymptotically optimal SMPs through different heuristics such as biased sampling [17] [18] [2] [3], lazy edge evaluation [7], and bidirectional tree generations [19] [20].

Biased sampling heuristics adaptively sample the robot state-space to overcome limitations caused by random uniform exploration in underlying SMP methods. For instance, P-RRT* [17] [18] incorporates artificial potential fields [6] into RRT* to generate goal directed trees for rapid convergence to an optimal path solution. In similar vein, Gammell et al. proposed the Informed-RRT* [2] and BIT* (Batch Informed Trees) [3]. Informed-RRT* defines an ellipsoidal region using RRT*’s initial path solution to adaptively sample the configuration space for optimal path planning. Despite improvements in computation time, Informed-RRT* suffers in situations where finding an initial path is itself challenging. On the other hand, BIT* is an incremental graph search method that instantiates a dynamically-changing ellipsoidal region for batch sampling to compute paths. Despite some improvements in computation speed, these biased sampling heuristics still suffer from the curse of dimensionality.

Lazy edge evaluation methods, on the other hand, have shown to exhibit significant improvements in computation speeds by evaluating edges only along the potential path solutions. However, these methods are critically dependent on the underlying edge selector and tend to exhibit limited performance in cluttered environments [21]. Bidirectional path generation improves the algorithm performance in narrow passages but still inherits the limitations of baseline SMPs [19][20].

Reinforcement learning (RL) [22] has also emerged as a prominent tool to solve continuous control and planning problems [23]. RL considers Markov Decision Processes (MDPs) where an agent interacts with the environment by observing a state and taking an action which leads to a next state and reward. The reward signal encapsulates the underlying task and provides feedback to the agent on how well it is performing. Therefore, the objective of an agent is to learn a policy to maximize the overall expected reward function. In the past, RL was only employed for simple problems with low-dimensions [24] [25]. Recent advancements have led to solving harder, higher dimensional problems by merging the traditional RL with expressive function approximators such neural networks, now known as Deep RL (DRL) [26] [27]. DRL has solved various challenging robotic tasks using both model-based [28] [29] [30] and model-free [31] [32] [33] approaches. Despite considerable progress, solving practical problems which have weak rewards and long-horizons remain challenging [34].

There also exist approaches that apply various learning strategies such as imitation learning to mitigate the limitations of motion planning methods. Zucker et al. [35] proposed to adapt the sampling for the SMPs using REINFORCE algorithm [36] in discretized workspaces. Berenson et al. [37] use a learned heuristic to store new paths, if needed, or to recall and repair the existing paths. Coleman et al. [38] store experiences in a graph rather than individual trajectories. Ye and Alterovitz [39] use human demonstrations in conjunction with SMPs for path planning. While improved performance was noted compared to traditional planning algorithms, these methods lack generalizability and require tedious hand-engineering for every new environment. Therefore, modern techniques use efficient function approximators such as neural networks to either embed a motion planner or to learning auxiliary functions for SMPs such as sampling heuristic to speed up planning in complex cluttered environments.

Neural Motion Planning has been a recent addition to the motion planning literature. Bency et al. [40] introduced recurrent neural networks to embed a planner based on its demonstrations. While useful for learning to navigate static environments, their method does not use environment information and therefore, is not meant to generalize to other environments. Ichter et al. [41] proposed conditional variational autoencoders that contextualize on environment information to generate samples through decoder network for the SMPs such as FMT* [42]. The SMPs use the generated samples to create a graph for finding a feasible or optimal path for the robot to follow. In a similar vein, Zhang et. al [43] learns a rejection sampling policy that rejects or accepts the given uniform samples before making them the part of the SMP graph. The rejection sampling policy is learned using past experiences from the similar environments. Note that a policy for rejection sampling implicitly learns the sampling distribution whereas Icheter et. al [41] explicitly generates the guided samples for the SMPs. Bhardwaj et al [44] proposed a method called SAIL that learns a deterministic policy which guides the graph expansion of underlying graph-based planner towards the promising areas that potentially contains the path solution. SAIL learns the guiding policy using the oracle Q-values (encapsulates the cost-to-go function), argmin regression, and full environment information. Like these adaptive sampling methods, MPNet can also generate informed samples for SMPs, but in addition, our approach is also outputs feasible trajectories with worst-case theoretical guarantees.

There has also been attempts towards building learning-based motion planners. For instance, Value Iteration Networks (VIN) [45] approximates a planner by emulating value iteration using recurrent convolutional neural networks and max-pooling. However, VIN is only applicable for discrete planning tasks. Universal Planning Networks (UPN) [46] extends VIN to continuous control problems using gradient descent over generated actions to find a motion plan connecting the given start and goal observations. However, these methods do not generalize to novel environments or tasks and thus require frequent retraining. The most relevant approach to our neural planner (MPNet) is L2RRT [47] that plans motion in learned latent spaces using RRT method. L2RRT learns state-space encoding model, agent’s dynamics model, and collision checking model. However, it is unclear that existence of a path solution in configuration space will always imply the existence of a path in the learned latent space and vice versa.

III MPNet: A Neural Motion Planner

MPNet comprises of two neural networks: an encoder network (Enet) and a planning network (Pnet). Enet takes the robot’s surrounding information such as a raw point-cloud or point-cloud converted to voxel depending on the underlying neural architecture that could be a fully-connected neural network or a 3D convolutional neural network (CNN), respectively. The output of the Enet is a latent space embedding of the given information. Pnet takes the encoding of the environment, the robot’s current state and goal state to output samples for either a path or tree generation. In remaining section, we describe the notations necessary to outline MPNet.

Let robot configuration space (C-space) be denoted as $\mathcal{C}\subset\mathbb{R}^{d}$ comprising of obstacle space $\mathcal{C}_{\mathrm{obs}}$ and obstacle-free space $\mathcal{C}_{\mathrm{free}}=\mathcal{C}\backslash\mathcal{C}_{\mathrm{obs}}$, where $d$ is the C-space dimensionality. Let robot’s surrounding environment, also known as workspace, be denoted as $\mathcal{X}\subset\mathbb{R}^{m}$, where $m$ is a workspace dimension. Like C-space, the workspace also comprise of obstacle, $\mathcal{X}_{\mathrm{obs}}$, and obstacle-free, $\mathcal{X}_{\mathrm{free}}=\mathcal{X}\backslash\mathcal{X}_{\mathrm{obs}}$, regions. The workspaces could be up to 3-dimensions whereas the C-space can have higher dimensions depending on the robot’s degree-of-freedom (DOF). Let robot initial and goal configuration space be $c_{\mathrm{init}}\in\mathcal{C}_{\mathrm{free}}$ and $c_{\mathrm{goal}}\subset\mathcal{C}_{\mathrm{free}}$, respectively. Let $\sigma=\{c_{0},\cdots,c_{T}\}$ be an ordered list of length $T$. We assume $\sigma_{i}$ corresponds to the $i$-th state in $\sigma$, where $i=[0,T]$. For instance $\sigma_{0}$ corresponds to state $c_{0}$. Furthermore, we consider $\sigma_{\mathrm{end}}$ corresponds to the last element of $\sigma$, i.e., $\sigma_{\mathrm{end}}=c_{T}$. A motion planning problem can be concisely denoted as $\{c_{\mathrm{init}},c_{\mathrm{goal}},\mathcal{C}_{\mathrm{obs}}\}$ where the aim of a planning algorithm is to find a feasible path solution, if one exists, that connects $c_{\mathrm{init}}$ to $c_{\mathrm{goal}}$ while completely avoiding obstacles in $\mathcal{C}_{\mathrm{obs}}$. Therefore, a feasible path can be represented as an ordered list $\sigma=\{c_{0},\cdots,c_{T}\}$ such that $c_{0}=c_{\mathrm{init}}$, $c_{T}=c_{\mathrm{goal}}$, and a path constituted by connecting consecutive states in $\sigma$ lies entirely in $\mathcal{C}_{\mathrm{free}}$. In practice, C-space representation of obstacles $\mathcal{C}_{\mathrm{obs}}$ is unknown and rather a collision-checker is available that takes workspace information, $\mathcal{X}_{\mathrm{obs}}$, and robot configuration, $c$, and determines if they are in collision or not. Another important problem in motion planning is to find an optimal path solution for a given cost function. Let a cost function be defined as $J(\cdot)$. An optimal planner provides guarantees, either weak or strong, that if given enough running time, it would eventually converge to an optimal path, if one exists. The optimal path is a solution that has the lowest possible cost w.r.t. $J(\cdot)$.

We consider a practical scenario, where MPNet plans feasible, near-optimal paths using raw point-cloud/voxel data of obstacles $x_{\mathrm{obs}}\subset\mathcal{X}_{\mathrm{obs}}$. However, like other planning algorithms, we do assume an availability of a collision-checker that verifies the feasibility of MPNet generated paths based on $\mathcal{X}_{\mathrm{obs}}$. Precisely, Enet, with parameters $\theta^{e}$, takes the environment information $x_{\mathrm{obs}}$ and compresses them into a latent space $Z$,

[TABLE]

Pnet, with parameters $\theta^{p}$, takes the environment encoding $Z$, robot’s current or initial configuration $c_{t}\in\mathcal{C}_{\mathrm{free}}$, and goal configuration $c_{\mathrm{goal}}\subset\mathcal{C}_{\mathrm{free}}$ to produce a trajectory through incremental generation of states $\hat{c}_{t+1}$ (Fig. 2 (b)),

[TABLE]

IV MPNet: Training

In this section, we present three training methodologies for MPNet neural models, Enet and Pnet: i) offline batch learning, ii) continual learning, and iii) active continual learning.

Offline batch learning method assumes the availability of complete data to train MPNet offline before running it online to plan motions for unknown/new planning problems. Continual learning enables MPNet to learn from streaming data without forgetting past experiences. Active continual learning incorporates MPNet into the continual learning process where MPNet actively asks for an expert demonstration when needed for the given problem. Further details on training approaches are presented as follow.

IV-A Offline batch learning

The offline batch learning requires all the training data to be available in advance for MPNet [8] [9]. As mentioned earlier, MPNet comprises of two modules, Enet and Pnet. In this training approach, both modules can either be trained together in an end-to-end fashion using planning network loss function or separately with their individual loss functions.

To train Enet and Pnet together, we back-propagate the gradient of Pnet’s loss function in Equation 4 through both modules. For the standalone training of Enet, we use an encoder-decoder architecture whenever there is an ample amount of environmental point-cloud data available for unsupervised learning. There exist several techniques for encoder-decoder training such as variational auto-encoders [48] and their variants [49] or the class of contractive autoencoders (CAE) [50]. For our purpose, we observed that the contractive autoencoders learn robust feature spaces desired for planning and control, and give better performance than other available encoding techniques. The CAE uses the usual reconstruction loss and a regularization over encoder parameters,

[TABLE]

where $\theta^{e}$, $\theta^{d}$ are the encoder and decoder parameters, respectively, and $\lambda$ denotes regularization coefficient. The variable $\hat{x}$ represents reconstructed point-cloud. The training dataset of obstacles’ point-cloud $x\subset\mathcal{X}_{\mathrm{obs}}$ is denoted as $D_{\mathrm{obs}}$ which contains point cloud from $N_{\mathrm{obs}}\in\mathbb{N}$ different workspaces.

The planning module (Pnet) is a stochastic feed-forward deep neural network with parameters $\theta^{p}$. Our demonstration trajectory is a list of waypoints, $\sigma=\{c_{0},c_{1},\cdots,c_{T}\}$, connecting the start and goal configurations such that the fully connected path lies in obstacle-free space. To train Pnet either end-to-end with Enet or separately for the given expert trajectory, we consider one-step look ahead prediction strategy. Therefore, MPNet takes the obstacles’ point-cloud embedding $Z$, robot’s current state $c_{t}$ and goal state $c_{T}$ as an input to output the next waypoint $\hat{c}_{t+1}$ towards the goal-region. The training objective is a mean-squared-error (MSE) loss between the predicted $\hat{c}_{t+1}$ and target waypoints $c_{t+1}$, i.e.,

[TABLE]

where $T_{j}$ is the length of $j$-th trajectory, $\hat{N}\in\mathbb{N}$ is the total number of training trajectories, and $N_{p}$ is the averaging term. Although we use MSE loss, one can consider other choices such as adversarial loss function [51]. In rigid body planning, such as planning in SE(3), if the rotations are represented as quaternions, we use the following loss:

[TABLE]

where $l_{p}$ and $l_{q}$ correspond to the MSE loss between positional $p$ and quaternion $q$ components of the waypoints in $\sigma$, and $\beta$ is a scaling factor. Since quaternion needs to be in a unit sphere the loss $l_{q}$ is defined as (for more details refer to [52]):

[TABLE]

IV-B Continual Learning

In continual learning settings, both modules of MPNet (Enet and Pnet) are trained in an end-to-end fashion, since both neural models need to adapt to the incoming data (Fig. 2(a)). We consider a supervised learning problem. Therefore, the data comes with targets, i.e.,

[TABLE]

where $s=(c_{t},c_{T},x_{\mathrm{obs}})$ is the input to MPNet comprising of the robot’s current state $c_{t}$, the goal state $c_{T}$, and obstacles information $x_{\mathrm{obs}}$. The target $y$ is the next state $c_{t+1}$ in the expert trajectory given by an oracle planner. Generally, continual learning using neural networks suffers from the issue of catastrophic forgetting since taking a gradient step on a new datum could erase the previous learning. The problem of catastrophic forgetting is also described in terms of knowledge transfer in the backward direction.

To describe backward transfer, we introduce few new notations as follow. Let a mean model success rate on a dataset $\mathcal{M}$ after learning from an expert example at time $t-1$ and $t$ be denoted as $\mathcal{A}_{\mathcal{M},t-1}$ and $\mathcal{A}_{\mathcal{M},t}$, respectively. A metric described as a backward transfer can be written as $\mathcal{B}=\mathrm{A}_{\mathcal{M},t}-\mathrm{A}_{\mathcal{M},t-1}$. A positive backward transfer $\mathcal{B}\geq 0$ indicates that after learning from a new experience, the learning-based planner performed better on the previous tasks represented as $\mathcal{M}$. A negative backward transfer $\mathcal{B}<0$ indicates that on learning from new experience the model on previous tasks deteriorated.

To overcome negative backward transfer leading to catastrophic forgetting, we employ the Gradient Episodic Memory (GEM) method for lifelong learning [53]. GEM uses the episodic memory $\mathcal{M}$ that has a finite set of continuum data seen in the past to ensure that the model update doesn’t lead to negative backward transfer while allowing only the positive backward transfer. For MPNet, we adapt GEM for the regression problem using the following optimization objective function.

[TABLE]

where $l=\|f^{t}_{\theta}(s)-y\|^{2}$ is a squared-error loss, $f^{t}_{\theta}$ is the MPNet model at time step $t$ (see Fig. 2). Furthermore, note that, if the angle between proposed gradient $(g)$ at time $t$, and the gradient over $\mathcal{M}$ $(g_{\mathcal{M}})$ is positive, i.e., $\langle g,g_{\mathcal{M}}\rangle\geq 0$, there is no need to maintain the old function parameters $f^{t-1}_{\theta}$ because the above equation can be formulated as:

[TABLE]

where $\hat{\mathbb{E}}$ denotes empirical mean.

In most cases, the proposed gradient $g$ violates the constraint $\langle g,g_{\mathcal{M}}\rangle\geq 0$, i.e., the proposed gradient update $g$ will cause increase in the loss over previous data. To avoid such violations, David and Razanto [53] proposed to project the gradient $g$ to the nearest gradient $g^{\prime}$ that keeps $\langle g^{\prime},g_{\mathcal{M}}\rangle\geq 0$, i.e,

[TABLE]

The projection of proposed gradient $g$ to $g^{\prime}$ can be solved efficiently using Quadratic Programming (QP) based on the duality principle, for details refer to [53].

Various data parsing methods are available to update the episodic memory $\mathcal{M}$. These sample selection strategies for episodic memory play a vital role in the performance of continual/life-long learning methods such as GEM [54]. There exist several selection metrics such as surprise, reward, coverage maximization, and global distribution matching [54]. In our continual learning framework, we use a global distribution matching method to select samples for the episodic memory. For details and comparison of different sample selection approaches, please refer to Section VII-B. The global distribution matching method, also known as reservoir sampling, uses random sampling techniques to populate the episodic memory. The aim is to approximately capture the global distribution of the training dataset since it is unknown in advance. There are several ways to implement reservoir sampling. The simplest approach accepts the new sample at $i$-th step with probability $\cfrac{|\mathcal{M}|}{i}$ to replace a randomly selected old sample from the memory. In other words, it rejects the new sample at $i$-the step with probability $1-\cfrac{|\mathcal{M}|}{i}$ to keep the old samples in the memory.

In addition to episodic memory $\mathcal{M}$, we also maintain a replay/rehearsal memory $\mathcal{B}^{*}$. The replay buffer lets MPNet rehearse by learning again on the old samples (Line 14-19). We found that rehearsals further mitigate the problem of catastrophic forgetting and leads to better performance, as also reported by Rolnick et al. [55] in reinforcement learning setting. Note that replay or rehearsal on the past data is done with the interval of replay period $r\in\mathbb{N}_{\geq 0}$.

Finally, Algorithm 1 presents the continual learning framework where an expert provides a single demonstration at each time step, and MPNet model parameters are updated according to the projected gradient $g^{\prime}$.

IV-C Active Continual Learning

Active continual learning (ACL) is our novel data-efficient learning strategy which is practical in the sense that the planning problem comes in streams and our method asks for expert demonstrations only when needed. It builds on the framework of continual learning presented in the previous section. ACL introduces a two-level of sample selection strategy. First, ACL gathers the training data by actively asking for the demonstrations on problems where MPNet failed to find a path. Second, it employs a sample selection strategy that further prunes the expert demonstrations to fill episodic memory so that it approximates the global distribution from streaming data. The two-level of data selection in ACL improves the training samples efficiency while learning the generalized neural models for the MPNet.

Algorithm 2 presents the outline of ACL method. At every time step $t$, the environment generates a planning problem ($c_{\mathrm{init}},c_{\mathrm{goal}},\mathcal{X}_{\mathrm{obs}})$ comprising of the robot’s initial $c_{\mathrm{init}}$ and goal $c_{\mathrm{goal}}$ configurations and the obstacles’ information $\mathcal{X}_{\mathrm{obs}}$ (Line 7). Before asking MPNet to plan a motion for a given problem, we let it learn from the expert demonstrations for up to $N_{c}\in\mathbb{N}_{\geq 0}$ iterations (Line 9-10). If MPNet is not called or failed to determine a solution, an expert-planner is executed to determine a path solution $\sigma$ for a given planning problem (Line 13). The expert trajectory $\sigma$ is stored into a replay buffer $\mathcal{B}^{*}$ and an episodic $\mathcal{M}$ memory based on their sample selection strategies (Line 14-15). MPNet is trained (Line 16-19) on the given demonstration using the constraint optimization mentioned in Equations 7-9. Finally, similar to continual learning, we also perform rehearsals on the old samples (Line 20-25).

V MPNet: Online Path Planning

MPNet can generate both end-to-end collision-free paths and informed samples for the SMPs. We denote our path planner and sample generator as MPNetPath and MPNetSMP, respectively. MPNet uses two trained modules, Enet and Pnet.

Enet takes the obstacles’ information $x_{\mathrm{obs}}$ and encodes them into a latent-space embedding $Z$. Enet is either trained end-to-end with Pnet or separately with encoder-decoder structure. Pnet, is a stochastic model as during execution it uses Dropout in almost every layer. The layers with Dropout [56] get their neurons dropped with a probability $p\in[0,1]$. Therefore, every time MPNet calls Pnet, due to Dropout, it gets a sliced/thinner model of the original planning network which leads to a stochastic behavior. The stochasticity helps in recursive, divide-and-conquer based path planning, and also enables MPNet to generate samples for SMPs. The input to Pnet is a concatenated vector of obstacle-space embedding $Z$, robot current configuration $\hat{c}_{t}$, and goal configuration $c_{T}$. The output is the robot configuration $\hat{c}_{t+1}$ for time step $t+1$ that would bring the robot closer to the goal configuration. We iteratively execute Pnet (Fig. 2(b)), i.e., the new state $\hat{c}_{t+1}$ becomes the current state $\hat{c}_{t}$ in the next time step and therefore, the path grows incrementally.

V-A Path Planning with MPNet

Path planning with MPNet uses Enet and Pnet in conjunction with our iterative, recursive, and bi-directional planning algorithm. Our planning strategy has the following salient features.

Forward-Backward Bi-directional Planning: Our algorithm is bidirectional as we run our neural models to plan forward, from start to goal, as well as to plan backward, from goal to start, until both paths meet each other. To connect forward and backward paths we use a greedy RRT-Connect [57] like heuristic.

Recursive Divide-and-Conquer Planning: Our planner is recursive and solves the given path planning problem through divide-and-conquer. MPNet begins with global planning (Fig. 3 (a)) that results in critical, collision-free states that are vital to generating a feasible trajectory. If any of the consecutive critical nodes in the global path are not connectable (Beacon states), MPNet takes them as a new start and goal, and recursively plans a motion between them (Figs. 3 (b-c)). Hence, MPNet decomposes the given problem into sub-problems and recursively executes itself on those sub-problems to eventually find a path solution.

In the remainder of this section, we describe the essential components of MPNetPath algorithm followed by the overall algorithm execution and outline.

V-A1 Bidirectional Neural Planner (BNP)

In this section, we formally outline our forward-backward, bidirectional neural planning method, outlined in Algorithm 3. BNP takes the environment representation $Z$, the robot’s initial $c_{\mathrm{init}}$ and target $c_{\mathrm{goal}}$ configurations as an input. The bidirectional path is generated as two paths, from start to goal $(\sigma_{a})$ and from goal to start $(\sigma_{b})$, incrementally march towards each other. The paths $\sigma_{a}$ and $\sigma_{b}$ are initialized with the robot’s start configuration $c_{\mathrm{init}}$ and goal configuration $c_{\mathrm{goal}}$, respectively (Line 1). We expand paths in an alternating fashion, i.e., if at any iteration $i$, a path $\sigma_{a}$ is extended, then in the next iteration, a path $\sigma_{b}$ will be extended, and this is achieved by swapping the roles of $\sigma_{a}$ and $\sigma_{b}$ at the end of every iteration (Line 10). Furthermore, after each expansion step, the planner attempts to connect both paths through a straight-line, if possible. We use $\mathrm{steerTo}$ (described later) to perform straight-line connection which makes our connection heuristic greedy. Therefore, like RRT-Connect [57], our method makes the best effort to connect both paths after every path expansion. In case both paths $\sigma^{a}$ and $\sigma^{b}$ are connectable, BNP returns a concatenated path $\sigma$ that comprises of states from $\sigma_{a}$ and $\sigma_{b}$ (Line 8-9).

V-A2 Replanning

The bidirectional neural planner outputs a coarse path of critical points $\sigma$ (Fig. 3 (a)). If all consecutive nodes in $\sigma$ are connectable, i.e., the trajectories connecting them are in obstacle-free space then there is no need to perform any further planning. However, if there are any beacon nodes in $\sigma$, a replanning is performed to connect them (Fig. 3 (b-c)). The replanning procedure is presented in Algorithm 4. The trajectory $\sigma$ consists of states $\sigma=\{c_{0},c_{1},\cdots,c_{T}\}$ given by BNP. The algorithm uses $\mathrm{steerTo}$ function and iterates over every connective state pairs $\sigma_{i}$ and $\sigma_{i+1}$ in a given path $\sigma$ to check if there exists a collision-free straight trajectory between them. If a collision-free trajectory does not exist between any of the given consecutive states, a new path is determined between them through replanning. To replan, the beacon nodes are presented to the replanner as a new start and goal pair together with the obstacles information. We propose two replanning methods:

(i) Neural replanning (NP): NP takes the beacon states and makes a limited number of attempts ($N_{r}$) to find a path solution between them using BNP. In case of NP, the $\mathrm{plan\_oracle}$ in Algorithm 4 is set to $\mathrm{False}$.

(ii) Hybrid replanning (HP): HP combines NP and oracle planner. HP takes beacon states and tries to find a solution using NP. If NP fails after a fixed number of trials $N_{r}$, an oracle planner is executed to find a solution, if one exists, between the given states (Line 9). HP is performed if boolean $\mathrm{plan\_oracle}$ is set $\mathrm{True}$ in Algorithm 4.

V-A3 Lazy States Contraction (LSC)

This process is often known as smoothing or shortcutting [58]. The term contraction was coined in graph theory literature [59]. We implement LSC as a recursive function that when executed on a given path $\sigma=\{c_{0},c_{1},\cdots,c_{T}\}$, leaves no redundant state by directly connecting, if possible, the non-consecutive states, i.e., $c_{i}$ and $c_{>i+1}$, where $i\in[0,T-1]$, and removing the lousy/lazy states (Fig. 3 (d)). This method improves the computational efficiency as the algorithm will have to process fewer nodes during planning.

V-A4 Steering ($\mathrm{SteerTo}$)

The $\mathrm{steerTo}$ function, as it name suggests, walks through a straight line connecting the two given states to validate if their connecting trajectory lies entirely in the collision-free space or not. To evaluate the connecting trajectory, the $\mathrm{steerTo}$ function discretize the straight line into small steps and verifies if each discrete node is collision-free or not. The discretization is done as $\sigma(\delta)=(1-\delta)c_{1}+\delta c_{2};\forall\delta\in[0,1]$ between nodes $c_{1}$ and $c_{2}$ using a small step-size. In our algorithm, we use different step-sizes for the global planning, replanning, and for final feasibility checks (for details refer to supplementary material).

V-A5 isFeasible

This function uses the $\mathrm{steerTo}$ to check whether the given path $\sigma=\{c_{0},c_{1},\cdots,c_{T}\}$ lies entirely in collision-free space or not.

MPNetPath Execution Summary: Algorithm 5 outlines our MPNetPath framework. Enet encodes the raw point-cloud $x_{\mathrm{obs}}$ into latent embedding $Z$ (Line 1). BNP takes the given planning problem $(c_{\mathrm{init}},c_{\mathrm{goal}},Z)$ and outputs a coarse path $\sigma$ (Line 2). The LSC function takes $\sigma$ to remove the redundant nodes and leaves only critical states crucial for finding a feasible path solution (Line 3). If a path constituted by connecting the consecutive nodes in $\sigma$ does not belong to the collision-free region, a neural replanning (NP), followed by LSC, is performed for a fixed number of iterations $N_{r}$ (Line 8-12). The neural-replanning recursively gets deeper and finer in connecting the beacon states whereas LSC function keeps on pruning out the redundant states after every replanning step. In most case, the neural replanner is able to compute a path solution. However, for some hard cases, where neural replanner fails to find a path between beacon states in $\sigma$, we employ hybrid planning (HP) using an oracle planner (Line 13-14). Note that, in case of hybrid replanning, the oracle planner is executed only for a small segment of the overall problem which helps MPNetPath retains its computational benefits while being able to determine a path solution if one exists.

V-B Informed Sampling with MPNet

As mentioned earlier, our planning network (Pnet) is a stochastic model as it has Dropout in almost every layer during the execution. We exploit the stochasticity of Pnet to generate multiple informed samples that would be used as a sample generator for a classical SMP. Because samples are informed in such a way that has high probability to be part of a connectable and near-optimal path, it allows the underlying SMP to find solutions efficiently and quickly.

Algorithm 6 outlines the procedure to integrate our MPNetSMP with any SMP. MPNet’s BNP performs one-step-ahead prediction, i.e., given obstacles representation $Z$, robot current state $\hat{c}_{t}$, and goal state $c_{T}$, it predicts a next state $\hat{c}_{t+1}$ closer to the goal than previous state $\hat{c}_{t}$. We execute MPNetSMP to incrementally generate samples from start to goal in a loop for a fixed number of iterations $N_{\mathrm{smp}}$ before switching to uniform random sampling (Line 5-8). The underlying SMP takes the informed samples (Line 9) and builds a tree starting from an initial configuration. Due to informed sampling, the tree, in the beginning, is biased towards a subspace that potentially contains a path solution and after certain iterations, the tree begins to expand uniformly. Hence, our MPNetSMP perform both exploitation and exploration. Once a path solution $\sigma$ is found, it is either returned as a solution or further optimized for a given cost function.

We also propose that our MPNetSMP can be adapted to generate samples for bidirectional SMPs [19]. MPNetSMP generates informed samples incrementally between the given start and goal, and just like the bidirectional heuristic of MPNetPath, it can be queried to generate bidirectional samples. A simple modification for bidirectional sampling in Algorithm 6 would be to initialize two random variable $c_{\mathrm{randS}}$ and $c_{\mathrm{randG}}$ with start and goal states, respectively, in place of Line 2. Then use $c_{\mathrm{randG}}$ instead of $c_{\mathrm{goal}}$ and swap the roles of $c_{\mathrm{randS}}$ and $c_{\mathrm{randG}}$ at the end of every iteration. The proposed modification would allow informed bidirectional generation of trees that are crucial for solving the narrow passage planning problems.

VI Results

In this section, we present results evaluating MPNet against state-of-the-art classical planners: RRT* [1], Informed-RRT* [2], and BIT* [3]. We use standard and efficient OMPL [60] implementations for classical planners. MPNet models were trained with the PyTorch Python API, exported using TorchScript 222https://pytorch.org/tutorials/advanced/cpp_export.html so that they could be loaded in OMPL for execution. Furthermore, for MPNet, we present the results of MPNetPath and MPNetSMP trained with offline batch learning (B), continual learning (C), and active continual learning (AC). The MPNetPath:NP and MPNetPath:HP uses neural and hybrid replanning, respectively. The hybrid replanning exploits neural replanner for a fixed number of iterations $N_{r}$. The system used for experiments has 3.40GHz$\times$ 8 Intel Core i7 processor with 32 GB RAM and GeForce GTX 1080 GPU.

VI-A Training & Testing Environments

We consider problems requiring the planning of 2D/3D point-mass robot, rigid-body-SE2, rigid-body-SE3, and a 7DOF Baxter robot manipulator. We used encoder-decoder based unsupervised learning in the case of point-mass robots, and rigid-body-SE2. In Baxter and rigid-body-SE3 settings, we train Enet and Pnet together in an end-to-end setting.

In planning of 2D/3D point-mass and rigid-body-SE2, we train MPNet over one hundred workspaces with each containing four thousand trajectories, and it is evaluated on two test datasets, i.e., seen-$\mathcal{X}_{\mathrm{obs}}$ and unseen-$\mathcal{X}_{\mathrm{obs}}$. The seen-$\mathcal{X}_{\mathrm{obs}}$ comprises of one hundred workspaces seen by MPNet during the training but two hundred unseen start and goal pairs in each environment. The unseen-$\mathcal{X}_{\mathrm{obs}}$ consists of ten new workspaces, not seen by MPNet during training, with each containing two thousand start and goal pairs. For the 7DOF Baxter manipulator, our training dataset containing ten cluttered environments with each having nine hundred trajectories, and our test dataset contained the same ten workspaces as in training but one hundred new start and goal pairs for each scenario. In rigid-body-SE3 planning, we consider OMPL’s [60] home-like environment, with training and testing dataset comprising two thousand trajectories and five-hundred unseen start and goal 3D poses of the rigid-body, respectively. For all planners presented in our experiments, we evaluate multiple trials on the given test dataset, and their mean performance metrics are reported.

VI-B MPNet Comparison with its Expert Demonstrator (RRT)*

We compare MPNet against its expert demonstrator RRT*. Fig. 4 shows the paths generated by MPNetPath (red), and its expert demonstrator RRT* (blue). The average computations times of MPNetPath and RRT* are denoted as $t_{\mathrm{R}}$ and $t_{\mathrm{MP}}$, respectively. The average Euclidean path cost of MPNetPath and RRT* solutions are denoted as $c_{\mathrm{R}}$ and $c_{\mathrm{MP}}$, respectively. It can be seen that MPNet finds paths of similar lengths as its expert demonstrator RRT* while retaining consistently low computational time. Furthermore, the computation times of RRT* are not only higher than MPNetPath computation time but also grows exponentially with the dimensionality of the planning problems. Overall, we observed that MPNetPath is at least $100\times$ faster than RRT* and finds paths that are within a 10% range of RRT*’s paths cost.

Fig. 5 presents informed trees generated by MPNetSMP with an underlying RRT* algorithm. We report average path computation times and Euclidean costs denoted as $t$ and $c$, respectively. The generated trees by MPNetSMP are in a subspace of given configuration space that most of the time contains path solutions. It should be noted that MPNetSMP paths are almost optimal and are observed to be better than MPNetPath and its expert demonstrator RRT* solutions. Moreover, our sampler not only finds near-optimal/optimal paths but also exhibit a consistent computation time of less than a second in all environments which is much lower than the computation time of RRT* algorithm.

VI-C MPNet Comparison with Advanced Motion Planners

We further extend our comparative studies to evaluate MPNet against advanced classical planners such as Informed-RRT* [2] and BIT* [3].

Table I presents the comparison results over the four different scenarios, i.e., simple 2D (Fig. 4 (a-b)), complex 2D (Fig. 4 (c-d)), complex 3D (Fig. 4 (e-f)) and rigid-body-SE2 (Fig. 4 (g-h)). Each scenario comprises of two test datasets, seen-$\mathcal{X}_{\mathrm{obs}}$ and unseen-$\mathcal{X}_{\mathrm{obs}}$. We let Informed-RRT* and BIT* run until they find a solution of Euclidean cost within $5\%$ range of the cost of MPNetPath solution. We report mean computation times with standard deviation over five trials. In all planning problems, MPNetPath:NP (with neural replanning), MPNetPath:HP (with hybrid replanning), and MPNetSMP exhibit a mean computation time of less than a second. The state-of-the-art classical planners, Informed-RRT* and BIT*, not only exhibit higher computation times than all versions of MPNet but, just-like RRT*, their computation times increase with the planning problem dimensionality. In simplest case (Fig. 4 (a-b)), MPNetPath:NP stand out to be atleast $80\times$ and $30\times$ faster than BIT* and Informed-RRT*, respectively. On the other hand, MPNetSMP provides about $8\times$ and $5\times$ computation speed improvements compared to BIT* and Informed-RRT*, respectively, in simple 2D environments. Furthermore, it can be observed that the speed gap between classical planner and MPNet (MPNetPath and MPNetSMP) keeps increasing with planning problem dimensions.

Fig. 6 and Fig. 7 present the mean computation time, in log-scale, of all MPNet variants, Informed-RRT* (IRRT*), and BIT* on seen-$\mathcal{X}_{\mathrm{obs}}$ and unseen-$\mathcal{X}_{\mathrm{obs}}$ test datasets, respectively. Note that MPNetPath trained with offline batch learning (B), continual learning (C), and active continual learning (AC) show similar computation times as can be seen by their plots in all planning problems. Furthermore, in Figs. 6-7, it can be seen that MPNetPath and MPNetSMP computation times remain consistently less than one second in all planning problems irrespective of their dimensions. The computation times of IRRT* and BIT* are not only high but also lack consistency and exhibits high variations over different planning problems. Although the computation speed of MPNetSMP is slightly lower than that of MPNetPath, it performs better than all other methods in terms of path optimality for a given cost function.

VI-D MPNet Data Efficiency & Performance Evaluation

Fig. 8 shows the number of training paths consumed by all MPNet versions and Table II presents their mean success rates on the test datasets in four scenarios- simple 2D, complex 2D, complex 3D, and rigid-body-SE2. The success rate represents the percentage of planning problems solved by a planner in a given test dataset. The models trained with offline batch learning provides better performance in term of a success rate compared to continual/active-continual learning methods. However, in our experiments, the continual/active-continual learning frameworks required about ten training epochs compared to one hundred training epochs of the offline batch learning method. Furthermore, we observed that continual/active-continual learning and offline batch learning show similar success rates if allowed to run for the same number of training epochs. The active continual learning is ultimately preferred as it requires less training data than traditional continual and offline batch learning while exhibiting considerable performance.

VI-E MPNet on 7DOF Baxter

Since BIT* performs better than Informed-RRT* in high dimension planning problems, we consider only BIT* as our classical planner in our further comparative analysis. We evaluate MPNet against BIT* for the motion planning of 7DOF Baxter robot in ten different environments where each environment comprised of one hundred new planning problems. Fig. 9 presents four out of ten environment settings. Each planning problem scenario contained an L-shape table of half the Baxter height with randomly placed five different real-world objects such as a book, bottle, or mug as shown in the figure. The planner’s objective is to find a path from a given robot configuration to target configuration while avoiding any collisions with the environment, or self-collisions. Table III summarizes the results over the entire test dataset. MPNet on average find paths in less than a second with about 80% success rate. BIT* also finds the initial path in less than a second with similar success rate as MPNet. However, the path costs of BIT* initial solution are significantly higher than the path cost of MPNet solutions. Furthermore, we let BIT* run to improve its initial path solutions so that the cost of the paths are not greater than 140% of MPNet path costs. The results show that BIT* mostly fails to find solutions as optimal as MPNet solutions but demonstrate about 56% success rate in finding solutions that are usually 40% larger in lengths/costs than MPNet path costs. Furthermore, the computation time of BIT* also increases significantly, requiring an average of about 9 seconds to plan such paths. Fig. 11 shows the mean computation time and path cost comparison of BIT* and MPNet over ten individual environments. It can be seen that MPNet computes paths in less than a second while giving incredibly optimized solutions and is much faster compared to BIT*.

Fig. 10 shows a multi-goal planning problem for 7 DOF Baxter robot. The task is to reach and pick up the target object while avoiding any collision with the environment and transfer it to another target location, shown as yellow block. Note that in previous experiments we let BIT* run until it finds paths within $40\%$ of MPNet path costs. Now, we let BIT* run until it finds a path within $10\%$ cost of the cost of MPNet path solution. Our result shows that MPNetPath:NP plans the entire trajectory in less than a second whereas BIT* took about 3.01 minutes to find a path of similar cost as our solution. Note that MPNet is never trained on trajectories that connect two configurations on the table. However, thanks to MPNet ability to generalize that it can solve completely unseen problems in no time compared to classical planners that have to perform an exhaustive search before proposing any path solution.

VI-F *MPNet planning in SE (3) *

To further extend our comparison of MPNet and BIT*, we also consider planning in SE (3) for a rigid-body in a cluttered home-like environment with multiple narrow passages [60], as shown in the Fig. 12. Our test dataset comprised 500 randomly sampled start and goal poses (not seen by MPNet during training), and the Fig. 12 shows two of those test cases. In these settings, MPNet exhibits a success rate of about $85\%$. The mean computation time to find an initial path solution for MPNet (B), MPNet (C), and BIT* were $0.96$, $1.61$, and $2.84$ seconds, respectively. We also observed that MPNet paths’ cost were significantly lower than the cost of BIT* solutions. The mean path costs of MPNet (B), MPNet (C), and BIT* were $457.80$, $512.73$, and $836.85$, respectively. Furthermore, we noticed that BIT* takes up to several minutes to find a path of similar cost as MPNet’s solution.

VII Discussion

This section presents a discussion on various attributes of MPNet, sample selection methods for continual learning, and a brief analysis of its completeness, optimality, and computational guarantees.

VII-A Stochastic Neural Planning & Replanning

In MPNet, we apply Dropout [56] to almost every layer of the planning network, and it remains active during path planning. The Dropout randomly selects the neurons from its preceding layer with a probability $p\in[0,1]$ and masks them so that they do not affect the model decisions. Therefore, at any planning step, Pnet would have a probabilistically different model structure that results in stochastic behavior.

Yarin and Zubin [61] demonstrate the application of Dropout to model uncertainty in the neural networks. We show that the Dropout can also be used in learning-based motion planners to approximate the subspace of a given configuration space that potentially contains several path solutions to a given planning problem. For instance, it is evident from the trees generated by MPNetSMP (Fig. 5) that our model approximates the subspace containing path solutions including the optimal path.

The perturbations in Pnet’s output, thanks to Dropout, also play a crucial role during our neural replanning (Algorithm 4). In neural replanning, MPNet takes its own global path and uses its stochastic neural planner (Algorithm 5) to replan a motion between any beacon states. Replanning is a crucial component of our planning algorithm. Although the global plan comprises collision-free nodes, the straight-line connection between those nodes might not be collision-free (Fig. 3 (b)). We observed that without replanning, MPNet exhibits a low success rate of about $60-70\%$. Since global planning decomposes a given problem into sub-problems, the replanning between beacon nodes to solve sub-problems through our stochastic neural planner helps our method to recover from any failures leading to a high success rate in complex, cluttered environments.

VII-B Sample Selection Strategies

In continual learning, we maintain an episodic memory of a subset of past training data to mitigate catastrophic forgetting when training MPNet on new demonstrations. Therefore, it is crucial to populate the episodic memory with examples that are important for learning a generalizable model from streaming data. We compare four MPNet models trained on a simple 2D environment with four different sample selection strategies for the episodic memory. The four selection metrics include surprise, reward, coverage maximization, and global distribution matching. The surprise and reward metrics give higher and lower priority to examples with large prediction errors, respectively. The coverage maximization maintains a memory of $k$-nearest neighbors. The global distribution matching, as described earlier, uses random selection techniques to approximate the global distribution from streaming data. We evaluate four MPNet models on two test datasets, seen-$\mathcal{X}_{\mathrm{obs}}$ and unseen-$\mathcal{X}_{\mathrm{obs}}$, from simple 2D environment, and their mean success rates are reported in Fig. 13. It can be seen that global distribution matching exhibits slightly better performance than reward and coverage maximization metrics. The surprise metric gives poor performance because satisfying the constraint in Equations 9 becomes nearly impossible when the episodic memory is populated with examples that have large prediction losses. Since global distribution matching provides the best performance overall, we have used it as our sample selection strategy for the episodic memory in the continual learning settings.

VII-C Batch offline & Active Continual Learning

In this section, we briefly highlight the merits of our training approaches. A batch offline learning is preferred when plenty of data is available for offline training, especially in cases where existing planners can be used to generate data. However, in cases where data is expensive to make, an active continual learning approach is preferable as it asks for demonstration only when needed leading to data-efficient training of our models. For instance, in semi-autonomous driving, the planning problems would usually come in streams, and our neural planner will only ask for a human demonstration when needed. Also, to realize classical life-long learning one can combine both batch offline and active continual learning where models are pre-trained with the former and further refined with the latter whenever needed.

VII-D Completeness

In this section, we discuss the completeness guarantees of MPNet, formally proposed as follow:

Proposition 1 (Feasible Path Planning)* Given a planning problem $\{c_{\mathrm{init}},c_{\mathrm{goal}},x_{\mathrm{obs}}\}$, and a collision-checker, MPNet finds a path solution $\sigma:[0,T]$, if one exists, such that $\sigma_{0}=c_{\mathrm{init}}$, $\sigma_{T}\in c_{\mathrm{goal}}$, and $\sigma\subset\mathcal{C}_{\mathrm{free}}$.

Proposition 1 implies that for a given planning problem, MPNet will eventually find a feasible path, if one exists. We show that the worst-case completeness guarantees of our approach rely on the underlying SMP for both path planning (MPNetPath) and informed neural sampling (MPNetSMP). In our experiments, we use RRT as our oracle SMP that exhibits probabilistic completeness.

The probabilistic completeness is described in Definition 1 based on the following notations. Let $\mathcal{T}^{AL}_{n}$ be a connected tree in obstacle-free space, comprising of $n$ vertices, generated by an algorithm $AL$. We also assume $\mathcal{T}^{AL}_{n}$ always originates from the robot’s initial configuration $c_{\mathrm{init}}$. An algorithm is probabilistically complete if it satisfies the following definition.

Definition 1 (Probabilistic Completeness)* Given a planning problem $\{c_{\mathrm{init}},c_{\mathrm{goal}},\mathcal{X}_{\mathrm{obs}}\}$ for which there exists a solution, an algorithm AL with a tree $\mathcal{T}^{AL}_{n}$ that originates at $c_{\mathrm{init}}$ is considered probabilistically complete iff $\lim_{n\rightarrow\infty}\mathbb{P}(\mathcal{T}^{AL}_{n}\cap c_{\mathrm{goal}}\neq\varnothing)=1$.

RRT* algorithm exhibits probabilistic completeness as it builds a connected tree originating from initial robot configuration and randomly exploring the entire space until it finds a goal configuration. Therefore, as the number of samples in the RRT* approach to infinity the probability of finding a path solution, if one exists, gets to one, i.e.,

[TABLE]

In remainder of the section, we present a brief analysis showing that MPNetPath and MPNetSMP also exhibit probabilistic completeness like their underlying RRT*.

VII-D1 Probabilistic completeness of MPNetPath

To justify MPNetPath worst-case guarantees, we introduce the following assumptions that are crucial to validate a planning problem.

Assumption 1 (Feasibility of States to Connect)* The given start and goal pairs $(c_{\mathrm{init}},c_{\mathrm{goal}})$, for which an oracle-planner is called to find a path, lie entirely in obstacle-free space, i.e., $c_{\mathrm{init}}\in\mathcal{C}_{\mathrm{free}}$ and $c_{\mathrm{goal}}\subset\mathcal{C}_{\mathrm{free}}$.

Assumption 2 (Existence of a Path Solution) Under Assumption 1, for a given problem $(c_{\mathrm{init}},c_{\mathrm{goal}},\mathcal{X}_{\mathrm{obs}})$, there exists at least one feasible path $\sigma$ that connects $c_{\mathrm{init}}$ and $c_{\mathrm{goal}}$, and $\sigma\not\subset\mathcal{X}_{\mathrm{obs}}$.

Based on Definition 1 and Assumptions 1-2, we propose in Theorem 1, with a sketch of proof, that MPNetPath also exhibits probabilistic completeness, just like its underlying oracle planner RRT.

Theorem 1 (Probabilistic Completeness of MPNetPath)* If Assumptions 1-2 hold, for a given planning problem $\{c_{\mathrm{init}},c_{\mathrm{goal}},\mathcal{X}_{\mathrm{obs}}\}$, and an oracle RRT* planner, MPNetPath will find a path solution, if one exists, with a probability of one as the underlying RRT* is allowed to run till infinity if needed.

Sketch of Proof:* Assumption 1 puts a condition that the start and goal states in the given planning problem lie in the obstacle-free space. Assumption 2 requires that there exist at least one collision-free trajectory for the given planning problem. During planning, MPNetPath first finds a coarse solution $\sigma$ that might have beacon (non-connectable consecutive) states (see Fig. 3). Our algorithm connects those beacon states through replanning. Assumption 1 holds for the beacon states as each generated state of MPNetPath is evaluated by an oracle collision-checker before making it a part of $\sigma$. In replanning, we perform neural replanning for a fixed number of trials to further refine $\sigma$, and if that fails to conclude a solution, we connect any remaining beacon states of the refined $\sigma$ by RRT*. Hence, if Assumption 1-2 hold for beacon states, MPNetPath inherits the convergence guarantees of the underlying planner which in our case is RRT*. Therefore, MPNetPath with an underlying RRT* oracle planner exhibits probabilistic completeness.

VII-D2 Probabilistic completeness of MPNetSMP

MPNetSMP generates samples for SMPs such as RRT*. Our method performs exploitation and exploration to build an informed tree. It begins with exploitation by sampling a subspace that potentially contains a solution for a limited time and switches to exploration via uniform sampling to cover the entire configuration space. Therefore, like RRT*, the tree of MPNetSMP is fully connected, originates from the initial robot configuration, and eventually expands to explore the entire space. Hence, the probability that MPNetSMP tree will find a goal configuration, if possible, approaches one as the samples in the tree reaches a large number, i.e.,

[TABLE]

VII-E Optimality

MPNet learns through imitating the expert demonstrations which could come from either an oracle motion planner or a human demonstrator. In our experiments, we use RRT* for generating expert trajectories, hybrid replanning in MPNetPath, and as a baseline SMP for our MPNetSMP framework. Optimal SMPs such as RRT* exhibits asymptotic optimality, i.e., a probability of finding an optimal path, if one exists, approaches one as the number of random samples in the tree approaches infinity (for proof, refer to [1]). RRT* gets weak-optimality guarantees from its rewiring heuristic. The rewiring incrementally updates the tree connections such that over the time, each node in the tree would be connected to a branch that ensures lowest cost path to the root state (initial robot configuration) of the tree.

In our experiments, we show that MPNetPath imitates the optimality of its expert demonstrator. In Fig. 4, it can be seen that MPNet path solutions are of similar Euclidean costs as its expert demonstrator RRT* path solutions. Therefore, the quality of the MPNet paths relies on the quality of its training data.

In the case of MPNetSMP, there exists optimality guarantees depending on the underlying SMP. Since we use RRT* as a baseline SMP for MPNetSMP, we formally propose in Proposition 2 that our method exhibits asymptotic optimality.

Proposition 2 (Optimal Path Planning)* Given a planning problem $\{c_{\mathrm{init}},c_{\mathrm{goal}},x_{\mathrm{obs}}\}$, a collision-checker, and a cost function $J(\cdot)$, MPNet can adaptively sample the configuration space for an asymptotic optimal SMP such that the probability of finding an optimal path solution, if one exists, w.r.t. $J(\cdot)$ approaches one as the number of samples in the graph approaches infinity.

In our sampling heuristic, MPNetSMP generates informed samples for a fixed number of iterations and performs uniform random sampling afterward. Therefore, RRT tree formed by the samples of MPNetSMP will explore the entire C-space and will be rewired incrementally to ensure asymptotic optimality. Since MPNetSMP only performs intelligent sampling and does not modify the internal machinery of RRT*, the optimality proofs will exactly be the same as provided in [1]. Hence, provided that MPNetSMP performs exploitation for fixed steps and pure exploration afterward, and the underlying SMP is RRT*, the optimality of our method is asymptotically guaranteed .

VII-F Computational Complexity

In this section, we present the computational complexity of our method. Both MPnetPath and MPNetSMP take the output of Enet and Pnet, which is only a forward-pass through a neural network. The forward pass through a neural network is known to have a constant complexity since it is a matrix multiplication of a fixed size input vector with network weights to generate a fixed size output vector. Since MPNetSMP produces samples by merely forward passing a planning problem through Enet and Pnet, it does not add any complexity to the underlying SMP. Therefore, the computational complexity of MPNetSMP with underlying RRT* is the same as RRT* complexity, i.e., $O(n\log n)$, where $n$ is the number of samples [1].

On the other hand, MPNetPath has a crucial replanning component that uses an oracle planner in the cases where neural replanning fails to determine an end-to-end collision-free path. The oracle planner in the presented experiments is RRT*. Therefore, in the worst-case, MPNetPath operates with complexity of $O(n\log n)$. However, we show empirically that MPNetPath succeeds, without any oracle planner, for up to 90% of the cases in a challenging test dataset. For the remaining 10% cases, oracle planner is executed only for a small segment of the overall path (see Algorithm 5). Hence, even for hard planning problems where MPNetPath fails, our method reduces a complicated problem into a simpler problem, thanks to its divide-and-conquer approach, for the underlying oracle planner. Thus, the oracle planner operates at a complexity which is practically much lower than its worst-case theoretical complexity, as is also evident from the results of MPNetPath with hybrid replanning (HB) in Table I.

VIII Conclusions and Future work

We present a learning-based approach to motion planning using deep neural networks. For a given planning problem, our method is capable of i) finding collision-free near-optimal paths; ii) generating samples for sampling-based motion planners in a subspace of a given configuration space that most likely contains solutions including the optimal path. We also present the active continual learning strategy to train our models with a significant improvement in training data-efficiency compared to naive training approaches. Our experimentation shows that our neural motion planner consistently finds collision-free paths in less than a second for the problems where other planners may take up to several minutes.

In our future works, one of our primary objectives is to tackle the environment encoding problem for motion planning. Environment encoding is one of the critical challenges in real-world robotics problems. Current perception approaches consider encoding of individual objects rather than the entire scene that retains the inter-object relational geometry which is crucial for motion planning. Henceforth, we aim to address the problem of motion planning oriented environments’ encoding from raw point-cloud data. Another future objective is to learn all fundamental modules required for motion planning that not only include the motion planner but also the collision checker and cost function. Recently, L2RRT [47] is proposed that learns the motion planner and collision checker in latent space. However, providing worst-case theoretical guarantees is notoriously hard for planning in latent spaces. Therefore, a modular approach is necessary that learns decision-making directly in robot configuration space so that it can be combined with any classical motion planner to provide theoretical guarantees. Furthermore, learning a cost function, using inverse reinforcement learning [62], is also crucial for kinodynamic and constraint motion planning problems where defining a cost function satisfying all constraints is challenging.