# Research on Kinematics Analysis and Trajectory Planning of Novel EOD Manipulator
## 3 Kinematics Analysis of 5-DOF Manipulator
### Forward Kinematics Analysis  
In the forward kinematics analysis, the joint angle variable \(\theta_{i}\) is known for each joint of the robot, and the position and attitude of the robot end are calculated. According to the coordinate transformation rule of the manipulator, the relationship between the two connecting rods can be represented by the homogeneous transformation matrix:  
\[i-1T_{i}=\left[\begin{array}{cccc}\cos\theta_{i}&-\sin\theta_{i}\cos\alpha_{ i}&\sin\theta_{i}\cos\alpha_{i}&a_{i}\cos\theta_{i}\\ \sin\theta_{i}&\cos\theta_{i}\cos\alpha_{i}&-\cos\theta_{i}\sin\alpha_{i}&a_{ i}\sin\theta_{i}\\ 0&\sin\alpha_{i}&\cos\alpha_{i}&d_{i}\\ 0&0&0&1\end{array}\right] \tag{1}\]  
The homogeneous transformation of each joint can be derived from Equation (1) and Table 1. To be concise, in the following, \(\sin\theta_{i}\) and \(\cos\theta_{i}\) are denoted by \(C_{i}\), and \(\sin(\theta_{m}+\theta_{n})\) and \(\cos(\theta_{m}+\theta_{n})\) are denoted by \(S_{mn}\) and \(C_{mn}\), respectively, where \(i,m,n\in\{0,1,2,3,4,5,6\}\).  
\[{}^{0}T_{1}=\left[\begin{array}{cccc}C_{1}&0&S_{1}&0\\ S_{1}&0&-C_{1}&0\\ 0&1&0&d_{1}\\ 0&0&0&1\end{array}\right] \tag{2}\]  
\[{}^{1}T_{2}=\left[\begin{array}{cccc}C_{2}&-S_{2}&0&a_{2}C_{2}\\ S_{2}&C_{2}&0&a_{2}S_{2}\\ 0&0&1&0\\ 0&0&0&1\end{array}\right] \tag{3}\]  
\[{}^{2}T_{3}=\left[\begin{array}{cccc}C_{3}&-S_{3}&0&a_{3}C_{3}\\ S_{3}&C_{3}&0&a_{3}S_{3}\\ 0&0&1&0\\ 0&0&0&1\end{array}\right] \tag{4}\]  
\[{}^{3}T_{4}=\left[\begin{array}{cccc}C_{4}&-S_{4}&0&a_{4}C_{4}\\ S_{4}&C_{4}&0&a_{4}S_{4}\\ 0&0&1&0\\ 0&0&0&1\end{array}\right] \tag{5}\]  
\[{}^{4}T_{5}=\left[\begin{array}{cccc}C_{5}&0&S_{5}&a_{5}C_{5}\\ S_{5}&0&-C_{5}&a_{5}S_{5}\\ 0&1&0&0\\ 0&0&0&1\end{array}\right] \tag{6}\]  
\[{}^{5}T_{6}=\left[\begin{array}{cccc}C_{6}&0&S_{6}&0\\ S_{6}&0&-C_{6}&0\\ 0&1&0&d_{6}\\ 0&0&0&1\end{array}\right] \tag{7}\]  
\begin{table}
\begin{tabular}{c c c c c c} \hline \hline
**Joint** & \(\text{d}_{i}\) & \(\text{a}_{i}\) & \(\text{a}_{i}\) & \(\text{\theta}_{i}\) & **Joint Range/\({}^{\circ}\)** \\ \hline Waist joint & \(\text{d}_{1}\) & 0 & Pi/2 & \(\text{\theta}_{1}\) & 0–180 \\ Shoulder joint & 0 & \(\text{a}_{2}\) & 0 & \(\text{\theta}_{2}\) & \(-60\)–180 \\ Elbow joint & 0 & \(\text{a}_{3}\) & 0 & \(\text{\theta}_{3}\) & \(-120\)–90 \\ Virtual joint & 0 & \(\text{a}_{4}\) & 0 & \(\text{\theta}_{4}\) & 90 \\ Wrist pitch joint & 0 & \(\text{a}_{5}\) & Pi/2 & \(\text{\theta}_{5}\) & \(-90\)–90 \\ Wrist rotation joint & \(\text{d}_{6}\) & 0 & 0 & \(\text{\theta}_{6}\) & \(-180\)–180 \\ \hline \hline \end{tabular}
\end{table}
Table 1: The D–H parameter table of manipulator after introducing virtual joint.  
By multiplying the above Equation (2) to Equation (7), the position and attitude of the end-effector center of the manipulator can be obtained. See Equation (8).  
\[{}^{0}T_{6}={}^{0}T_{1}{}^{1}T_{2}{}^{2}T_{3}{}^{3}T_{4}{}^{4}T_{5}{}^{5}T_{6}= \left[\begin{array}{cccc}n_{x}&o_{x}&a_{x}&p_{x}\\ n_{y}&o_{y}&a_{y}&p_{y}\\ n_{z}&o_{z}&a_{z}&p_{z}\\ 0&0&0&1\end{array}\right] \tag{8}\]  
By inputting the initial joint angle \(\theta_{i}=0^{\circ},(i=1,2,3,5,6),\theta_{4}=90^{\circ}\) into (8), the following can be obtained:  
\[{}^{0}T_{6}=\left[\begin{array}{cccc}0&0&1&1.075\\ 0&-1&0&0\\ 1&0&0&0.6410\\ 0&0&0&1\end{array}\right] \tag{9}\]  
The results are completely consistent with those obtained from the Rkine forward kinematics solution function in the MATLAB Robotic Toolbox. Fkine is a function name of the software MATLAB Robotic Toolbox. At the same time, it is consistent with the D-H coordinate system created in this paper. It can be seen that the position and attitude vector of the end-effector center of the manipulator and the forward kinematics equation are correct.