Computer Science
Computer Science-2021.gz:143284
Computer Science
1cf68dec25614deb75d7976d380692bc247e660d
MPC-MPNet: Model-Predictive Motion Planning Networks for Fast, Near-Optimal Planning under Kinodynamic Constraints

Kinodynamic Motion Planning (KMP) is to find a robot motion subject to concurrent kinematics and dynamics constraints. To date, quite a few methods solve KMP problems and those that exist struggle to find near-optimal solutions and exhibit high computational complexity as the planning space dimensionality increases. To address these challenges, we present a scalable, imitation learning-based, Model-Predictive Motion Planning Networks framework that quickly finds near-optimal path solutions with worst-case theoretical guarantees under kinodynamic constraints for practical underactuated systems. Our framework introduces two algorithms built on a neural generator, discriminator, and a parallelizable Model Predictive Controller (MPC). The generator outputs various informed states towards the given target, and the discriminator selects the best possible subset from them for the extension. The MPC locally connects the selected informed states while satisfying the given constraints leading to feasible, near-optimal solutions. We evaluate our algorithms on a range of cluttered, kinodynamically constrained, and underactuated planning problems with results indicating significant improvements in computation times, path qualities, and success rates over existing methods.


I. INTRODUCTION
The motion planning problem is to find a path connecting the given start and goal states while satisfying all the desired constraints on the robot motion. Geometric motion planning is a simple instance that considers only collision-avoidance constraints with states representing the robot's kinematics variables (e.g., position, joint-angles) [1]. However, in KMP, the state comprises position, velocity, and sometimes acceleration, and the robot motions are required to satisfy both kinematic (e.g., collision-avoidance) and dynamics (e.g., velocity and acceleration) constraints, which makes the problem PSPACE-hard and computationally demanding [1].
The KMP has a broad range of applications from torqueconstrained robot manipulation to speed racing [2] and performing acrobatic motions [3]. The existing state-of-theart methods solve KMP through Sampling-based Motion Planners (SMPs) [1] by constructing a tree that originates from a start state and expands by searching the entire statespace to reach the given goal state. The edges of the tree, connecting any two intermediate states, are formed by a local steering function. The local steering often requires solving a boundary-value problem (BVP) through trajectory optimization, which is known to be NP-hard [4] and fails when the boundary constraints are not satisfied. A recent development led to another sampling-based KMP approach [5], called Stable Sparse-RRT (SST), that circumvents solving BVPs by using a random shooting method for steering and achieves asymptotic optimality. However, it still takes long computation times from tens of seconds to minutes as environments become more complicated due to uniform exploration of state and control spaces.
In this paper, we propose an imitation learning-based approach named Model-Predictive Motion Planning Networks (MPC-MPNet) 1 . It has a deep neural networks-based generator and discriminator, which, once trained using expert data, outputs feasible paths that satisfy the given kinodynamic constraints. Our approach extends the Motion Planning Networks (MPNet) [6] and leverages collision-aware Model Predictive Control (MPC) methods in the planning loop for parallelizable local steering. Note that the original MPNet framework [6] considered only geometric planning problems and relied on bidirectional path expansions and re-planning steps for finding path solutions to given problems. In KMP, the bidirectional path extensions and re-planning steps to repair in-collision path segments often lead to discontinuity and infeasible paths. Therefore, in MPC-MPNet, we propose novel planning algorithms that perform only forward path expansion and avoid re-planning by building a set of informed possible paths. We evaluate our approach in challenging under-actuated robotics problems in complex, cluttered environments. Our results show that MPC-MPNet outperforms state-of-the-art planning algorithms in computational speed and generalizes to unseen planning problems with high success rates.

