NeuroSMPC: A Neural Network guided Sampling Based MPC for On-Road
  Autonomous Driving
In this paper we show an effective means of integrating data driven
frameworks to sampling based optimal control to vastly reduce the compute time
for easy adoption and adaptation to real time applications such as on-road
autonomous driving in the presence of dynamic actors. Presented with training
examples, a spatio-temporal CNN learns to predict the optimal mean control over
a finite horizon that precludes further resampling, an iterative process that
makes sampling based optimal control formulations difficult to adopt in real
time settings. Generating control samples around the network-predicted optimal
mean retains the advantage of sample diversity while enabling real time rollout
of trajectories that avoids multiple dynamic obstacles in an on-road navigation
setting. Further the 3D CNN architecture implicitly learns the future
trajectories of the dynamic agents in the scene resulting in successful
collision free navigation despite no explicit future trajectory prediction. We
show performance gain over multiple baselines in a number of on-road scenes
through closed loop simulations in CARLA. We also showcase the real world
applicability of our system by running it on our custom Autonomous Driving
Platform (AutoDP).
Kaustab Pal, Aditya Sharma, Mohd Omama, Parth N. Shah, K. Madhava Krishna
# NeuroSMPC: A Neural Network guided Sampling Based MPC for On-Road Autonomous Driving

###### Abstract

In this paper we show an effective means of integrating data driven frameworks to sampling based optimal control to vastly reduce the compute time for easy adoption and adaptation to real time applications such as on-road autonomous driving in the presence of dynamic actors. Presented with training examples, a spatio-temporal CNN learns to predict the optimal mean control over a finite horizon that precludes further resampling, an iterative process that makes sampling based optimal control formulations difficult to adopt in real time settings. Generating control samples around the network-predicted optimal mean retains the advantage of sample diversity while enabling real time rollout of trajectories that avoids multiple dynamic obstacles in an on-road navigation setting. Further the 3D CNN architecture implicitly learns the future trajectories of the dynamic agents in the scene resulting in successful collision free navigation despite no explicit future trajectory prediction. We show performance gain over multiple baselines in a number of on-road scenes through closed loop simulations in CARLA. We also showcase the real world applicability of our system by running it on our custom Autonomous Driving Platform (AutoDP).

## I Introduction

On road autonomous driving entails persistent and consistent real-time decision making in often highly dynamic and evolving scenes. To accomplish this most trajectory planning frameworks employ a scheme of generating multiple candidate trajectory proposals and scoring them according to an appropriate cost function [3, 23, 8]. The candidate trajectories are typically obtained from large scale driving data [3] or by sampling from a parameter distribution that defines a trajectory [23]

As an alternative sampling based optimization paradigms have been popular in the high dimensional planning literature that seamlessly integrate dynamical model of the systems over which the trajectory plans are computed. The major concern however here is the disadvantage in rolling out trajectories in real-time

In this paper we propose a novel framework that interleaves neural network driven prediction of finite horizon controls with samples drawn from the variance centred around the mean predicted by the network. The deep network outputs a vector of controls that constitute the optimal mean control over a finite horizon. The network is supervised with the best controls from the elite samples of the sampling based control framework [2]. Typically this mean is expected to be close to the global optimum [1, 15]. The best rollout sample is one that optimizes a scoring or cost function detailed later.

We retain the advantages accrued due to the original framework by not executing the network output controls but choosing the best rollout candidate from the samples around the network output controls. At the same time we overcame the curse of time complexity of such frameworks by selecting the optimal mean obtained from the network that is at-least three orders faster. It is to be noted that almost all such formulations involve many iterations of the following two steps to converge to the optimal control :

* Selection of a subset of the candidate samples (the elite set) based on a scoring function.
* Update of the mean in accordance with the elite samples and re-sampling from the newly updated distribution.

Apart from this, the network through its spatio-temporal convolution (3D-CNN) architecture implicitly learns the evolution of dynamic actions over time without resorting to explicit trajectory prediction. Despite the lack of explicit trajectory prediction the executed trajectory is collision free in diverse set of on-road navigation scenarios with many dynamic actors. We attribute this to extremely low latency, high frequency control roll-outs possible through the deep network.

