Explicit Model Predictive Control of a Magnetic Flexible Endoscope

Abstract

In this paper, explicit model predictive control is applied in conjunction with nonlinear optimisation to a magnetically actuated flexible endoscope for the first time. The approach is aimed at computing the motion of the external permanent magnet, given the desired forces and torques. The strategy described here takes advantage of the nonlinear nature of the magnetic actuation and explicitly considers the workspace boundaries, as well as the actuation constraints. Initially, a simplified dynamic model of the tethered capsule, based on the Euler-Lagrange equations is developed. Subsequently, the explicit model predictive control is described and a novel approach for the external magnet positioning, based on a single step, nonlinear optimisation routine, is proposed. Finally, the strategy is implemented on the experimental platform, where bench-top trials are performed on a realistic colon phantom, showing the effectiveness of the technique. The work presented here constitutes an initial exploration for model-based control techniques applied to magnetically manipulated payloads, the techniques described here may be applied to a wide range of devices, including flexible endoscopes and wireless capsules. To our knowledge, this is the first example of advanced closed loop control of magnetic capsules.

I. INTRODUCTION

MAGNETIC Flexible Endoscopes (MFE) and Magnetic Tethered Capsules (MTC) have gained great relevance in the field of robotic exploration of the lower Gastro Intestinal (GI) tract [1].One of the primary motivations for using magnetic actuation is the safe application of clinically relevant forces from an external permanent magnet (EPM) to an internal device, without the need for complex locomotion mechanisms. Previous results [2] have shown the feasibility of this approach in clinically relevant scenarios, where the capsule is indirectly controlled by the operator through the open-loop teleoperation of the magnet. However, the length of the procedure has been shown to increase by at least 100%. Given the highly nonlinear nature of the magnetic coupling, and the complexity of the biological environment, indirectly controlling the motion of the capsule by actuating the EPM is a difficult, unintuitive task. Moreover, a great advantage of MFEs and MTCs is the enhanced level of autonomy [3] achievable by means of the robotic actuation, which could, in a future scenario, replace the operator in the most repetitive parts of a procedure.

In both MFEs and MTCs, the adoption of a closed-loop control system capable of controlling the capsule (or tip) pose is a crucial step. Focusing on the case of magnetic capsules, several localisation techniques have been developed and experimented both on tethered and untethered capsules [4], [5], [6], [7], [8]. In the case of tethered capsules, previous papers [9], [10] have proposed a proprioceptive technique based on Hall effect sensors, that capitalises on the magnetic field of the EPM to localise the capsule. Although capsule localisation significantly improves the endoscope navigation, the open loop teleoperation of the EPM has demonstrated to be feasible, but extremely slow[2]. For this reason, closed-loop control schemes have been proposed on several platforms, including the control of a 2D system [11], the position control of a 3D untethered magnetic device in water [12] and the orientation control of a capsule actuated by a single rotating magnet [13]. In previous papers, control schemes for a MFE platform, based on Proportional Integral (PI) regulators have been proposed [14], [15], [16]. These schemes rely on a linearised relation between EPM movements and forces applied to the capsule. Extensive tests on bench-top platforms, as well as porcine live specimen [17], have demonstrated that the main limiting factor affecting the manoeuvrability of the capsule is the hostile environment that characterises the colon. Here, haustral folds and tight bends obstruct the capsule, thus minimising the relevance of the capsule’s dynamics in the computation of the control action. The anatomical obstructions are significant sources of disturbances and cannot be neglected as this can lead to unexpected and suboptimal application of torques and forces to the capsule. This is particularly true with PI controllers, as the workspace limits and nonlinear nature of the actuation are not taken into account.

In order to overcome this issue, a control approach based on explicit Model Predictive Control (eMPC) is discussed in this paper. Conventional MPC is a well known and widely adopted technique, originally developed for chemical processes [18]. MPC has the unique characteristic of solving an optimal control problem explicitly considering constraints on actuation and state, thus being particularly beneficial with respect to the issues discussed above [19]. On the other hand, it requires the on-line solution of an optimisation problem, which can significantly increase the computational burden. For this reason, we use the eMPC approach [20], which shifts most of the computational burden off-line, in return for a limited flexibility in the definition of constraints. This freed computational power is then used to propose a new approach to the solution of the magnetic problem, i.e. the pose of the EPM with respect to the capsule, required to obtain a desired force/torque. The proposed technique is based on the solution of a single-step optimisation procedure, with the aim of controlling the absolute position of the EPM rather than the incremental displacement, as done in [14], [15]. To the knowledge of the authors, this is the first example of application of MPC to the control of a magnetic endoscope. It must be pointed out that the proposed approach could easily be applied to untethered capsules, provided that a localisation feature is available. This is an initial benchtop feasibility study that aims to explore whether MPC is an appropriate control strategy.