II. RELATED WORK
This section presents a brief overview of related work done in the past for solving KMP problems. KMP algorithms depend on the local steering function to satisfy the motion constraints between any two given states. The local steering methods are usually implemented either as (i) random shooting methods which uniformly samples control sequences [7][8] [5], (ii) predefined motion primitives [9], or (iii) local trajectory optimization solvers [10] [11][12] [13].
Sampling random control sequences for shooting method has the advantage of worst-case theoretical guarantees. For instance, the SST approach [5] leverages random exploration of control and state spaces to provide the worstcase asymptotic completeness and optimality guarantees. However, these methods struggle in higher-dimensional and  Fig. 1: MPC-MPNetPath In each iteration, the neural generator predicts a batch of next states from a given current and select a collision-free state with a minimum estimated cost for the tree expansion using MPC.
cluttered planning spaces since uniform exploration takes large computation times to determine a feasible path solution.
In contrast, the motion primitives methods generate a local database of optimal controllers in their offline stage [9] and use them for steering under KMP algorithms. However, as the motion primitive set is finite, it usually does not capture the entire control space and lacks completeness guarantees. The trajectory optimization methods formulate local steering as an optimization problem and iteratively solve them for computing action sequences connecting the given states. For instance, [14][12] locally linearize the system and obtain Linear Quadratic Regulators' (LQR) parameters using optimization for steering. Tedrake et. el [13] extend LQR methods to construct a tree connecting states within stability regions defined by LQR funnels. However, these methods add a substantial computational burden when constructing trees and do not apply to online planning problems. In a similar vein, Xie et. el [10] formulated local steering as BVP and used optimization to find their solutions. These BVP solvers operate in conjunction with traditional SMPs [1] for finding solutions to KMP problems. However, their approach often collapse when the boundary constraints are not satisfied.
Another class of methods in KMP learn local controllers for steering through reinforcement and imitation learning. For instance, [15] uses reinforcement learning (RL) to acquire local policy for steering within SMP methods. Similarly, [16] [17] constructs high-level landmarks, which are later connected by local RL policies. However, these methods inherit RL limitations such as requiring exhaustive interactions with their environments for learning. In the imitation learning paradigm, [18] and [19] use expert demonstrations to learn local policies for connecting given states, but they still rely on classical planners to generate those state sequences.
Recent developments overcome the limitations of classical motion planning by incorporating learned planning distributions into their algorithms [6][20][21] [22]. These learningbased planners quickly generate feasible motion sequences online for finding path solutions but mostly consider geometric planning problems. Among them, MPNet [6] [20] generates end-to-end paths by exploiting learned distributions in a bidirectional manner throughout its planning and replanning process. MPNet has also been extended to consider kinematic [23] and non-holonomic [24] constraints. However, these extensions still rely on bidirectional planning, which often leads to discontinuities and infeasible paths in KMP problems.

III. PROBLEM DEFINITION
Let C denote a configuration space (C-space) of a mechanical system, where collision and collision-free regions are represented as C obs and C f ree = C\C obs , respectively. Let X denote the state space in which a state, x = (c,ċ) ∈ X , contains a configuration c ∈ C and the time derivativeċ. Like C-space, the state space also contains the collision X obs and collision-free X f ree state spaces. The system's dynamics model, represented by an implicit set of equations, can be formulated asẋ = f (x, u), where u denotes the control input to a system from a feasible control set U. In general, the objective of KMP for the given initial state x init and goal region X goal ⊂ X f ree is to find a collision-free

IV. MODEL PREDICTIVE MOTION PLANNING NETWORKS
In this section, we present MPC-MPNet, an end-to-end learning-based KMP algorithm. MPC-MPNet iteratively generates waypoints and local steering trajectories to construct collision-free paths connecting the start and goal states. It includes a neural generator, discriminator, and two novel planning algorithms named MPC-MPNetPath and MPC-MPNetTree. The main components of our approach are described as follows.

A. Observation Encoder
The observation encoder embeds the workspace information, represented as voxel maps v, into latent features Z containing critical anchor points for the underlying neural generator and discriminator. The voxel maps are volumetric with dimensions L × W × H × C, corresponding to length, width, height, and number of channels, respectively, and are usually processed by 3D convolutional neural networks (CNNs). However, we convert the voxel maps into voxel patches with dimensions L × W ×Ĉ, whereĈ = HC, and use the 2D-CNNs for learning the embeddings. This is because 3D-CNNs are known to be computationally inefficient as their representations are inherently cubic and usually contain empty volumes [25].

B. Neural Generator
The neural generator G, with parameters θ g , is a stochastic neural model that outputs intermediate waypointsx t+1 for

Fig. 2: MPC-MPNetTree
The neural generator predicts a batch of next states from nearest neighbors of random states inside the search tree. Our parallelized MPC finds the local controllers between randomly selected start and neurally generated next states for extending multiple branches of tree simultaneously towards the given goal state.
the given environment encoding Z, robot's current state x t ∈ X f ree and goal state x goal ∈ X goal , i.e., The neural generator adopts Dropout [26] in almost every layer to generate stochastic samples. The Dropout randomly selects neurons in each trained network layer, resulting in a sliced model in every forward pass. This operation leads to randomness adapted from expert data. In contrast, other techniques, such as input Gaussian noise, are agnostic of underlying data distribution and incurs difficulty in training deep neural models [27]. Our neural generator is trained end-to-end with the observation encoder by optimizing the following mean square error between the predicted statesx and the demonstration trajectories' states x * , i.e., where N p is the total number of paths and T i is the length of each path i in the dataset.

