1 Introduction

Based on ecological environment, lifestyle, food safety, and other factors, the incidence of gastrointestinal diseases is increasing, and non-invasive diagnosis and treatment by capsule robots have become hot research topics in recent years [1]. For traditional medical methods, such as ultrasound, CT, and cable gastroscopy, the capsule robots can clearly observe internal conditions of the gastrointestinal tract without harming surface tissue [2,3,4]. However, due to the peristaltic state, folds, and liquid environment of the gastrointestinal tract, the stable movement of the capsule robots is still a difficulty in current research [5, 6].

Considering the influence of nonlinear factors, robust control system is important research to achieve excellent motion performance of capsule robots [7,8,9,10]. So, many scholars have proposed different control approaches to improve the application ability of capsule robots in complex environments [11,12,13]. Turan et al. [14] presented a deep reinforcement learning method to train the continuous control of capsule robots, which simplified the tedious modeling of nonlinear environments to reduce the motion error. Aiming at the uncertain disturbance, Barducci et al. [15] proposed an adaptive control method to improve the motion speed of the medical robot. Furthermore, Lee et al. [16] designed an electromagnetic drive system with multi-coil combination, which could adjust various motion modes of capsule robots such as rolling, deflection, and translation. Similarly, Ryan et al. [17] adopted multiple permanent magnets to drive micro-robot with five degrees of freedom, increasing the intensity and gradient of magnetic field. Based on fuzzy impact factor, Sun et al. [18] optimized a turning control strategy of capsule robots, and solved the optimal value of average target turning speed and trajectory. Zhang et al. [19] proposed an underactuated double-hemisphere capsule robot, and presented a human–computer interactive control strategy of space magnetic field. Subsequently, Liu et al. [20] proposed a nonlinear control strategy based on Hamilton–Jacobi inequality theory, where the control errors from external disturbances could be suppressed.

In terms of the mechanical arm-permanent magnet drive mode, the trajectory control of manipulator is prerequisite to improve the stability of the magnetic field [21,22,23]. Dong et al. [24] added PID feed-forward with variable parameters to the traditional impedance controller of mechanical arm, and presented an adaptive adjustment method of PID parameters by Lyapunov stability theorem. In addition, Lu et al. [25] proposed an augmented Lagrange multiplier-compression particle swarm optimization algorithm, and used a time-acceleration optimal trajectory planning of manipulator. Aimed at the problems of time-varying uncertainties, input delay, and disturbances, Tao et al. [26] developed an indirect iterative learning control scheme, the effectiveness of which was verified by comparing with the direct type. Wang et al. [27] presented a general control strategy based on trajectory oscillation damping and the parameters of which were optimized by using particle swarm algorithm. Furthermore, Wang et al. [28] proposed a variable domain fuzzy control method through online optimization of the expansion factor, and used a time-optimal trajectory planning algorithm to obtain the expected joint position of the manipulator.

From literature reviews, capsule robots are mainly actuated by electromagnetic coil and permanent magnet [29,30,31]. The electromagnetic coil system is easy to control, but which has some shortcomings, such as weak magnetic field strength and limited magnetic field space. A permanent magnet can produce a strong magnetic field with low energy consumption, but the magnetic field gradient of which is large and uneven. In clinical application, capsule robots mainly adopt magnetic field with mechanical arm-permanent magnet [32, 33]. But the coupling of each joint in mechanical arm is strong, which requires a large working space and high equipment cost. Meanwhile, the magnetic field of motor in the joint may aggravate the magnetic noise in space. To ensure accurate motion of capsule robots, magnetic field instrument and corresponding control strategy have become important content of research.

In this work, a manipulation instrument of capsule robots with integrated motion control approach is proposed. Firstly, a steering fixture controlling permanent magnet is designed, and the manipulation instrument with five degrees of freedom is built by linear modules. Furthermore, an integrated motion control approach is presented including a primary motion subsystem with fuzzy controller and an auxiliary motion subsystem with PID controller. The calculation accuracy of Gray Wolf Optimization Algorithm (GWOA) is improved by using parallel optimization strategy; the main parameters of the controllers are tuned by the improved GWOA to enhance performance of the system. Finally, some motion tests are carried out in different planes and slopes, as well as simulated stomachs and real pig stomachs, and the experiments verify the effectiveness of the instrument and approach by comparing with traditional control systems.

The rest of this paper is organized as follows. Manipulation instrument and control mode are described in Sect. 2. The design of integrated motion control approach is introduced in Sect. 3. Parameter modification of integrated motion control approach is presented in Sect. 4. Experimental tests are carried out in Sect. 5. Section 6 concludes this work.

2 Manipulation instrument and control mode

2.1 Construction of manipulation instrument

As shown in Fig. 1a, the external permanent magnet (EPM) is regulated by four linear modules and a steering fixture to achieve five degrees of freedom (three linear and two rotational motions) in working area. The linear modules of X, Y, and Z axes can control translation of the EPM in X, Y, and Z directions, respectively. Utilizing the steering fixture and shift module, the linear module of U axis can control two rotating motions of the EPM by the axial and radial axes of rotation. The range of the four linear modules and corresponding motors is shown in Table 1. Figure 1b shows cross section of the steering fixture, where the driving shaft and the driven shaft are in the off position, also in the middle state shown in Fig. 1c. The driven shaft can only slide along the axis in the center hole of the friction disk, and the pressure spring locks the driven shaft by squeezing the friction disk in the middle state. Furthermore, the disk type turning pin and thrust bearing on the driving shaft have no contact with the rotary table and can rotate freely.