II. METHODS

The system comprises two main components, shown in Fig.1. A robotic manipulator is used to move the EPM in six degrees of freedom. The capsule on the tip of the MFE is equipped with an Internal Permanent Magnet (IPM) and a flexible circuit, carrying six Hall effect sensors and an IMU, to enable the localisation of the capsule position and orientation with respect to the EPM (details are discussed in [14]). In addition, an endoscopic camera and an LED light source are embedded in the capsule. These features allow the flexible endoscope tip to be used in teleoperation as well as to be driven along pre-planned trajectories. For proof-of-concept, we focus on following a pre-defined trajectory in a clinically relevant colon phantom. The adoption of ill-planned trajectories as well as user inputs that force the capsule in a constrained direction would result in an ineffective control action. This issue cannot be tackled at a motion control level, but rather requires an additional layer of intelligence with knowledge of the environment. This interesting topic will be discussed in future works.

Fig. 1.

The Magnetic tethered capsule developed at the StormLab UK.

A. Capsule Model

The design of the capsule has been described previously in [1], [17], [2]. However, the dynamic model, used for control design purposes, is described here for the first time. The derivation of the motion equations is carried out in the Lagrangian framework and the Euler-Lagrange equation has been adopted. The dynamic model of the capsule can be represented as:

$$\frac{d}{dt}\frac{\partial \mathcal{L}}{\partial \dot{q}}-\frac{\partial \mathcal{L}}{\partial q}=\mathit{\xi }$$ (1)

with $\mathcal{L}=\mathcal{T}-\mathcal{U}$, where $\mathcal{T}$ and $\mathcal{U}$ are the kinetic and potential energy terms, respectively. Adopting the usual meaning of the terms in the Lagrangian equations, the generalised coordinates q are defined in the global reference frame as q(t) = [x(t), y(t), ψ(t), θ(t), ϕ(t)] where the ZYX Euler parametrisation has been chosen. Finally, the vector of external forces ξ consists of the forces applied to the capsule and the friction due to sliding:

$$\mathit{u}=\left[f_x,f_y,{\tau }_{\psi },{\tau }_{\theta },{\tau }_{\varphi }\right]$$ (2)

$${\mathit{f}}_{\mathit{h}}=\mathit{\mu }\frac{d\mathit{q}}{dt}$$ (3)

$$\mathit{\xi }=\mathit{u}-{\mathit{f}}_{\mathit{h}}$$ (4)

where μ is the vector of friction coefficients, u is the vector of external forces applied by the magnetic coupling and f_h is the vector of forces describing friction. It must be pointed out that the friction in the real environment is unknown and may widely vary depending on the lubrication and status of the tissue. Much work on friction in lower GI tract is available in literature [21], [22], but the identification of the parameters requires dedicated experimental work. In this paper, reasonable friction coefficients have been adopted and subsequently tuned during the experimental work. Although generous lubrication has been adopted in the experimental setup (i.e. low friction), variations during the experiments can be considered as unmodelled disturbances.

As represented in Fig.2, the capsule is modelled as a rigid body, virtually hinged in the point of contact with the tissue. This approach has been chosen as a consequence of the navigation strategy. In order to promote the traversing of obstacles, the capsule is constantly heading in the opposite direction, with respect to the contact surface (positive φ angle, in Fig. 2). It must be recalled that the contact surface is always the portion of the colon’s wall between the EPM and the IPM as the magnet attracting the capsule is operating on top of the patient. This approach can be compared to “surfing”, where the tip of a sliding object angles away from the surface in order to facilitate the motion. Considering the capsule in contact with the upper wall of the colon, the dynamic on z(t) is neglected. This approach avoids the computation of the potential energy ($\mathcal{U}=0$), constituted only by gravity. It is worth mentioning that this approach is robust with respect to variations in z(t) of the colon’s wall, as the magnetic coupling with the EPM is constantly attracting the capsule toward the top surface. The kinetic energy of the capsule is described as:

$$\mathcal{T}=\frac{1}{2}m{\mathit{v}}_b^{\prime }{\mathit{v}}_b+\frac{1}{2}\frac{d{\mathit{\gamma }}^{\prime }}{dt}{\mathit{J}}_{\overline{\omega }}^{\prime }\overline{\mathit{I}}{\mathit{J}}_{\overline{\omega }}\frac{d\gamma }{dt}$$ (5)