C. Neural Discriminator
Due to stochasticity in the neural generator, the predictions are usually scattered towards the given target. To select the best state from the given set, we introduce a neural discriminator network that predicts a given state's time-toreach the desired target. Our planning procedures leverage the time-to-reach predictions as a cost to prune outputs of the neural generator.
Hence, the discriminator D, with parameters θ d , takes environment embedding Z, the robot state x t and x goal as input, and predicts the cost as: The neural discriminator is trained to minimize the mean square error between the predicted costs and the real costs: where d * i,k is the cost to goal at waypoint k in the path i. To balance the positive and negative training samples and enhance the discriminator's performance, we augment the training data by assigning large penalties to in-collision waypoints and unreachable transition pairs. As a result, the trained discriminator predicts high costs for invalid states and eliminates anomalous waypoints.

D. Model Predictive Control
To satisfy the kinodynamic constraints during tree/path expansion, we utilize MPC as a steering function, a strategy widely used for optimal control problems. We use MPC for several reasons, including implementation simplicity, lower computational complexity, and parallel computation potential. Furthermore, compared to BVP solvers, MPC models the trajectory generation process as an initial-value problem. Therefore, MPC does not collapse when the terminal state is unreachable and instead returns a nearest, valid terminal state. In contrast, BVP solvers fail in such situations because boundary conditions are not satisfied.
MPC takes a current x t , a target statex t+1 , and generates an optimized trajectory σ t = [(x, u, τ ) t ] minimizing the cost between the propagated terminal state andx t+1 . Our MPC solver is implemented with Cross Entropy Method (CEM) [28] and time-elastic-band approach to optimize both control and their duration sequences [29]. CEM takes advantage of the Monte Carlo method and importance sampling to estimate the optimal trajectories distribution iteratively.
Algorithm 1 outlines our MPC approach. Using sampled controls and durations from a parameterized distribution (u, τ ) i ∼ D(u, τ ; θ mpc ), we propagate and generate the steering trajectory σ i , from the given starting state x t . These propagations are ranked through a cost function defined as is the distance metrics between given states and d c (·) is a collision penalty function. The cost function selects the elite samples, i.e., the propagated states with the lowest scores. These elite states are used to update MPC parameters θ mpc by minimizing the cross-entropy loss between the distributions of steered terminal states and the target statex t+1 . Note that, the collision penalty discourages in-collision trajectory expansions. Furthermore, in our implementation, we assume the distribution D(u, τ ; θ mpc ) of controls and their durations at each time step to be Gaussian distributions.

E. Parallelization
We use neural networks to process multiple waypoints as a batch to improve performance and implement our MPC algorithm using tensors on Graphic Processing Units (GPUs) for parallel processing. With parallel computation, Select elite samples based on their scores; 6 Update θ mpc with elite samples; 7 Update optimal trajectory with the best σ * i 8 return σ * i ; the neural networks and MPC are accelerated to process up to N B ∈ N samples simultaneously. To represent batch parallel processing, we introduce new notations denoting all vectorized inputs in batch form using the symbol B, i.e., where B t , B goal , and B Z correspond to the batch of current states, desired states, and observation encodings.
During execution, our stochastic generator outputs a variety of next states B t+1 , and the discriminator predicts their associated costs B dt+1 as: For the given start B t and targetB t+1 states, our parallelized MPC generates their corresponding local kinodynamic trajectories as: where is a batch of local trajectories at time t. if Reached(T, x goal ) then 13 return ExtractPath(T ); 14 return ∅; if Reached(T, x goal ) then 10 return ExtractPath(T ); 11 return ∅;

F. Planning Algorithms
In this section, we present our planning algorithms that balance exploration-exploitation in their different ways for quickly finding a path solution with a unidirectional tree expansion. Figure 1 and Algorithm 2 outline our MPC-MPNetPath algorithm. The procedure begins by generating a batch of new samples using the neural generator G. The discriminator D prunes the generated batch B t by selecting a samplex t+1 with a minimum cost d to reach the given target x goal . The MPC module takes the current node x t and selected next statex t+1 to perform the kinodynamic steering, leading to a local trajectory σ t . The terminal state of σ t is the resulting valid state x t+1 (as close as possible tox t+1 while satisfying constraints) after the execution of u on state x t for time duration τ . The local trajectory is added to the tree if it is valid. In the case of invalid local trajectory, i.e., not collision-free, a random node is selected from the tree and is treated as new current state x t for the next planning iteration. Once, the goal is reached, an end-to-end path is extracted connecting the given start and goal states under kinodynamical constraints.

