Robot Motion Planning as Video Prediction: A Spatio-Temporal Neural Network-based Motion Planner

Interpreting Robot Motion as Video. Based on the above described environment representation scheme, the discrete process of robot motion planning can be viewed as composing a video clip. Suppose we are monitoring the discrete motion of a robot in a 2D/3D environment, where the robot keeps moving from the start state to a fixed goal state. The full set of discrete statuses of the planning task can be then viewed as a sequence of video frames $\mathcal{X}_{0},\mathcal{X}_{1},...,\mathcal{X}_{T}$, where the first frame $\mathcal{X}_{0}$ corresponds to the initial status of the task and the last frame $\mathcal{X}_{T}$ describes the final status of the task (see Figure 1). Noticing that a planned path is essentially the sequential connection of multiple waypoints $x(t)$, we make the design choice that each video frame $\mathcal{X}_{t}$ represents the spatial information of $x(t)$ together with the corresponding environment and goal state $x(T)$ at timestamp $t$.

Pixel Patching. Recall that in principle, the robot state and the goal state can be represented using only one pixel in the second and third channels, respectively. However, in practice, NN models may not be able to capture and learn such one-pixel information efficiently, and thereby potentially affecting planning performance. To address this issue, we propose to augment the representation of the state information using more pixels. To be specific, we mark all the free-space pixels within the patches of size $p$ centered on the robot state pixel and goal state pixel as ones, respectively. Figure 1 provides an example of the patch augmentation. In general, a large $p$ reduces the state resolution, while a small $p$ increases the difficulty of the model to capture the state representation. Therefore, the value of patch size $p$ is properly determined by balancing such trade-offs in our experiments.

Performing Motion Planning as Video Prediction. With such video-based interpretation, the path finding in 2D/3D workspace is essentially equivalent to a video prediction task. In general, given the current and previous observations $\mathcal{X}_{0},\mathcal{X}_{1},...,\mathcal{X}_{t}$ at timestamp $t$, our goal is to predict the new frame $\mathcal{X}_{t+1}$, whose second channel indicates a sample map of the next waypoint $x(t+1)$ with probabilities in $[0,1]$ (see Figure 2). This process is repeated until the next waypoint reaches the goal state $x(T)$. Notice that here we aim to find the path with minimal Euclidean distance, as measured by the cost function

Inspired by the current success of neural networks in video processing applications, we propose to build an end-to-end spatio-temporal neural network to perform this video prediction-based motion planning task. Here, distinct from existing CAE-equipped neural motion planners, our proposed network model directly uses the raw spatial information as the input without any CAE-based encoding. Specifically, for our target motion planning task, if the neural network is properly designed that they are able to directly process the non-encoded environmental and state information, the originally inevitable information loss incurred by CAE encoding can be completely avoided, thereby bringing better planning performance. In such scenarios, the neural network can perform end-to-end learning to predict the most probable length-$K$ sequences of video frames in the future given the previous $J$ observations as follows:

To achieve this goal, we build a spatio-temporal LSTM as the backbone network architecture. As illustrated in Figure 2, the entire neural network consists of four ConvLSTM layers [25], which can directly process our prepared video frame as the input. The key equations are as follows:

where $\sigma$, $*$ and $\odot$ represent the sigmoid activation function, the convolution operator and element-wise product, respectively. Here, for 2D tasks, all the inputs $\mathcal{X}_{t}$, cell outputs $\mathcal{C}_{t}$, hidden states $\mathcal{H}_{t}$, and gates $i_{t},f_{t},g_{t},o_{t}$ are 3D tensors in $\mathbf{R}^{K\times n\times n}$, where $K$ is the number of input dimensions or the number of feature maps (for hidden representations). For 3D tasks, these are 4D tensors in $\mathbf{R}^{K\times n\times n\times n}$ instead. Tensors $\mathcal{W}$ and $b$ are the training parameters, which are represented by $\mathcal{W}_{1-4}$ in Figure 2 for simplicity of notations.