Fig. 1
figure 1

a Assembly drawing of the manipulation instrument. b Cross-section diagram of the steering fixture. c Switching schematic diagram of two rotating motions of the EPM

Full size image
Table 1 Main parameters of linear modules
Full size table

The combination of the shift module and the steering fixture can transfer kinetic energy from linear module of U axis to the EPM, and switch two rotating motions in different directions. As shown in Fig. 1c, the shift module is in three different states, corresponding to three different positions of the driving shaft and the driven shaft in the steering fixture. As can be seen from the section diagram of the shift module, two electromagnetic coils are installed on the two supporting rods respectively, and circular permanent magnet is fixed on the driving shaft in the middle. Attraction and repulsion of circular permanent magnet are controlled by the upper and lower electromagnetic coils, so as to adjust the sliding of the driving shaft along the axis. When the EPM is required to rotate around the radial center line, the shift module is in the upper state shown in Fig. 1c, where the driving shaft moves up to push the friction disk away from the rotary table, engaging with the driven shaft, and the thrust bearing on the driving shaft pushes the friction rod to lock rotary table. When the EPM needs to rotate around the axial center line, the shift module first changes from the upper state to middle state, and the driving shaft rotates slightly to adjust the position of the disk type turning pin to align with chute of the rotary table. Then, the shift module changes from the middle state to lower state, and the disk type turning pin engages with the chute of rotary table. So, the driving shaft drives all the structures on the rotary table, and the rotation of the EPM around the axial center line is realized.

Hardware framework of the manipulation instrument is shown in Fig. 2, and two DC power supplies provide 24 V voltage for the drivers and motion controller, the product parameters of which are shown in Tables 2 and 3. According to the established manipulation instrument, the control flow of capsule robots is shown in Fig. 3. Firstly, the motion control parameters are set through the upper computer platform, and the trigger signal is transmitted to the motion controller by shift knob. Then, the motion controller manages the four drivers to activate the four linear modules, respectively. The EPM can change the angle at any position in the workspace under the adjustment of the linear modules and the steering fixture. At the same time, in terms of the position of the EPM, the upper and lower magnetic sensor arrays measure composite magnetic field, then which is transmitted to host computer to solve the positions of capsule robots. The upper computer platform uses the calculated position information of capsule robots to adjust output signal of the controller. So, the motion state of the EPM is controlled in real time to reduce the position error of capsule robots.

Fig. 2
figure 2

Hardware framework of the manipulation instrument for capsule robots

Full size image
Table 2 Product specifications of drivers
Full size table
Table 3 Product specifications of motion controller
Full size table
Fig. 3
figure 3

Control flow chart of capsule robots

Full size image

2.2 Movement route planning of capsule robots

Under the influence of gravity and magnetic force, the capsule robots mainly adopt a turning motion, which rotates symmetrically with the EPM [34]. The capsule robots make contact with the inner surface of the stomach during the movement and clearly observe the surface tissue. In order to improve the accuracy of gastric disease examination, the movement route of capsule robots needs to be reasonably planned to traverse every area on the inner surface of the stomach.

The human stomach is composed of cardia, fundus, body, and pylorus, where the fundus and body of the stomach account for most of the region, showing an irregular spatial structure. As shown in Fig. 4a, the stomach is projected onto the X–Y plane, and the forward and reverse projections along the Z axis are projective planes 1 and 2, respectively. Projective planes 3 and 4 are the corresponding planes in the forward and reverse direction along the X axis. When the patient is examined in supine, left-side, prone, and right-side positions on the X–Y plane, the projections of the stomach onto the detection platform are on projective planes 2, 3, 1, and 4, respectively. Therefore, the spatial structure of the stomach is simplified into 4 projection planes by changing examination posture, that is, the capsule robots can be assumed to carry out inspection on the four projective planes. Although there are overlapping areas in each plane, the simplified rule can effectively avoid missing.

Fig. 4
figure 4

a The projection of the human stomach onto the four planes. b Schematic diagram of rotation motion for capsule robots

Full size image

Based on the above analysis of the stomach space planarization and the motion characteristics of the EPM, the movement routes of capsule robots are regularly divided. Figure 4b shows a circuitous route motion mode of capsule robots on the projective plane 2, that is, the capsule robots mainly reciprocate along the X axis. When reaching the edge area with a high slope, capsule robots adjust the observation angle or detect on other projective planes. Adopting the same motion mode in other projective planes, the edge areas of each plane can be effectively covered. At the same time, the manipulation of capsule robots for doctors is reduced, and the universal applicability of the equipment is improved.

3 Design of integrated motion control approach

3.1 Framework of integrated motion control approach

When a capsule robot is turning along the X direction, the linear module of X axis controls the EPM to translate, and the linear module of U axis adjusts the radial rotation motion, where the rotation center line is parallel to the Y axis. Therefore, the control of the linear modules of X and U axes is defined as the primary motion subsystem. In the process of movement, the capsule robot may produce motion errors along the Y direction, which is corrected by adjusting the linear module of Y axis. So, the control of the linear module of Y axis is defined as the auxiliary motion subsystem. On the contrary, the steering fixture is rotated 90°, that is, the EPM rotates 90° by axial rotation motion, and the capsule robot turns along the Y direction. Then, the control of linear modules of Y and U axes is defined as the primary motion subsystem; The control of linear module of X axis is defined as the auxiliary motion subsystem.

