## 4 Joint Motion Planning Based on Modified Hermite Interpolation
### Modified Cubic Hermite Interpolation
It must be noted that two new knots \(P\) and \(Q\) need to be built between knot \(B\) and \(C\). If only one new knot is built between knots \(B\) and \(C\), set of Equations cannot be solved because the number of Equations is greater than that of the unknowns. Similarly, we need to find out the unknowns \(r_{1}\), \(r_{2}\), \(r_{3}\), \(r_{4}\), \(\dot{r}_{1}\), \(\dot{r}_{2}\), \(\dot{r}_{3}\), \(\dot{r}_{4}\) of the newly-built knots except the first and the last one. Figure 8: Piecewise cubic Hermite interpolation curves with four initial knots: (**a**) Interpolation with initial knots; (**b**) Interpolation with rebuilt knots; (**c**) Comparison between two kinds of interpolation. In the same way, the initial knot \(B(x_{1},y_{1})\) and its derivative \(\dot{y}_{1}\) must meet the interpolation function \(r_{2}(t)\). The initial knot \(C(x_{2},y_{2})\) and its derivative \(\dot{y}_{2}\) must meet the interpolation function \(r_{4}(t)\), shown in Figure 8b. Then put \(x_{1},y_{1},\dot{y}_{1}\) and \(x_{2},y_{2},\dot{y}_{2}\) into \(r_{2}(t)\) and \(r_{4}(t)\) derived from formula (18), and we can get four Equations similar to Equations (26) and (27). When it is extended to more knots, Equations (30) and (31) can be obtained after sorting and inducting. \[\left[ \begin{array}{cc}1-\frac{3K_{1}^{2}}{H_{j+1}}-\frac{2K_{1}^{3}}{H_{j+ 1}}&\frac{3K_{1}^{2}}{H_{j+1}}+\frac{2K_{1}^{3}}{H_{j+1}}&-K_{i}-2K_{i}^{2}-K_{ i}^{3}&-K_{i}^{2}-K_{i}^{3}\end{array} \right]\left[ \begin{array}{c}r_{j}\\ r_{j+1}\\ \dot{r}_{j}\\ \dot{r}_{j+1}\end{array} \right]=y_{i}\quad \begin{array}{c}(i=1,2,3,\cdots, n-2)\\ (j=1,3,5,\cdots,m-3)\end{array}  \tag{30}\]  
\[\left[ \begin{array}{cc}\frac{6K_{i}}{H_{j+1}}+\frac{6K_{i}^{2}}{H_{j+1}}&- \frac{6K_{i}}{H_{j+1}}&-\frac{6K_{i}^{2}}{H_{j+1}}&1+4K_{i}+3K_{i}^{2}&2K_{i}+3K _{i}^{2}\end{array} \right]\left[ \begin{array}{c}r_{j}\\ r_{j+1}\\ \dot{r}_{j}\\ \dot{r}_{j+1}\end{array} \right]=\dot{y}_{i}\quad \begin{array}{c}(i=1,2,3, \cdots,n-2)\\ (j=1,3,5,\cdots,m-3)\end{array}  \tag{31}\]  
where, \(n\) is the number of the initial knots (\(n\geq 3\)), and \(m\) (\(m=2n-2\)) is the number of the newly-built knots. The subscript \(i\) is related to initial knots while subscript \(j\) is related to newly-built knots. Array \(\boldsymbol{y}\) is the ordinates of initial knots, \(y_{0},y_{1},\,\ldots,\,y_{n-1}\), which are all knowns. The array \(\dot{y}\) is the first-order derivatives of initial knots, \(\dot{y}_{0}\dot{y}_{1},\,\ldots,\,\dot{y}_{n-1}\), which are all knowns. Array \(\boldsymbol{r}\) is the ordinates of newly-built knots, \(r_{0},\,r_{1},\ldots,\,r_{m-1}\), in which \(r_{0}\) and \(r_{m-1}\) are knowns and others are unknowns. The array \(\dot{r}\) is the first-order derivatives of newly-built knots \(\dot{r}_{0}\),\(\dot{r}_{1},\,\ldots,\,\dot{r}_{m-1}\), in which \(\dot{r}_{0}\) and \(\dot{r}_{m-1}\) are knowns and others are unknowns. So, \(r_{1},r_{2},\,\ldots,\,r_{m-2}\) and \(\dot{r}_{1},\,\dot{r}_{2},\,\ldots,\,\dot{r}_{m-2}\) are unknown. The expressions of array \(\boldsymbol{K}\) and array \(\boldsymbol{H}\) are shown in Equations (32) and (33) below respectively, seen as in Figure 8c for graphic representation. Since all of the initial knots' abscissas \(x_{0},\,x_{1},\,\ldots,\,x_{n-1}\) are given and the newly-built knots' abscissas \(t_{0},\,t_{1},\,\ldots,\,t_{m-1}\) can be considered as the knowns described in the latter section, array \(\boldsymbol{K}\) and array \(\boldsymbol{H}\) are known constants. Hence, Equations (30) and (31) are linear ones. \[K_{i}=x_{i}-t_{j}(i=1,2,3,\cdots,n-2,j=1,3,5,\cdots,m-3) \tag{32}\]  
\[H_{j}=t_{j}-t_{j-1}\ (\ j=1,2,\cdots,m-1) \tag{33}\]  
Synthesizing formulas (30) and (31), there are \(4(n-2)\) unknowns and \(2(n-2)\) Equations. Hence, the set of Equations has no unique solution unless other Equations are to be added. Similarly, in order to guarantee \(\mathbb{C}^{2}\) continuity at newly-built knots, the positions and first-order derivatives of new knots \(A,M,N,P,Q\) and \(D\) in Figure 8b must meet the formula (22).