# Research on Kinematics Analysis and Trajectory Planning of Novel EOD Manipulator
## 5 Simulation and Experiment
### Experimental Verification of Manipulator  
The prototype is made to verify its running stability and grasping ability, and the trajectory planning process of grasping suspected explosives is carried out. In order to ensure that the manipulator can execute the command accurately, the motion accuracy of each servo motor of the manipulator is adjusted before the experiment. Therefore, we introduce PID algorithm. The classical PID controller [21] can reduce the add-on error caused by the robot arm in the motion process and improve the accuracy of the robot arm joint tracking. This algorithm is used as the robot arm joint control strategy, as shown in Formula (18):  
\[u(k)=K_{p}[e(k)+\frac{T}{T_{i}}\sum_{j=1}^{k}e(j)+\frac{T_{d}}{T}(e(k)-e(k-1))] \tag{18}\]  
\[\Delta u(k)=u(k)-u(k-1)=K_{p}(e(k)-e(k-1))+K_{i}e(k)+ \tag{19}\]  
\(K_{d}(e(k)-2e(k-1)+e(k-2))\)  
The meanings of the symbols in the above formula are shown in Table 3.  
The diagram of wrist rotation joint motor control is shown in Figure 16 below. We obtain the desired value by adjusting various parameters of the motor, such as K\({}_{p}\), K\({}_{t}\) and K\({}_{d}\). Figure 16: The diagram of wrist rotation joint motor control. \begin{table}
\begin{tabular}{c c} \hline \hline
**Symbol** & **Meaning** \\ \hline \(u(k)\) & Computer output value at the k-th sampling time \\ \(K_{p}\) & Proportional coefficient \\ \(K_{i}\) & Integral coefficient \\ \(K_{d}\) & Differential coefficient \\ \(e(k)\) & Deviation value input at the k-th sampling time \\ \(T\) & Sampling time \\ \hline \(T_{i}\) & Integration time \\ \(T_{d}\) & Differential time \\ \(k\) & Sampling serial number \\ \hline \hline \end{tabular}
\end{table} 
Table 3: Symbols and definitions. The change of the servo motor current loop before and after adjustment is shown in Figure 17.  
As shown in Figure 17, the error between the actual current and the rated current is basically eliminated after adjustment, which meets the control requirements of the manipulator. The data from the starting point A to the ending point B obtained in the above simulation are applied to the actual motion control of the manipulator. Experimental process of manipulator trajectory planning is shown in Figure 18. By saving the data of the end-effector of the manipulator during the movement and using the data analysis software, the curve of the points passed can be drawn, as shown in Figure 19.  
Figure 17: (a) Current change curve before debugging; (b) Current change curve after debugging. Figure 18: Experimental process of manipulator trajectory planning: (**a**) Control the mechanical claw to grasp the object; (**b**) The manipulator retracts the arm to the highest position; (**c**) Control the mechanical arm to the position above the explosion-proof tank; (**d**) Control the mechanical claw to grasp the object and put it into the explosion-proof tank. Figure 19: The simulated trajectory of each point of the manipulator. Figure 18 shows that the motion process of the manipulator is stable and the running track is basically consistent with the simulation trajectory. Figure 20 shows the complete simulation path diagram. Comparing Figures 18 and 20, it can be seen that during the experiment, the manipulator can reach the planned middle path point. Figure a corresponds to path point C, and Figure 18b corresponds to path point D. The movement process is smooth without jitter, so it can complete the blasting task. Finally, it can be proved in combination with Figure 19 that the manipulator can reach the expected position and orientation when the starting and ending point of the path are specified and the path point is given, and the error can be basically ignored. Through the above simulation experiments and prototype experiments, the manipulator basically meets the design requirements.