# Time-Optimal Trajectory Planning for Woodworking Manipulators Using an Improved PSO Algorithm
## 2 Materials and Methods
### 3-5-3 Piecewise Polynomial Interpolation  
By inverse kinematics, the angle of each joint when the manipulator's end-effector passes through the path point can be obtained. Planning the trajectory between each path point is necessary to determine the joint trajectory interpolation function and calculate the joint angle, velocity, and acceleration motion parameters between the path points. Polynomial functions are commonly used as trajectory interpolation functions. However, when only a cubic polynomial is used, the angular acceleration is discontinuous, and when only using a quintic polynomial to pass through multiple path points, it leads to complex calculations. Therefore, we used the 3-5-3 piecewise polynomial interpolation [23] to conduct trajectory planning, which reduces computational complexity, effectively ensuring the manipulator's stability while passing through multiple path points. The 3-5-3 piecewise polynomial interpolation is divided into three segments, with the first and third segments being cubic polynomial and the second segment being quintic polynomial. Taking joint 1 as an example, the joint trajectory interpolation functions of each segment are shown in the following equation:  
\[\theta_{1}(t)= a_{10}+a_{11}t+a_{12}t^{2}+a_{13}t^{3},\] (4) \[\theta_{2}(t)= a_{20}+a_{21}t+a_{22}t^{2}+a_{23}t^{3}+  
To ensure the continuous and smooth trajectory of each segment during operation, the joint angular displacement, velocity, and acceleration must satisfy the constraints as per the following equation:  
\[ \begin{cases}\theta_{1}(0)=\theta_{0}\\ \theta_{2}(0)=\theta_{1}(1)\\ \theta_{3}(0)=\theta_{2}(1)\\ \theta_{3}(1)=\theta_{f}\end{cases} , \tag{13}\]  
\[ \begin{cases}\dot{\theta}_{1}(0)=\dot{\theta}_{1}=0\\ \dot{\theta}_{2}(0)=\dot{\theta}_{1}(1)\\ \dot{\theta}_{3}(0)=\dot{\theta}_{2}(1)\\ \dot{\theta}_{3}(1)=\dot{\theta}_{f}=0\end{cases} , \tag{14}\]  
\[ \begin{cases}\ddot{\theta}_{1}(0)=\ddot{\theta}_{1}=0\\ \ddot{\theta}_{2}(0)=\ddot{\theta}_{1}(1)\\ \dot{\theta}_{3}(0)=\ddot{\theta}_{2}(1)\\ \dot{\theta}_{3}(1)=\ddot{\theta}_{f}=0\end{cases} , \tag{15}\]  
where \(\theta_{i}(0)\), \(\dot{\theta}_{i}(0)\), and \(\ddot{\theta}_{i}(0)\) are the joint angular displacement, velocity, and acceleration at the beginning of the \(i\)th segment, respectively; and \(\theta_{i}(1)\), \(\dot{\theta}_{i}(1)\) and \(\ddot{\theta}_{i}(1)\) are the angular displacement, velocity, and acceleration at the end of the \(i\)th segment. When the manipulator starts and stops, the angular velocity and acceleration must be 0 to ensure safety. Then, the polynomial coefficient matrix \(a\) can be solved by Equations (16)-(18), where \(i_{i}\) of \(A\) matrix is \(i\)th segment interoperation time and \(X_{i}\) is the \(i\)th path point interpolation position of joint 1. \[A= \begin{bmatrix}1&t_{1}&t_{1}^{2}&t_{1}^{3}&-1&0&0&0&0&0&0&0&0\\ 0&1&2t_{1}&3t_{1}^{2}&0&-1&0&0&0&0&0&0&0\\ 0&0&2&6t_{1}&0&0&-2&0&0&0&0&0&0\\ 0&0&0&0&1&t_{2}&t_{2}^{2}&t_{3}^{3}&t_{4}^{4}&t_{5}^{5}&-1&0&0&0\\ 0&0&0&0&0&1&2t_{2}&3t_{2}^{2}&4t_{3}^{2}&5t_{4}^{4}&0&-1&0&0\\ 0&0&0&0&0&0&2&6t_{2}&12t_{2}^{2}&20t_{3}^{3}&0&0&-2&0\\ 0&0&0&0&0&0&0&0&0&0&1&t_{3}&t_{3}^{2}&t_{3}^{3}\\ 0&0&0&0&0&0&0&0&0&0&1&2t_{3}&3t_{3}^{2}\\ 0&0&0&0&0&0&0&0&0&0&2&6t_{3}\\ 1&0&0&0&0&0&0&0&0&0&0&0&0\\ 0&1&0&0&0&0&0&0&0&0&0&0&0\\ 0&0&2&0&0&0&0&0&0&0&0&0\\ 0&0&0&0&1&0&0&0&0&0&0&0&0\\ 0&0&0&0&0&0&0&0&0&1&0&0&0\\ \end{bmatrix} , \tag{16}\]  
\[b= \begin{bmatrix}0&0&0&0&0&0&X_{3}&0&0&X_{0}&0&0&X_{1}&X_{2}\end{bmatrix} ^{ \mathsf{T}}, \tag{17}\]  
\[a=A^{-1}\;b= \begin{bmatrix}a_{10}&a_{11}&a_{12}&a_{13}&a_{20}&a_{21}&a_{22}&a_ {23}&a_{24}&a_{25}&a_{30}&a_{31}&a_{32}&a_{33}\end{bmatrix} ^{\mathsf{T}}, \tag{18}\]  
Figure 3 shows the joint 1 trajectory planning results when the interpolation time of each section of 3-5-3 polynomial interpolation is 5 s. The results show that the joint displacement and velocity curves are smooth, and there are no abrupt changes in the acceleration curve.