# Time-Optimal Trajectory Planning for Woodworking Manipulators Using an Improved PSO Algorithm
## 2 Materials and Methods
### Mechanism of Woodworking Manipulator
#### 2.1.2 Kinematic Model of Woodworking Manipulator  
The relative motion components in the woodworking manipulator are treated as rigid connecting rods represented by straight lines. The motion joints are represented as moving and rotating pairs. Thus, the woodworking manipulator can be simplified. The modified D-H parameter method is commonly used for modeling different kinds of manipulators' connecting rod and joint. It is defined by four parameters: connecting rod torsion angle,  
Figure 1: Woodworking manipulator. connecting rod length, connecting rod offset, and joint angle, which can achieve coordinate system transformations through homogeneous transformation matrices and describe the pose and position of the end-effector [33]. Compared with the D-H parameter method, the modeling results are the same, but the position of the coordinate system is different. In our study, the modified D-H parameter method is more convenient. Then, the simplified manipulator's connecting rod and joint model, established by the modified D-H method, is shown in Figure 2, and the parameters are shown in Table 1.  
The modified D-H parameter method was used for the kinematic model, obtaining a homogeneous transformation matrix \({}^{i-1}A_{i}\)[33] between adjacent connecting rod coordinate systems. \[{}^{i-1}A_{i}= \begin{bmatrix}C\theta_{i}&-S\theta_{i}&0&a_{i-1}\\ S\theta_{i}C\alpha_{i-1}&C\theta_{i}C\alpha_{i-1}&-S\alpha_{i-1}&-S\alpha_{i-1} d_{i-1}\\ S\theta_{i}S\alpha_{i-1}&C\theta_{i}S\alpha_{i-1}&C\alpha_{i-1}&C\alpha_{i-1}d_{i-1} \\ 0&0&0&1\end{bmatrix} , \tag{1}\]  
where \(C\theta\) is cosine function, and \(S\theta\) is sine function. \begin{table}
\begin{tabular}{c c c c c} \hline \hline
**Joint \(i\)** & \(\theta_{i}\) **(rad)** & \(d_{i}\) **(mm)** & \(\alpha_{i-1}\) **(rad)** & \(\alpha_{i-1}\) **(mm)** & **Joint Range** \\ \hline
1 & \(-\pi/2\) & \(d_{1}\) & 0 & 0 & 0–1000 mm \\
2 & \(\pi/2\) & \(d_{2}\) & \(-\pi/2\) & 0 & 0–1500 mm \\
3 & \(\theta_{3}\) & \(d_{3}\) & \(-\pi/2\) & 0 & \(-180^{\circ}\)–\(180^{\circ}\) \\
4 & \(\theta_{4}\) & \(\pi/2\) & 0 & \(-180^{\circ}\)–\(180^{\circ}\) \\ \hline \hline \end{tabular} where \(\theta_{i}\) is the joint angle, \(\theta_{3}\) and \(\theta_{4}\) are variable; \(d_{i}\) is connecting rod offset; \(d_{1}\) and \(d_{2}\) are variable; \(d_{3}\) = 672 mm; \(d_{4}\) = 286.5 mm; \(\alpha_{i-1}\) is connecting rod torsion angle; and \(a_{i-1}\) is connecting rod length; O represents the position of the base coordinate system; \(\mathcal{O}_{1}-\mathcal{O}_{4}\) represent the position of joint 1–joint 4 coordinate system; and O\({}_{5}\) represents the position of the tool coordinate system.  
\end{table} 
Table 1: Modified D-H parameters of woodworking manipulator. Figure 2: Simplified woodworking manipulator. Then, calculate the manipulator's forward kinematics \(T\)-matrix through the variable values of each joint in Table 1, which shown in the following equation:  
\[ \begin{array}{c}T=T_{0}{}^{0}A_{1}{}^{1}A_{2}{}^{2}A_{3}{}^{3}A_{4}T_{tool}\\ =\begin{bmatrix}r_{11}&r_{12}&r_{13}&p_{x}\\ r_{21}&r_{22}&r_{23}&p_{y}\\ r_{31}&r_{32}&r_{33}&p_{z}\\ 0&0&0&1\end{bmatrix}=\begin{bmatrix}-5\theta_{3}C\theta_{4}&5\theta_{3}S \theta_{4}&C\theta_{3}&d_{2}&+d_{4}C\theta_{3}+yS\theta_{3}S\theta_{4}\\ S\theta_{3}&C\theta_{4}&0&d_{3}+yC\theta_{4}\\ -C\theta_{3}C\theta_{4}&C\theta_{3}S\theta_{4}&-S\theta_{3}&d_{1}+z-d_{4}S \theta_{3}+yC\theta_{3}S\theta_{4}\\ 0&0&0&1\end{bmatrix},\end{array}  \tag{2}\]  
where \(T_{0}\) is the base coordinate transformation matrix; \(T_{tool}\) is tool coordinate system transformation matrix and \(y\), \(z\) are the parameters in them. \(r_{ij}\) represents the pose information of the end-effector, and \(p_{x}\), \(p_{y}\) and \(p_{z}\) represent the position information of the end-effector.