# A Time-Optimal Continuous Jerk Trajectory Planning Algorithm for Manipulators
## 4 Solving the Time Optimal Problem  
The time-optimal trajectory planning problem for the manipulator is to solve and normalize the sequence of time nodes corresponding to the time node vector \(\mathbf{u}\) that minimizes the normalized total time, which is a nonlinear constrained optimization problem  
\[f(\mathbf{x})=\min\sum_{i=0}^{n-1}x_{i} \tag{19}\] \[\mathrm{s.t.}c_{m}(\mathbf{x})=\max_{j=1,2,\cdots,n+6}\left\{ \begin{array}{l}d_{mj}^{1}(\mathbf{x})\\ d_{mj}^{2}(\mathbf{x})\\ \end{array}\right\}-c_{vm}\leq 0\] \[c_{2N+m}(\mathbf{x})=\max_{j=3,4,\cdots,n+6}\left\{\begin{array} []{l}d_{mj}^{3}(\mathbf{x})\\ \end{array}\right\}-c_{jm}\leq 0\]  
where \(x=(x_{0},x_{1},\cdots,x_{n-1})^{\mathrm{T}}\) and \(x_{i}=\triangle t_{i}=t_{i+1}-t_{i}\), where \(\triangle t_{i}\) has the lower bound, that is, each element of \(T_{L}=(\triangle t_{L0},\triangle t_{L1},\cdots,\triangle t_{L(n-1)})\) satisfies  
\[\Delta t_{\mathrm{L}i}\geq\max_{m=1,2,\cdots,N}\left(\frac{p_{m(i+1)}-p_{mi}}{ c_{vm}}\right) \tag{20}\]  
Let  
\[\left\{\begin{array}{l}k_{1}=\max\left(\frac{d_{mj}^{1}}{c_{vm}}\right)\quad j =1,2,\cdots,n+6\quad m=1,2,\cdots,N\\ k_{2}=\max\left(\frac{d_{mj}^{2}}{c_{vm}}\right)\quad j=2,3,\cdots,n+6\quad m =1,2,\cdots,N\\ k_{3}=\max\left(\frac{d_{mj}^{2}}{c_{jm}}\right)\quad j=3,4,\cdots,n+6\quad m =1,2,\cdots,N\end{array}\right. \tag{21}\]  
Determining the initial value of the time node vector according to \(k_{1},k_{2}\) and \(k_{3}\), which can improve the search efficiency of the optimization algorithm, the initial value is  
\[\mathbf{x}_{0}=\max\left(\begin{array}{cc}1&k_{1}\quad\sqrt{k_{2}}\quad \sqrt{k_{3}}\end{array}\right)\times\mathbf{T}_{\mathrm{L}} \tag{22}\]  
The sequential quadratic programming method with superlinear convergence performance is used to solve the nonlinear constrained optimization problem described in Equation (19). The Lagrangian function is constructed to linearize the nonlinear constraints. The Lagrangian function is given by  
\[L(\mathbf{x},\lambda)=f(\mathbf{x})-\lambda^{\mathrm{T}}C(\mathbf{x}) \tag{23}\]where \(\lambda=(\lambda_{1}\lambda_{2}\cdots\lambda_{3N})^{\text{T}}\) is the Lagrange multiplier, \(C(\mathbf{x})=(c_{1}(\mathbf{x})c_{2}(\mathbf{x})\cdots c_{3N}(\mathbf{x}))^{ \text{T}}\). When the gradient of the Lagrange function is \(\
abla L(\mathbf{x}^{*},\lambda)=\
abla f(\mathbf{x}^{*})-\lambda^{\text{T}} \
abla C(\mathbf{x}^{*})=0\), \(\mathbf{x}^{*}\) is the K-T point of the nonlinear optimization problem, that is, the solution of the time-optimal problem.  
The KTH quadratic programming subproblem of the sequential quadratic programming method is obtained by simulating the Newton-Raphson method:  
\[\begin{split}&\min_{d\in\mathbf{R}^{n}}\left(\mathbf{g}_{\text{ k}}\text{T}\mathbf{d}+\frac{1}{2}\mathbf{d}^{\text{T}}\mathbf{B}_{k}\mathbf{d} \right)\\ \text{s.t.}& a_{i}(\mathbf{x}_{k})^{\text{T}} \mathbf{d}+c_{i}(\mathbf{x}_{k})\leq 0\quad i=1,2,\cdots,3N\end{split} \tag{24}\]  
where \(A(\mathbf{x}_{k})=(a_{1}(\mathbf{x}_{k}),a_{2}(\mathbf{x}_{k}),\cdots,a_{3N}( \mathbf{x}_{k}))^{\text{T}}=\
abla C(\mathbf{x}_{k})^{\text{T}}\), \(\mathbf{g}_{k}=\mathbf{g}(\mathbf{x}_{k})=\
abla f(\mathbf{x}_{k})\) and \(\mathbf{B}_{k}\in\mathbf{R}^{n\times n}\) is an approximation of the Hessian matrix of the Lagrangian function.  
Through successive solving of the quadratic programming problems, the optimal trajectory of manipulator time node vector \(x\) converges to the optimal value \(x^{*}\).