## 4 Joint Motion Planning Based on Modified Hermite Interpolation
### Modified Cubic Hermite Interpolation
Then Equations (26) and (27) could be obtained respectively after sorting out. \[\left[1-\frac{3K_{1}^{2}}{H_{2}}-\frac{2K_{1}^{3}}{H_{2}}\frac{3K_{1}^{2}}{H_{ 2}}+\frac{2K_{1}^{3}}{H_{2}}\;-K_{1}-2K_{1}^{2}-K_{1}^{3}-K_{1}^{2}-K_{1}^{3} \right]\left[ \begin{array}{c}r_{1}\\ \end{array} \right]=y_{1} \tag{26}\]  
\[\left[ \begin{array}{c}6K_{1}\\ \end{array} \right.\frac{6K_{1}}{H_{2}}-\frac{6K_{1}^{2}}{H_{2}}-\frac{6K_{1}^{ 2}}{H_{2}}\;1+4K_{1}+3K_{1}^{2}\;2K_{1}+3K_{1}^{2}\right]\left[ \begin{array}[] {c}r_{1}\\ \end{array} \right]=\dot{y}_{1} \tag{27}\]  
where, \(x_{1}-t_{1}\) is marked as \(K_{1}\), and \(t_{2}-t_{1}\) is marked as \(H_{2}\), shown in Figure 7c. As mentioned above, the calculation of \(t_{0}\), \(t_{1}\),..., \(t_{m}\) will be described in detail in a later section, so \(K_{1}\) and \(H_{2}\) could be considered as the knowns. Besides, \(y_{1}\) and \(\dot{y}_{1}\) are certainly the knowns of initial knot \(B\). Therefore, Equations (26) and (27) are linear equations of the unknowns \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\), \(\dot{r}_{2}\). We need to solve out the unknowns \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\), \(\dot{r}_{2}\) while there are only two Equations so that another two linear equations are required. In order to guarantee second-order continuity at connection points, positions and first-order derivatives of new knots \(A\), \(M\), \(N\) and \(C\) must meet formula (22). Every three adjacent new knots among four new knots need to meet the formula (22). Specifically, knots \(A\), \(M\) and \(N\) all together should meet formula (22). After sorting out, function (28) can be obtained. \[\left(\frac{3H_{2}}{H_{1}}\;\frac{3H_{1}}{H_{2}}-\frac{3H_{2}}{H_{1}}-\frac{3H _{1}}{H_{2}}\;H_{2}\;2(H_{1}+H_{2})\;H_{1}\right) \tag{28}\]  
where, \(t_{1}-t_{0}\) and \(t_{2}-t_{1}\) are marked as \(H_{1}\) and \(H_{2}\) respectively, shown in Figure 7. As mentioned above, \(H_{1}\) and \(H_{2}\) could be considered as the knowns. Since \(r_{0}\) and \(\dot{r}_{0}\) are the ordinate and 1 st derivative of knot \(A\), they are known constants. Hence, Equation (28) is a linear one of the unknowns \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\) and \(\dot{r}_{2}\). Moreover, new knots \(M\), \(N\) and \(C\) all together should also meet the formula (22). Similarly, Equation (29) can be obtained after sorting out. \[\left[\frac{3H_{3}}{H_{2}}\;\frac{3H_{2}}{H_{3}}-\frac{3H_{3}}{H_{2}}-\frac{3H _{2}}{H_{3}}\;H_{3}\;2(H_{2}+H_{3})\;H_{2}\right]\left[ \begin{array}{c}r_{1} \\ r_{2}\\ r_{3}\\ \dot{r}_{1}\\ \dot{r}_{2}\\ \dot{r}_{3}\end{array} \right]=0 \tag{29}\]  
where, \(t_{2}-t_{1}\) and \(t_{3}-t_{2}\) are marked as \(H_{2}\) and \(H_{3}\). \(H_{2}\) and \(H_{3}\) could be considered as known constants. Since \(r_{3}\) and \(\dot{r}_{3}\) are the ordinate and 1 st derivative of knot \(C\), they are the knowns. Hence, Equation (29) is also a linear one of the unknowns \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\) and \(\dot{r}_{2}\). These four Equations (26)-(29) are all linear ones of the unknowns \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\) and \(\dot{r}_{2}\). By solving the Equations, the values of \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\) and \(\dot{r}_{2}\) can be obtained. After acquiring the positions and the 1 st derivatives at newly-built knots \(A\), \(M\), \(N\), and \(C\), modified cubic Hermite interpolation curves can be obtained according to formula (18). Now let us look at the situation with four initial points interpolated. As seen in Figure 8, there are four initial knots to be interpolated \(A(x_{0}\), \(y_{0}\)), \(B(x_{1}\), \(y_{1})\), \(C(x_{2}\), \(y_{2})\), and \(D(x_{3}\), \(y_{3})\). The first-order derivatives at knots \(A\), \(B\), \(C\) and \(D\) are assigned willfully to \(\dot{y}_{0}\), \(\dot{y}_{1}\), \(\dot{y}_{2}\), and \(\dot{y}_{3}\). Rebuild new data points. As seen in Figure 8, new knot \(M\) is built between the first and the second knots \(A\) and \(B\), and new knot \(N\) between the penultimate and the last knot \(C\) and \(D\).