Based on the above definition, taking the motion direction of the capsule robot as a guide, the system is divided into parts I and II, as shown in Fig. 5. When the capsule robot is turning along the X direction, the steering fixture, shift module, and Z axis linear module are initially adjusted, and then, the shift knob of X axis is pressed; when the capsule robot needs to turn along the Y direction, the steering fixture, shift module, and shift knob of the Y axis are adjusted, and the system automatically switches to another part. Therefore, the two parts automatically switch according to the primary motion direction of capsule robot, and the integrated motion control approach has different primary/auxiliary motion subsystems to ensure the accuracy of the capsule robot in X/Y directions.

Fig. 5
figure 5

Schematic diagram of integrated motion control approach for two motion directions

Full size image

3.2 Primary motion subsystem based on fuzzy controller

In this section, taking the capsule robot moving in the X direction as an example, the primary motion subsystem based on fuzzy controller is presented to dynamically adjust the turning velocity of the EPM.

  1. (1)

    Define variables of the fuzzy logic controller

When capsule robot turns along X direction, the speed of the linear module of X axis is set as constant, and the linear module of U axis is adjusted. The velocity error E and the error change rate EC are defined as the input of the fuzzy logic controller, and the velocity regulating factor U of the linear module of U axis is defined as the output. According to the motion characteristics of capsule robot, the basic domain of input and output variables is determined as shown in Table 4.

  1. (2)

    Fuzzy variables

Table 4 Input and output variables
Full size table

The input and output variables are fuzzified by setting seven fuzzy states, namely (NB, NM, NS, ZO, PS, PM, PB). The Gaussmf function is used for the membership function of both input and output variables, the expression of which is as follows:

$${\mathrm{y}} \, = \, {\mathrm{e}}\frac{ ({\mathrm{x}}_{\mathrm{ls}}-{\mathrm{c}}_{\mathrm{ls}}{)}^{2}}{{{\sigma }_{\mathrm{ls}}}^{2}}$$
(1)

where xls is the basic domain of variables, cls is the center point of the function, and σls is the curve width of the function.

  1. (3)

    Design fuzzy rule base

The magnetic field of capsule robot and the EPM always tend to be parallel, so when the capsule robot appears small velocity error, the EPM should maintain original motion state. Then, the capsule robot in the process of movement will be adjusted, and restor the symmetric turning motion with the EPM in a certain range. When the velocity of the capsule robot slows down and the acceleration is negative, the output variable U should be adjusted to a large range, that is, the turning velocity of the EPM is much faster than the straight motion, so as to improve the velocity of the capsule robot. On the contrary, when the velocity of the capsule robot increases significantly and the acceleration is positive, the output variable U should be adjusted to a small range. In this case, the turning velocity of the EPM is far less than the straight motion, so as to reduce the velocity of the capsule robot. Based on the above analysis, the established fuzzy control rules are shown in Table 5.

  1. (4)(4)

    Defuzzification

Table 5 Fuzzy control rules of U
Full size table

The fuzzy value of the output can be obtained by the above fuzzy reasoning. At this time, the fuzzy value needs to be defuzzified to get the specific output. The weighted average method is adopted in this work, and the expression is as follows:

$${\mathrm{u}} \, = \, \frac{\sum_{{\mathrm{i}}= 1 }^{\mathrm{m}}{{\mathrm{x}}}_{\mathrm{i}}{\mu }_{\mathrm{n}}\mathrm{(}{\mathrm{x}}_{\mathrm{i}}{)}}{\sum_{{\mathrm{i}}= 1 }^{\mathrm{m}}{\mu }_{\mathrm{n}}\mathrm{(}{\mathrm{x}}_{\mathrm{i}}{)}}$$
(2)

where xi is the proportion of rule i in the output; μn(xi) is the output result of rule i.

3.3 Auxiliary motion subsystem based on PID controller

Due to irregular slopes in the inner wall of the stomach, displacement deviation may occur in the Y direction when the capsule robot turns along the X. With the capsule robot flips forward, the displacement deviation will be automatically adjusted in a small range. While the deviation is large, it is necessary to move the EPM to pull the capsule robot and make it return to preset route. Therefore, when the capsule robot moves along the X direction, by detecting the displacement deviation in the Y direction and adjusting the position of the EPM, an auxiliary motion subsystem based on PID controller is designed to ensure the motion accuracy of the robot.

According to the above approach, when the primary motion subsystem is in the X direction, the auxiliary motion subsystem regulates the displacement error of capsule robot in real time, controlling the EPM to move in Y direction. After the capsule robot and the EPM return to symmetric state, the auxiliary motion subsystem controls the EPM to original route, and the system continues to perform the primary motion control. The target displacement of the auxiliary motion subsystem is automatically calculated by upper computer to realize the adjustment of the capsule robot outside the primary motion.

4 Parameter modification of integrated motion control approach

4.1 Improved Gray Wolf Optimization Algorithm (IGWOA)

The GWOA is a meta-heuristic algorithm based on group optimality, which simulates four kinds of gray wolves: α, β, δ, and ω. Wolf-based hunting steps include hunting prey, encircling prey, and completing hunting, which correspond to finding, surrounding, and obtaining the optimal solutions of problems, respectively. It is defined that there are s gray wolves in population, and the search space is d dimensions, that is, the solution of the problem represented by the gray wolf i is xi = (xi1, xi2, xi3, …xid). The α, β, and δ wolves represent the first three optimal solutions of the problems, and the remaining solutions are denoted by ω wolves. Wolves need to surround prey before hunting, so the distance between wolves and prey can be expressed as follows:

$${\mathrm{D}} \, \mathrm{=} \, \mathrm{|}{\mathrm{C}} \, \times { \, {\mathrm{x}}}_{\mathrm{p}}\mathrm{(}{\mathrm{t}}\mathrm{)} \, - \, {\mathrm{x}}\mathrm{(}{\mathrm{t}}\mathrm{)|}$$
(3)
$${\mathrm{x}}\mathrm{(}{\mathrm{t}}\mathrm{+1)} \, \mathrm{=} \, {\mathrm{x}}_{\mathrm{p}}\mathrm{(}{\mathrm{t}}\mathrm{)} \, - \, {\mathrm{A}} \, \times \, {\mathrm{D}}$$
(4)

where t is the current iteration number; xp (t) is the position of prey; x (t) is the position of gray wolf; C and A represent the direction coefficient, which can be expressed as follows:

$${\mathrm{A}} \, \mathrm{=} \, {2}{\mathrm{a}} \, \times { \, {\mathrm{r}}}_{1} - \, {\mathrm{a}}$$
(5)
$${\mathrm{C}} \, \mathrm{=} \, {2} \, \times { \, {\mathrm{r}}}_{2}$$
(6)

where a linearly decreases from 2 to 0 during algorithm iteration; r1 and r2 are random direction vectors in the range of [0, 1].

When the location of prey is determined, α, β, and δ wolves represent the optimal solution of problems and direct wolves to launch the hunt. Based on the positions of the first three wolves, the positions of other gray wolves can be expressed as follows:

$$\begin{array}{c}D_\alpha=\vert C_1\times x_\alpha(t)-x(t)\vert\\{\mathrm{D}}_\beta\,=\,\mathrm{|}{\mathrm{C}}_2\,\times\,{\mathrm{x}}_\beta\mathrm{(}\mathrm{t}\mathrm{)}\,-\,\mathrm{x}\mathrm{(}\mathrm{t}\mathrm{)|}\\{\mathrm{D}}_\delta\,=\,\mathrm{|}{\mathrm{C}}_{3\,}\times\,{\mathrm{x}}_\delta\mathrm{(}\mathrm{t}\mathrm{)}\,-\,\mathrm{x}\mathrm{(}\mathrm{t}\mathrm{)|}\end{array}$$
(7)
$$\begin{array}{c}x_1=x_\alpha-A_1\times D_\alpha\\{\mathrm{x}}_2\,=\,{\mathrm{x}}_{\beta\,}-{\,\mathrm{A}}_{2\,}\times{\,\mathrm{D}}_\beta\\{\mathrm{x}}_3\,=\,{\mathrm{x}}_\delta-{\,\mathrm{A}}_3\,\times\,{\mathrm{D}}_\delta\end{array}$$
(8)
$${\mathrm{x}}_{\mathrm{p}}\mathrm{(}{\mathrm{t}} \, \mathrm{+} \, \mathrm{1)} \, \mathrm{=} \, \frac{{\mathrm{x}}_{1} \, \mathrm{+} \, {\mathrm{x}}_{{2} \, }\mathrm{+} \, {\mathrm{x}}_{3}}{3}$$
(9)

In the GWOA, α, β, and δ wolves lead the whole wolf pack to hunt, which is the endogenous power of wolf evolution. However, wolves are managed under the control of unified leadership that limits the diversity of population to a certain extent. When the three wolves of the leadership are in the same local optimal region at the same time, the algorithm is easy to fall into the local optimum and cannot get the global optimal solution. Therefore, in order to improve the comprehensive performance of global exploration and local deepening of the algorithm, a wolf parallel optimization strategy is introduced into the GWOA, where two different wolf update methods are adopted for parallel iteration to achieve different optimization abilities of the wolf pack in different iteration stages.

The direction coefficient A has a direct effect on the updating of ω wolves, which is a random number in [− a, a]. If the value of A is large, the global search ability of the algorithm is enhanced; While the value of A is small, the search range decreases, and the algorithm has superior local optimization ability. The ω wolves are divided into two parts, namely ω1 and ω2. The ω1 wolves account for the majority of the ω wolves, and adopt the traditional updating method, where the parameter a linearly decreases and is denoted as a1. The ω2 wolves account for a small part of the ω wolves, and Logistic curve is utilized to modify parameter a denoted as a2, as shown in Fig. 6a. According to the modification of parameter a, the ω2 wolves can maintain a large range of search in the early stage of the iteration, and quickly converge in the later stage, which is helpful to determine the optimal value of the algorithm.

Fig. 6
figure 6

a Variation curve of coefficient a. b Schematic diagram of wolf searching range of the IGWOA

Full size image

By dividing the ω wolves into the ω1 and ω2 with different updating ways, the optimal solution is selected after each iteration, which is utilized to update three historical optimal solutions (namely α, β, and δ wolves) until meeting the solution accuracy of problems. As shown in Fig. 6b, ω2 wolves are added to the GWOA, which can expand the search range in an early stage of the algorithm, and carry out a small range of optimization in the later stage. Moreover, the solutions obtained by ω2 wolves are compared with the current ω1 wolves, and the optimal solution is recorded through competition. Keeping the population number unchanged, the diversity of the population is improved by adding an update method, and the optimal solution of problems is selected by using the competition mechanism. Figure 7 is the flow chart of the IGWOA.

Fig. 7
figure 7

Flowchart of the IGWOA