MPC-MPNetTree:
This method leverages (i) the innate capacity of neural networks to process batches, and (ii) our parallelized MPC framework to expand multiple nodes of the search tree simultaneously, directed towards the given target states B goal . The algorithm is summarized in Figure 2 and Algorithm 3. In each iteration, MPC-MPNetTree samples a set of random states B rand using the RandomSample function and finds their corresponding nearest neighbors in the tree by calling the NearestNeighbor function. These nearest nodes are treated as current set of states B t for the underling MPC-MPNet procedures, i.e., the generator outputs the next batch of samples and MPC computes their local trajectories B σt . The valid local trajectories are added to the search tree, and the final path is extracted once the tree reaches to the given goal state.

V. IMPLEMENTATION DETAILS
This section describes the necessary implementation details of our frameworks with their training and testing environments. We implement our neural modules using Pytorch and export them to C++ with Torchscript. Our parallelized MPC algorithm follows standard GPU programming. For the training and testing environments, we consider the following kinodynamically constrained, cluttered environments with schematics shown in Fig. 4.
2) Cartpole: The cartpole dynamics are used as specified in [31]. The state space is defined as: 2]. and the control space is defined as [−300, 300]. We randomly place seven rectangular obstacles in the environment to challenge and restrict the cartpole motion.
3) Car: This is a 2D first-order car system, where the state space is of 3DOF, including position and orientation. We randomly place five rectangular obstacles in the workspace, and limit the distance between obstacles to ensure narrow passages.

4) Quadrotor:
We define the quadrotor dynamics as in [32]. The state space is defined as: [p, q,ṗ, ω], where p and q denote the position and orientation of the quadrotor, respectively, with their corresponding time derivatives, indicating velocity, represented asṗ and ω. The control space is 4 dimensional with bound [−15, −5] × [−1, 1] 3 . We randomly place 10 obstacles in the workspace space and ensure the scene is cluttered.
In each of the cases mentioned above, we set up 10 environments by random placement of the obstacles, each with 1000-2000 randomly sampled start and goal state pairs. The 10 − 20% of data is used for testing, and the remaining for the training. In our test dataset, we also include two unseen environments for each problem, created by random placement of obstacles. We ensure these environments to be different from the ten seen settings, and for each, we randomly sample 100-200 valid start and goal pairs for evaluation. Hence, in total, our test dataset contains 12 environments for the given setups. The demonstration trajectories for the training data are obtained using the SST algorithm. Furthermore, we obtain the point-cloud of the environments, by randomly sampling the obstacle space and processing them into voxel v of size 32 × 32 × 32.

VI. RESULTS
We present a set of experiments to compare the computation time, path quality, and success rate of MPC-MPNetPath, MPC-MPNetTree, and SST planning algorithms. All experiments were performed on the same system with 32GB RAM, GeForce GTX 1080 GPU, and 3.40GHz×8 Intel Core i7 processor.

A. Comparative Studies
This study compares the computation time, success rate, and path quality (measured by time-to-reach) of MPC-MPNetPath, MPC-MPNetTree, and SST algorithms in Cart-Pole, Acrobot, Car, and Quadrotor environments. All evaluation planning problems were unique and not seen during the training, presenting non-trivial and cluttered environments with underactuated systems. Table I presents the mean computation times with standard deviations across different scenarios in both seen and unseen environments. Figs. 3 show the box-plots of all methods comparing their computation time and path quality inter-quartile ranges for solving all kinodynamic planning problems. Fig. 5 to Fig. 8 show example qualitative results from MPC-MPNet and SST. In all these problems, including unseen scenarios, MPC-MPNetTree and MPC-MPNetPath success rates were between 90 − 100% and 85 − 95%, respectively, comparable to the SST success rates in the given time limit.
It can be seen that compared to SST, MPC-MPNet methods take significantly lower computation times to find similar quality path solutions with comparable success rates. Our experiments also show that for environments with highdimensional state and control spaces such as in Quadrotor, SST's computation times increase exponentially. In contrast, MPC-MPNet still retains its computational benefits from informative waypoint sampling and outperforms SST by a large margin.
Among MPC-MPNetTree and MPC-MPNetPath, we observed that the former method achieves higher success rates and finds better quality path solutions by exploiting GPU-accelerated, parallel computation. Nevertheless, we present CPU-based MPC-MPNetPath and GPU-based MPC-MPNetTree to highlight that both approaches perform better than traditional gold-standard planning methods. Moreover, our algorithms balance exploration and exploitation through randomly sampling current configurations within trees to provide better completeness and comparable success rates as conventional methods.

