Introduction

In the last decades, open surgery was the standard approach to perform surgeries. Nowadays, minimally invasive surgery (MIS) has been introduced to overcome some of the main drawbacks related to open surgery [1,2,3,4,5]. Indeed, with MIS, the size of the surgical wounds is small; therefore, intraoperative and postoperative conditions are preferable: the bleeding is significantly limited and thus, the need for blood transfusions is reduced as well. Moreover, postoperative pain and complications are reduced, and the recovery from the surgery is faster than in the case of open surgery [6, 7]. In addition to hemorrhage occasions where open surgery must be implemented, the application of MIS has aroused more attention [8]. Among them, one of the main minimally invasive techniques is laparoscopy, performed only inside the abdominal wall and the pelvic cavities for both diagnostic and surgical purposes [9, 10]. It can be seen that laparoscopy has gradually become a frequently applied technique for several medical interventions, such as gynecological operations and digestive system surgeries [8, 11].

However, performing a minimally invasive operation is more difficult than traditional open surgery because the surgeon has only a bidimensional vision of the operative field, which is projected on 2D images. Also, the incisions are very small, and the surgeon must be cautious in moving the instrumentation [12]. To conclude, disadvantages of MIS compared to open surgery include a long learning curve, poor vision and depth perception, limited range of movement and dexterity, and the lack of haptic feedback, which leads to poor outcomes if the surgeon is not well trained in the intervention [7, 12].

With the development of robotics and strict operational requirements in MIS, using the robot to replace the traditional open surgery has become a popular trend and manifests chances that can partly debate shortcomings of MIS, and related works have boomed the research field. First, many robots used in the medical field assure a three-dimensional view of the operative workspace with adjustable magnification. Besides, they provide better dexterity correcting the tremor of the surgeon and improving their movements through motion scaling. Finally, robots that contain high numbers of degrees of freedom (DOFs) and wristed instruments resemble human motion well [12].

In robot-assisted MIS, the robot must implant both an endoscope and the surgical tools needed for the operation into the patient’s body so that the surgeon can observe the internal environment and perform the surgery. The robot’s end-effector must enter the patient’s body through narrow apertures, and the whole operation must be performed without applying excessive forces on the edges of these incisions. In order not to provoke damage to the patient, the end-effector must only translate along the tool axis and the entering point. Thus, the incision point represents a kinematic constraint that the robot cannot violate and that is called Remote Center of Motion (RCM) [13].

RCM constraint is extremely important for a robot to successfully implement the MIS, but this rigorous control requirement is extremely challenging to implement. Generally, the RCM constraint can be accomplished either mechanically or by software [14, 15]. In the former case, also known as passive RCM constraint, the definition of the pivot is obtained by mechanically constraining the pivot itself to the mechanism kinematics employing circular tracking arcs, dual parallelograms, or synchronous spherical linkages [14, 16,17,18]. However, these methods are expensive and wear out with time. With the development of control methods [19,20,21,22,23], in the latter case, that is the programmable or active RCM, the constraint is accomplished by multi-joints’ coordinated control, usually exploiting industrial redundant robot [24]. Implementation of the active method is less expensive than the passive one and allows greater flexibility. Also, this method is quite robust, but it brings problems related to space-occupation and lesser maneuverability, thus different software methods and their RCM tackling effects are becoming a prominent research topic. Different methods and controllers have been proposed and studied in the literature, including the so-called RCM-constrained Jacobian [25], a dual quaternion-based kinematic controller [26], a task-space augmentation method [27, 28], fuzzy approximation [29, 30], decoupled controllers [15, 31, 32], etc. Furthermore, optimization approaches have been proposed, and these techniques treat RCM constraint as an equality constraint [33].

As for the RCM-constrained Jacobian, the Jacobian matrices of the robot and the tool’s endpoint are used to directly compute the endpoint Jacobian; then the obtained Jacobian matrix is inserted in a feedback kinematic controller so that the robot can follow the desired trajectory [25]. Regarding the dual quaternion-based controller, the RCM is programmable, and the software maintains it through a kinematic controller that operates in the unit dual quaternion space [26]. The task-space augmentation method couples the coordinates of the RCM constraint and the task space to achieve two main tasks: the respect of the RCM constraint and the correct performance of the tool-tip trajectory [27]. Finally, it has been demonstrated that one of the best performing methods is the null-space and task-space decoupled control [15, 27]. In this method, the null space and the task space are hierarchically considered, then they can be combined together by utilizing specific weights. Moreover, to overcome the impossibility of using the Jacobian matrix with redundant robots, it has been introduced a pseudo-inverse Jacobian that allows solving the inverse kinematics of the robot [15].