The paper contributes specifically in the following ways:

1. A neural network interleaved sampling based optimal control that computes finite horizon control rollout in real-time thereby making it suitable for real-time self driving for on-road scenes.
2. We show 3D convolutions that convolve spatial and temporal components of a time sequenced Birds Eye View (BEV) layout learns implicitly the future trajec

Fig. 1: **Run on a real car: We show that our model trained on synthetic data from a simulation is able to run successfully in a real world setting. We ran the model on a real self driving car that avoids an obstacle in front of it in a controlled on-campus scene to ensure safety. The sequences of images in the top row shows that the ego-vehicle (highlighted in green) is able to avoid the obstacle (highlighted in yellow). The sequences of images in the bottom row shows the corresponding accuracy grid maps along with it’s predicted trajectory (red) and the best sampled trajectory (green) in the ego-vehicle coordinate frame.**
tories of the dynamic obstacles so much so the planner based on the network output avoids collision with dynamic obstacles despite lack of explicit trajectory prediction into the future.
3. A number of closed loop simulations in diverse scenarios in the CARLA simulator as well as real-time on campus navigation through our AutoDP confirm the efficacy and real-time veracity of the proposed framework.
4. Moreover the comparative analysis on compute time with other sampling based optimal control frameworks [2, 1] clearly depicts the vast performance gain of the proposed method.
5. Further, we propose _RoadSeg_, a system for on board real-time road segmentation in known environments that can efficiently generate BEVs in resource constrained setups.

## II Related Work

A large portion of on-road driving literature is devoted to getting layout representations [11, 16, 14], agent trajectory prediction [9, 20] and end to end trajectory generation [14, 23]. Most of the trajectory generation frameworks involve choosing the best possible trajectory out of an elite set that is typically obtained from large dataset of driving examples or expert trajectories [23, 14]. However the expert trajectories despite the diversity can be sub-optimal and need not be the best response for a given input scenario. Most of these methods do not show their trajectory planning in closed loop simulation scenarios as they are evaluated in pre-recorded datasets that prevent perception and planning in coupled closed loop settings. Also none of the above methods showcase their formulation on a real self driving vehicle in real world on-road scenes.

Elsewhere, in the manipulation planning community, sampling based optimal control [17, 22] have become popular essentially due to the derivative free blackbox optimization feature of such frameworks. Such sampling based trajectory optimization methods suffer from the curse of time complexity with a number of variants being proposed to overcome this disadvantage. For instance [2] resorts to efficient parallelization of controls while [15] contributes through sample efficient methods. Whereas in [1] gradient based updates of sample parameters leads to enhanced performance. Nonetheless these methods still need iterative improvement of the sample parameters to reach optimum mean control values that preclude their adaptation to on-road real time autonomous driving applications.

In contrast to above methods we showcase real time closed loop simulations on a number of CARLA scenes along with a closed loop implementation on our AutoDP to achieve point to point on campus autonomous driving. Moreover our low latency control rollout facilitates navigation in dynamic scenes without collisions despite the lack of explicit trajectory prediction into the future of the dynamic actors.

## III Methodology

### _Problem statement_

For an on road driving scene, given a point cloud and the global path that our AutoDP needs to follow, we consider the problem of generating occupancy grid maps from the point cloud data and use the occupancy grid map to generate controls that drives the AutoDP towards obstacle free regions on the road while moving along its global path. The state of the vehicle at timestep \(h\) is represented by the vector \(\widetilde{\mathbf{x}}_{h}=\left[x_{h},y_{h},\theta_{h}\right]\) where \(x_{h}\) and \(y_{h}\) represents the \(x\) and the \(y\) coordinate of the vehicle respectively and \(\theta_{h}\) is the orientation of the vehicle. The controls for the vehicle at timestep \(h\) are represented by the vector \(\mathbf{u_{h}}=\left[v_{h},\omega_{h}\right]\) where \(v_{h}\) and \(\omega_{h}\) are the velocity and the angular-velocity of the vehicle respectively.

