# Research on Kinematics Analysis and Trajectory Planning of Novel EOD Manipulator
## 3 Kinematics Analysis of 5-DOF Manipulator
### Inverse Kinematics Analysis  
The inverse kinematics is known to be the position and attitude of the robot end, and all the joint variables of the corresponding position of the robot are solved, namely, the joint angle \(\theta_{i}\). In the field of engineering, inverse kinematics has a higher application value and is the basis of robot motion planning and trajectory planning [15]. To obtain this closed-form solution, two sufficient conditions need to be followed [16; 17; 18]:  
(1) Three adjacent joint axes intersect at one point;  
(2) There are three adjacent joint axes parallel to each other.  
The manipulator designed in this paper satisfies the second condition: shoulder joint, elbow joint and wrist pitch joint are parallel to each other. Therefore, the closed solution of robot inverse kinematics can be obtained. For the transformation matrix of the end-effector of the manipulator, the inverse transformation method is used to list multiple equations, the analytical solution \(\theta_{4}\) of the waist joint can be solved, and the analytical solutions of each subsequent joint can be obtained in turn.  
\[{}^{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{10}\]  
By arranging Equation (10) and pre-multiplying both sides of Equation (10) by \({}^{0}T_{1}{}^{-1},^{1}T_{2}{}^{-1},^{2}T_{3}{}^{-1},^{3}T_{4}{}^{-1},^{4}T_{5 }{}^{-1},^{5}T_{6}{}^{-1}\) in turn, one can obtain different equations [19], so as to obtain the value of each joint angle, as follows:  
\[\theta_{1}=atan2\big{(}p_{y},p_{x}\big{)} \tag{11}\]  
\[\theta_{6}=atan2\big{(}n_{x}S_{1}-n_{y}C_{1},o_{x}S_{1}-o_{y}C_{1}\big{)} \tag{12}\]  
\[\theta_{2345}=atan2\big{(}a_{x}C_{1}+a_{y}S_{1},-a_{z}\big{)} \tag{13}\]  
\[\theta_{3}=atan2(a_{3},a_{4})-atan2\Big{(}A^{2}+B^{2}-a_{2}^{2}-a_{3}^{2}-a_{4 }^{2}\pm\sqrt{\left[4a_{2}^{2}\Big{(}a_{3}^{2}+a_{4}^{2}\Big{)}-\left(A^{2}+B^ {2}-a_{2}^{2}-a_{3}^{2}-a_{4}^{2}\right)\right]}\,\Big{)} \tag{14}\]  
In the formula:  
\[A=p_{x}\,C_{1}+p_{y}S_{1}-a_{5}C_{2345}-d_{6}S_{2345}\] \[B=p_{z}-d_{1}-a_{5}S_{2345}+d_{6}C_{2345}\] \[\theta_{4}=pi/2 \tag{15}\]\[\theta_{5}=atan2\left(\pm\sqrt{\left(M-\left(P^{2}+Q^{2}+X^{2}+Y^{2}-a_{2}^{2} \right)^{2}\right)},P^{2}+Q^{2}+X^{2}+Y^{2}-a_{2}^{2}\right)-atan2(2\times(Q \times Y-P\times X),2\times(P\times Y+Q\times X)) \tag{16}\]  
In the formula:  
\[N=n_{z}C_{6}-o_{z}S_{6}\] \[P=-(-a_{5}a_{z}+d_{6}\big{(}a_{x}C_{1}+a_{y}S_{1}\big{)}-p_{x}C_ {1}-p_{y}S_{1}).\] \[Q=-\big{(}a_{5}\big{(}a_{x}C_{1}+a_{y}S_{1}\big{)}+d_{6}a_{z}-p_{ z}+d_{1}\big{)}\] \[X=a_{3}a_{z}+a_{4}N,\,X=a_{3}a_{z}+a_{4}N\] \[M=4\times\Big{(}(Q\times Y-P\times X)^{2}+(P\times Y+Q\times X)^{ 2}\Big{)}\] \[\theta_{2}=\theta_{2345}-\theta_{3}-\theta_{4}-\theta_{5} \tag{17}\]  
As can be seen from (14) and (16), there may be \(2^{2}\) joint combinations of the manipulator in the same position and orientation. However, due to the existence of the ultrasonic limiter on the manipulator, the value range of each joint variable is shown in Table 1. During the solution process, given the end target position and orientation pose T, only one group of four original solutions satisfies the range of joint variables, so the multiple solution optimization problem is not involved in the inverse kinematics solution process of the manipulator.