where m is the mass of the capsule (supplemented to include the weight of the initial tract of the tether), γ = [ψ(t), θ(t), ϕ(t)] is the subset of the generalised coordinates related to the rotational degrees of freedom, Ī is the constant inertia matrix of the capsule expressed in the local reference frame and the term ${\mathit{J}}_{\overline{\omega }}$ is the matrix relating the derivatives of the Euler angles $d\mathit{\omega }/dt$ to the angular velocities expressed in the local frame, not reported here for the sake of brevity. The term v_b is computed as:

$${\mathit{v}}_b={\mathit{R}}_{lg}*{[x(t),y(t),0]}^{\prime }+\overline{\mathit{\omega }}\times {[0,-c,-d]}^{\prime }$$ (6)

where R_lg is the transformation matrix from global to local frame, the local angular velocities $\overline{\mathit{\omega }}$ can be computed as $\overline{\mathit{\omega }}=J_{\overline{\mathit{\omega }}}\mathit{\gamma }$ and the terms c, d are the coordinates of the center of mass of the capsule with respect to the virtual universal joint, as shown by Fig.2.

Fig. 2.

Dynamic model of the capsule.

The kinetic energy has been computed in terms of local coordinates to simplify the computation of the inertia matrix (Ī) which is constant in this approach. Moreover, the computation of the rotational velocity is easier.

The equations of motion of the capsule can be written in the form:

$$\ddot{\mathit{q}}=\mathit{M}{(\mathit{q})}^{-1}(-\mathit{C}(\mathit{q},\dot{\mathit{q}})\dot{\mathit{q}}+\mathit{\xi }).$$ (7)