### _Pipeline_

We show the pipeline of our proposed framework in Fig. 2. The point-cloud from our AutoDP is first passed through the RoadSeg Network to segment the points that belong to the road. The points that do not belong to the road are considered as obstacles. A down-projection operation is performed to the obstacle points to create a birds-eye view (BEV) occupancy grid map. 5 of these BEVs are then stacked together and passed through a 3D-CNN CNN based architecture to get the mean controls. These mean controls are used as the mean of a sampling based MPC to sample more trajectories. The best trajectory is chosen from amongst these trajectories to be executed by our AutoDP.

#### Iii-B1 **RoadSeg Network**

For the system to work in real time on a resource constrained setup, its essential that the individual building blocks have minimal GPU footprint and the highest possible FPS. The major bottleneck in our pipeline is road-segmentation, which is usually a GPU-heavy process. We can take advantage of the fact that our AutoDP is meant to operate in a known map. We can pre-segment the road using heavy neural networks, then distill this information with a smaller network that can work in real time. The resultant network is like an implicit representation of the operational area wherein the robot can navigate [7]. To do this, we first create a map of the operational area of our vehicle using LEGO-LOAM [19]. This map is used for global localization and global planning, similar to other autonomous driving platforms [6]. The map is created with our custom calibrated-lidar-camera setup. Since, we have 3D points and corresponding RGB values with the calibrated setup, we can use off the shelf segmentation models to segment out the road in the 3D map offline. The choice of the segmentation model is arbitrary. Once, the road points are segmented out, we can distill this information with a smaller neural network, which we call the RoadSeg network. This network takes in the \(x,y,z\) points which are passed to a NERF-like positional encoder [12] followed by few MLP layers. The network's job is to predict if the current points belong to the road or not and is trained using the offline-segmented LEGO-LOAM map as ground-truth. Though the LEGO-LOAM map is very
sparse, the positional encoding allows the network to learn general spatial trends and makes it capable of segmenting out the road of the entire (much denser) lidar point clouds at inference. Its also capable of segmenting out regions that were previously not seen in the map creation process (like those occupied by other vehicles). The RoadSeg network is shown in Fig 3. We show the advantage of using our RoadSeg Network over other baselines in Sec. IV-B

#### Iv-A2 **3D-CNN based Neural Network**

The occupancy grid map at the current timestep \(T\) along with the past 4 occupancy grid maps and the global path are concatenated into a \(6\times H\times W\) spatio-temporal tensor. This spatio-temporal tensor is then passed into our 3D-CNN based encoder architecture to extract a feature vector. The feature vector is then passed through 4 fully-connected layers to get an output vector of dimension \(H\times 2\) which represents the optimal controls (velocity and angular-velocity). The feature vector from the encoder architecture encodes the spatio-temporal correlations between the occupancy grid maps. For our implementation we have used a slightly modified version of MobileNet-V2 [18] as the encoder.

#### Iv-A3 **Sampling based MPC**

The objective is to generate controls for short horizons of \(H\) timesteps into the future. The output of the 3D-CNN based Neural Network is used as a mean for a Gaussian distribution. We now sample \(N\) control sequences of length \(H\) from this Gaussian distribution and roll them out using the unicycle kinematics model of the vehicle to get \(N\) trajectories. Each of these trajectories are then scored with two cost functions:

