# Research on the Path Planning Algorithm of a Manipulator Based on GMM/GMR-MPRM
## 4 MPRM Module
### Variable Density Resampling  
Each point on the task path obtained by the GMM/GMR algorithm has a mean and a covariance. The mean value is represented by \(\mu\), and the covariance is represented by \(\Sigma\). This paper used the \(mvrnd\) function in MATLAB to generate the sampling space. The function can generate a dataset that conforms to the multivariate Gaussian distribution. Therefore, resampling the \(k\)-th point on the path based on the Gaussian distribution can be expressed as:  
\[\bar{x}^{k}=mvrnd(u_{k},\Sigma_{k},N_{k}) \tag{11}\]\[\tilde{x}=\{\tilde{x}^{1},\tilde{x}^{2},\ldots,\tilde{x}^{K}\} \tag{12}\]  
where \(\tilde{x}^{k}\) is the resampled dataset of the \(k\)-th point, \(N_{k}\) is the sample quantity of resampling, and \(\tilde{x}\) is the resampled dataset with a sample quantity of \(\sum\limits_{k=1}^{K}N_{k}\).  
If the sample quantity \(N_{k}\) of resampling for each Gaussian component on the task path is the same, then there will be a large number of resampling points near the obstacle when the Gaussian component close to the obstacle is resampled, which reduces the success rate of path planning. In addition, the covariance of the Gaussian component also has an impact on the quantity of resampling, that is the Gaussian component with a smaller covariance has higher importance, and the quantity of resampling for this component should be increased, while the Gaussian component with a larger covariance has lower importance, and the quantity of resampling for this component should be reduced.  
Therefore, a variable density resampling method is proposed, that is the Gaussian component that is closer to the obstacle generates more sampling points, and the Gaussian component that is farther from the obstacle generates fewer sampling points. The Gaussian component with a small covariance generates more sampling points, and the Gaussian component with a high covariance generates fewer sampling points. This can not only increase the sampling speed and the success of path planning, but also improve the consistency of the planned path and the task path. The quantity of resampling \(N_{k}\) can be defined as:  
\[N_{k}=\left\{\begin{array}{cc}\frac{n_{\Sigma}}{\|\Sigma\|}+n_{0}\frac{d_{t }}{d_{k}}&d_{k}<d_{t}\\ \frac{n_{\Sigma}}{\|\Sigma\|}+n_{0}&d_{k}\geq d_{t}\end{array}\right. \tag{13}\]  
where \(d_{k}\) is the shortest distance between the \(k\)-th Gaussian component and the obstacle, \(d_{t}\) is the threshold of the distance, \(\|\Sigma\|\) is the covariance of the \(k\)-th Gaussian component, and \(n_{\Sigma}\) and \(n_{0}\) are the reference quantity of resampling based on the covariance and distance, respectively. Since there is no collision detection during resampling, some sampling points will fall into the position of obstacles. If the roadmap is constructed directly in this sampling space, the number of collision detections and invalid queries will increase. Therefore, it is necessary to recalculate the minimum distance \(L_{\text{min}}\) between the sampling point and the obstacle surface. If \(L_{\text{min}}\leq L_{threshold}\), the point is considered as the collision point and removed; otherwise, the point is retained.