where the gravitational term (usually G(q) is null, as a result of $\mathcal{U}=0$. The matrix M(q), describing the generalized inertia of the system, is computed solving the appropriate term of eq.(1) with respect to $\dot{\mathit{q}}$, considering that:

$${\left[\frac{\partial \mathcal{L}}{\partial \dot{q}}\right]}^T=\mathit{M}(\mathit{q})\dot{\mathit{q}}$$ (8)

Conversely, the computation of the term C(q, $\dot{\mathit{q}}$) is not unique, thus the method based on the Christoffel symbols [23] has been adopted here. Finally, eq.(7) has been linearised around the equilibrium point $\overline{\mathit{q}}=\mathbf{0}$, i.e. in the condition of capsule horizontal and static, thus obtaining a linear system in state space form. The choice of linearising the system in this condition has been adopted as a consequence of the slow movements and changes in force experienced by the capsule.

The mathematical steps described above have been carried out using the MATLAB symbolic manipulation toolbox [24] thus obtaining an LTI system suitable for the implementation of MPC, as described in the next section.

B. Explicit MPC

As described in the beginning of the section, this work focuses on the tracking of a predefined trajectory. This problem has been tackled by means of the explicit MPC (eMPC) technique [20]. The control scheme has been initially described as a classical MPC over a linear, discrete time system, where the control action (i.e. the forces and torques applied to the capsule, here described with u) are computed by minimising the cost function:

$$\begin{gathered} \underset{\text{Δ}\mathit{u}}{\text{min}}\mathit{J}(\text{Δ}\mathit{u},\mathit{q}(t))= \\ \sum _{k=0}^{N-1}{\left({\mathit{q}}_k-\mathit{r}(t)\right)}^{\prime }{W}_q\left({\mathit{q}}_k-\mathit{r}(t)\right) \\ +\text{Δ}{\mathit{u}}_k^{\prime }{W}_u\text{Δ}{\mathit{u}}_k+{\mathit{q}}_N^{\prime }{W}_N{\mathit{q}}_N \end{gathered}$$ (9)

$$\text{subj. to}{\mathit{q}}_{k+1}=\mathit{A}{\mathit{q}}_k+\mathit{B}{\mathit{u}}_k,$$ (10)

$${\mathit{q}}_0=\mathit{q}(t),$$ (11)

$${\mathit{u}}_{k+1}={\mathit{u}}_k+\text{Δ}{\mathit{u}}_k,$$ (12)

$$\begin{gathered} \text{Δ}{\mathit{u}}_{\min }\le \text{Δ}{\mathit{u}}_k\le \text{Δ}{\mathit{u}}_{\max }, \\ k=i\dots N-1, \end{gathered}$$ (13)

$${\mathit{u}}_{\min }\le {\mathit{u}}_k\le {\mathit{u}}_{\max },k=i\dots N-1,$$ (14)

$${\mathit{q}}_{\min }\le {\mathit{q}}_k\le {\mathit{q}}_{\max },k=i\dots N-1$$ (15)

where N is the prediction horizon, r(t) is the set point of the state over the horizon, $\text{Δ}{\mathit{u}}_k^{\prime }$ is the increment of the control action at every step, and W_q, W_N, W_u are the weight matrices of the state, the terminal state and the input respectively.

Eqs.(10–11) represent the linearised dynamics of the capsule over the prediction horizon. Eq.(12) is an integrator aimed at computing the value of the control action, Δu is adopted as a minimisation variable (i.e. the control algorithm is in velocity form), thus allowing tracking of the reference signal r(t) by avoiding the static error (see [25] for details).

Eqs.(13)–(15) describe the actuator limits and the admissible portion of the state space. In the context of eMPC, the numerical values of u_min/u_max and q_min/q_max are fixed during the off-line computation of the optimal control action. Therefore, only constraints related to the actuation and the available workspace are considered here.

Eqs.(9–14) implicitly define u(t) as a function of q(t), and therefore, would normally require the solution of a quadratic program (QP) in real-time, as usually done in the implicit MPC technique. Conversely, the approach described in [20] tackles the problem by solving the optimisation off-line for all the feasible values of q(t). The result of this procedure is a continuous piecewise affine relation, i.e.:

$$\mathit{u}(\mathit{q})=\{\begin{array}{ll}{\mathit{F}}_1\mathit{q}+{\mathit{G}}_1\hfill & \text{ if }{\mathit{H}}_1\mathit{q}<{\mathit{K}}_1\hfill \\ ⋮\hfill & ⋮\hfill \\ {\mathit{F}}_M\mathit{q}+{\mathit{G}}_M\hfill & \text{ if }{\mathit{H}}_M\mathit{q}<{\mathit{K}}_M.\hfill \end{array}$$ (16)

where H_1...M, K_1...M define a finite number of convex regions of the state space and F_1...M, G_1...M are the terms of the affine optimal state feedback for every region. A complete description of the computation of the terms of eq.(16) is beyond the scope of the paper (details can be found in [20]). However, it must be pointed out that the number of regions depend on the number of constraints and increasing them does not affect the computational burden of the control scheme, as the terms of Eq.(16) are computed off-line.

C. The Magnetic Actuation

In Sec.II–B, the approach adopted for the computation of forces and torques applied on the capsule has been shown. Given the nonlinear nature of the magnetic coupling between EPM and IPM, the application of the desired forces/torques is not straightforward. The relation between the pose of the magnets and the forces(f_m) and torques (τ_m) exerted on the capsule can be computed by means of the dipole/dipole model ([15]):

$$\begin{gathered} {\mathit{f}}_{\mathit{m}}\left(\mathit{p},{\mathit{m}}_a,{\mathit{m}}_c\right)=\frac{3{\mu }_0\Vert {\mathit{m}}_a\Vert {\mathit{m}}_c\Vert }{4\pi \Vert \mathit{p}{\Vert }^4}. \\ ({\widehat{\mathit{m}}}_a{\widehat{\mathit{n}}}_c^{\top }+{\widehat{\mathit{m}}}_c{\widehat{\mathit{m}}}_a^{\top }+ \\ \left({\widehat{\mathit{m}}}_c^{\top }Z{\widehat{\mathit{m}}}_a\right)\mathit{I})\widehat{\mathit{p}} \end{gathered}$$ (17)

$$\begin{gathered} {\mathit{\tau }}_{\mathit{m}}\left(\mathit{p},{\mathit{m}}_a,{\mathit{m}}_c\right)=\frac{{\mu }_0\Vert {\mathit{m}}_a\Vert \Vert {\mathit{m}}_c\Vert }{4\pi \Vert \mathit{p}{\Vert }^3}{\widehat{\mathit{m}}}_c\times \\ D(\widehat{\mathit{p}}){\widehat{\mathit{m}}}_a \end{gathered}$$ (18)

where p_c and p_a are the capsule and EPM positions in the global frame, p is the relative position of the EPM with respect to the capsule (p = p_c – p_a), ${\widehat{\mathit{m}}}_c$ and ${\widehat{\mathit{m}}}_a$ are the magnetic moment of the IPM and EPM in the world frame, respectively. $D=3\widehat{\mathit{p}}{\widehat{\mathit{p}}}^{\top }-\mathit{I}$, being I the identitiy matrix. In previous works on a similar platform ([14], [15]) the control action has been computed in terms of variation of forces and torques as the output of a PI controller. Subsequently, a linearized variational relationship between the joint angles of the manipulator and the control action has been adopted, namely:

$$\dot{\mathit{\xi }}={\mathit{J}}_{\mathcal{A}\mathcal{F}}{\dot{\mathit{q}}}_m$$ (19)

where ${\mathit{J}}_{\mathcal{A}\mathcal{F}}$ is the actuating-force-torque Jacobian matrix as described in [15] and ${\stackrel{\mathbf{.}}{\mathit{q}}}_m$ are the velocities of the manipulator’s joints. This approach greatly simplifies the computation of the joint values but neglects the manipulator’s limits in terms of absolute position of the EPM and joint velocities. Moreover, the feasibility of the desired $\dot{\mathit{\xi }}$ is not explicitly considered during the computation of the manipulator’s motion. Given the desired forces and torques computed as described in the previous section (${\mathit{\xi }}_{\mathrm{des}}=\left[{\mathit{f}}_{\mathrm{des}},{\mathit{\tau }}_{\mathrm{des}}\right]$ in the following), the position and orientation of the EPM at each time step is computed by solving an optimisation problem as follows:

$$\begin{gathered} \underset{\left[{\mathit{p}}_a,{\widehat{\mathit{m}}}_a\right]}{\text{min}}{\mathit{J}}_{\mathit{\xi }}\left({\mathit{p}}_a,{\widehat{\mathit{m}}}_a,{\mathit{m}}_c\right)= \\ {\mathit{W}}_{\mathit{f}}{\Vert {\mathit{f}}_{\mathrm{des}}-{\mathit{f}}_m\left(\mathit{p},{\mathit{m}}_a,{\mathit{m}}_c\right)\Vert }^2+ \\ {\mathit{W}}_{\mathit{\tau }}{\Vert {\mathit{\tau }}_{\mathrm{des}}-{\mathit{\tau }}_m\left(\mathit{p},{\mathit{m}}_a,{\mathit{m}}_c\right)\Vert }^2 \end{gathered}$$ (20)

$$\text{subj}.\text{ to }\Vert {\mathit{m}}_a\Vert =\Vert {\mathit{m}}_{a,init}\Vert ,$$ (21)

$$p_{a,z}-p_{c,z}\ge z_{lim},$$ (22)

$$\begin{gathered} {\mathit{l}}_{q_m,safe}<dt{\dot{\widehat{\mathit{q}}}}_m+{\mathit{q}}_{m,0} \\ <{\mathit{u}}_{q_m,safe}, \end{gathered}$$ (23)

$${\stackrel{\mathbf{.}}{\widehat{\mathit{q}}}}_m\le {\stackrel{\mathbf{.}}{\mathit{q}}}_{m,safe}$$ (24)

where

$${\stackrel{\mathbf{.}}{\widehat{\mathit{q}}}}_m={\mathit{J}}_{\mathcal{R}}^{†}\left({\mathit{q}}_{m,0}\right)\left[\begin{array}{c}{\mathit{p}}_a-{\mathit{p}}_{a,0}\\ {\widehat{\mathit{m}}}_a-{\widehat{\mathit{m}}}_{a,0}\end{array}\right]$$ (25)

Eq.(20) describes the cost function of the minimisation problem, where W_f and W_τ are weights used to prioritise the translational/rotational movements. Eq.(21) accounts for the constant module of the magnetic field of the EPM, Eq.(22) implements a safety limit which aims to avoid the contact with the patient, z_lim can be fixed or vary during the procedure, de pending on the patient’s pose. Eq.(23) describes the available workspace in terms of the joint limits of the manipulator. In order to express the constraint in linear form with respect to the optimisation variables, the relation between $\left[{\mathit{p}}_a,{\widehat{\mathit{m}}}_a\right]$ and q_m is approximated by multiplying the length of the time step (i.e. dt) and the approximated joint velocities ${\stackrel{\mathbf{.}}{\widehat{\mathit{q}}}}_m$. The latter are computed by means of Eq.(25) where ${\mathit{J}}_{\mathcal{R}}^{†}\left({\mathit{q}}_{m,0}\right)$ is the pseudoinverse of the manipulator’s Jacobian, computed at the beginning of the optimization step. It must be mentioned that q_m,0 is the value of the joint angles at the beginning of the optimization. Similarly, p_a,0 and ${\widehat{\mathit{m}}}_{a,0}$ are the position and orientation of the EPM at the beginning of the step.

Finally, Eq.(24) expresses a boundary on the commanded joint velocities in order not to exceed the maximum values allowed by the robot’s control system. Eqs.(20–24) describe a non-convex optimisation problem with linear inequality constraints, which represents a complex problem from the computational point-of-view. Conversely, the number of optimisation variables is limited $\left(\left[{\mathit{p}}_a,{\widehat{\mathit{m}}}_a\right]\in {ℝ}^6\right)$. In the next section, an efficient implementation will be discussed.

III. EXPERIMENTAL VALIDATION

The endoscopic probe is constituted by a tethered rigid capsule (length 20.6mm, diameter 18.1mm), described in Sec II and in greater details in [10]. The robotic manipulator is a KUKA LBR Med R820 manipulator (KUKA GMBH, Germany) has the EPM (axially magnetized, N52, 4” diameter and length, 1.48T, Magnet Experts Ltd, UK) as an end-effector. The colon phantom (Kyoto Kagaku M40) is widely used as a clinical training platform for colonoscopy. It is soft, deformable and comprises haustral fold features (obstacles).

The software implementation of the current project is the result of an incremental work, thoroughly described in several papers [15], [10], [14], [17]. The code takes advantage of the Robotic Operating System (ROS) middleware [26] and is written in Python, for which several optimisation tools are available [27], [28]. Although theoretically possible, the computation of eq.(16) in this framework would require an implementation of the eMPC from scratch. In order to simplify the process, the Multi Parametric Toolbox for Matlab [29] has been used to automatically generate a set of piecewise linear functions, subsequently imported in Python as described in [30]. The length of the optimisation horizon has been chosen equal to 20 steps.

The implementation of the on-line optimisation procedure described in Sec.II–C is not straightforward. Given the iterative and computational-intensive nature of the approach adopted, the interpreted structure of Python may negatively affect performances if a blind implementation of eqs.[20–24] is carried out. We choose to use the optimisation functions available in the Scipy package [27], thus relying on the Sequential Least SQuares Programming (SLSQP) algorithm. The bottleneck of this approach is the numerical computation of the Jacobians of J_ξ and constraint equations. For this reason, we chose to symbolically compute the Jacobians by means of the MATLAB Symbolic Manipulation Toolbox [24], generate a set of C functions with the Code Generation Toolbox and call the external functions from the main code in Python, thus explicitly feeding the numerical Jacobians to the optimisation routine. A cycle-time of 0.05 sec, has been chosen, given that the motion of the capsule is slow and no phenomenon concerning high-bandwidth dynamics has to be controlled. As will be shown later, the choice of this parameter is related to the computational burden of the optimisation.

A. PI control limitations

In order to demonstrate the motivation behind the adoption of an eMPC controller, a simple experiment with the PI control discussed in [14], [15] is reported in this section. These previous works focused on trajectory following in environments that lacked significant obstacles. In this experiment, the capsule is commanded to follow a straight trajectory in the initial segment of a colon phantom (laid out similar to that shown in Fig. 5), placed between two plastic sheets. The gains of the PI have been set to 10.13 N/m, 2.2 Nm/rad and 0.33 N/ms, 0.09 Nm/rads respectively. The experiment has been repeated three times with similar results.

Fig. 5.

Experimental setup for the validation of the proposed approach - A: The colon phantom is shaped with two straight segments divided by a 90° turn - B: In the first tract, an obstacle, represented by the green cylinder, is present.

Fig. 4 shows some relevant quantities recorded during the experiment. In Fig. 4–A the position of the capsule is shown. After travelling a distance of 3 cm, the capsule is not able to move forward as it is obstructed by a fold. Subsequently, the PI control applies more force as the error increases, as shown by Fig. 4–C. Due to the nonlinear nature of the magnetic coupling, the increase in force implies a tilting of the EPM, which entails a tilt in the capsule, shown in Fig. 4–B. This behaviour, mentioned previously, can be explained by the blindness of the control with respect to the nonlinear nature of the actuation, as well as the lack of environmental awareness. The experiment described in the next section will demonstrate how the approach based on eMPC can overcome these limitations.

Fig. 4.

Experiment with PI controller - A: Position of the capsule over time. B: Orientation of the capsule over time. C: Pulling force applied to the capsule.

B. Experimental validation

Figs.5–A and 5–B show the experimental setup chosen for the validation of the control approach. The silicone phantom representing a human colon has been shaped with an initial straight tract of length 20 cm and an obstacle, represented by a green solid cylinder of diameter 20 mm, length 45 mm and weight 18 g (shown in detail in Fig. 5–A) in the approximate middle. After the initial tract, a 90° turn is present, followed by another straight segment. A tight turn has been chosen as, during endoscopic procedures, it is more common to alternate forward movements and orientation changes, rather than performing curved trajectories.

The phantom has been placed between two flat surfaces and the capsule was commanded to proceed through the initial tract (X direction) with a constant speed of 0.01 m/s while simultaneously maintaining a negative tilting angle of 0.2 Rad (11.5°). When the capsule has reached the end of the initial tract, a user input commanded the capsule to perform a 90° rotation on the XY plane and subsequently proceed in the Y direction. The same experiment has been repeated three times with analogous results. A video recording of the experiment is available in the enclosed media. Figs. 6–9 refer to one of the repetitions, the ranges of variation within the experiments are provided as numerical results. To enhance the clarity of presentation, the two phases of the experiment are shown in separate plots. Figs 6–7 refer to the initial part of the experiment, where the capsule travels the first straight tract of the phantom and overcomes the obstacle. Figs. 8–9 refer to the second part of the experiment where the capsule is commanded to turn and then advance through the last part of the phantom. For this reason, Figs. 8–9 start from t = 25s.

Fig. 6.

First part of the experiment - A: Path travelled by the capsule in the colon phantom. B: Force applied to the capsule in the direction of motion. Measurements in solid blue line, setpoints in dashed red line. The area of the plot highlighted in blue represents the part of the experiment where the capsule is trying to overcome the obstacle.

Fig. 9.

Second part of the experiment - A: Heading of the capsule during the experiment. B: Torque applied to the capsule along the Y axis. Measurements in solid blue line, setpoints in dashed red line.

Fig. 7.

First part of the experiment - A: Heading of the capsule during the experiment. B: Torque applied to the capsule along the Y axis. Measurements in solid blue line, setpoints in dashed red line. The area of the plot highlighted in blue represents the part of the experiment where the capsule is trying to overcome the obstacle.

Fig. 8.

Second part of the experiment - A: Path travelled by the capsule in the colon phantom. B: Force applied to the capsule in the direction of motion. Measurements in solid blue line, setpoints in dashed red line. The area highlighted in green is where the capsule is commanded to turn.

Fig.6 shows the path travelled by the capsule and the force applied to it in the direction of motion (aligned with the global x shown in Fig. 2 - the most influential direction for locomotion).). The forces in the vertical and lateral direction are omitted here for the sake of brevity. As shown by Fig.6–A, the positional set-point is followed (Maximum error ±47 mm, RMSE 35 ± 4.8 mm), although several steps can be observed. This can be explained by a stick-slip motion of the capsule, caused by folds of the upper wall of the phantom and by the uneven lubrication of the surface. The phantom is mildly hydrophobic and hence the water-based lubricant is readily wiped off to leave a high friction contact. Moreover, a clear step can be observed around t ≈ 12.5s where the capsule overcomes the obstacle. The maximum error is approximately twice the length of the capsule. While this is significant, it must be pointed out that the difficult environment dominates the task. Moreover, in this complex and highly variable environment, gross locomotion has a much higher priority than precise trajectory following and precise capsule pose adjustments can be made while stationary, e.g. for performing diagnostic or therapeutic tasks.

Fig.6–B shows the longitudinal force applied to the capsule. As displayed by the red-dashed lines, the desired force generated by the eMPC is almost constant for the majority of the experiment, with exception of the initial tract and the proximity of the obstacle. An undesired peak of force must be highlighted at t ≈ 15s (maximum error of ±0.48N), this is due to the increase in required torque. The evolution of the measured longitudinal force with respect to the set-point shows the effectiveness of the optimisation procedure. Nevertheless, particularly in the initial tract and when in contact with the obstacle, a significant error can be highlighted (RMSE 0.11 ± 0.042 N). The range of forces applied to the capsule, including the peaks caused by the stick-slip motion, are well below those that would cause tissue damage. In [31], up to 3 Bar of water pressure was tolerated before tissue rupture. Assuming a force of 1 N seen during these experiments, the required contact area would need to be in the order of 3.3 mm² - an extremely unlikely scenario given the rounded shape of the capsule and the highly deformable tissue.

The evolution of the capsule heading during the initial part of the experiment is described by Fig.7. The desired heading (i.e. ψ) is constant along the path. In this case, the measured heading (Fig.7–A) is subject to significant but acceptable, variations (Maximum error ±0.2 Rad, RMSE 0.083 0.011 Rad). This can be explained by the capsule overcoming the haustral folds and thus being subject to substantial discontinuities in the sliding surface. Moreover, the a peak in torque and angle is highlighted around t = 13s as a consequence of transversing the obstacle, which involves a sudden change in capsule height as well as forward position. Fig.7–B shows the desired and measured torque acting on the capsule. The torque applied on the Y axis is subject to fluctuations (Maximum error ±0.041 Nm, RMSE 0.014±0.0021 Nm) in a similar way to Fig.7–A, as a consequence of the disturbances applied by the obstacle and the uneven surface. The torques do not have a significant influence on the stress applied to tissues in clinical scenarios, as the capsule is free to move. For this reason, the values described by Fig.7–B are clinically acceptable.

Similarly to Figs. 6–7, Figs. 8–9 show the behaviour of the capsule in the second part of the experiment. In particular, Fig. 8–A shows the path travelled by the capsule in the negative Y direction. From t = 25s to t = 40s the capsule is not advancing as steering is occurring. At t = 40s the capsule is commanded to advance in the Y direction for 20cm. The path is followed with an acceptable error (Maximum error ±41 mm, RMSE 11±5.8 mm). The force applied to the capsule is shown in Fig. 8–B, where a similar behaviour with respect to the first straight tract is exhibited. Moreover, Fig. 9 shows the horizontal orientation of the capsule and the steering torque applied to overcome the turn. In particular, Fig. 9–A shows the Y component of the capsule’s angular position. The orientation of the capsule reaches π/2 rad around t = 40s, while the maximum error is ±0.18 rad with and RMSE of 0.09 ± 0.012 rad over the repetitions. The torque applied to the capsule is shown in Fig. 9–B (Maximum error ±0.02 Nm, RMSE 0.009 ± 0.0011 Nm).

The computational burden of the eMPC is negligible with respect to the online optimisation, showing an average time of 3.7 × 10⁻³ s and a maximum of 7 × 10⁻³ s. Conversely, the burden of the nonlinear optimization stage is not completely satisfactory, as the average computational time is 0.041 s, but peaks of 0.075 s were experienced, which can negatively affect the performance of the control system. This can be explained by the choice of Python as framework and SciPy as optimisation tool - known to be computationally inefficient. Having set the cycle-time of the algorithm to 0.05 s, the average time required by the optimisation is compatible with the requirements. Nevertheless, the peaks show that the implementation is not adequately robust in the worst-case scenarios. In light of this, further work will be carried out in future developments.

IV. CONCLUSIONS

In this study, an advanced scheme aimed at controlling the tip of an MFE, by means of an external permanent magnet, is proposed. The conjunction of eMPC for the computation of desired forces/torques and nonlinear optimisation for the motion of the EPM was shown to be a feasible control strategy. Nevertheless, further improvements in terms of implementation will be required in order to shorten the computational time required to carry out the optimisation stage. The adopted approach was shown to produce satisfactory results in terms of capsule manoeuvrability in a realistic colon phantom, although the current implementation considered only pre-planned trajectories and environmental features such as visible obstacles are not explicitly taken into account. In light of this, the adoption of an implicit MPC approach, where evolving constraints can be considered, will constitute an interesting extension of this work. In particular, a significant advantage of the MPC approach will be the ability to consider visible folds as obstacles and adapt the planned trajectory over the prediction horizon accordingly. The techniques adopted in this work demonstrate the feasibility and highlight the critical points of the approach. In particular, the computational burden of the nonlinear optimisation stage is close to the tolerable upper bound. For this reason, a different implementation will be required in order to solve additional computationally intensive steps. Future work in this direction will involve the computation of the optimisation in a compiled C routine, taking advantage of the CPLEX libraries [32].

Regarding the dynamic modelling, future work might be dedicated to the modelling of friction. The common clinical practice adopts continuous lubrication of the tether as well as the tip of the endoscopes in order to maintain a constant and generous level of lubrication. In this paper, the same approach has been adopted. The capsule, the tether and the colon phantom have been generously lubricated. The friction has been considered in the capsule modelling, although the coefficients have been manually tuned, as a detailed modelling of the friction in an unknown and unconstrained environment is extremely challenging. The variations of friction can be considered as a disturbance and the experimental results show the robustness of the approach with respect to these variations. Future developments will involve the adoption of estimation techniques, as well as improved control approaches to achieve greater robustness with respect to variations in friction.

Finally, it is worth mentioning that the experimental setup described here constitutes an initial result, further validation of the platform will require tests on more complex shapes, where the anatomy and the physical properties of the human colon are reproduced.

Fig. 3.

Overall control scheme.