* _Smoothness cost_: The smoothness cost ensures that we always give more preference to a trajectory with a smooth change in it's linear and angular velocities. \[\hat{c}_{ang}(\mathbf{\hat{x}_{\_}{\_}{\mathit{l}}},\mathbf{u_{\_}{ \mathit{l}}})=\sqrt{\sum_{\_}{h=1}^{H-1}(u_{w_{\_}{h}}-u_{w_{\_}{h-1}})^{2}}\] (1) \[\hat{c}_{\_}{\mathit{l}in}(\mathbf{\hat{x}_{\_}{\mathit{l}}}, \mathbf{u_{\_}{\mathit{l}}})=\sqrt{\sum_{\_}{h=1}^{H-1}(u_{v_{\_}{h}}-u_{w_{ \_}{v-1}})^{2}}\] (2) where \(u_{\_}{v}\) and \(u_{\_}{w}\) represents the linear and angular velocity components of the sampled controls respectively.
* _Obstacle avoidance cost_: The obstacle avoidance cost ensures that the trajectories are not colliding with any obstacles (occupied cells in the occupancy grid map). \[d(\mathbf{\widetilde{x}_{\_}{\mathit{h}}},\mathbf{\widetilde{\mathit{o}}_{\_} {\mathit{h}}})=\mid\mathbf{\widetilde{x}_{\_}{\mathit{h}}}-\mathbf{\widetilde{ \mathit{o}}_{\_}{\mathit{h}}}\mid\mid_{2}\] (3) Here \(\mathbf{\widetilde{x}_{\_}{\mathit{h}}}\) and \(\mathbf{\widetilde{o}_{\_}{\mathit{h}}}\) are the states of the agent and the obstacle respectively at time-step \(h\). The agent and the obstacles are represented as circles. Let \(r_{\_}A\) and \(r_{\_}O\) be the radius of the agent and the obstacle respectively. The state \(\mathbf{\widetilde{x}_{\_}{\mathit{h}}}\) is said to be in collision with the obstacle state \(\mathbf{\widetilde{o}_{\_}{\mathit{h}}}\) if the euclidean distance \(d(\mathbf{\widetilde{x}_{\_}{\mathit{h}}},\mathbf{\widetilde{o}_{\_}{\mathit{h }}})\) between the

Fig. 3: The RoadSeg network takes in the point postions as input, passes them thorough a nerf-like positional encoding followed by a fully connected layer. The result is a segmentation score (road/not-road) for each point.

Fig. 2: **Pipeline: Each Point-cloud from our AutoDP is first passed through the RoadSeg Network to segment the points that belong to the road. The non-road points are considered as obstacles. A down-projection operation is performed to the obstacle points to create a BEV occupancy grid map. 5 past BEVs are then stacked together and passed through a 3D-CNN architecture to get the mean controls for a short-horizon. These mean controls are then used as the mean of a sampling based MPC to sample more trajectories. The best trajectory is chosen from amongst these trajectories to be executed by our AutoDP.**
agent's position and the obstacle's position is less than or equal to the sum of their radius.

\[\hat{c}_{obs}(\mathbf{\hat{x_{i}}},\mathbf{u_{i}})=\begin{cases}\infty\,\ \text{if}\ d(\mathbf{ \hat{x_{h}}},\mathbf{\hat{o_{h}}})\leq r_{A}+r_{O},\ h\in[0,H)\\ 0\,\ \text{otherwise}\end{cases} \tag{4}\]

where \(\mathbf{\hat{x_{i}}}\) and \(\mathbf{\hat{u_{i}}}\) are the i-th trajectory and controls out of the \(N\) sampled trajectories and controls.

The final cost \(\hat{C}(\mathbf{\hat{x_{i}}},\mathbf{u_{i}})\) for each trajectory is calculated as the weighted sum of the three costs

\[\hat{C}(\mathbf{\hat{x_{i}}},\mathbf{u_{i}})=w_{ang}\hat{c}_{ang}(\mathbf{\hat {x_{i}}},\mathbf{u_{i}})+w_{lin}\hat{c}_{lin}(\mathbf{\hat{x_{i}}},\mathbf{u_{i}})+\] \[w_{o}\hat{c}_{obs}(\mathbf{\hat{x_{i}}},\mathbf{u_{i}}) \tag{5}\]

where \(w_{ang}\), \(w_{lin}\) and \(w_{o}\) are the weights chosen by the user. The trajectory with the smallest cost is chosen as the best trajectory and the controls at \(h=1\) are used to drive the ego-vehicle after which we recompute a new trajectory again. Figure 4 shows the output of the neural network and the best trajectory.

