Trajectory Tracking Algorithm Study of Coal Mine Water Detector Drilling Bar Installation

Abstract

Mechanical water detection is recognized as the most reliable and safe production technology for coal mines, mainly for the detection of water hazards in pre-mining operations. Intelligent water detectors are currently the main research direction in mechanical water detection, and the automatic installation of drilling bars is the key to achieving intelligent water detection. Improving the connection accuracy in the process of installing drilling bars is an important research topic for the improvement of control links. To improve the connection accuracy of the drilling bars at the time of supplying material, we used the modified Denavit–Hartenberg method to analyze the motion gestures of the supplied material device and the Lagrange equation to establish a dynamic analysis model. We aimed at better control precision by improving the sliding mode control algorithm and at increasing the convergence rate of tracking errors with a sliding controller based on an exponential approximation law and using saturated functions instead of the symbol functions in the reaching law to weaken the vibration in the control process. We then used particle swarm optimization (PSO) to find the optimum combination parameters of the sliding mode controllers and test the performance of the sliding mode controllers before and after PSO with MATLAB/Simulink. The results showed that the optimized controller has a strong resistance to parameter fluctuations, and the system responds quickly, achieves a good performance, and improves the convergence rate of tracking errors.

1. Introduction

Detecting water is an important factor in safe production in coal mines. Mechanical water detection is recognized as the most reliable water detection technology, and its intelligent transformation is an inevitable development, in accordance with the development of science and technology, as well as the need for industrial development. At the beginning of the 21st century, Epiroc, Sandvik, Boart Longyear, and other companies entered a period of rapid development in drilling technology, developing the Smart ROC-Series intelligent drill with automated drilling holes, drilling bar loading, drilling, and other functions, the Pit Viper-Series automatic drill with remote control and automatic drilling bar switching functions, and the RDS Robot system drilling bar installation module, as shown in Figure 1. The method of using human-assisted installation and dismantling drilling bars means that the worker’s labor intensity is high, and people and machines working together can result in safety accidents [1]. An example of an artificial drilling operations scenario is shown in Figure 2. In 2019, China’s Coal Mine Safety Supervisory Authority published the “Coal Mine Robot Focus R&D Catalogue”, which requires that the water detector can be automatically installed and that drilling bar functions can be dismantled. In 2020, “the Guidelines on Accelerating the Intelligent Development of Coal Mines” clearly proposed raising the level of intelligence of coal mines to basic intelligence levels by 2025. The “14th Five-Year’ High Quality Development Guidelines for the Coal Industry”, issued in 2021, clearly proposed to significantly reduce the number of underground operators. China University of Mining and Technology and Harbin Institute of Technology jointly developed a drill robot that can automatically transport, install, and dismantle drilling bars. Chongqing Research Institute achieved fully automatic drilling of a drill Z through research and testing in the areas of automatic installation and dismantling drilling bars and automated drilling.

In the intelligent operation of mechanical water detection, how to ensure the connection accuracy of drilling bars when automatically completing the assembly tasks is the core technical issue for the development of water detection machinery [2]. The study of control algorithms for supplied material devices to optimize the trajectory tracing controller is a concrete safeguard measure to improve the connection accuracy [3,4]. Based on the theory of inverse kinematics, the application of the trajectory tracking control algorithm converts the Cartesian trajectory tracking control problem to a joint trajectory tracking control problem to study the accuracy of the control methods of the supplied material device [5].

Interference factors in the underground environment are numerous and uncertain, such as external disturbances of the controlled system, limiting the increase in tracking control accuracy [6,7]. At the same time, the supplied material device, as a nonlinear strong coupling system [8], has higher requirements for the control algorithm. The sliding mold control is essentially a special type of nonlinear control, in the design of which the sliding modules are unrelated to the parameters and disturbances of the controlled object, which can lead to effects of uncertainty and nonlinearity in most practical systems [9,10]. The good characteristics of the dynamic response make this control algorithm widely used in nonlinear control systems [11]. Therefore, this paper improves the control of the supplied material device by improving the sliding mold controller. In response to the need for the automatic injection of intelligent water detectors in coal mines, the supplied four-degrees-of-freedom material device which is the object of study here demonstrates a sliding-model control strategy based on PSO to optimize the trajectory tracking controller for the purpose of improving its joint trajectory tracking accuracy. First, we established a dynamic model of the supplied material device. Then, we designed a sliding mold controller based on the exponential approximation law and used the saturation function to replace the symbol function in the reaching law to weaken the vibration. Finally, using PSO to optimize the above controller parameters, the controller’s optimal combination of parameters is obtained. The simulation results showed that the controller optimized by PSO was better.

