# A Time-Optimal Continuous Jerk Trajectory Planning Algorithm for Manipulators
## 2 Jerk Continuous Trajectory Construction  
According to the requirements of the trajectory tracking accuracy of a manipulator, a trajectory with a certain geometry in the task space is discretized, and the pose matrix sequence \(T_{i}\) and the corresponding time node sequence \(t_{i}\) are obtained. The joint position vector \(\mathbf{p}_{i}\) corresponding to \(T_{i}\) is calculated by using the inverse kinematics, and then the joint position-time sequence is formed:  
\[\mathbf{S}=\left(\mathbf{p}_{i},t_{i}\right)\quad i=0,1,\cdots,n \tag{1}\]  
where \(\mathbf{p}_{i}=\left(p_{1i}p_{2i}\cdots p_{Ni}\right)^{\mathrm{T}}\). The jerk continuous trajectory requires each joint trajectory \(p_{m}=f_{m}(t)\) to be at least \(C^{3}\) continuity, and \(p_{mi}=f_{m}(t_{i})\), that is, through the joint position sequence. Adopting the \(k\)-th degree non-uniform rational B-spline with the \(C^{k-1}\) continuity structure, the joint trajectory for the manipulator can be uniformly described as  
\[\mathbf{p}(u)=\sum\limits_{j=0}^{n}\mathbf{d}_{j}N_{j,k}(u)=\sum\limits_{j=i-k}^ {i}\mathbf{d}_{j}N_{j,k}(u) \tag{2}\]  
where \(d_{j}\in R^{N\times 1}\) is the control top point vector, \(j=0,1,\cdots,n,u\in[u_{i},u_{i+1}]\subseteq[u_{k},u_{n+1}]\subset[u_{0},u_{n+ k+1}]\), for the normalized time variable; \(p(u)\in R^{N\times 1}\) is the joint position vector at the moment \(u\); and \(N_{j,k}(u)\) is the \(k\)-th degree canonical B-spline basis function. The joint trajectory curve is related to the local support of the B-spline curve and the quality of \(C^{k-1}\) continuity. The joint velocity, acceleration and jerk at time \(u\in[u_{i},u_{i+1}]\) correspond to the R-order derivatives \(p^{\prime}(u)\) of the B-spline trajectory curve, \(r=1,2,3\), respectively, which can be found by using the de [PERSON] recurrence formula  
\[\mathbf{p}^{r}(u)=\sum\limits_{j=i-k+r}^{T}\mathbf{d}_{j}^{r}N_{j,k-r}(u) \tag{3}\]  
\[\mathbf{d}_{j}^{r}=\left\{ \begin{array}{l}\mathbf{d}_{j},l=0\\ (k+1-l)\frac{\mathbf{d}_{j}^{l-1}-\mathbf{d}_{j-1}^{l-1}}{u_{j+k+1-l}-u_{j}},l =1,2,\cdots,r,j=i-k+l,\cdots,i\end{array} \right. \tag{4}\]  
Since the trajectory curve needs to strictly satisfy each position-time constraint, the control points of the B-spline interpolation trajectory curve need to be inverted according to this constraint. By normalizing the time node \(t_{i}\) through the cumulative duration parameterization method, we obtain the \(k\)-th degree B-spline trajectory domain node vector \(u=(u_{0},u_{1},\cdots,u_{n+2k})\in[0,1]\), that is  
\[u_{0}=u_{1}=\cdots=u_{k}=0 \tag{5}\]  
\[u_{n+k}=u_{n+k+1}=\cdots=u_{n+2k}=1 \tag{6}\]  
\[u_{i}=u_{i-1}+\frac{|\Delta t_{i-k-1}|}{\sum\limits_{j=0}^{n-1}|\Delta t_{j}|} i=k+1,k+2,\cdots,n+k-1 \tag{7}\]  
where \(0\leq u_{0}\leq u_{1}\leq u_{2}\leq...\leq u_{n+2k}\leq 1\). In this paper, we set \(u_{0}\) and \(u_{n+k}\) to indicate multiple nodes, and the other nodes are distributed between \(u_{k}\) and \(u_{n+k}\). The \(n+1\) equations corresponding to the position-time constraints can be listed as  
\[\mathbf{p}(u_{i+k})=\sum\limits_{j=i}^{i+k}\mathbf{d}_{j}N_{j,k}(u_{i+k})= \mathbf{p}_{i} \tag{8}\]  
where \(i=0,1,\cdots,n\), \(d_{j}\) is the \(n+k\) control vertex vector of the B-spline trajectory, \(j=0,1,\cdots,n+k-1\). The ideal joint trajectory requires that the joint start-stop velocity, acceleration and jerk can be set. Providing six boundary conditions (taking \(k=7\)) results in \(n+7\) equations:  
\[\left\{ \begin{array}{l}\mathbf{p}(u_{i+7})=\sum\limits_{j=i}^{i+7}\mathbf{ d}_{j}N_{j,7}(u_{i+7})=\mathbf{p}_{i}\quad i=0,1,\cdots,n\\ \mathbf{p}^{\prime}(u)|_{u=u_{7}}=\mathbf{v}_{s,}\mathbf{p}^{\prime}(u)|_{u=u _{n+7}}=\mathbf{a}_{e}\\ \mathbf{p}^{\prime\prime}(u)|_{u=u_{7}}=\mathbf{j}_{s,}\mathbf{p}^{\prime \prime}(u)|_{u=u_{n+7}}=\mathbf{j}_{e}\end{array} \right.