## 4 Joint Motion Planning Based on Modified Hermite Interpolation
### Modified Cubic Hermite Interpolation  
Firstly, take three points of cubic Hermite interpolation as an example. Here are three data points to be interpolated \(A(x_{0},y_{0})\), \(B(x_{1},y_{1})\), \(C(x_{2},y_{2})\), and the first-order derivatives at knots \(A\), \(B\), \(C\) are assigned willfully to \(\dot{y}_{0}\), \(\dot{y}_{1}\) and \(\dot{y}_{2}\), as can be seen in Figure 7a. Since \(\dot{y}_{0}\), \(\dot{y}_{1}\) and \(\dot{y}_{2}\) are specified willfully without following formula (22), the cubic Hermit interpolation curves \(y(x)\) at connection point \(B\) is not C\({}^{2}\) continuous. Figure 7: Piecewise cubic Hermite interpolation curves with three initial knots: (**a**) Interpolation with initial knots; (**b**) Interpolation with rebuilt knots; (**c**) Comparison between two kinds of interpolation. In order to specify first-order derivatives willfully at points \(A\), \(B\), \(C\) and guarantee C\({}^{2}\) continuity, new points are built. As seen in Figure 7b, new point \(M\) is built between initial point \(A\) and \(B\), and new point \(N\) is built between initial point \(B\) and \(C\). In order to distinguish the new interpolation function from the old one, the new function is marked as \(r(t)\) instead of \(y(x)\). Newly-built knots become \(A(t_{0},r_{0})\), \(M(t_{1},r_{1})\), \(N(t_{2},r_{2})\) and \(C(t_{3},r_{3})\), and 1 st derivatives at \(A\), \(M\), \(N\), and \(C\) are marked respectively as \(\dot{r}_{0}\), \(\dot{r}_{1}\), \(\dot{r}_{2}\) and \(\dot{r}_{3}\). The first and last point are unchanged, \((x_{0},y_{0})=(t_{0},r_{0})\), \((x_{2},y_{2})=(t_{3},r_{3})\), \(\dot{y}_{0}=\dot{r}_{0}\), \(\dot{y}_{2}=\dot{r}_{3}\). Newly-built knots \(A\), \(M\), \(N\), and \(C\) instead of initial knots \(A\), \(B\), and \(C\) are interpolated by cubic Hermite interpolation, and interpolating function \(r(t)\) is obtained. Function \(r(t)\) must meet the following situations: (1) Interpolating curves must pass by the knots \(A\), \(B\), and \(C\); (2) First-order derivatives at point \(A\), \(B\), and \(C\) are equal to \(\dot{y}_{0}\), \(\dot{y}_{1}\) and \(\dot{y}_{2}\); (3) Second-order derivatives at newly-built knots \(M\), and \(N\) are continuous. According to formula (18), positions and 1 st derivatives at every two newly-built knots are needed to carry out interpolation. Since the first and the last point are unchanged and known, positions and 1 st derivatives of new knots except point A and C must be found out. That is new points positions \(M(t_{1},r_{1})\), \(N(t_{2},r_{2})\), and their derivatives \(\dot{r}_{1}\dot{r}_{2}\) must be found out. Because the calculation of new knots' ordinates \(t_{0}\), \(t_{1}\),..., \(t_{m-1}\) will be described in detail in a later section, \(t_{0}\), \(t_{1}\),..., \(t_{m-1}\) can be considered as the knowns. Therefore, firstly explain how to find out \(r_{1}\), \(r_{2}\), \(\dot{r}_{1}\dot{r}_{2}\). As shown in Figure 7b, the interpolation curve \(r_{2}(t)\) must pass by the initial knot \(B\). According to formula (18), the second interpolation curve \(r_{2}(t)\) can be expressed function (24) as below. \[r_{2}(t)=\left[1 \begin{array}{cccc}t-t_{1}&(t-t_{1})&t_{2}-t_{1}\\ \end{array} \right]^{2}\left( \begin{array}{cccc}t-t_{1}\\ \end{array} \right)^{3}\right]\left[ \begin{array}{cccc}1&0&0&0\\ 0&0&1&0\\ -3&3&-2&-1\\ 2&-2&1&1\end{array} \right]\left[ \begin{array}{cccc}r_{1}\\ r_{2}\\ (t_{2}-t_{1})\dot{r}_{1}\\ (t_{2}-t_{1})\dot{r}_{2}\end{array} \right]t\in[t_{1},t_{2}] \tag{24}\]  
where \(t\) is the variable, and \((t_{1},r_{1})\), \(\dot{r}_{1}\), \((t_{2},r_{2})\) and \(\dot{r}_{2}\) are positions and 1 st derivatives at new knot \(M\) and \(N\), respectively. The initial knot \(B(x_{1},y_{1})\) and its derivative \(\dot{y}_{1}\) must meet the interpolation function \(r_{2}(t)\) in formula (24), which can be expressed as the following Equation (25). \[\left\{ \begin{array}{ll}r_{2}(x_{1})=y_{1}\\ \dot{r}_{2}(x_{1})=\dot{y}_{1}\end{array} \right. \tag{25}\]  
Put \(x_{1}\), \(y_{1}\) and \(\dot{y}_{1}\) into formula (24).