Model Predictive Control (MPC) has been vastly applied in various industrial applications. However, this control method requires accurate modeling of the controlled object, and it is also very challenging to control the surgical robot. Therefore, in laparoscopic applications, there are only very few and old studies at present [34,35,36,37]. Nevertheless, in the case that the motion equation of the robot is clear, MPC has the following significant advantages. MPC considers only the task space, so it is easier to be implemented than the decoupled one. Furthermore, it is a model-based and an optimization-based control method; in particular, this control method can predict the future trajectory that the robot should follow, and thus, starting from the system model, the control input is set to the optimal value [38]. Lastly, with the MPC strategy, it is possible to introduce constraints on the inputs and the state of the system, for example, it can be imposed a velocity constraint such that the joint velocity does not reach high value [39,40,41]. For the reasons listed above, we decided to apply the MPC method to realize the trajectory following and RCM constraint in MIS. Furthermore, direct fuzzy adaptive controllers are known to work in the presence of a large uncertainty or unknown variation in plant parameters and disturbances [42]. In [43], unknown time-varying periodic disturbances from human–robot interaction are compensated by an adaptive fuzzy approximation.

In this work, we propose and simulate the application of MPC for active RCM constraint on an industrial serial 7-DOF robot, the KUKA LWR4+. The control objectives of the minimally invasive surgery in this paper are to control the robot following a desired trajectory and in the meantime, maintaining the surgical tool passing through the RCM point, that is, the end position and orientation of the surgical tool should be controlled simultaneously. Since the surgical tool is attached to the end of the robot, namely, the position and orientation of the last joint (i.e., the wrist joint) should be controlled. First, a two-dimensional (2D) trajectory tracking and RCM constraint for the surgical tool are considered. The surgical tool is modeled as a virtual dynamic system with virtual velocity along the two axes and virtual angular velocity around the end of the surgical tool. Thereby, the trajectory tracking and RCM constraint problem are transferred into the control problem of the virtual dynamic system. Then, MPC can be applied to design the controller of the virtual dynamic system. Second, since the workspace of the robot is 3D, thus, to simplify the 3D problem, it is projected into xy-plane and yz-plane, respectively. Then, the above-mentioned 2D MPC solution can be used to solve these two 2D problems. The actual position of the end of the surgical tool and its orientation can be obtained through the mapping from 2D to 3D, and the actual position and orientation of the last joint of the robot can also be determined. By utilizing the inverse kinematics of the robot, we can control the robot to get to the actual pose to realize the trajectory tracking and RCM constraint. By simplifying the trajectory tracking and RCM constraint problem into controlling a virtual surgical tool, it is easy to replicate the solutions on other manipulators. Furthermore, the fuzzy approximation is introduced to manage the kinematic uncertainties and external disturbance existing in the MPC control. Finally, the efficiency and accuracy of the proposed approach are validated with the KUKA LWR4+ robot on simulation environment, with comparing to the decoupled method and the MPC method without fuzzy approximation.

The paper is organized as follows: the model of the robot’s kinematics and the mathematical solution of the RCM constraint are explained in the second section. In the third section, the control design method of the MPC and the fuzzy approximation are taken into account. The fourth section illustrates the results of the paper and compares them to the decoupled method and MPC method without fuzzy approximation. Finally, the conclusions are reported in the last section.

Problem statement

RCM constraint

In minimally invasive surgery, the surgical instrument enters through an incision point in the abdominal wall and must not hurt the edges of the wound during the surgical procedure. Therefore, it can only translate along the tool axis and the incision point. This type of constraint is called remote center of motion (RCM) constraint. Generally, the surgical tools used in MIS consist of a slender shaft, a cutting or grasping end-effector. Therefore, the surgical tool is often treated as a probe, with a specific length and moves under RCM constraints, to simulate the behavior of the surgical tool during surgery. Similarly, in this paper, we approximate the surgical tool as a line segment with a specific length l to describe the RCM constraint model in a two-dimensional space. The simulated model of the surgical tool is shown in Fig. 1.

Fig. 1
figure 1

Virtual model of the surgical tool in 2D. The surgical tool is connected to the robot end-effector, that is, the last joint of the robot. The end of the surgical tool should follow the desired trajectory \({\varvec{T}}_\mathrm{des}\), and the tool should pass through the RCM point \({\varvec{P}}_\mathrm{rcm}\) at the same time

Full size image

In this constraint model, \({\varvec{T}}_\mathrm{des}\) describes the desired trajectory, and \({\varvec{P}}_\mathrm{rcm}\) indicates the RCM constraint point. The surgical tool tip needs to track the desired trajectory \({\varvec{T}}_\mathrm{des}\) while also satisfying the RCM constraint, i.e., crossing the RCM point. In Cartesian space, the pose of the surgical tool that can satisfy these two conditions at the same time is unique, that is, the position and orientation of the surgical tip are determined uniquely by \({\varvec{T}}_\mathrm{des}\) and \({\varvec{P}}_\mathrm{rcm}\). Here, we define a tool vector that points from the tip of the surgical instrument center to the center of the RCM constraint to represent the surgical instrument’s desired location under the RCM constraint.

