# A Random Sampling-Based Method via Gaussian Process for Motion Planning in Dynamic Environments
## 2 Dynamic Gaussian Local Planner
### Gaussian Random Path Sampling (GRPS)  
The function SampleConstraintPaths\((\cdot)\) in Algorithm 1 is based on the GRPS method. For a \(d\) Dof robot, it has \(d\)-dimensional C-space. The C-space and robot workspace can be transformed into each other. The paths for the robot in the C-space can be planned by sampling-based methods. In \(d\)-dimensional C-space, let \(\delta\) be a local path represented by \(n_{s}\) discrete nodes \(x_{i},\ i=1,2,\ldots,n_{s}\), i.e., \(\delta=\{x_{1},x_{2},\ldots x_{n_{s}}\}\). Each \(d\)-dimensional node \(x_{i}\) consists of \(d\) 1-dimensional node component \(x_{i}^{j},\ j=1,2,\ldots,d\), i.e., \(x_{i}=\left(x_{1}^{1},x_{2}^{2},\ldots,x_{n_{s}}^{d}\right)^{\top}\). The local path component of the \(j\)'th dimension is \(\delta^{j}=\left\{x_{1}^{j},x_{2}^{j},\ldots,x_{n_{s}}^{j}\right\}\). The intuitive relationship is illustrated in Figure 2. In this paper, the non-bold symbols represent variables \(\in\mathbb{R}^{1\times 1}\), \(\mathbb{R}^{1\times n}\), or \(\mathbb{R}^{1\times n}\), and the bold italicized symbols are variables \(\in\mathbb{R}^{n\times n}\) or \(\mathbb{R}^{n\times m}\). Figure 1: The DGLP plans the local path. The \(x_{\text{init}}\) and \(x_{\text{goal}}\) are the initial node and the goal node of the local re-planning. The \(v_{\text{init}}\) and \(v_{\text{goal}}\) are the velocities at the \(x_{\text{init}}\) and \(x_{\text{goal}}\). Dashed curves represent a set of random paths. (**a**) A part of the global path is invalid because of the dynamic obstacle. (**b**) The DGLP samples \(n_{p}\) random paths that satisfy the path end orientation constraints. (**c**) The DGLP selects the high-quality collision-free local path from the random path set. The DGLP employs the Gaussian process model to randomly generate the local path component \(\delta^{l}\). The Gaussian process model is determined by a mean function \(\mu(s)\) and a kernel function \(k(s,s^{\prime})\), where \(s\in\mathbb{S}\) is path node index. In order to ensure the smoothness of random paths, we set the mean function \(\mathbf{\mu}(s)\) to 0 and select the Squared Exponential (SE) kernel function as the kernel function \(k(s,s^{\prime})\). The SE kernel function is  
\[k(s,s^{\prime})=\sigma_{f}^{2}\exp\left[-\left(s-s^{\prime}\right)^{2}/2\sigma _{l}^{2}\right]. \tag{1}\]  
The \(\sigma_{f}\) can affect the fluctuation amplitude of random paths. The \(\sigma_{f}\) is determined by the sampling range \(R\) and the amplitude factor \(v_{a}\), i.e., \(\sigma_{f}=v_{a}\cdot R\) in the DGLP. The \(\sigma_{l}\) controls the complexity of random paths. Let a random path \(\delta=\left\{x(s),s\in S\right\}\) be a Gaussian process, where \(x(s)\) denotes the path node corresponding to the index \(s\). The random path is described as  
\[\delta\sim\mathcal{GP}\left(\mathbf{\mu}(s),k\big{(}s,s^{\prime}\big{)}\right). \tag{2}\]  
The path probability distribution based on the Gaussian process regression model is  
\[\left\{ \begin{aligned} p\left(\delta_{p}\big{|}S_{p},S_{t}, \delta_{t}\right)&=N\big{(}\mathbf{\mu}_{p},\Sigma_{p}+\sigma_{n}^{2} I\big{)}\\ \mathbf{\mu}_{p}&=\mathbf{k}_{pt}\big{(}\mathbf{K}_{t}+\sigma_{n} ^{2}I\big{)}^{-1}\delta_{t}\\ \Sigma_{p}&=k_{p}-\mathbf{k}_{pt}\big{(}\mathbf{K}_{t}+ \sigma_{n}^{2}I\big{)}^{-1}\mathbf{k}_{pt}^{\mathrm{T}}\end{aligned} \right. \tag{3}\]  
\(\mathbf{S}_{t}=\left\{s_{1},s_{N}\right\}\) and \(\delta_{t}=\left\{x_{\mathrm{init}},x_{\mathrm{goal}}\right\}\) are a node index set and a given path node set. \(S_{p}=\left\{s_{i}|i=1,2,\ldots,N\right\}\) and \(\delta_{p}=\left\{x_{i}|i=1,2,\ldots,N\right\}\) are a node index set and path node set of a random path. Note that the \(N\) is equivalent to the \(n_{p}\) in Algorithm 1. The noise variance is represented by \(\sigma_{n}^{2}\), and \(\sigma_{n}=0\) in this paper. \(\mathbf{k}_{pt}=k\big{(}\mathbf{S}_{p},\mathbf{S}_{t}\big{)}\in\mathbb{R}^{N\times 2}\) is a kernel matrix of \(\mathbf{S}_{p}\) and \(\mathbf{S}_{t}\).