2. Motion Attitude Analysis Based on the MDH Method

In Figure 3 below, 3 is the base seats, 4, 5, 7, 8 are joints, 6 is the functional connectors, and 9 is the mechanical tongs, composing the supplied material device. The drilling bars are placed on the storage rack, and the supplied material device transports the new drilling bars from the storage rack to the planned installation location. A precise connection of the new drilling bars with the front drilling bars’ position can be achieved by adjusting the positional attitude of the mechanical tongs. A diagram of the relative positions is shown in Figure 3.

We preliminarily established the dynamic analysis model of the supplied material device by precisely defining its positional attitude. The supplied material device, formed by a series of functional connectors through the rotary joint, is a type of open-motion chain, and its characteristics comply with the MDH method [12]. Based on the above methods, the establishment of kinematic structural model was achieved, as shown in Figure 4. Based on the location of the rotary joint, four rectangular coordinates were established, with serial numbers of 0, 1, 2, and 3. The base coordinate corresponds to the 0 coordinate. The origin coordinate is at the bottom seat of the supplied material device, with the remaining three coordinates being the second and three rotary joints and the arm joints, respectively. The MDH coordinate model is shown in Figure 5.

Based on the coordinate model in Figure 4, the D-H parameters of the supplied material device are listed in

Table 1. D-H parameter table of the supplied material device.

i	ai−1 (mm)	αi−1 (°)	di (mm)	θi (°)
1	0	0	0	0
2	0	−90	0	−90
3	L1	0	0	0
h(4)	L2	0	0	0

. The meaning of the parameters is as follows: a is the length of the connecting rod, representing the distance between joint z_i and z_i+1 along the x axis; α is the angle of rotation, representing the changes in the rotation angle of joint z_i to z_i+1 around the x axis; d is the offset distance, representing the distance between joint x_i and x_i+1 along the z axis; θ is the angle of the connecting rod, representing the changes in the rotation angle of joint x_i to x_i+1 around the z axis, where x₁ and x₀ coincide when θ₁ is equal to 0° [13].

The angular displacement of each joint was designed using the forward kinematics of the robot [14]. At the same time, the positional attitude of the end coordinates of the hand were solved in relation to the base coordinate system. We resolved the positional attitude of the hand using known linear parameters and joint variables. The homogeneous transformation matrix of the adjacent connecting rod coordinate system, based on the rigid body posture attitude transformation theory, is as follows [15]:

$${ }_{\mathrm{i}}^{\mathrm{i}-1}\mathrm{T}=\left[\begin{array}{cc}\mathrm{R}& \mathrm{P}\\ 0& 1\end{array}\right]=\left[\begin{array}{cccc}\cos {\theta }_{\mathrm{i}}& -\sin {\theta }_{\mathrm{i}}& 0& {\mathrm{a}}_{\mathrm{i}-1}\\ \sin {\theta }_{\mathrm{i}}\cos {\alpha }_{\mathrm{i}-1}& \cos {\theta }_{\mathrm{i}}\cos {\alpha }_{\mathrm{i}-1}& -\sin {\alpha }_{\mathrm{i}-1}& -{\mathrm{d}}_{\mathrm{i}}\sin {\alpha }_{\mathrm{i}-1}\\ \sin {\theta }_{\mathrm{i}}\sin {\alpha }_{\mathrm{i}-1}& \cos {\theta }_{\mathrm{i}}\sin {\alpha }_{\mathrm{i}-1}& \cos {\alpha }_{\mathrm{i}-1}& -{\mathrm{d}}_{\mathrm{i}}\cos {\alpha }_{\mathrm{i}-1}\\ 0& 0& 0& 1\end{array}\right]$$

(1)

By substituting the D-H parameter values in Table 1 into Formula (1) for calculation, the coordinate transformation matrix from joint1 to joint4 of the profiling mechanism can be obtained as follows:

$${ }_1^0\mathrm{T}=\left[\begin{array}{cccc}\cos {\theta }_1& -\sin {\theta }_1& 0& 0\\ \sin {\theta }_1& \cos {\theta }_1& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(2)

$${ }_2^1\mathrm{T}=\left[\begin{array}{cccc}\cos {\theta }_2& -\sin {\theta }_2& 0& 0\\ 0& 0& 1& 0\\ -\sin {\theta }_2& -\cos {\theta }_2& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

(3)

$${ }_3^2\mathrm{T}=\left[\begin{array}{cccc}\cos {\theta }_3& -\sin {\theta }_3& 0& {\mathrm{L}}_1\\ \sin {\theta }_3& \cos {\theta }_3& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(4)

$${ }_4^3\mathrm{T}=\left[\begin{array}{cccc}\cos {\theta }_4& -\sin {\theta }_4& 0& {\mathrm{L}}_2\\ \sin {\theta }_4& \cos {\theta }_4& 0& 0\\ 0& 0& 1& 0\\ 0& 0& 0& 1\end{array}\right]$$

(5)

The positional attitude transformation matrix of the mechanical tongs relative to the base coordinates, using Formulas (2)–(5), is as follows:

$${}_4{}^0\mathrm{T}={\displaystyle \prod _{\mathrm{i}=1}^4{ }_{\mathrm{i}}^{\mathrm{i}-1}\mathrm{T}}=\left[\begin{array}{cc}{}_4{}^0\mathrm{R}& {}_4{}^0\mathrm{P}\\ \begin{array}{ccc}0& 0& 0\end{array}& 1\end{array}\right]=\left[\begin{array}{cccc}{\mathrm{T}}_{11}& {\mathrm{T}}_{12}& -\sin {\theta }_1& {\mathrm{L}}_1\cos {\theta }_1\cdot \cos {\theta }_2-\sin {\theta }_1\\ {\mathrm{T}}_{21}& {\mathrm{T}}_{22}& \cos {\theta }_1& {\mathrm{L}}_2\sin {\theta }_1\cdot \cos ({\theta }_2+{\theta }_3)\\ {\mathrm{T}}_{31}& {\mathrm{T}}_{32}& 0& {\mathrm{L}}_2\sin ({\theta }_2+{\theta }_3)-{\mathrm{L}}_1\sin {\theta }_2\\ 0& 0& 0& 1\end{array}\right]$$

(6)

In the formula,

$${\mathrm{T}}_{11}=\cos {\theta }_4\cdot \cos {\theta }_1\cdot \cos ({\theta }_2+{\theta }_3)-\sin {\theta }_4\cdot \cos {\theta }_1\cdot \sin ({\theta }_2+{\theta }_3);$$

$${\mathrm{T}}_{12}=-\sin {\theta }_4\cdot \cos {\theta }_1\cdot \cos ({\theta }_2+{\theta }_3)-\cos {\theta }_4\cdot \cos {\theta }_1\cdot \sin ({\theta }_2+{\theta }_3);$$

$${\mathrm{T}}_{21}=\cos {\theta }_4\cdot \sin {\theta }_1\cdot \cos ({\theta }_2+{\theta }_3)-\sin {\theta }_4\cdot \sin {\theta }_1\cdot \sin ({\theta }_2+{\theta }_3);$$

$${\mathrm{T}}_{22}=-\sin {\theta }_4\cdot \sin {\theta }_1\cdot \cos ({\theta }_2+{\theta }_3)-\cos {\theta }_4\cdot \sin {\theta }_1\cdot \sin ({\theta }_2+{\theta }_3);$$

$${\mathrm{T}}_{31}=-\cos {\theta }_4\cdot \sin ({\theta }_2+{\theta }_3)-\sin {\theta }_4\cdot \cos ({\theta }_2+{\theta }_3);$$

$${\mathrm{T}}_{32}=\sin {\theta }_4\cdot \sin ({\theta }_2+{\theta }_3)-\cos {\theta }_4\cdot \cos ({\theta }_2+{\theta }_3).$$

The MATLAB R2016b robotic toolbox can be used to construct a motion model of the supplied material device to obtain the spatial position of the terminal executor at its initial state, as shown in Figure 6.

3. Establishment of the Dynamic Model of the Supplied Material Device

Use the Lagrange equation to construct the dynamic analysis model by describing the joint control’s input force matrix and the relationship between the position, speed, and acceleration of the supplied material device. The specific process is as follows:

Based on the definition of the Lagrange function L, first establish the Lagrange function, defining the difference between the system’s kinetic energy K and potential energy P as follows [16]:

$$\mathrm{L}=\mathrm{K}-\mathrm{P}$$

(7)

Here, the coordinate representation method of K and P is not restricted. Defining the dynamic equation of the system, namely, the Lagrange equation, is achieved using the following formula [16]:

$${\mathrm{F}}_{\mathrm{i}}=\frac{\mathrm{d}}{{\mathrm{d}}_{\mathrm{t}}}\frac{\partial \mathrm{L}}{\partial {\dot{\mathrm{q}}}_{\mathrm{i}}}-\frac{\partial \mathrm{L}}{\partial {\mathrm{q}}_{\mathrm{i}}},\mathrm{i}=1,2,\dots \mathrm{n}$$

(8)

In the formula, ${\mathrm{q}}_{\mathrm{i}}$ represents the coordinates of the system’s kinetic energy and potential energy, ${\dot{\mathrm{q}}}_{\mathrm{i}}$ is the corresponding speed, n is the number of connecting rods, and F_i is the force matrix of the ith joint on the supplied material device. Finally, the formulation, taking into account external interference and unmodeled dynamics, is modeled as follows [17]:

$$\mathrm{M}(\mathrm{q})\ddot{\mathrm{q}}+\mathrm{C}(\mathrm{q},\dot{\mathrm{q}})\dot{\mathrm{q}}+\mathrm{G}(\mathrm{q})+\mathrm{d}(\mathrm{t})=\mathrm{u}(\mathrm{t})$$

(9)

In the formula, $\mathrm{q}={\left[{\mathrm{q}}_1,{\mathrm{q}}_2,\dots ,{\mathrm{q}}_{\mathrm{n}}\right]}^{\mathrm{T}}$ represents the joint position vector of the supplied material device; $\dot{\mathrm{q}}\in {\mathrm{R}}^{\mathrm{n}}$ and $\ddot{\mathrm{q}}\in {\mathrm{R}}^{\mathrm{n}}$ represent the angular velocity vector and angular acceleration vector, respectively; $\mathrm{M}(\mathrm{q})\in {\mathrm{R}}^{\mathrm{n}\times \mathrm{n}}$ represents the symmetrical inertia matrix of 2 × 2; $\mathrm{C}(\mathrm{q},\dot{\mathrm{q}})\in {\mathrm{R}}^{\mathrm{n}\times \mathrm{n}}$ represents the Cosmic force and centrifugal matrix of 2 × 2; $\mathrm{G}(\mathrm{q})\in {\mathrm{R}}^{\mathrm{n}}$ represents the gravitational vector of 2 × 1; $\mathrm{d}(\mathrm{q})\in {\mathrm{R}}^{\mathrm{n}}$ represents the external interference and unmodeled dynamics of the supplied material device of 2 × 1; and $\mathrm{u}\in {\mathrm{R}}^{\mathrm{n}}$ represents the joint-controlled force matrix vector of 2 × 1.

The expected angle q_d and its derivative are bounded as a specific study premise, which is carried out by optimizing the control algorithm, achieving the positioning accuracy control of the supplied material device, while effectively shielding it from external interference.

4. Stability Design of the Controller System

4.1. Design of Controller

Given the characteristics of the upper part of the material device, as well as the sliding mold control algorithm, use the sliding mold control to optimize the trajectory tracking control of the supplied material device. The expected position of the joint angle of the supplied material device is q_d, and the tracking error function is defined as follows:

$$\mathrm{e}(\mathrm{t})={\mathrm{q}}_{\mathrm{d}}-\mathrm{q}$$

(10)

The design of the sliding surface is as follows:

$$\mathrm{s}(\mathrm{t})=\mathrm{c}\cdot \mathrm{e}(\mathrm{t})+\dot{\mathrm{e}}(\mathrm{t})$$

(11)

In the formula, $\mathrm{c}=\mathrm{d}\mathrm{i}\mathrm{a}\mathrm{g}\left({\mathrm{c}}_1, {\mathrm{c}}_2, \dots {\mathrm{c}}_{\mathrm{n}}\right), {\mathrm{c}}_{\mathrm{i}} > 0$. When $\mathrm{t}\to \infty$, the index of the tracking error of the joint corner trajectory converges; the greater the value of c is, the faster the convergence rate is. Improve the dynamic performance of the supplied material device system by optimizing the design of the diagonal matrix c. This is obtained from Formulas (10) and (11):

$$\dot{\mathrm{s}}(\mathrm{t})=\mathrm{c}\dot{\mathrm{e}}(\mathrm{t})+\ddot{\mathrm{e}}(\mathrm{t})=\mathrm{c}\dot{\mathrm{e}}(\mathrm{t})+{\ddot{\mathrm{q}}}_{\mathrm{d}}-\ddot{\mathrm{q}}$$

(12)

Although the slide control improves the robustness of the system, at the same time, the temporal and spatial delays of the switching surface also lead to the control system vibrating. The vibration cannot be eliminated, there are only ways to weaken the vibration. In order to suppress the vibration, this paper uses the exponential approach law and replaces its symbol function with the saturation function; the expression of the reaching law is as follows:

$$\dot{\mathrm{s}}=-\epsilon \cdot \mathrm{s}\mathrm{a}\mathrm{t}(\mathrm{s})-\mathrm{k}\cdot \mathrm{s},\epsilon >0,\mathrm{k}>0$$

(13)

In the formula, the saturation function is $\mathrm{s}\mathrm{a}\mathrm{t}(\mathrm{s})=\left\{\begin{array}{l}1,\mathrm{s}>\mathrm{\Delta }\\ \mathrm{k}\mathrm{s},\left|\mathrm{s}\right|\le \mathrm{\Delta }\\ -1,\mathrm{s}<-\mathrm{\Delta }\end{array}\right.$.

Larger ε values can cause vibration, and this vibration can be weakened by adjusting the parameter ε of the reaching law to ensure the quality of the sliding module’s arrival process.

4.2. Proof of the Stability of the Supplied Material Device’s Control System

To prove the stability of the reaching law, select the Lyapunov function validation:

$$\mathrm{V}=\frac{1}{2}{\mathrm{s}}^2$$

(14)

It can be obtained from Formulas (13) and (14) that

$$s\dot{s}=-s\cdot \epsilon \cdot \mathrm{s}at(s)-k\cdot s^2<0$$

(15)

From Formula (15), it is obtained, based on saturation functions, that the exponential approach law satisfies Lyapunov’s theory of stability [18].

The system control rates obtained from Formulas (12) and (13) are as follows:

$$\mathrm{u}(\mathrm{t})=\mathrm{C}(\mathrm{q},\dot{\mathrm{q}})\dot{\mathrm{q}}+\mathrm{G}(\mathrm{q})+\mathrm{d}+\mathrm{M}[\mathrm{c}\dot{\mathrm{e}}(\mathrm{t})+{\ddot{\mathrm{q}}}_{\mathrm{d}}+\epsilon \cdot \mathrm{s}\mathrm{a}\mathrm{t}(\mathrm{s})+\mathrm{k}\cdot \mathrm{s}]$$

(16)

5. Parameter Optimization of the Controller Based on PSO

Since the sliding surface coefficient c and the parameters of the reaching law ε and k can affect the tracking effect of the supplied material device when designing the slider controller, it is necessary to optimize the design of the above three parameters. The efficient search capability of PSO helps find better solutions, and its global search feature can find the best solution in a larger range and avoid falling into the local best. Therefore, this paper uses particle swarm optimization, seeking the optimal combination of the above three parameters to improve the robustness of the control system. PSO is a method to search for the best result through a large number of parameters to optimize an optimal result, with structural simplicity and rapid convergence [19]. The optimal combination of parameters can be found through crowd search, and track tracking control of the supplied material device can be achieved based on the assumption that the target function J obtains the minimum value. The specific design steps are as follows [20]:

5.1. Determination of the Initial State of the Parameter

Suppose that an initial controller particle group PSO is randomly composed of n particles in a D-dimensional space, i.e., $\mathrm{P}\mathrm{S}\mathrm{O}=\left({\mathrm{p}}_1, {\mathrm{p}}_2, \dots {\mathrm{p}}_{\mathrm{n}}\right)$, and ${\mathrm{p}}_{\mathrm{i}}=(\mathrm{c}\mathrm{o}{\mathrm{n}}_1^{{\mathrm{p}}_{\mathrm{i}}},\mathrm{c}\mathrm{o}{\mathrm{n}}_2^{{\mathrm{p}}_{\mathrm{i}}},\mathrm{c}\mathrm{o}{\mathrm{n}}_3^{{\mathrm{p}}_{\mathrm{i}}})$ represents the three parameters of the controller to be optimized. The particle iteration has the corresponding flight speed ${\mathrm{v}}_{{\mathrm{p}}_{\mathrm{i}}}$ and position ${\mathrm{x}}_{{\mathrm{p}}_{\mathrm{i}}}$ and needs to meet $-{\mathrm{v}}_{\max }<{\mathrm{v}}_{{\mathrm{p}}_{\mathrm{i}}}<{\mathrm{v}}_{\max }$, $-{\mathrm{x}}_{\max }<{\mathrm{x}}_{{\mathrm{p}}_{\mathrm{i}}}<{\mathrm{x}}_{\max }$.

5.2. Determination of Controller Parameters Based on PSO

During the iteration, the particle dynamically adjusts the speed and position through individual and global extremums to constantly approach the optimal target at a certain rate [21]. In the t + 1 iteration, the speed and position of the particle update are as follows:

$$\begin{array}{l}{\mathrm{v}}_{\mathrm{i}\mathrm{d}}(\mathrm{t}+1)={\mathrm{\omega }}_{\mathrm{p}\mathrm{s}\mathrm{o}}{\mathrm{v}}_{\mathrm{i}\mathrm{d}}(\mathrm{t})+{\mathrm{c}}_1{\mathrm{r}}_1({\mathrm{P}}_{\mathrm{i}\mathrm{d}}(\mathrm{t})-{\mathrm{x}}_{\mathrm{i}\mathrm{d}}(\mathrm{t}))+{\mathrm{c}}_2{\mathrm{r}}_2({\mathrm{G}}_{\mathrm{i}\mathrm{d}}(\mathrm{t})-{\mathrm{x}}_{\mathrm{i}\mathrm{d}}(\mathrm{t}))\\ {\mathrm{x}}_{\mathrm{i}\mathrm{d}}(\mathrm{t}+1)={\mathrm{x}}_{\mathrm{i}\mathrm{d}}(\mathrm{t})+{\mathrm{v}}_{\mathrm{i}\mathrm{d}}(\mathrm{t}+1)\end{array}$$

(17)

In the formula, ${\mathrm{P}}_{\mathrm{i}\mathrm{d}}\left(\mathrm{t}\right)$ is the individual extremum of particle i based on its own flight experience, ${\mathrm{G}}_{\mathrm{i}\mathrm{d}}\left(\mathrm{t}\right)$ is the global extremum obtained from the group of particles’ flight experience, ω_pso is the inertia weight factor, and c₁, c₂ is the acceleration constant.

5.3. Determine Fitness Function

Use the fitness function to evaluate the properties of the particles [22], using the control system as the object of study to find a set of optimal combinations of parameters for the sliding surface coefficient c, as well as the approximation law parameters ε and k, so that the tracking error of the system is minimized. Based on the above requirement, take the sum of the errors of the two joints as the function to find the optimal parameter for the controller; the objective functions are as follows:

$$\mathrm{J}={\mathrm{e}}_1(\mathrm{t})+{\mathrm{e}}_2(\mathrm{t})$$

(18)

5.4. Algorithm Flow

In this algorithm, the input parameter is the particle group’s parameter and the acceleration constant of the sliding mold controller of the supplied material device, and the output result is the optimal parameter of the sliding mode controller. The specific processes are shown in Figure 7.

First, initialize the algorithm parameter and set the number of iterations, t, and number of sliding mode controller group particles, n. While randomly defining the initial position, flight speed, and acceleration constant of the particles, assume that the individual extremum of each particle is its current optimal position, and that the global extremum is the best particle position in the individual extremum. Calculate the fitness of each particle in the group using Formula (18); then, compare the particle fitness values to the individual extremum and the global extremum. If the current position of the particle is superior to the individual extremum, update the individual extremum, and if the particle swarm position is superior to the global extremum, update the global extremum. Obtain the particle in question’s up-to-date location and flight speed based on the updated individual extremum, the global extremum, and Formula (17). Finally, if it is found that the number of iterations exceeds the set maximum number, the iteration is stopped and the optimal solution is obtained, i.e., the optimal parameter of the sliding mode controller for the trajectory of the supplied material device; otherwise, Formula (18) is used to calculate the fitness of each particle in the group.

After the MATLAB/Simulink R2016b simulation iteration, the optimized results of PSO are obtained, as shown in Figure 8 and Figure 9.

As shown in Figure 8, after 15 iterations, the PSO has found the global extremum, at which point the target function achieves the minimum value. As shown in Figure 9, the red line represents the c value; the green line represents the k value; and the blue line represents the ε value, so the controller parameters have reached the optimum: c = 4.27, k = 1.5, ε = 2.1.

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 Figure 10 and Figure 11.

The matrix parameters in the dynamic model are as follows:

$$\mathrm{M}(\mathrm{q})=\left[\begin{array}{cc}\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{array}\right];$$

$$\mathrm{C}(\mathrm{q},\dot{\mathrm{q}})=\left[\begin{array}{cc}-{\mathrm{m}}_2{\mathrm{l}}_1{\mathrm{l}}_2{\dot{\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{\dot{\mathrm{q}}}_1\sin {\mathrm{q}}_2& 0\end{array}\right];$$

$$\mathrm{G}(\mathrm{q})=\left[\begin{array}{c}({\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{array}\right]$$

In the formula, p = (m₁ + m₂)l₁². The actual trajectories of the two connecting rods are q₁ and q₂, respectively. Create a dynamic model of the supplied material device in MATLAB/Simulink and simulate the process, with the simulation parameter set to m₁ = m₂ = 0.5 kg, and L₁ = L₂ = 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 Figure 12 and Figure 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.

As shown in Figure 14, the sliding mold controller with the manually determined parameter traces the expected trajectory of joint4 in 2.52 s. As shown in Figure 15, after optimization of the controller parameter by PSO, joint4 traces the expected trajectory in 2.03 s under the control of that controller. It can be seen by comparing Figure 14 and Figure 15 that the new model controller improves the control of joint4 after optimizing its parameters by PSO, resulting in a 19.4% reduction in the time taken to trace the expected trajectory.

As shown in Figure 16, the sliding mold controller with the manually determined parameter traces the expected speed of joint3 in 2.62 s. As shown in Figure 17, after optimization of the controller parameter by PSO, joint3 traces the expected speed in 2.19 s under the control of that controller. It can be seen by comparing Figure 16 and Figure 17 that the new model controller improves the control of joint3 after optimizing its parameters by PSO, resulting in a 16.4% reduction in the time taken to trace the expected speed.

As shown in Figure 18, the sliding mold controller with the manually determined parameter traces the expected speed of joint4 in 2.91 s. As shown in Figure 19, after optimization of the controller parameter by PSO, joint4 traces the expected speed in 2.03 s under the control of that controller. It can be seen by comparing Figure 18 and Figure 19 that the new model controller improves the control of joint4 after optimizing its parameters by PSO, resulting in a 30.2% reduction in the time taken to trace the expected speed.

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 Figure 20 and Figure 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. Details of this are shown in

Table 2. Optimized pre- and post-situation comparison.

						Time(s)
	Time Spent on Position Tracking before Optimizing	Time Spent on Position Tracking after Optimizing	Saved Time	Time Spent on Speed Tracking before Optimizing	Time Spent on Speed Tracking after Optimizing	Saved Time
joint3	2.24	2.16	0.08	2.62	2.19	0.43
joint4	2.52	2.03	0.49	2.91	2.03	0.88

.

7. Conclusions

In order to improve the connection accuracy of drilling bars in the automatic drilling process of intelligent water detectors, the trajectory tracking control of the drilling device was studied. First, the MDH method described the positional attitude of the supplied material device and was then used to construct the dynamic model based on the Lagrange equation. The sliding mold controller was designed on the basis of the dynamics model. The controller used the exponential approximation law and replaced the symbol function with the saturated function to weaken the inevitable vibration in the sliding mold control. Finally, using PSO to optimize the parameters of the controller, compared to the manually determined controller parameters, the combination of parameters obtained by PSO leads to sliding mold controller having a better effect on joints3 and -4 of the supplied material device, which can be summarized by the following points:

(1)

The time of the expected trajectory of joint3’s tracking decreased by approximately 36.6%. The time of the expected trajectory of joint4’s tracking decreased by approximately 19.4%. The above-detailed description of an optimized sliding mold controller increases the speed of the expected position on the tracking of the joints.

(2)

The expected speed of joint3’s tracking decreased by approximately 16.4%. The expected speed of joint4’s tracking decreased by approximately 30.2%. The above-detailed description of an optimized sliding mold controller decreases the expected speed of the tracking of the joints.

(3)

The sliding mold controller, before optimizing, used 2.8126 s to converge the overall tracking error of the supplied material device to 0.0012. Compared to the above effects, it took 2.0321 s to converge the overall tracking error of the supplied material device to 0.0010. The above data show that the sliding mold controller improves the convergence rate of the control error after optimizing the parameters using PSO.