$$\begin{aligned} \mathrm{tool} = {\varvec{P}}_\mathrm{rcm} - {\varvec{T}}_\mathrm{des}. \end{aligned}$$
(1)

Then, the angle of the vector can be calculated by the following equation:

$$\begin{aligned} \varvec{{\bar{\theta }}} = \mathrm{acos}\left( {\frac{{{{\varvec{x}}_\mathrm{rcm}} - {{\varvec{x}}_\mathrm{actual}}}}{{\left| \mathrm{tool} \right| }}} \right) , \end{aligned}$$
(2)

where \(|\mathrm{tool}| = \sqrt{{{\left( {{{\varvec{x}}_\mathrm{rcm}} - {{\varvec{x}}_\mathrm{actual}}} \right) }^2} + {{\left( {{{\varvec{y}}_\mathrm{rcm}} - {{\varvec{y}}_\mathrm{actual}}} \right) }^2}} \). It should be noted here that the angle is chosen to correspond to the x-axis.

In geometric space, utilizing the tool vector, the pose of the last robot joint can be described by the desired trajectory \( {\varvec{T}}_\mathrm{des}\) and the RCM point \({\varvec{P}}_\mathrm{rcm}\). The specific relationship is shown in the following equations:

$$\begin{aligned}&{\varvec{\theta } _\mathrm{robot}} = \varvec{{\bar{\theta }}}, \end{aligned}$$
(3)
$$\begin{aligned}&{{\varvec{P}}_\mathrm{robot}} = {{\varvec{T}}_\mathrm{des}} - l \cdot \frac{\mathrm{tool}}{{\left| \mathrm{tool} \right| }}. \end{aligned}$$
(4)

Thus, a definite correspondence is established between the last robot joint pose and the desired trajectory point that satisfies the RCM constraint. In this work, the position and orientation of the robot are the main control objectives.

2D RCM constraint

In practice, the motion control of the surgical tool is accomplished by a series of discrete control points distributed over the desired trajectory, which is expressed in the control model as a control relation with time parameters [44,45,46,47,48]. In the motion process, the wrist joint pose and surgical tool position still follow the RCM constraint relations given in the previous section, but the velocity must be considered. Here, as shown in Fig. 2, \(x_\mathrm{actual}\), \(y_\mathrm{actual}\), and \(\theta _\mathrm{actual}\) were used to describe the pose achieved by the tool tip at a certain time; moreover, the velocity of the tool tip is defined by \(v_x\), \(v_y\) (virtual linear velocities), and \(\omega \) (virtual angular velocity). Similarly, the coordinates of the RCM constraint were defined as (\(x_\mathrm{rcm}\), \(y_\mathrm{rcm}\)), and finally, we have chosen the desired trajectory in the form of a vector of organized as (\(x_d, y_d, \theta _d\)). It should be mentioned that (\(x_d, y_d, \theta _d\)) is the desired pose trajectory of the robot, which means that if the robot can be controlled to follow the desired pose trajectory, the control objectives of the desired trajectory and RCM constraint can be maintained simultaneously. It should be noted here that the kinematic modeling of the tool tip is within the virtual space, which aims at simplifying the complexity of the model.

Fig. 2
figure 2

Kinematic diagram of the surgical tool shown in 2D space. The surgical tool frame is established at the end of the surgical tool, and it is consistent with the robot base frame (in blue). It is assumed that the surgical tool can move along x and y axes with virtual linear velocity \(v_x\) and \(v_y\). In addition, it can rotate around the end with a virtual angular velocity \(\omega \)

Full size image

At the initial moment, the surgical tool position coincides with the initial point of the desired trajectory. After considering the time and velocity parameters, the pose of the surgical tool at time t can be given by the following kinematic model:

$$\begin{aligned} \begin{aligned} x(t)=x(t-1)+v_xT, \\ y(t)=y(t-1)+v_yT, \\ \theta (t)=\theta (t-1)+\omega T, \end{aligned} \end{aligned}$$
(5)

where T is the time interval between t and \(t-1\). Furthermore, the kinematic model (6) can be rewritten in a more general matrix form by distinguishing the state parameters and output parameters:

$$\begin{aligned} \left[ {\begin{array}{*{20}{l}} {x(t)}\\ {y(t)}\\ {\theta (t)} \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} 1&{}0&{}0\\ 0&{}1&{}0\\ 0&{}0&{}1 \end{array}} \right] \left[ {\begin{array}{*{20}{l}} {x(t - 1)}\\ {y(t - 1)}\\ {\theta (t - 1)} \end{array}} \right] + \left[ {\begin{array}{*{20}{l}} T&{}0&{}0\\ 0&{}T&{}0\\ 0&{}0&{}T \end{array}} \right] \left[ {\begin{array}{*{20}{l}} {{v_x}}\\ {{v_y}}\\ \omega \end{array}} \right] .\nonumber \\ \end{aligned}$$
(6)

