Adaptive Decoupling Control of a Serial Redundant Robot for
Teleoperated Minimally Invasive Surgery
Hang Su, Giancarlo Ferrigno, Senior Member, IEEE, and Elena De Momi, Senior Member, IEEE
Abstract— In this paper, we present an adaptive decoupling control scheme using a serial redundant robot for teleoperated Minimally Invasive Surgery (MIS). In presence of uncertain interaction between the surgical tip and the patient body during teleoperated surgery, the accuracy of the end-effector position should be secured, while guaranteeing a Remote Center of Motion (RCM) constraint. Adaptive fuzzy approximation is adopted to estimate the dynamical uncertainties in physical interaction between the surgical tip and the abdominal wall to enhance the accuracy and keep the RCM constranit. The effectiveness of the proposed control approach was verified in a lab setup environment by using the LWR 4+ (KUKA) slave robot and Sigma7 (Force Dimension) master device.
I. INTRODUCTION
To serve as the surgical arms for laparoscopic surgery, the end-effector of the manipulator must go through small inci-sions on the patient’s abdominal wall, which is commonly known as the Remote Center of Motion (RCM) constraint. Various studies have been conducted with serial robots for MIS tasks for RCM constraint [1][2]. Sandoval et al. pro-posed an improved dynamic control approach for redundant robots with Cartesian Admittance Control (CAC) in the task-space and RCM constraint [3]. However, the control scheme is only validated with simulation and is not competent for practical application. Furthermore, in presence of uncertain disturbances during human-robot interaction, the accuracy of the surgical tip and the RCM constraint should be secured. Direct fuzzy adaptive controllers are known to estimate a large uncertainty or unknown variation in plant parameters and disturbance [4][5]. The unknown time-varying periodic disturbances from physical interaction can be compensated online. In this paper, we proposed an adaptive decoupling control scheme to enhance the accuracy and keep the RCM constraint for the teleoperated laparoscopic surgery.
II. METHODOLOGY A. Modelling of the serial robot
The dynamic model of n-DoF serial manipulator in the Lagrangian formulation can be expressed as:
M(q) ¨q+ C(q, ˙q)˙q + g(q) = τC−τe (1)
where q ∈ Rn is the joint values vector, M(q) ∈ Rn×n is the
inertia matrix, C(q, ˙q) ∈ Rn×n is a matrix representing the
*This work was supported by the European Unions Horizon 2020 research and innovation program under SMARTsurg project grant agreement No. 732515 and the China Scholarship Council.
Hang Su, Giancarlo Ferrigno and Elena De Momi are with the Department of Electronics, Information and Bioengineering, Politecnico di Milano, 20133, Milan, Italy. hang.su, giancarlo.ferrigno, elena.demomi@polimi.it
Coriolis and Centrifugal effects, and g(q) ∈ Rn is the vector
of gravity torques. The torque vectors τC ∈ Rn and τe∈ Rn
represent the control torques and the external torque vectors, respectively. 𝑞" 𝑞# 𝑞$ RCM Target Abdominal Wall Tool 𝑞$ 𝐹& 𝑑 𝑋 𝑃*
Fig. 1. Minimally Invasive Surgery scenario: d is the distance between the RCM point (Pt) and the tool (the small incision is zoomed for a better
understanding). The tool tip position X is controlled by teleoperation to reach the target in the patient’s abdomen.
B. Adaptive decoupling control scheme
To drive the tool tip, the torque controller is defined as: τC= τd+ ˆC(q, ˙q)˙q + ˆg(q) + ˆτe (2)
where τC ∈ Rn, ˆC(q, ˙q) ∈ Rn×n and ˆg(q) ∈ Rn are
the estimated compensation terms, ˆτe is the filtered torque
computed from external torque sensors, and τd ∈ Rn is the
term introduced to conduct the desired surgical task. 1) Main task control: The end-effector position X ∈ R3is
utilized to reach the desired position. The task control torque τT can be defined as
τT= JT(KXX − D˜ XX)˙ (3)
where J(q) ∈ R3×n is the Jacobean matrix from the base to the end-effector, ˜X = Xd− X(q) ∈ R3×n, KX∈ R3×3 is the
diagonal stiffness matrix, DX∈ R3×3is the diagonal damping
matrix, and ˙X ∈ R3 is the actual Cartesian velocity.
2) Null-space control: The null-space controller τN∈ Rn
can be utilized to achieve the RCM constraint:
𝑷𝒕 𝑿𝒅, 𝑿̇𝒅 Task-Space controller Master device 𝝉𝒅 𝑪) 𝒒,𝒒̇ 𝒒̇ + 𝒈- 𝒒 Robot RCM 𝝉𝑪 Null-space Projector 𝑭𝒆 Fuzzy Approximation 𝑿 𝒒 , 𝑿̇ 𝒒 𝑭𝑵 𝝉𝑻𝟏 𝝉𝑻 𝝉𝑵𝟏 𝝉𝑻𝟐 𝒒, 𝒒̇ 𝑿 𝒒 , 𝑿̇ 𝒒 𝝉𝑵𝟐 𝝉𝒆
Fig. 2. Block diagram representing the proposed control architecture: The “Task-space control” block is used to achieve the desired pose of the end effector, the “RCM Constraint” calculates the virtual force applied on the null space, the “Null-space Projector” maps the virtual force to joints torque, the “Fuzzy Compensation” compensates the unknown disturbance in both task space and null-space, the “Robot ” is robot arm dynamic model with uncertain physical interaction.
where J2(q) ∈ R3×n is the Jacobean matrix from base to
the last joint, J(q)+M) is the inertia-weighted pseudo-inverse matrix [6], FN∈ R3 in Fig. 1 is the force applied on the last
joint to keep the RCM constraint: FN= (ksd − kdd)˙
− →
FN (5)
where d = ||(Pt− X) × u|| ∈ R, Pt ∈ R3 is the RCM point,
u ∈ R3 is the tool tip vector, k
s, kd ∈ R are stiffness and
damping parameter,→−FN= (X−Ptd)×u×u is the direction vector
of the force. In addition to the decoupled control torque, adaptive fuzzy approximation is introduced to estimate the uncertain physical interaction, enhancing the task accuracy and keeping RCM constraint. The whole control diagram can be seen in Fig. 2.
III. EXPERIMENTAL EVALUATION User 1 Patient phantom RCM Camera User 2 Vision interface Master device
Fig. 3. Experimental setup procedures: Firstly, hands-on control is utilized to allow user 1 locate the RCM constraint by hand on the patient phantom; then user 2 use the master device to control the tool tip for surgical tasks.
As it is shown in Fig. 3, two subjects were enrolled to setup the experimental scene with the developed teleoper-ated MIS system implemented with Fast Research Interface (FRI) [7]. The proposed adaptive decoupling control method (ADC) was compared with the method (DCAC) [3]. The accuracy of the effector and the RCM constraint error are recorded and shown in Fig. 4.
50 60 70 80 90 100 110 Time [s] 0 0.1 0.2 0.3 Error [cm]
(a) Cartesian position error
50 60 70 80 90 100 110 Time [s] 0 0.5 1 1.5 2 Error [cm] (b) RCM constraint error (𝑑) (𝐸%)
ADC
DCAC
ADC
DCAC
Fig. 4. Performance comparison between DCAC and ADC
IV. CONCLUSIONS
This paper proposes an adaptive decoupling control scheme using a serial robot for teleoperated laparoscopic surgery. Compared with the method proposed in [3], it shows promising accuracy improvement of the surgical tip in static error, and the error of RCM constraint also was converged into a smaller area.
References
[1] N. Aghakhani, M. Geravand, N. Shahriari, M. Vendittelli, and G. Oriolo, “Task control with remote center of motion constraint for minimally invasive robotic surgery,” in Robotics and Automation (ICRA), 2013 IEEE International Conference on. IEEE, 2013, pp. 5807–5812. [2] R. C. Locke and R. V. Patel, “Optimal remote center-of-motion
loca-tion for robotics-assisted minimally-invasive surgery,” in Robotics and Automation, 2007 IEEE International Conference on. IEEE, 2007, pp. 1900–1905.
[3] J. Sandoval, G. Poisson, and P. Vieyres, “Improved dynamic formulation for decoupled cartesian admittance control and rcm constraint,” in Robotics and Automation (ICRA), 2016 IEEE International Conference on. IEEE, 2016, pp. 1124–1129.
[4] H. Su, H. Zhang, Z. Li, and C.-Y. Su, “Adaptive fuzzy control of operation space constrained exoskeletons under unmodelled dynamics,” in Intelligent Control and Automation (WCICA), 2014 11th World Congress on. IEEE, 2014, pp. 3277–3282.
[5] Z. Li, C.-Y. Su, G. Li, and H. Su, “Fuzzy approximation-based adaptive backstepping control of an exoskeleton for human upper limbs,” IEEE Transactions on Fuzzy Systems, vol. 23, no. 3, pp. 555–566, 2015. [6] O. Khatib, “A unified approach for motion and force control of robot
manipulators: The operational space formulation,” IEEE Journal on Robotics and Automation, vol. 3, no. 1, pp. 43–53, 1987.
[7] G. Schreiber, A. Stemmer, and R. Bischoff, “The fast research interface for the kuka lightweight robot,” in IEEE Workshop on Innovative Robot Control Architectures for Demanding (Research) Applications How to Modify and Enhance Commercial Controllers (ICRA 2010), 2010, pp. 15–21.