After the feature extraction at each timestamp , the output layer is connected with an additional convolutional layer to convert the last hidden state into the video frame for prediction results. In other words, the network performs the learning on the input video frame and output the prediction in the same frame format for the next-step planning, thereby fully preserving and leveraging the spatial correlation information at each timestamp in a consistent and end-to-end way. At the same time, because our full neural network exhibits LSTM characteristics by design, compared with existing CNN or MLP-based motion planners, our approach can more properly capture and leverage the sequential dependence and correlation among different timestamps. All in all, with the carefully designed end-to-end convolutional LSTM architecture, the rich and important spatio-temporal information exhibited in the entire motion planning procedure can be fully identified, extracted and processed, thereby significantly improving the planning performance.

Neural Network Training (Offline Phase). To train the neural network on obtaining path planning capability, we prepare the training dataset via using the discrete motion process created by an “expert” planner across different tasks. Specifically, we generate the near-optimal paths using RRT* and extract their waypoints to form the video clips. During training, STP-Net takes the observations over the first $J$ timestamps as the input sequence $\hat{\mathcal{X}}_{in}=\{\hat{\mathcal{X}}_{0},\hat{\mathcal{X}}_{1},...,\hat{\mathcal{X}}_{J}\}$, and predicts the remaining frames $\tilde{\mathcal{X}}_{out}=\{\tilde{\mathcal{X}}_{J+1},\tilde{\mathcal{X}}_{J+2},...,\tilde{\mathcal{X}}_{T}\}$. In general, if we denote the function of neural network model $\theta$ as $f_{\theta}$, the training goal is to find the optimal parameters $\theta^{*}$ that minimizes the average cross-entropy loss between the predicted sequences and true targets across all the training data points $(\{\hat{\mathcal{X}}_{in},\hat{\mathcal{X}}_{out}\})_{n}$ as:

Here, the standard stochastic gradient descent (SGD)-based training strategy is used to train the desired STP-Net model.

Single-Path Construction (Online Phase). After the network training phase, during the online path construction we use the well-trained STP-Net to generate the sample distribution for the waypoint. As described in Algorithm 1, given a well trained STP-Net model $f$, the initial frame $\mathcal{X}_{0}$ and the start/goal states, the robot state is first initialized as $x_{start}$ (Line 1), and there is only one frame $\mathcal{X}_{0}$ in the current observation (Line 2). Then STP-Net performs the prediction in an iterative manner (Line 3). At each iteration, the NN model $f$ takes the newest observation $\mathcal{V}$ and predicts the next frame $\mathcal{X}_{next}$, which shares the same size as the input frame (Line 4). Recall that here the second channel of $\mathcal{X}$ records the current state of the robot, while the other two channels are the static environment and the fixed goal state, respectively. Also, because all the pixels are between $0$ and $1$ which indicates their probability of being the next waypoint, we can then sort the entries of the predicted frame $\mathcal{X}_{next}$ by their pixel values in the second channel (Line 5). In this way, the foremost element in $\mathcal{S}$ has the greatest probability to be the next waypoint. Notice that the elements in $\mathcal{S}$ are sequentially visited and checked if they are not used or collision-free (Line 6-8). Specifically, the function $ColDet$ checks whether the line connecting the current state $x_{current}$ and the next candidate state $x_{next}$ has collision with the obstacles or not. If $x_{next}$ is valid (not used and collision-free), the robot moves to the new state and appends it into the path (Line 9). The path is set to be found once $x_{current}$ reaches the region around the goal state (Line 11-13). Otherwise, we formulate the current task status as the new frame $\mathcal{X}_{new}$ and append it into the observation $\mathcal{V}$ (Line 14-15). The function $GenFrame$ produces the new frame by marking the patch corresponding to $x_{current}$ in the second channel (start state channel), while the other two channels (environment and goal state channel) are exactly the same as the initial frame $\mathcal{X}_{0}$. If the path still can not be found after $max\_itr$ iterations, an empty solution is returned (Line 17).