Then, in a more compact way, it can be written as:

$$\begin{aligned} {\varvec{X}}(t) = {\varvec{A}} \cdot {\varvec{X}}(t - 1) + {\varvec{B}} \cdot {\varvec{u}}(t), \end{aligned}$$
(7)

where \({\varvec{u}}(t)\) is the velocity vector of the surgical tool, which is also the control term in the kinematic model. Furthermore, the new actual position achieved by the surgical tool:

$$\begin{aligned} \left[ {\begin{array}{*{20}{l}} {{x_\mathrm{actual}}}\\ {{y_\mathrm{actual}}}\\ {{\theta _\mathrm{actual}}} \end{array}} \right] = \left[ {\begin{array}{*{20}{l}} 1&{}0&{}0\\ 0&{}1&{}0\\ 0&{}0&{}1 \end{array}} \right] \left[ {\begin{array}{*{20}{l}} {x(t)}\\ {y(t)}\\ {\theta (t)} \end{array}} \right] . \end{aligned}$$
(8)

That can be written as:

$$\begin{aligned} {\varvec{Y}}(t) = {\varvec{C}} \cdot {\varvec{X}}(t). \end{aligned}$$
(9)

Once the kinematic model for the actual tool position control has been determined, the MPC control method can be designed for the control term \({\varvec{u}}(t)\) to track the desired trajectory.

Fig. 3
figure 3

The projection of the 3D RCM constraint in the 2D plane

Full size image

3D RCM constraint

As the problem of the RCM constraint in 3D space resembles with it in 2D space, passing modification, the expression of the RCM constraint problem in two dimensions can be extended to three dimensions. Nevertheless, there is an easier way to solve this problem, which is decomposing the RCM constraint equations. In this chapter, a dimensionality reduction method based on coordinate decomposition is proposed, which projects the surgical tool and RCM constraint relationship in tri-dimensional space onto the mutually orthogonal planes, \(x{-}y\) and \(x{-}z\) planes. Thus, the problem in 3D space is converted into 2D space RCM constraint problems that are already discussed. The specific projection relationship is shown in Fig. 3. In the picture, line AB represents the surgical tool, where point A is the tool handle connected to the end of the robot, point B is the tool tip, and point C represents the RCM point. In Cartesian space, the projections on two orthogonal planes can determine the pose of the line segment in 3D space, and here we choose the \(x{-}y\) and \(x{-}z\) projection planes for our analysis. Similarly, using the projection decomposition method we can also obtain the expected trajectory of the tool tip in both \(x{-}y\) and \(x{-}z\) planes, and carry out the 2D RCM constraint analysis in each of the two planes. For the \(x{-}y\) plane, it is easy to obtain the vectors containing the x and y coordinates of the tip and its orientation \(\theta _{xy}\) among the time by means of geometric projection laws. Analogously, from the \(x{-}z\) plane, we can also obtained x, z, and the angle \(\theta _{xz}\), where the coordinate x is equal to the one obtained from the \(x-y\) plane. Then, the coordinates of the wrist joint satisfying the RCM constraint can be expressed as:

$$\begin{aligned} \begin{array}{*{20}{l}} {{x_\mathrm{robot}} = x_{tip} + l \cdot \cos ({\theta _{xy}}) \cdot \cos ({\theta _{xz}})},\\ {{y_\mathrm{robot}} = y_{tip} + l \cdot \sin ({\theta _{xy}}) \cdot \cos ({\theta _{xz}})},\\ {{z_\mathrm{robot}} = z_{tip} + l \cdot \cos ({\theta _{xy}}) \cdot \sin ({\theta _{xz}})}, \end{array} \end{aligned}$$
(10)

where l is the tool length, \(x_\mathrm{tip}, y_\mathrm{tip},\) and \(z_\mathrm{tip}\) are the position of the surgical tool, and \(x_\mathrm{robot}, y_\mathrm{robot},\) and \(z_\mathrm{robot}\) represents the position of another end of the surgical tool. With this trajectory of the last joint of the robot, the robot can be controlled to maintain the desired trajectory and RCM constraint in 3D space. The detailed framework of the projection decomposition method is shown in Fig. 4.

Fig. 4
figure 4

Conceptual scheme of the projection decomposition method

Full size image

Control system design

Controller design method

The MPC is a predictive model and it can be described through a prediction horizon P and a control horizon M. At the time instant i, the model predicts the possible outputs of the next P steps, but to compute the output at the instant \(i+1\), it considers only the first M predicted frames.

Fig. 5
figure 5

