# Trajectory Tracking Algorithm Study of Coal Mine Water Detector Drilling Bar Installation
## 6 Analysis of the Trajectory Tracking Control of the Joint  
Examples of two joints of a four-degrees-of-freedom serially supplied material device are given in this paper. Build a dynamic model of the supplied material device, then design and optimize the sliding mode controller, and ultimately optimize the controller parameters using PSO. Simulate and analyze the effect of the sliding mode controller before and after PSO on the trajectory tracking control accuracy of the supplied material device. The control structure of the supplied material system and the construction of the simulation module are shown in Figures 10 and 11. Figure 8: Iterative change curve of fitness function. Figure 9: Curves of parameters’ variation. The matrix parameters in the dynamic model are as follows:  
\[\mathrm{M}(\mathrm{q})= \begin{bmatrix}\mathrm{P}+\mathrm{m}_{2}\mathrm{l}_{2}^{2}+ 2\mathrm{m}_{2}\mathrm{l}_{1}\mathrm{l}_{2}\cos\mathrm{q}_{2}&\mathrm{m}_{2} \mathrm{l}_{2}^{2}+\mathrm{m}_{2}\mathrm{l}_{1}\mathrm{l}_{2}\cos\mathrm{q}_{ 2}\\ \mathrm{m}_{2}\mathrm{l}_{2}^{2}+\mathrm{m}_{2}\mathrm{l}_{1}\mathrm{l}_{2} \cos\mathrm{q}_{2}&\mathrm{m}_{2}\mathrm{l}_{2}^{2}\end{bmatrix} ;\]  
\[\mathrm{C}(\mathrm{q},\dot{\mathrm{q}})= \begin{bmatrix}-\mathrm{m}_{2}\mathrm{l }_{1}\mathrm{l}_{2}\mathrm{q}_{2}\sin\mathrm{q}_{2}&-\mathrm{m}_{2}\mathrm{l} _{1}\mathrm{l}_{2}(\dot{\mathrm{q}}_{1}+\dot{\mathrm{q}}_{2}\sin\mathrm{q}_{ 2})\\ -\mathrm{m}_{2}\mathrm{l}_{1}\mathrm{l}_{2}\mathrm{q}_{1}\sin\mathrm{q}_{2}&0 \end{bmatrix} ;\]  
\[\mathrm{G}(\mathrm{q})= \begin{bmatrix}(\mathrm{m}_{1}+\mathrm{m}_{2})\mathrm{ g}\mathrm{l}_{1}\cos\mathrm{q}_{2}+\mathrm{m}_{2}\mathrm{g}\mathrm{l}_{2}\cos( \mathrm{q}_{1}+\mathrm{q}_{2})\\ \mathrm{m}_{2}\mathrm{g}\mathrm{l}_{2}\cos(\mathrm{q}_{1}+\mathrm{q}_{2}) \end{bmatrix} \]  
In the formula, \(\mathrm{p}=(\mathrm{m}_{1}+\mathrm{m}_{2})\mathrm{l}_{1}\)\({}^{2}\). The actual trajectories of the two connecting rods are \(\mathrm{q}_{1}\) and \(\mathrm{q}_{2}\), respectively. Create a dynamic model of the supplied material device in MATLAB/Simulink and simulate the process, with the simulation parameter set to \(\mathrm{m}_{1}=\mathrm{m}_{2}=0.5\) kg, and \(\mathrm{L}_{1}=\mathrm{L}_{2}=0.8\) m. They are the mass and length of the two connecting rods, respectively. The control effect of the sliding mode controller on the track tracking of the supplied material device under two different parameter determination methods can be determined. As shown in Figure 12, the sliding mold controller with the manually determined parameter traces the expected trajectory of joint3 in 2.24 s. As shown in Figure 13, after optimization of the controller parameter by PSO, joint3 traces the expected trajectory in 1.60 s under the control of that controller. It can be seen by comparing Figures 12 and 13 that the new controller model improves the control of joint3 after optimizing its parameters by PSO, resulting in a 36.6% reduction in the time taken to trace the expected trajectory. Figure 11: Construction of simulation module. Figure 10: The control structure of the supplied material system. As shown in Figure 20, the slider mold controller uses 2.8126 s to converge the track tracking error of the supplied material device to 0.0012 when the parameter is manually determined. As shown in Figure 21, after optimization of the controller parameter by PSO, the slider mold controller uses 2.0321 s to converge the track tracking error of the supplied material device to 0.0010 under the control of that controller. It can be seen by comparing Figures 20 and 21 that the new model controller has better controls after optimizing its parameters by PSO, resulting in smaller eventual convergence errors and a shorter usage time. In summary, the slide mold controller after the optimization of parameters with PSO has several advantages in terms of the position tracking control and speed tracking control of the joints, and it reduces the convergence time of tracking errors.