Extension to Multi-Path Construction. The proposed STP-Net can also be readily extended to serve as a multi-robot path planner, which is described in Algorithm 2. To that end, for each additional robot under the same environment, the raw input frame $\mathcal{X}_{0}$ is stacked with two extra channels, which represent the start/goal states of the new robot. For instance, for 2D and 3D planning tasks of five robots, the sizes of input frames are $n\times n\times 11$ and $n\times n\times n\times 11$, respectively. Notice that the predicted frame also contains the corresponding probability map of the next waypoint for each robot. Similar to the single-path construction, at each iteration the waypoint for each robot is sequentially determined based on the corresponding predicted probability distribution. To avoid the collision between robots, once a waypoint is occupied, such waypoint cannot be taken by other robots at that timestamp (Line 15-20). The new video frame is then updated according to the new robot states of all robots, and appended into the video clip (Line 27-28). The procedure is repeated until all robots reach their goal regions (Line 29-34).

We demonstrate the effectiveness of STP-Net on three benchmark tasks. The first two tasks are path planning on 2D maps of size $64\times 64$, and the third one is on 3D maps of size $20\times 20\times 20$. Specifically, the first type of maps is in the format of random forests, where $15$ obstacles of different shapes are randomly placed in different positions on 2D spaces. The second type is maze-like environments, which are generated using a randomized depth-first search. For the 3D environments, we sample $8$ out of a total of $11$ obstacle blocks with different shapes and place them randomly. For each training task, a feasible, near-optimal path is generated using RRT*. As described in Section III, each waypoint, the corresponding workspace and the fixed goal state, can be transformed together into a video frame, and hence each training data is interpreted as a video clip. Notice that to facilitate batch training, the video clips of different tasks are all padded into the same length by repeating their last frames, so that all the training data share the same dimensions.

For both 2D and 3D planning tasks, we train STP-Net over $100$ workspaces, each of which contains $800$ ground truth video clips. The evaluation is performed on both seen and unseen environments. The set of seen environments consists of all the environments used during the training procedure but with $200$ different pairs of start/goal configuration in each environment. The set of unseen environments is composed of $20$ new workspaces which are not used in the training, and each of them contains $400$ different start/goal pairs. We report the accuracy and path cost by averaging over all the computed paths for new tasks from seen/unseen environments.

For the multi-path planning tasks, the data are prepared using a similar procedure. In particular, the video frames of each multi-robot task are generated by stacking the start/goal frame channels of five different tasks under the same environment and at the same timestamp.

We evaluate the performance of STP-Net by comparing it with two classical motion planners (Informed-RRT* and BIT*) and two NN-based ones (MPNet and OracleNet). Considering the original OracleNet has limited performance because of the lack of sufficient map information, for fair comparison we use a performance-enhanced OracleNet-CAE as the baseline, which is equipped with a CAE module (like MPNet does) to better incorporate the encoding vector of the environment into the input.

Our STP-Net model consists of $4$ stacked ConvLSTM layers to learn the hidden representations, and all the hidden dimensions are set to $64$ for 2D tasks and $32$ for 3D tasks. The sizes of convolution kernels in the ConvLSTM are set to $5\times 5$ for 2D tasks and $5\times 5\times 5$ for 3D tasks. At the output end of the model, an additional $1\times 1$ or $1\times 1\times 1$ convolutional layer is allocated to convert the last hidden representation into the output frame that shares the same dimension as the input. In the training phase, ADAM optimizer [26] is selected with batch size of $128$ for 2D tasks and $64$ for 3D tasks, respectively. The learning rate is set as $0.0003$ and the entire model is trained for $20000$ iterations. The pixels of states are augmented by patches of size $5$ for 2D tasks and $3$ for 3D tasks. Besides, the length of input sequence $J$ from Equation 2 is set as $5$. The experiments are conducted on a computer equipped with an AMD EPYC 74202P 24-Core Processor and an NVIDIA RTX A6000 GPU. The neural networks are trained and tested using the PyTorch framework. We implement the neural network of STP-Net based on the official code of PredRNN [27]. We report the results of Informed-RRT* and BIT* using their C++ OMPL [28] implementation.

We first perform an ablation study on STP-Net, which justifies the necessity of predicting the static obstacles in future frames. To this end, we modify STP-Net slightly to obtain variant called STP-Net (Prob), which only predicts the probability maps of the next waypoint. From Table I, our STP-Net that predicts the 3-channel video frames outperforms STP-Net (Prob) significantly, with respect to both the time cost and path cost. Furthermore, to find a path under 2D Random Forest environments, STP-Net (Prob) visits around four times more waypoints than STP-Net, which causes a greater number of collision detection operations. Given the ablation study, STP-Net (Prob) is omitted from further evaluation.

