# Research on the Path Planning Algorithm of a Manipulator Based on GMM/GMR-MPRM
## 4 MPRM Module
### B-Spline Trajectory Smoothing  
After shortening the length of the path by using the Shortcut algorithm, B-spline interpolation is used to smooth the pruned path. At the same time, in order to ensure the  
Figure 3: Schematic diagram of path pruning. continuous speed of the connection, the speed boundary condition is set. The B-spline curve is essentially a set of piecewise continuous polynomials, and complex curves can be described by low-order polynomials. The B-spline curve can be represented by:  
\[C(u)=\sum\limits_{i=0}^{n}N_{i,p}(u)D_{i} \tag{16}\]  
where \(N_{i,p}(u)\) is the B-spline basis function, \(D_{i}\) is the control vertex, and \(p\) is the order of the polynomial. The value of \(u\) is defined by the node vector:  
\[U=\{u_{0},u_{1},\ldots,u_{p},u_{p+1},\ldots,u_{n},u_{n+1},\ldots,u_{n+p+1} \tag{17}\]  
Take \(u_{0}=u_{1}=,\ldots,u_{p}=0,u_{n+1}=,\ldots,u_{n+p+1}=1\), and use the Cox-de Boor recursion formula for the basis function of the B-spline; the recursion formula can be given by:  
\[N_{i,0}(u)=\left\{ \begin{array}{ll}1,&\text{if}\quad u_{i}\leq u\leq u_{i+1 }\\ 0,&\text{others}\end{array} \right. \tag{18}\]  
\[N_{i,k}=\frac{u-u_{i}}{u_{i+k}-u_{i}}N_{i,k-1}(u)+\frac{u_{i+k+1}-u}{u_{i+k+1} -u_{i+1}}N_{i+1,k-1}(u) \tag{19}\]  
The shape of the B-spline curve is determined by the node vector and the control vertices. There are various parameterization methods to parameterize the node vector, such as uniform parameterization, chord length parameterization, and centripetal parameterization. Centripetal parameterization can obtain a smoother curve; therefore, this paper adopted this parameterization method:  
\[u_{k}=u_{k-1}+\frac{d_{k}}{d} \tag{20}\]  
where \(d=\sum\limits_{k=0}^{r}d_{k}\), \(d_{k}=|q_{k+1}-q_{k}|^{\frac{1}{2}}\) and \(q_{k}\) are the points to be interpolated. For the cubic B-spline curve, when performing the interpolation calculation, there is:  
\[C(u_{i})=\sum\limits_{j=i-3}^{i-1}d_{j}N_{j,3}(u_{i})=q_{i-3},i=3,4,\ldots,n+1 \tag{21}\]  
This formula can be expressed in the form of the matrix equation:  
\[\left[ \begin{array}{cccc}N_{1,3}(u_{4})&N_{2,3}(u_{4})&N_{3,3}(u_{4})\\ \ddots&\ddots&\ddots\\ &N_{n-3,3}(u_{n})&N_{n-2,3}(u_{n})&N_{n-1,3}(u_{n})\end{array} \right]\left[ \begin{array}{c}d_{1}\\ \vdots\\ d_{n-1}\end{array} \right]=\left[ \begin{array}{c}q_{1}\\ \vdots\\ q_{n-3}\end{array} \right] \tag{22}\]  
For the cubic B-spline curve, the first and last nodes in the node vector are quadruple nodes, and this causes the first (end) vertex to be the same as the first (end) data point. After removing the first and last two equations of the above formula, the matrix equation can be expressed as:  
\[\left[ \begin{array}{cccc}b_{1}&c_{1}&&&a_{1}\\ a_{2}&b_{2}&&&c_{2}\\ &\ddots&\ddots&&\ddots\\ &a_{n-2}&&b_{n-2}&c_{n-2}\\ &c_{n-1}&&a_{n-1}&b_{n-1}\end{array} \right]\left[ \begin{array}{c}d_{1}\\ \vdots\\ d_{n-1}\end{array} \right]=\left[ \begin{array}{c}e_{1}\\ \vdots\\ e_{n-1}\end{array} \right] \tag{23}\]Add two boundary conditions and simplify; the matrix equation can be expressed as:  
\[\left[ \begin{array}{ccccc}b_{1}&c_{1}&&a_{1}&&\\ a_{2}&b_{2}&&c_{2}&&\\ &\ddots&\ddots&\ddots&\\ &a_{n-2}&&b_{n-2}&c_{n-2}\\ &c_{n-1}&&a_{n-1}&b_{n-1}\end{array} \right]\left[ \begin{array}{c}d_{1}\\ \vdots\\ d_{n-1}\end{array} \right]=\left[ \begin{array}{c}e_{1}\\ \vdots\\ e_{n-1}\end{array} \right] \tag{24}\]  
where,  
\[\left\{ \begin{array}{ll}a_{i}=\frac{(\Delta_{i_{2}i_{2}})^{2}}{\Delta_{i}+ \Delta_{i+1}+\Delta_{i+2}}\\ b_{i}=\frac{(\Delta_{i}+\Delta_{i+1})\Delta_{i+2}}{\Delta_{i}+\Delta_{i+1}+ \Delta_{i+2}}+\frac{(\Delta_{i+2}+\Delta_{i+3})\Delta_{i+1}}{\Delta_{i+1}+ \Delta_{i+2}+\Delta_{i+3}}\\ c_{i}=\frac{(\Delta_{i+1})^{2}}{\Delta_{i+1}+\Delta_{i+2}+\Delta_{i+3}}\\ e_{i}=(\Delta_{i+1}+\Delta_{i+2})q_{i-1}\end{array} \right. \tag{25}\]  
The velocity boundary conditions are:  
\[\left\{ \begin{array}{ll}b_{1}=1,c_{1}=a_{1}=0,e_{1}=q_{0}+\frac{\Delta_{3}}{ 3}q_{0}\\ c_{n-1}=a_{n-1}=0,b_{n-1}=1,e_{n-1}=q_{n-1}-\frac{\Delta_{n}}{3}\dot{q}_{n-2} \end{array} \right.