### _Dataset_

Each sample in our dataset consists of a sequence of 5 occupancy-grid maps along with the global path as the input and the short horizon optimal control as the output. We used the CARLA simulator [5] to create a dataset. Obstacles were spawned with varying velocities randomly amongst the CARLA maps. Each occupied cell in the occupancy grid map is used as an obstacle while planning the optimal controls using the Sampling based MPC (SMPC). Since our SMPC operates in the center line reference frame (CRF), we transform all the obstacles to the frame attached to the global path[21]. This allows us to treat curved roads as straight roads and plan feasible trajectories easily without taking the curvature bound constraints. To plan the trajectories that avoids collision with dynamic obstacles whose velocities are known from the CARLA ground truth data, we used the SMPC with the MPPI update rule. The optimal trajectories are then transformed back to the global frame so that the trajectories are within the curvature bounds of the roads in the global frame.

During inference the BEV doesnot need to be converted from the global frame to the CRF which is a big advantage of our method. Also the 3D-CNN layers learns the dynamic nature of the scene implicitly so we also don't need a separate obstacle trajectory predictor.

## IV Experimental Evaluation

### _Qualitative Analysis_

In this section, we show a qualitative comparison between the trajectories generated by our NeuroSMPC (NSMPC) formulation and trajectories generated by Model Predictive Path Integral (MPPI) and gradient based Cross-Entropy Method (GradCEM) approaches. All the methods generate a short horizon (30 timesteps) control (velocity and angular-velocity) sequence. These controls are rolled out using the unicycle kinematics model to generate the trajectory. The NSMPC takes the past 5 birds-eye view occupancy grid maps and the global path as input. For both the MPPI and GradCEM approach, the occupied cells in the occupancy grid map are considered as obstacles. In both the approaches the distribution is updated iteratively to get the mean controls. In Fig. 5 we show the trajectory generated by our method compared to the trajectories generate by MPPI and GradCEM. We observe that the trajectory generated from our NSMPC formulation is qualitatively similar to the trajectories generated from the iterative approaches in an empty straight and curved road scenario and in a road with obstacles.

### _Quantitative Analysis_

**RoadSeg Network:** The RoadSeg Network, though only limited to the current operational area, allows for a much faster and accurate road segmentation with minimal GPU footprint. We compare our RoadSeg Network with two other baselines: RANSAC based plane segmentation and LSeg[10] in terms of computation time and GPU memory utilization. From Table I, we can observe that we outperform other approaches by a significant margin. The lesser GPU footprint of RoadSeg allows us to run the entire NeuroSMPC pipeline on a single laptop in our AutoDP.

**Neural guided Sampling based MPC:** We compare our method with two other sampling based MPC baselines: MPPI and GradCEM, during inference with respect to the number of iterations required to reach an optimal trajectory. In Table

Fig. 4: This figure demonstrates the output trajectory (red) of the neural network, the sampled trajectories (blue) from a Gaussian distribution with the neural network output as the mean, and the best trajectory (green) selected from the sampled trajectories after scoring them with the smoothness and obstacle avoidance cost functions.

\begin{table}
\begin{tabular}{|c|c|c|} \hline
**Approach** & **GPU Memory Utilization (MB)** & **Inference Time (sec)** \\ \hline \hline RoadSeg & 790 & 0.0015 \\ \hline RANSAC & - & 0.08 \\ \hline LSeg & 3492 & 0.018 \\ \hline \end{tabular}
\end{table} TABLE I: RoadSeg Performance compared against RANSAC and LSeg. RoadSeg results in lesser GPU footprint and faster inference.
II, we show that using our method, we achieve an optimal trajectory in single-shot, whereas using MPPI or GradCEM we need to iteratively update the mean to get an optimal trajectory. For MPPI we need to update the mean of the distribution for 5 iterations whereas for GradCEM we need to update the mean for 3 iterations.

### _Sim2Real_