We then evaluate the performance of STP-Net in 2D Random Forest environments. Figure 3 illustrates how we generate a path step by step via using the predicted frames output from STP-Net model. Initially, the input of the model is a single video frame $\mathcal{X}_{0}$ (the top-left) containing the full spatial information of the environment plus start and goal states. The model then predicts the next frame $\tilde{\mathcal{X}}_{1}$ (first row, second column), whose second channel (second row, second column) contains the estimated probabilities of being the next waypoint for each position. Here the brighter yellow pixels indicate higher probabilities. A valid waypoint is determined by selecting the collision-free position with the highest probability. Once the next predicted waypoint is determined, we generate the new observation $\hat{\mathcal{X}}_{1}$ (third row, second column) and predict the next frame $\tilde{\mathcal{X}}_{2}$ based on the previous two observations. We repeat this process until the waypoint reaches the goal state at timestamp $t=6$.

From this example, we can further obtain two important observations. First, our proposed end-to-end processing strategy, which directly processes the raw environmental map as the input data, can preserve the spatial information very well. As illustrated in the first row (“Frames”) of Figure 3, each predicted frame, which is essentially the output of STP-Net at the previous timestamp, properly retains all the important spatial information of the map. Such promising properties translate to better planning performance, especially when compared with the current CAE-based encoding strategy that always causes information loss. Second, STP-Net learns the collision-free path very well and hence it provides high-quality prediction for the waypoint. Comparing the second row (“Distributions”) and third row (“Decisions”) of Figure 3, it is seen that in most cases the determined next waypoint lies in the most probable sampling region (the brightest yellow). In other words, this means our neural network model can always predict the collision-free waypoint candidate with high confidence. Such property is very important for improving the planning speed. This is because if the predicted waypoint is further denied after the collision check, the neural planner has to perform more rounds of collision detection until it finds a satisfactory one. Fortunately, the experiments show that the highest probable waypoint candidates predicted by STP-Net are usually collision-free, and thereby naturally saving unnecessary planning cost. Figure 4 further amplifies the importance of high-quality prediction. It is seen that because STP-Net can provide collision-free prediction with very limited failed trials (marked as red lines), the overall planning speed is very fast.

Figure 5 visualizes the path search and generation in various maze environments with different task difficulties. It is seen that in such narrow passages-abundant scenarios, STP-Net can still predict the next reachable waypoint in a single shot for most cases. As illustrated in Figure 5, all the paths can be efficiently found within very limited trials of predictions. The visualization of the generated path of STP-Net in 3D Random Forest workspaces is shown in Figure 6.

Table I compares the performance of our proposed STP-Net with other four baseline motion planners. Experimental results show that all the planners produce $99.8\%-100\%$ accuracy across all the environments. It is seen that STP-Net outperforms the reference approaches with respect to time costs, with the competitive path quality. To be specific, STP-Net is at least $5\times$, $2.6\times$ and $1.8\times$ faster than the other two NN-based methods (MPNet and OracleNet-CAE), on 2D Random Forest, 2D Maze and 3D Random Forest environments, respectively. Simultaneously, STP-Net achieves higher accuracy than MPNet and OracleNet-CAE on all the 2D/3D tasks and regardless whether environments have been seen. While it is expected that anytime methods like BIT* should produce better path if enough time is granted, as shown in Table I, when BIT* is given the same time budget as required by STP-Net, STP-Net produces higher quality paths, which can be observed in Figure 8(a).

Figure 8(b) summarizes the average numbers of predicted waypoints (including the selected ones and the denied ones due to collision) for all the NN-based motion planners. A smaller number here indicates a better prediction produced by a neural network. STP-Net requries at least 22 times fewer predictions of waypoints as compared to the existing learning-based solutions across different planning tasks.

We further evaluate the performance of STP-Net on five-robot path planning problems. Figure 7 shows that STP-Net can produce near-optimal paths for five robots simultaneously with much less time as compared to computing all the paths separately. Figure 8(c) compares the time costs of these two different approaches. It is seen that STP-Net demonstrates strong scalability for multi-robot path planning with three times faster planning speed than the separate single-path construction solution.