The proposed fuzzy approximation-based MPC control framework for maintaining trajectory tracking and RCM constraint. \({\varvec{x}}_{d,xy}\) represents the desired position and angle trajectory \((x_d,y_d,\theta _d)\) in \(x{-}y\) plane, \({\varvec{x}}_{\mathrm{actual},xy}\) is the actual position and angle trajectory of the surgical tool in \(x{-}y\) plane. \({\varvec{u}}_{xy}(k)\) if the output of the \(x{-}y\) plane MPC controller, \({\varvec{u}}_{xy,\mathrm{fuzzy}}(k)\) denotes the compensation item, and \({\varvec{u}}_{d,xy}(k) = {\varvec{u}}_{xy}(k) + {\varvec{u}}_{xy,\mathrm{fuzzy}}(k)\). The definitions of the these values in x-z plane are omitted here for the sake of simplification

Full size image

To implement the MPC control method we start from the discrete kinematic model defined in the RCM Constraint subsection [49]:

$$\begin{aligned} \begin{array}{*{20}{l}} {{{\varvec{x}}_i}(k + 1) = {\varvec{A}} \cdot {{\varvec{x}}_i}(k) + {{\varvec{B}}_i} \cdot {{\varvec{u}}_i}(k) + {{\varvec{d}}_i}\left( k \right) }, \\ {{\varvec{y}}(k) = {{\varvec{C}}_i} \cdot {{\varvec{x}}_i}(k)}, \end{array} \end{aligned}$$
(11)

where \({{\varvec{x}}_i} \in {R^n},{{\varvec{u}}_i} \in {R^r},{\varvec{y}} \in {R^q}\). \({{\varvec{d}}_i} \in {R^n}\). n is the state dimensionality, r is the input dimensionality, and q is the output dimensionality. It should be noted that during practical scenarios, the motion of organs is rhythmic due to the rhythm of breathing. The motion of organs is a disturbance for the tasks of trajectory tracking and RCM constraints. The dynamics of the external disturbance induced by the rhythmic motion of the organs is unknown and nonlinear. Therefore, the external disturbance induced by the uncertain motion of organs is defined as \({{\varvec{d}}_i} \in {R^n}\).

The discrete model in (8) and (10) can be written through the predictive model as [50]:

$$\begin{aligned} {\varvec{Y}}(k) = {{\varvec{F}}_y} \cdot {\varvec{x}}(k) + {{\varvec{G}}_y} \cdot {\varvec{U}}(k), \end{aligned}$$
(12)

where:

$$\begin{aligned}&{\varvec{Y}}(k) = {\left[ {\begin{array}{*{20}{l}} {{\varvec{y}}(k + 1)}\\ \vdots \\ {{\varvec{y}}(k + p)} \end{array}} \right] _{qP \times 1}}, \end{aligned}$$
(13)
$$\begin{aligned}&{{\varvec{U}}_i}(k) = {\left[ {\begin{array}{*{20}{l}} {{{\varvec{u}}_i}(k)}\\ \vdots \\ {{{\varvec{u}}_i}(k + M - 1)} \end{array}} \right] _{Mr \times 1}}, \end{aligned}$$
(14)
$$\begin{aligned}&{{\varvec{F}}_y} = {\left[ {\begin{array}{*{20}{l}} {{\varvec{C}}{{\varvec{A}}_i}}\\ \vdots \\ {{\varvec{C}}{\varvec{A}}_i^p} \end{array}} \right] _{pq \times n}}, \end{aligned}$$
(15)
$$\begin{aligned}&{{\varvec{G}}_y} = {\left[ {\begin{array}{*{20}{l}} {{\varvec{C}}{{\varvec{B}}_i}}&{}{\varvec{0}}&{}{\varvec{0}}\\ \vdots &{} \vdots &{}{\varvec{0}}\\ {{\varvec{C}}{\varvec{A}}_i^{{\varvec{M}} - 1}{{\varvec{B}}_i}}&{} \cdots &{}{{\varvec{C}}{{\varvec{B}}_i}}\\ \vdots &{} \vdots &{} \vdots \\ {{\varvec{C}}{\varvec{A}}_i^{{\varvec{P}} - 1}{{\varvec{B}}_i}}&{} \cdots &{}{{\varvec{C}}\sum {_{i = 0}^{P - M}{\varvec{A}}_i^i{{\varvec{B}}_i}} } \end{array}} \right] _{pq \times Mr}}, \end{aligned}$$
(16)

where p is the number of prediction steps and M is the number of control steps.

Once we have computed the possible outputs \({\varvec{Y}}(k)\), we must find the best path for the robot by minimizing a cost function [51]:

$$\begin{aligned} \mathop {\min }\limits _{{{\varvec{U}}_i}(k)} = \left\| {{\varvec{W}}(k) - {\varvec{Y}}(k)} \right\| _{{\varvec{Q}}_y}^2 + \left\| {{\varvec{U}}(k)} \right\| _{{\varvec{R}}_y}^2, \end{aligned}$$
(17)

where \({\varvec{W}}(k)\) is a \(t\times qp\) matrix containing the desired pose of the tip at the instantk, and t is the number of the simulation time intervals. The solution that minimizes the cost function is given by [51]:

$$\begin{aligned} {\varvec{U}}(k) = {\left( {\varvec{G}}_y^T{{\varvec{Q}}_y}{{\varvec{G}}_y} + {{\varvec{R}}_y}\right) ^{ - 1}}{\varvec{G}}_y^T{{\varvec{Q}}_y}\left( {\varvec{W}}(k) - {{\varvec{F}}_y}{\varvec{x}}(k)\right) .\nonumber \\ \end{aligned}$$
(18)

So we have defined:

$$\begin{aligned} {\varvec{u}}(k) = {{\varvec{d}}^T}{\varvec{U}}(k), \end{aligned}$$
(19)

where

$$\begin{aligned} {\varvec{d}} = \left[ {\begin{array}{*{20}{l}} 1&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0\\ 0&{}1&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0\\ 0&{}0&{}1&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0&{}0 \end{array}} \right] . \end{aligned}$$
(20)

Finally, we have computed the new values of x and y as in (12). The procedure is repeated for all the t time intervals of the simulation.

Fuzzy approximation

As mentioned above, the motion of organs is a disturbance for the tasks of trajectory tracking and RCM constraints, which is defined as \({{\varvec{d}}_i} \in {R^n}\). To achieve the implementation of a task by controlling the position of the surgical tip regardless of the external disturbance, the fuzzy approximation method proposed in [42] is utilized in this paper to compensate for the external disturbance. As mentioned in [42], a nonlinear disturbance can be represented by the function of \(f\left( {\varvec{Z}} \right) :{R^n} \rightarrow {R^m}\) as \(f({\varvec{Z}})=\varvec{\theta }^{T} {\varvec{S}}({\varvec{Z}})+\varvec{\varepsilon }({\varvec{Z}})\), where \(\varvec{\theta } \in {R^l}\) is the adaptable weight, the vector \({\varvec{Z}} = \left[ {{z_1},{z_2}, \ldots ,{z_n}} \right] \in {R^n}\) is the input vector of the function model, \({\varvec{S}}({\varvec{Z}})\in {R^n}\) denote the vector of the kernel function. \(\varepsilon \in R\) represents the error of the approximation performance, which should meet the condition: \(\exists {\bar{\varepsilon }} \in R > 0\), \(|\varepsilon ({\varvec{Z}})| \leqslant {\bar{\varepsilon }} \), \(\forall {\varvec{Z}} \in {\varvec{\Omega } _Z}\) [42, 52]. In our work, the adaptive rule [53] for the approximation model is selected as:

$$\begin{aligned} {\xi _j} = \frac{{\prod \nolimits _{i = 1}^n {{\mu _{A_i^l\left( {{z_i}} \right) }}} }}{{\sum \nolimits _{j = 1}^m {\prod \nolimits _{i = 1}^n {{\mu _{A_i^l\left( {{z_i}} \right) }}} } }},j = 1, \ldots ,m. \end{aligned}$$
(21)

The weight parameters [53] \(\varvec{\Theta } \) is adjusted with \(\varvec{{\dot{\Theta }}} = \left[ {{{\varvec{{\dot{\theta }}} }_1},{{\varvec{{\dot{\theta }}} }_2}, \ldots } \right. \left. , {{{\varvec{{\dot{\theta }}} }_m}} \right] \in {R^{m \times l}},{\varvec{{\dot{\theta }}} _i} \in {R^l},i = 1,2, \ldots ,m\):