One of the key challenges of a deep learning based system is to ensure that a model trained on a dataset collected from a simulator can also run effectively in the real world. While it is very easy to generate large volumes of data from a simulator, it is very difficult to accurately model real world physical phenomenons like friction, impact and uncertainty in a simulator. This results in the dataset from simulator not representing the real physical world accurately. We were able to overcome this issue by using a simplified form of data. The occupancy grid map from the simulator lidar is very similar to the occupancy grip map from the real world Lidar. This is because the lidar configuration in the simulator is the same as our AutoDP. The only thing missing in the simulator is the sensor noise which we overcame by perturbing the lidar points by a small amount randomly. After training the model on synthetic data collected from a simulator, we were able to run the model on our AutoDP. Fig. 1 shows the execution of the model on our AutoDP while avoiding an obstacle in a controlled on campus scene. While there were still some instances where the model's output was colliding with the obstacle, the sampling nature of our formulation ensures that we still get an obstacle avoiding trajectory from the distribution of trajectories sampled from the model's output as mean.

### _Why Sample?_

The Neural Network's output is stochastic in nature and is not always guaranteed to avoid collisions with the obstacles. To ensure safety we need to make sure that the trajectory being executed by the AutoDP is always collision free. To solve this we sample a distribution of trajectories by using our model's output as a mean. The sampled trajectories are then scored using the cost functions mentioned in Section III-B.3. The best trajectory is then selected from the sampled trajectories and executed by the AutoDP. Fig. 6 shows an example scenario where the Neural Network output is colliding with the obstacle in front. The best trajectory (green) chosen from sampled trajectories is then executed by the AutoDP to avoid colliding with the obstacle.

## V Implementation and Training

The MPPI, GradCEM and our Neural guided Sampling based MPC has been implemented using the PyTorch [13] library and trained on a system with a Intel Core i7-5930K (6 Core 12 threads) CPU and one NVIDIA RTX A4000 GPU with 16 GB VRAM and 64 GB RAM. We used the Lion optimizer [4] with a learning rate of 0.001 to train our network parameters.

## VI Conclusion

This paper proposed NeuroSMPC, a novel neural network guided Sampling Based Optimal Controller as an effective mechanism of interleaving single shot optimal inference with sampling based frameworks. NeuroSMPC overcomes the entailment of iterative resampling of sampling based optimal control frameworks by inferring single shot optimal trajectory rollouts and yet retains the advantage of sample diversity by sampling controls around the predicted rollouts. NeuroSMPC trajectory rollouts are similar to sampling based optimal control formulations [1, 2] yet come at a much faster clip thus making it suitable for real time settings like on-road autonomous driving. NeuroSMPC's spatio-temporal convolution architecture learns implicitly the dynamic nature

\begin{table}
\begin{tabular}{|c|c|} \hline
**Approach** & **No. of iteration** \\ \hline \hline NSMPC (Ours) & 0 \\ \hline MPPI & 5 \\ \hline GradCEM & 3 \\ \hline \end{tabular}
\end{table} TABLE II: Comparison of SMPC (our method) with MPPI and GradCEM in terms of no. of update iterations required to get an optimal trajectory.

Fig. 5: We show a qualitative comparison of the trajectories generated by NSMPC (our method), MPPI and GradCEM in three different driving conditions: Straight empty road, Curved empty road and a road with Obstacles. The grey line denotes the future trajectory of the dynamic obstacles.

Fig. 6: **Why sample?** Since the NN output may not always be collision free, we sample a distribution of trajectories (red) by using the NN output (blue) as the mean of a Gaussian distribution. The best trajectory (green) is then selected to be executed by the AutoDP.
of scenes without the need for explicit prediction of future states as it seamlessly avoids dynamic obstacles through an implicit understanding of their future evolution. NeuroSMPC has been tested in various CARLA scenes and a sim2real transfer on AutoDP (AutonomousDrivingPlatform) for on-road campus autonomous driving establishes its efficacy.

## VII Acknowledgements

This work has partially been funded by Centre for Artificial Intelligence & Robotics (CAIR).