B. Ablation Studies
To show the impact of the neural discriminator in the planning pipeline, we present an ablation study, comparing the planning time and path quality between (i) MPC-MPNetPath without neural discriminator, which predicts only one state at every iteration, and (ii) MPC-MPnetPath, which generates a batch of waypoints with the neural generator, and selects the best based on an estimated cost by the neural discriminator. All experiments are conducted in the same environmental setup as in the comparative studies.
From the results shown in Table II, we can observe that the neural discriminator contributes to reducing the mean and standard deviation of planning time. It also helps with generating trajectories with a better cost quality. In MPC-MPNet, stochasticity is introduced by dropout layers in the neural generator. This operation generates a variety of samples, some of which might require replanning for the connections. Our process eliminates those cases using the neural discriminator by selecting a state with the minimum time to reach the given target, also resulting in fewer planning iterations and better path quality.

VII. DISCUSSION
In this section, we highlight the worst-case properties of our proposed algorithms named MPC-MPNetPath and MPC-MPNetTree. The former operates MPC without GPU processing and uses discriminator to remove unnecessary states to save computational resources. The latter builds on GPU programming and parallelly computes various nodes for simultaneous local extensions with MPC. Our methods plan without relying on bidirectional tree generation or any replanning as was required in the original MPNet and its variants. However, similar to MPNet, our neural generator does explore the sub-space of given state-space that potentially contains a path solution, thanks to Dropout, that implants randomness into our neural generator adapted from expert demonstrations. Since our generated state-spaces are confined to a subspace, the resulting informed tree also grows in that region due to directed extensions with MPC. To enable our methods to explore the state-space outside the generator's learned state-spaces, we propose a notion of stage-wise exploration.
Our stage-wise exploration strategy balances global exploitation-exploration in three phases. In the first phase, our proposed algorithms are operated for a fixed number of planning iterations N 1 . In the second phase, i.e., after N 1 iterations and until N 2 iterations, our algorithm replaces the MPC-based local controller with a random shooting method. This phase allows exploration in the action-spaces while still keeping state-spaces informed as given by the neural generator. Finally, after N 2 steps and beyond in the third phase, our approach performs full exploration by randomly sampling both state and action spaces, like the SST algorithm. These three phases allow our trees to expand from an inner region, potentially containing a path solution, to an outer region in the worst-case for finding a solution if one exists. In our experimentation, we validated that, similar to MPNet, incorporating staged-wise exploration ensures 100% success rate while still retaining the computational benefits and performing better than classical planning approaches.    Since our methods perform stage-wise exploration of the state and action spaces and eventually explore them entirely over a large number of planning iterations, the resulting approach exhibits probabilistic-completeness. It implies that MPC-MPNet finds a path, if one exists, with the probability approaching to one as the number of planning iterations reaches infinity. The formal proofs can be derived in the same  The problem requires finding a kinodynamically constrained motion of a 12 DOF quadrotor in challenging environments. In these scenarios, our methods were at least 50 times faster than SST, showing our method's scalability to high-dimensional KMP problems.
way as reported in [5]. Furthermore, note that our MPC-MPNetTree method also uses the nearest neighbor search for extending trees. This method is adopted from SST, which selects the best neighbors in terms of their cost from the given start/root state and removes tree edges with relatively higher costs. Based on this nearest-neighbor selector and the random exploration, the SST method [5] also guarantees asymptotic-optimality, i.e., over a large number of planning iterations, their planner will eventually find a minimum cost path solution, if one exists. Since MPC-MPNetTree also does exploration, though in stages, and uses SST's like nearest node selector and pruner, it also exhibits asymptoticoptimality with proofs being similar to as presented in [5].

VIII. CONCLUSIONS
We propose Model Predictive Motion Planning Network or MPC-MPNet, a learning-based algorithm capable of finding solutions in seconds with near-optimal path quality in complex kindynamically constrained scenarios where other state-of-the-art methods take upto minutes for finding a similar quality path solutions. Besides, our experiments show that MPC-MPNet generalizes to unseen tasks and adapts to high-dimensional problems with high success rates. We also show that MPC-MPNet allows parallel computing and exhibits worst-case theoretical completeness and optimality guarantees, making it an ideal planner for practical problems ranging from robot navigation to full-body humanoid motion planning. In our future research direction, we would like to investigate our models' generalization to new planning problem domains. We are also interested in extending our approach to planning under dynamic and manifold kinematic constraints for robot manipulation problems.