$$ \begin{aligned} \varvec{\dot{\theta }}_{i}=\left\{ \begin{array}{ll} \varvec{\gamma } {\varvec{e}}_{i} {\varvec{P}} \varvec{\xi }^{T}({\varvec{Z}}), &{} \left\| \varvec{\theta }_{i}\right\|<{\varvec{M}}_{\varvec{\theta }_{i}} \text{ or } \left( \left\| \varvec{\theta }_{i}\right\| \ge {\varvec{M}}_{\varvec{\theta }_{i}}\right. \\ &{} \left. \& \varvec{\gamma } {\varvec{E}} {\varvec{P}} \varvec{\xi }^{T}({\varvec{Z}})<0\right) \\ \varvec{\Gamma }({\varvec{Z}}), &{} \left\| \varvec{\theta }_{i}\right\| \ge {\varvec{M}}_{\varvec{\theta }_{i}} \& \varvec{\gamma } {\varvec{e}}_{i} {\varvec{P}} \varvec{\xi }^{T}({\varvec{Z}}) \ge 0, \end{array}\right. \end{aligned}$$
(22)

where \(\varvec{\Gamma }({\varvec{Z}})=\varvec{\gamma } {\varvec{e}}_{i} {\varvec{P}}\left( 1-\frac{\varvec{\theta }_{i} \varvec{\theta }_{i}^{T}}{\left\| \varvec{\theta }_{i}\right\| ^{2}}\right) \varvec{\xi }^{T}({\varvec{Z}})\), and \(\varvec{\gamma } \in R^{m \times m}\) denotes the updating speed matrix of the compensator. In addition, the vector of system output error is defined by \({\varvec{E}} = \left[ {{{\varvec{e}}_1},{{\varvec{e}}_2}, \ldots ,{{\varvec{e}}_m};{{\varvec{\dot{e}}}_1}} \right. ,{\varvec{\dot{e}}_2}, \ldots ,{\varvec{\dot{e}}_m}]\). Based on the Lyapunov stability theorem, the \({\varvec{P}} \in R^{2 \times 1}\) is chosen. The following motion compensation method is adopted to estimate the external disturbances. \({{\varvec{X}}_d}\), \({\varvec{\dot{X}}_d}\), \({\varvec{X}}\) and \(\varvec{\dot{X}}\) are the input of the fuzzy system, and the output is the compensated angular velocity on each axis. After approximation of the uncertain disturbance, an adaptive fuzzy term \({\varvec{u}}_\mathrm{fuzzy}(k)\):

$$\begin{aligned} {{\varvec{u}}_\mathrm{fuzzy}}\left( k \right) = {\varvec{\Theta } ^T}S\left( {{{\varvec{X}}_d},{{\varvec{\dot{X}}}_d},{\varvec{X}},\varvec{\dot{X}}} \right) . \end{aligned}$$
(23)

is introduced to compensate the uncertain external disturbances. The desired control term can be expressed as:

$$\begin{aligned} {{\varvec{u}}_d}\left( k \right) = {\varvec{u}}\left( k \right) + {{\varvec{u}}_\mathrm{fuzzy}}\left( k \right) . \end{aligned}$$
(24)
Fig. 6
figure 6

Simulation setup in Matlab

Full size image
Fig. 7
figure 7

Simulation trajectories

Full size image
Fig. 8
figure 8

Experiment one—Zig-Zag trajectory

Full size image
Table 1 Root mean square rrror (RMSE) of the trajectory tracking and RCM constraint in experiment one and two
Full size table
Fig. 9
figure 9

Experiment one—Sine trajectory

Full size image
Fig. 10
figure 10

Experiment two—Zig-Zag trajectory

Full size image
Fig. 11
figure 11

Experiment two—Sine trajectory

Full size image

Control framework

To solve the 3D space RCM constraints, we choose the projection decomposition method to transform the problem into two 2D RCM constraint problems, and obtain the coordinate projections and velocity components of the wrist joint using MPC in both planes. Since the kinematic control is performed in joint space, we need to combine the position and velocity components of the wrist into 3D space through geometric relations. Once the wrist joint pose and velocity is obtained, the angle and angular velocity of each joint of the robot can also be solved by inverse kinematics, which in turn drives the robot motion and completes the surgical tool trajectory tracking. Furthermore, the fuzzy approximation method is introduced to compensate for the external disturbance induced by the motion of organs. The detailed control framework is shown in Fig. 5.

Simulation results

The comparative simulations are performed for the following purposes:

  1. 1.

    To show that the proposed MPC-based method is able to improve the trajectory tracking and RCM maintaining performance, in comparison with the typical decoupled method;

  2. 2.

    To demonstrate that fuzzy approximation-based compensation is more effective to maintain the performance when there are external disturbances induced by the motion of organs, in comparison with the proposed MPC-based method but without disturbance compensation.

The performance of the proposed method is verified through the simulation in Matlab. An environment in which a KUKA LWR4+ robot is loaded, the initial and the structural information have been given. In the virtual environment, the RCM constraint (the red circle) and the tip (in blue) were added as well, which is shown in Fig. 6. While the RCM is kept fixed, the tip moves following the chosen trajectories. In the simulation, two trajectories are chosen to conduct the experiment, that is, the zigzag and sine trajectories, which are shown in Fig. 7. It should be noted that to simplify the trajectory, the Z-axis positions of these two trajectories are kept fixed. To better demonstrate the superiority of the proposed method, two comparative experiments are conducted. First, the proposed method without fuzzy approximation is compared with the decoupled method [15] to show the better performance of the MPC-based method. Second, the proposed method with fuzzy approximation is compared with the one without fuzzy approximation to verify the effectiveness of the fuzzy approximation when there are some external disturbances. It is noted that during surgeries, although the human body remains stationary, the motion of organs is rhythmic, often following the rhythm of breathing. The motion of organs is a disturbance for the tasks of trajectory tracking and RCM constraints. Therefore, in the simulation, we set the disturbance as a periodic sinusoidal disturbance. Some metrics of evaluation are introduced here to analyze the simulation results:

$$\begin{aligned}&\begin{aligned} {e_{\mathrm{tra}{\mathrm{j}_x}}} = {x_\mathrm{actual}} - {T_{\mathrm{des},x}}, \\ {e_{\mathrm{tra}{\mathrm{j}_y}}} = {y_\mathrm{actual}} - {T_{\mathrm{des},y}}, \\ {e_{\mathrm{tra}{\mathrm{j}_z}}} = {z_\mathrm{actual}} - {T_{\mathrm{des},z}}, \\ \end{aligned} \end{aligned}$$
(25)
$$\begin{aligned}&{e_\mathrm{traj}} = \sqrt{e_{\mathrm{tra}{\mathrm{j}_x}}^2 + e_{\mathrm{tra}{\mathrm{j}_y}}^2 + e_{\mathrm{tra}{\mathrm{j}_z}}^2}, \end{aligned}$$
(26)
$$\begin{aligned}&{e_\mathrm{rcm}} = \frac{{\left\| {\left( {{{\varvec{P}}_\mathrm{rcm}} - {{\varvec{X}}_\mathrm{actual}}} \right) \times \mathrm{tool}} \right\| }}{{\left\| {{{\varvec{P}}_\mathrm{rcm}} - {{\varvec{X}}_\mathrm{actual}}} \right\| }}. \end{aligned}$$
(27)

As for the control part, with our MPC method, the prediction horizon p and the control horizon m are chosen as 10 and 4, respectively. It is noted that for the sake of the computational time, the predictive horizon has been set to a bigger value than the control horizon. Furthermore, the weight parameters \({\varvec{Q}}\) and \({\varvec{R}}\) are determined through several cycles of the trial with different combinations of the parameters. At the end, a high \({\varvec{Q}}\) and a low \({\varvec{R}}\) have been chosen to maintain a small tracking error and quite reactive behavior that allows us to change the input according to what has been advised by other MPC applications [54].

The results of the first simulation are shown in Figs. 8 and 9 with Fig. 8 displaying the Zig-Zag trajectory and Fig. 9 presenting the Sine trajectory. It indicates that the performance of the proposed MPC-based method is significantly better than the decoupled method, both in the trajectory error and the RCM error. The overall trajectory error of the proposed MPC method is within 1 mm, while that of the decoupled method would exceed 1 mm. As for the RCM error, the proposed MPC-based method can maintain near 0 at all the timestamps. However, the RCM error of the decoupled method could reach up to 5 mm.

The results of the second simulation are listed in Figs. 10 and 11 with Fig. 10 displaying the Zig-Zag trajectory and Fig. 11 presenting the Sine trajectory. It can be seen from the experimental results that the performance would be greatly effected by the external disturbances without the fuzzy approximation-based disturbance compensation. While introducing the fuzzy approximation-based compensation method, the performance are better, the trajectory and RCM error would decrease to 0 gradually, which means that the fuzzy approximation method can accurately estimate and compensate for the external disturbances.

Based on the above discussion of the experimental results, we summarize the advantages of the proposed method. The significance lies in the higher trajectory accuracy and RCM accuracy compared to the decoupling method, and also in the effective suppression of external disturbances induced by rhythmical motions of organs.

Conclusion

In this section, we demonstrate the advantages and potential of the MPC approach to robot-assisted laparoscopy. The specific contributions are summarized below.

  1. 1.

    The 3D trajectory tracking and RCM constraint maintaining problem has been projected into 2D space, which could simplify the problem.

  2. 2.

    The virtual kinematic model of the surgical tool is established, and the trajectory and RCM constraint are transferred into controlling the position and orientation of the virtual surgical tool.

  3. 3.

    The fuzzy approximation method is introduced to compensate for the external disturbances induced by the rhythmical motion of organs.

Simulations have been conducted to verify the effectiveness of the proposed method, both in comparison with the decoupled method and the proposed method without fuzzy approximation-based disturbance compensation. The results shows that the performance of the proposed method is better than the decoupled method, and the fuzzy approximation-based compensation method can accurately estimate the external disturbances and compensate for the disturbances. However, there are only simulation results in this paper. In the future, there shall be improvements such as the introduction of constraints on the tool velocity according to the different scenarios of minimally invasive surgeries. After improvements, the method could be implemented directly in the control of a KUKA LWR4+, or any other industrial robots, by re-tuning the MPC parameters, changing the values of the D-H parameters, and re-setting the initial configuration. In addition, the proposed method can be extended to investigate other research areas, such as mobile robots, soft robots [55, 56], etc.