Full size image

In order to test the performance of the IGWOA, four test functions are adopted to verify the effectiveness of the algorithms, where the performance of the algorithm is evaluated by the solution accuracy and the changing trend of the fitness function in iteration process. In this work, Sphere, Griewank, Sum fourth power, and Levy are selected as the test functions, respectively. In the IGWOA, the ω1 wolves account for 70% of original ω wolves, and the ω2 wolves account for 30%. The GWOA and IGWOA are set in the same initial conditions, where the population number is 30, and the maximum iteration is 600. The variation trends of fitness values in iteration process are shown in Fig. 8, and the operation results in the test functions are shown in Table 6.

Fig. 8
figure 8

Comparison of convergence curves of algorithm fitness values

Full size image
Table 6 Solution results of the test function
Full size table

According to the comparison results of the above four typical test functions, the GWOA and IGWOA have similar calculation accuracy and convergence speed at the beginning of the iteration. With the increase of iteration number, the IGWOA can accelerate the convergence and effectively avoid the local optimal value in the later stage, so as to get more accurate solution. Therefore, the improved method proposed in this work enhances the optimization ability of GWOA.

4.2 Simulation of primary motion subsystem

In this section, the primary motion subsystem is analyzed, the transfer function of which is established. Furthermore, the IGWOA is adopted to optimize the amplitude parameters of membership function in fuzzy controller. Finally, the simulation verification is carried out.

Transmission devices adjusted in the primary motion subsystem include linear module of U axis, hybrid stepper motor, synchronous belt, supporting rod, drive shaft, steering fixture, and the EPM. Firstly, the motion parameters of each linear module are set in the upper computer of system, and the operator activates the system to work by shift knob. The expected velocity wr(t) is calculated and sent to the fuzzy controller. Subsequently, the fuzzy controller analyzes the error E and error change rate EC, and outputs speed regulating factor U to correct the signal wr(t) that is then imported to transmission devices. Finally, the actual velocity wc(t) of capsule robot is measured as the output of the system. Compared with the motion response of the capsule robot, the errors of signal processing and information delay of circuit component in the upper computer can be ignored. The effect of transmission devices on the capsule robot can be expressed by a second-order transfer function model in a certain range. In this work, unit excitation response experiments are carried out, and the velocity change of the capsule robot is measured by machine vision equipment. Using the system identification toolbox in MATLAB, the input data and the collected output data are identified, and the closed-loop transfer function of the transmission devices can be obtained as follows:

$${\mathrm{G}}_{\mathrm{I}}\mathrm{(}{\mathrm{s}}\mathrm{)} \, \mathrm{=} \, \frac{29.15}{{\mathrm{s}}^{2} \, \mathrm{+} \, \mathrm{3.2}{\mathrm{s}} \, \mathrm{+} \, \mathrm{29.15}}$$
(10)

The range of membership function in fuzzy controller has an important impact on system performance, which is optimized by using the IGWOA. The amplitude parameters of 21 membership functions (Gaussmf functions) are optimized for input and output variables. In terms of the established primary motion subsystem, the optimal parameter values of the membership functions are obtained through continuous iterative unit step signal simulation tests. ITSE performance index is used as the objective function of parameter optimization, which is defined as follows:

$${\mathrm{ITSE}}_{\mathrm{obj}}\mathrm{ } = \, {\int }_{0}^{\infty }{\mathrm{t}} \, \times \, {\mathrm{e}}^{2}\mathrm{(}{\mathrm{t}}\mathrm{)}{\mathrm{dt}}$$
(11)

The population of the IGWOA is set to 30; the dimension of each wolf is set to 21, that is, 21 parameter values to be optimized; and the iteration number is set to 100. In order to evaluate the performance of the IGWOA more intuitively, the GWOA with the same parameter configuration is used as the comparison term. The tuning results of the above three parameters are shown in Table 7.

Table 7 Optimization parameters of input and output membership functions
Full size table

The parameter optimization schemes of the above three fuzzy controllers are imported to the primary motion subsystem, and the simulation tests with unit step signal are conducted. The output curves of the simulation tests are shown in Fig. 9. The response curves of the unit step signal in each scheme are analyzed, and the comparison results are shown in Table 8.

Fig. 9
figure 9

Output curves of unit step signals based on different fuzzy controller parameters

Full size image
Table 8 Control performance evaluation indicators of primary motion subsystem
Full size table

When the primary motion subsystem is only closed-loop regulated, the rise time of the system response is short, but the overshoot is too large. Compared with the fuzzy controllers based on GWOA and empirical value, IGWOA fuzzy controller can shorten the rise time by 52.4% and 54.2%, decrease the overshoot by 26.5% and 24.6%, and shorten the adjustment time by 50% and 42.2%, respectively. From simulation and comparative analysis, the fuzzy controller based on IGWOA can be utilized in the primary motion subsystem of the capsule robots.

4.3 Simulation of auxiliary motion subsystem

The hardware part of the auxiliary motion subsystem includes motion controller, hybrid stepper motor, timing belt, and supporting rod. Ignoring the delay error of the motion controller, the input of the auxiliary motion subsystem is step angle θr, and the actual output is θc. Based on the theory of small oscillation, and ignoring the effects of magnetic leakage, hysteresis, eddy current, and saturation, the voltage balance equation of hybrid stepper motor can be expressed as follows:

$$\begin{array}{c}U_a=Ri_a+L\frac{di_a}{dt}-K_m\omega_csin(Z_r\theta_d)\\{\mathrm{U}}_\mathrm{b}\,\mathrm{=}\,\mathrm{R}{\mathrm{i}}_\mathrm{b}\,\mathrm{+}\,\mathrm{L}\frac{\mathrm{d}{\mathrm{i}}_\mathrm{b}}{\mathrm{d}\mathrm{t}}\,\mathrm{+}\,{\mathrm{K}}_\mathrm{m}\omega_\mathrm{c}\mathrm{cos(}{\mathrm{Z}}_\mathrm{r}\theta_\mathrm{d}\mathrm{)}\end{array}$$
(12)

where Ua, Ub, ia, and ib are phase voltage and phase current of A and B, respectively; R and L are the resistance and inductance of the motor winding, respectively; Zr, θd, and ωc are the teeth number, angular displacement, and angular velocity of the rotor, respectively.

According to Eq. (12), the torque balance equation of the auxiliary motion subsystem based on linear module can be expressed as follows:

$${\mathrm{T}}_e\,=\,\mathrm{J}\frac{\mathrm{d}\omega_\mathrm{c}}{\mathrm{dt}}\,+{\,\mathrm{D}}_\mathrm{z}\omega_\mathrm{c}\,+\,{\mathrm{T}}_\mathrm{l}\,=\,-{\mathrm{K}}_\mathrm{m}{\mathrm{i}}_\mathrm{a}\mathrm{sin(}{\mathrm{Z}}_\mathrm{r}\theta_\mathrm{d})\,+{\,\mathrm{K}}_\mathrm{m}{\mathrm{i}}_\mathrm{b}\mathrm{cos(}{\mathrm{Z}}_\mathrm{r}\theta_\mathrm{d}\mathrm{)}$$
(13)

where Dz, Km, Te, and Tl are the damping coefficient of the control system, the back electromotive force coefficient of motor, the electromagnetic force, and the load torque, respectively.

The stepper motor is set as unidirectional excitation. According to Eqs. (12) and (13), the motion equation of the auxiliary motion subsystem can be deduced as follows:

$${\mathrm{J}}\frac{{\mathrm{d}}^{2}{\theta }_{\mathrm{d}}}{{\mathrm{d}}^{2}{\mathrm{t}}} \, \mathrm{+} \, {\mathrm{D}}_{\mathrm{z}}\frac{{\mathrm{d}}{\theta }_{\mathrm{d}}}{\mathrm{dt}} \, - \, \frac{{\mathrm{Z}}_{\mathrm{r}}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}\mathrm{sin(}{\mathrm{Z}}_{\mathrm{r}}{\theta }_{\mathrm{d}}\mathrm{)=0}$$
(14)

At the initial state, dθd/dt = 0, the incremental equation of motion can be expressed as follows:

$${\mathrm{J}}\frac{{\mathrm{d}}^{2}\mathrm{(}\Delta\theta_{\mathrm{d}}\mathrm{)}}{{\mathrm{d}}^{2}{\mathrm{t}}} \, \mathrm{+}{ \, {\mathrm{D}}}_{\mathrm{z}}\frac{{\mathrm{d}}\mathrm{(}\Delta\theta_{\mathrm{d}}\mathrm{)}}{\mathrm{dt}} \, - \, \frac{{\mathrm{Z}}_{\mathrm{r}}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}\mathrm{sin(}{\mathrm{Z}}_{\mathrm{r}}\Delta\theta_{\mathrm{d}}\mathrm{)} \, \mathrm{=} \, {0}$$
(15)

where Δθd = θc-θr is small and can be linearized. Equation (15) can be simplified as follows:

$${\mathrm{J}}\frac{{\mathrm{d}}^{2}{\theta }_{\mathrm{c}}}{{\mathrm{d}}^{2}{\mathrm{t}}} \, \mathrm{+} \, {\mathrm{D}}_{\mathrm{z}}\frac{{\mathrm{d}}{\theta }_{\mathrm{c}}}{\mathrm{dt}} \, \mathrm{+} \, \frac{{{\mathrm{Z}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}{\theta }_{\mathrm{c}} \, \mathrm{=} \, \frac{{{\mathrm{Z}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}{\theta }_{\mathrm{r}}$$
(16)

Laplace transform is applied to both sides of Eq. (16), the initial state is zero, and the following can be obtained follows:

$$\mathrm{(}{\mathrm{J}}{\mathrm{s}}^{2} \, \mathrm{+}{ \, {\mathrm{D}}}_{\mathrm{z}}{\mathrm{s}} \, \mathrm{+} \, \frac{{{\mathrm{Z}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}{\mathrm{)}\theta }_{\mathrm{c}}\mathrm{(s)} \, \mathrm{=} \, \frac{{{\mathrm{Z}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}}{2}{\theta }_{\mathrm{r}}\mathrm{(s)}$$
(17)

The transfer function of the auxiliary motion subsystem can be expressed as follows:

$${\mathrm{G}}_{\mathrm{II}}\mathrm{(s)} \, \mathrm{=} \, \frac{{{\mathrm{Z}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}\mathrm{/2}{\mathrm{J}}}{{\mathrm{s}}^{2} \, \mathrm{+} \, {\mathrm{D}}_{\mathrm{z}}\mathrm{/}{\mathrm{J}}{\mathrm{s}} \, \mathrm{+}{{ \, {\mathrm{Z}}}_{\mathrm{r}}}^{2}{\mathrm{L}}{{\mathrm{i}}_{\mathrm{a}}}^{2}\mathrm{/2}{\mathrm{J}}}$$
(18)

The motor parameters of the four linear modules are shown in Table 9. Damping coefficient of the auxiliary motion subsystem is set to 0.39, so the transfer function of the auxiliary motion subsystem based on the linear module of Y axis can be calculated as follows:

Table 9 Parameters of hybrid stepper motor
Full size table
$${\mathrm{G}}_{\mathrm{II}}\mathrm{(s)} \, \mathrm{=} \, \frac{38.97}{{\mathrm{s}}^{2} \, \mathrm{+} \, \mathrm{1.9}{\mathrm{s}} \, \mathrm{+} \, \mathrm{38.97}}$$
(19)

The auxiliary motion subsystem of the capsule robots is mainly regulated by PID controller, the parameter selection of which plays a key role in the performance of system. Therefore, the IGWOA is utilized to optimize the kp, ki, and kd in the PID controller. Based on the established transfer function of the auxiliary motion subsystem, the optimal parameters of PID controller are obtained through continuous iterative unit step signal simulation tests. The gray wolf population of the IGWOA is set to 30, the dimension of each wolf is 3, and the iteration number is 100. The IGWOA is adopted to adjust the parameters of PID controller, and the optimal parameters are kp = 62.00, ki = 13.52, kd = 1.95. In addition, using the GWOA with the same configuration as the IGWOA, kp = 40.15, ki = 8.01, and kd = 1.16 are obtained; Finally, setting the same initial value, Particle Swarm Optimization Algorithm (PSOA) is used to optimize the parameters of PID controller, and the optimization results are as follows: kp = 33.46, ki = 23.94, and kd = 1.16. The above three PID controller optimization schemes are placed in the auxiliary motion subsystem, and the unit step signal simulation tests are carried out. The comparative response curve of the unit step signal is shown in Fig. 10, and the dynamic performance analysis results are shown in Table 10.

Fig. 10
figure 10

Output curves of unit step signals based on different PID controller parameters

Full size image
Table 10 Control performance evaluation indicators of the auxiliary motion controller
Full size table

Compared with PSOA and GWOA of PID control systems, the response rise time based on IGWOA is shortened by 25.3% and 15.1%, the overshoot is decreased by 88.5% and 88.8%, and the adjustment time is shortened by 78.3% and 35.9%, respectively. Through simulation and comparative analysis, the PID controller based on IGWOA can be utilized in the auxiliary motion subsystem of the capsule robots.

5 Experimental tests of capsule robots

5.1 Experimental tests of symmetric turning motion

As a basic motion mode of the capsule robots, symmetric turning motions are tested on surfaces with different roughness and slopes using the manipulation instrument. The designed magnetically controlled capsule robot model and prototype are shown in Fig. 11a and b [34], the test platform is shown in Fig. 11c, and the specific parameters of which are listed in Table 11.

Fig. 11
figure 11

a Magnetically controlled capsule robot model. b The prototype of the designed capsule robot. c Experimental test platform for the turning motion of the capsule robot

Full size image
Table 11 Parameters of the designed capsule robot and the EPM
Full size table

The capsule robot is placed on two planes with friction coefficient of fμ = 0.1 and 0.9, respectively. The primary motion subsystem is in X direction, and the motion speed of the linear module of X axis is set to 45 mm/s. The proposed integrated control system is adopted, where the ratio u of rotational to straight motion is automatically adjusted. For comparison, two traditional open-loop control schemes are adopted, namely, the comparative system i and the comparative system ii. According to the symmetric turning motions property, the ratio u is taken as empirical values of 1.1 and 1.6 in the two comparative systems, respectively. The time-displacement changes of the capsule robot corresponding to the three motion control schemes are shown in Fig. 12. As shown in Fig. 12a, compared with other motion systems, the comparative system ii has a larger ratio u, which makes the capsule robot flip faster, and the average movement speed is high. But the comparative system ii also produces a large forward error and has motion instability. Compared with Fig. 12a, the motion speeds of the comparative systems i and ii in Fig. 12b are improved, and the larger surface friction coefficient can promote the forward motion of the capsule robot. From Fig. 12a and b, the proposed integrated control system has a relatively stable displacement output, which is basically consistent with the ideal value, and the system is less affected by the surface friction coefficient.

Fig. 12
figure 12

Displacement of capsule robot on planes. a Smooth surface (fμ = 0.1). b Rough surface (fμ = 0.9)

Full size image

Furthermore, in order to test the control characteristics on slope surface, the capsule robot is placed on the slope with an angle of 22°, and the friction coefficients fμ = 0.1 and 0.9 are set on the slope surface, respectively. Similar to the above planar motion, the capsule robot makes turning motion along the X direction, and the motion speed of the linear module of X axis is set to 22 mm/s. Using the comparative systems i and ii described above, the time-displacement changes of the capsule robot are shown in Fig. 13. Due to the presence of slope surface, the hysteresis displacement of the capsule robot is reduced, and the motion speeds of the systems i and ii are increased to a certain extent. However, as the distance between the capsule robot and the EPM increases along the slope, the system driving effect weakens. In addition, comparing Fig. 13a and b, when the friction coefficient fμ is large, the capsule robot has better climbing ability.

Fig. 13
figure 13

Displacement of capsule robot on slopes. a Smooth surface (fμ = 0.1). b Rough surface (fμ = 0.9)

Full size image

By comprehensive analysis of the above four motion test results, the capsule robot has a small motion error fluctuation under the action of the proposed integrated system, and can quickly adjust the ratio u, so as to achieve relatively stable and continuous motion. The average displacement errors of the three control schemes in the four motion environments are shown in Table 12. The average displacement errors of the proposed integrated control system are not higher than 4.2 mm, which meets the basic motion control requirements (less than 5.88 mm) [35].

Table 12 Comparison of average displacement errors based on different control systems
Full size table

5.2 Experimental tests in simulated stomach environments

In order to further test the motion control performance of the system, a 3D-printed stomach model and a real pig stomach are utilized to simulate the human stomach environment, and some experiments are carried out in this section.

According to the size of the human stomach, a similar stomach model is 3D printed with resin material. At the same time, a flexible film is pasted into the inner cavity of the model to simulate the folds of the stomach, and the liquid environment of the stomach is created by filling with warm water. The capsule robot is controlled to move along X direction, and the straight speed of the EPM is set as 15.62 mm/s, and the initial rotation speed is set as 1.61 rad/s. A camera is installed on the upper part of the experimental device to take real-time shots of the capsule robot. Using the proposed integrated control system, the motion screenshots are shown in Fig. 14. Furthermore, experimental tests of the comparative systems i and ii are carried out; the actual movement trajectories and theoretical route of the capsule robot are shown in Fig. 15.

Fig. 14
figure 14

Motion screenshots of the capsule robot in a simulated stomach

Full size image
Fig. 15
figure 15

Displacement curves of the capsule robot in a simulated stomach

Full size image

As shown in Fig. 15, the motion control trajectories of comparative systems i and ii show a large error from the ideal trajectory. At the same time, the operating results of the proposed system also show a deviation from the ideal trajectory. Because the flexible folds in the simulated stomach hinder the movement of the capsule robot, the liquid environment greatly reduces the friction force of surface contact, which affects the flipped forward of the capsule robot. So, the phenomenon of skid and retreat occurred during movement. By collecting 20 displacement points of the capsule robot in each experimental test, the average displacement errors of the comparative system i and the comparative system ii, and the proposed system can be obtained as 7.6 mm, 8.2 mm, and 5.8 mm, respectively. Therefore, it can be seen that the proposed integrated control system can reduce the influence of flexible folds and liquid environment and has a good control effect.

In addition, a real pig stomach is utilized to the experiments in this section, as shown in Fig. 16. The inner cavity of the pig stomach is a bumpy and flexible surface, and an arbitrary point is selected to mark the lesion shown in Fig. 16b. Since the stomach is in a state of peristalsis in living environment, a simulated peristalsis device is set up in the experiment, as shown in Fig. 16a and c. The stepper motor pulls the dither rod periodically through the cam and transmission belt, and regular shaking of the pig stomach is achieved. Finally, the inner cavity of the pig stomach is filled with warm water to simulate the gastric fluid environment, and the control performance of the capsule robot is tested by inspecting the location of the lesion. The capsule robot is placed at one end of the pig stomach, and the lesion is at the other end. Micro-camera and wireless transmission module in the capsule robot are used to observe the stomach condition, and the operator can check the lesion by adjusting the integrated control system from the upper computer. In the primary motion subsystem, the straight speed of the EPM is set as 15.62 mm/s, and the initial rotation speed is 1.15 rad/s. In the auxiliary motion subsystem, the offset speed of the EPM is 7.81 mm/s.

Fig. 16
figure 16

a Experimental manipulation instrument based on pig stomach. b Real pig stomach and simulated lesion. c Simulated peristalsis device

Full size image

Based on the experimental setup described above, the capsule robot takes pictures of the lesion in the pig stomach as shown in Fig. 17. According to the positioning information of the capsule robot and the wirelessly transmitted photos of the pig stomach, the operator adjusts the EPM through the integrated control system to realize the motion of the capsule robot. After 32.5 s, the operator can control the capsule robot to find the lesion from the initial position, which verifies the effective actuation ability of the manipulation instrument. Due to the presence of a large number of white broken mucosa in the pig stomach, the clarity of gastric fluid environment is not high, which leads to some blurred images taken in the experiments.

Fig. 17
figure 17

Images of lesion taken by the capsule robot

Full size image

6 Conclusions and future works

In terms of the motion mechanism of the magnetic capsule robots, a manipulation instrument based on EPM with the five degrees of freedom is designed. An integrated motion control approach, composed of primary motion subsystem and auxiliary motion subsystem, is proposed to decompose the control variables of system. Furthermore, a wolf parallel optimization strategy is adopted to improve the accuracy of the GWOA, and optimization ability of the improved algorithm is verified by test functions. The IGWOA is used to adjust the parameters of the integrated control system, that is, the parameters of the fuzzy controller in the primary motion subsystem and the PID controller in the auxiliary motion subsystem are optimized, respectively. Through the simulation test of unit step signal, the IGWOA is compared with other optimization algorithms, and the effectiveness of parameter adjustment is verified. Finally, some experiments of the designed capsule robot are carried out in different planes and slopes, as well as simulated stomach and real pig stomach, respectively. By comparing with two simple control systems (the comparative systems i and ii), the motion control error of the proposed integrated control system is 4.2 mm in the test platform, which meets the application control requirements.

In this work, the capsule robot has been tested in a variety of experimental environments, but there is still a certain gap with the real human stomach, such as acidic liquid, peristalsis, and irregular surface. Therefore, it is necessary to improve the safety and comfort of the capsule robot, and further carry out live animal experiments to evaluate the comprehensive performance of the capsule robot.