## Abstract

In this paper, we present an integrated robotic arm with a flexible endoscope for laparoscopy. The endoscope holder is built to mimic a human operator that reacts to the surgeon's push while maintaining both the incision opening through the patient's body and the center of the endoscopic image. An impedance control algorithm is used to react to the surgeon's push when the robotic arm gets in the way. A modified software remote center-of-motion (RCM) constraint formulation then enables simultaneous RCM and impedance control. We derived the kinematic relationship between the robotic arm and line of sight of the flexible endoscope for image center control. Using this kinematic model, we integrated the task control for RCM and surgeon cooperation and the endoscope image centering into a semi-autonomous system. Implementation of the control algorithm with both matlab simulation and the HIWIN RA605-710 robotic arm with a MitCorp F500 flexible endoscope demonstrated the feasibility of the proposed algorithm.

## 1 Introduction

Minimally invasive surgery (MIS), the advantages of which include a small surgical wound and rapid recovery, has replaced open surgery on many occasions. MIS, such as laparoscopic surgery, involves the use of an endoscope to visualize the internal tissues of a patient. Because the surgeon performing MIS needs to use both their hands for operating the laparoscopic tools, an assistant surgeon is required to hold the endoscope in place. This task, which is relatively routine, can now be performed using robotic endoscope holders [13].

To provide a useful perspective for surgery, an endoscope holder must point the camera toward the tips of the instrument. However, the endoscope holder sometimes obstructs the surgeon's movement during the procedure and may need to move aside while maintaining the trocar's insertion point to not tearing the wound. This maneuver is straightforward for a human assistant but requires extraordinary capability when being performed by a robot. Moreover, the aforementioned movement can change the camera view; this necessitates a flexible endoscope with adequate control to compensate for the endoscope's pointing angle. This paper presents a robotic flexible video-endoscope-holder system that integrates a flexible endoscope with a six-degrees-of-freedom robotic arm that reacts to a surgeon's nudge while maintaining both the insertion point and endoscopic view. The surgeon could be busy with the procedure, and one should try to minimize the surgeon's effort. In this research, we fitted the endoscope holder on the robot arm through a six-degree-of-freedom load cell. The surgeon can give the endoscope holder a nudge with his elbow, and the endoscope holder will react. We also carried out the analysis in an adequate manner for the robot reaction.

The design of a flexible endoscope holder has three requirements. First, the endoscope-holder system must respond to the surgeon's push when directed to move aside. Second, the robotic system must observe the remote center-of-motion (RCM) constraint. Third, a suitable control algorithm must be applied to control the orientation of the endoscope. Accordingly, we employed the commonly used impedance control algorithm to execute surgeon–robot interactions [4]. The first requirement will be handled by the conjunction of task control and impedance control in this research. Regarding the second requirement, two main approaches have been employed: hardware RCM through mechanism design and software RCM through control. The conventional MIS manipulator systems that use hardware RCM are RAVEN-II [5], Telelap ALF-X [6], and da Vinci [7,8] surgical systems. These systems use parallelogram-based joint mechanisms [2,3]. Another commonly used design involves circular arc tracking and synchronous belt transmission [913]. However, all these approaches have a hard mechanical constraint. Software-generated RCM provides more freedom in the setup and is becoming popular in commercial systems. The Versius system (from CMR surgical) is an example of this RCM ability [14]. Depending on the coordinates used to describe the attitude, approaches can be divided into world frame and trocar frame-based [15]. In the world frame-based approach, the relationship between the tooltip and insertion point is established with respect to the patient's body [1618], whereas in the trocar frame-based approach, this relationship is established with respect to the tool itself [1921]. The earlier world frame-based approach demonstrates the difficulty of algorithmic singularity for null space projection in task control [16,17]. Sandoval et al. proposed a modified formulation of the RCM constraint to avoid this difficulty [18], and their proposed method was thus adopted in the current research.

The third design requirement concerns the pointing control of the flexible endoscope to restore the image center. The flexible endoscope for laparoscopy comprises a distal controllable bending tip in front of a flexible shaft. In this research, we fixed the flexible shaft through a rigid cannula, inserted it through the trocar, and modified the navigation wheel mechanism for motorized control. By combining the rigid part of the endoscope with the sixth joint, the cannula for the flexible endoscope could be treated as part of the sixth joint. We then adopted the kinematic model developed in Ref. [22] to control the position and attitude of the bending tip. The inertia of the bending tip is small, and kinematic control is sufficient for the pointing [23,24]. Through analysis of the kinematics of the flexible endoscope and robotic arm, we established the relationship between the tip yaw angle, tip position, and robot configuration. With the dynamic model of the robot, a control mechanism that provided the required lens movement could be designed. Herein, we propose combined kinematic–dynamic control for the overall flexible endoscope-holder system. The system consists of a flexible endoscope with one degree-of-freedom bending tip and six axes robotic arm to constitute seven axes of motion. The RCM constraint and the target viewpoint impose three-degrees-of-freedom constraint. We still have one more degree-of-freedom left for surgeon collaboration. This control gives doctors a user-friendly environment for maneuvering without compromising the operating view. Because of the algorithm, the laparoscope-holder system can not only hold the laparoscope in a manner that approximates a human operator but also achieve visual angle control that is difficult to implement using regular feedback from the manipulator angles.

In this study, we modified a commercial flexible laparoscope for motorized control and mounted the rigid shaft on an industrial robot. We also developed the related software control interfaces for the operator. With impedance control, doctors can adjust the robot to their preferred posture for inserting the laparoscope through the trocar and then change the target monitor view through the control interface. Once the center of the view has been fixed, the flexible laparoscope holder can automatically hold the target view, allowing the doctor to push the robotic arm around.

The paper is organized as follows. Section 2 presents the system configuration and the dynamics and geometric modeling of the robot manipulator and the flexible laparoscope. Section 3 introduces the proposed control algorithm. Section 4 gives a brief description of the system hardware. Section 5 presents the simulation and experimental results, and Section 6 presents the conclusions.

## 2 System Configuration and Modeling

This section introduces the system architecture and the dynamic and geometric modeling. As shown in Fig. 1, the flexible laparoscope holder system consists of a robotic arm holding a flexible laparoscope. We designed a special holder to fix the rigid cannula onto the sixth joint of the robot. The cannula is a standard surgical tool used in MIS surgery, which guides the flexible laparoscope through the trocar, and the distal bending tip points the lens toward the point of interest. The robotic arm moves the rigid cannula under the remote-center-of-motion constraint to maintain the trocar position. A rack-and-pinion set then drives the cable to bend the lens toward the point of interest. The purpose of this research is to design a controller that moves the laparoscope around by the surgeon's order under the RCM constraint. At the same time, maintain the laparoscope image centered around the point of interest of the operation.

Because of the different levels of manipulation, we separated the description into four sections: A, the model for the robotic manipulator; B, the task space RCM constraint; C, the mathematical description for the bending-tip geometry; and D, the correction for the displacement introduced by the bending-tip.

This research designed a coupler device (Fig. 2) to fix the torque sensor and the endoscope to Joint 6 of the robot. To enable motorized control of the flexible endoscope, we have also replaced the original hand adjusting lever mechanism (Fig. 3(a)) with a rack-and-pinion mechanism of our design (Fig. 3(b)).

### 2.1 Robot Manipulator Modeling.

This section briefly describes the RCM constraint as in Ref. [18] and the laparoscope bending tip model as in Ref. [22]. We then present the kinematics model for the robot–laparoscope interaction.

With the rigid cannula treated as part of the sixth joint, the regular robot dynamic model with n degrees-of-freedom is suitable for describing the arm movements.
$D(q)q¨+C(q,q˙)q˙+G(q)=τ−τe$
(1)

where $q$, $q˙$, and $q¨$ are the angle, angular velocity, and angular acceleration, respectively; moreover, $τ$ and $τe∈Rn$ are the control and external torque, respectively. $D(q)∈Rn×n$ is the symmetric positive-definite inertial matrix, $C(q,q˙)q˙∈Rn$ is the centrifugal and Coriolis terms, and $G(q)∈Rn$ is the gravitational force term.

#### 2.1.1 RCM and Cannula Tip Constraint.

In MIS, the trocar cannula must never push against the wound. Therefore, in addition to the end control of the robotic arm, the system must follow the RCM constraint.

In Fig. 4, $Ptrocar$ denotes the trocar position on the patient, $P6$ the end of the sixth link (i.e., the proximal end of the cannula), and $Ptool$ the base of the bending tip (i.e., the distal end of the cannula). Moreover, $PRCM$ denotes the location of the point on the cannula that coincides with the trocar, defined as
$PRCM(q)=P6(q)+λ(Ptool(q)−P6(q))$
(2)

where $λ$ is the ratio of the distance between $Ptrocar$ and $P6$ to the length of the cannula.

Note that $PRCM$ is floating on the cannula to create the shortest distance to the trocar. From Ref. [18], the derivative of $PRCM(q)$ gives
$p˙RCM=[J6T+λ(JtoolT−J6T)ptoolT−p6T] [q˙λ˙]=JRCM(q,λ)[q˙λ˙]$
(3)

where $p˙RCM$ is the velocity of the RCM, $J6$ the mapping matrix between the joint velocity $q˙$ and velocity $P6˙$, and $Jtool$ the mapping matrix between $q˙$ and $P˙tool$. The last expression in Eq. (3) enables task-space control for the RCM constraint.

In the current case, the control task is not merely the RCM constraint. Simultaneous control of the base of the bending tip is essential for image compensation. Therefore, rather than following the dynamic formulation in Ref. [18] further, we used a modified formulation to include both the RCM and the end of the cannula $Ptool$. Therefore, a combined RCM and tip constraint can be presented as
$[p˙toolp˙RCMλ˙]=[Jtool03×1JRCM01×61] [q˙λ˙]$
(4)
where the dimensions of the combined Jacobian matrix are 7 × 7. Notably, the laparoscope cannula is integrated along the axial direction of the sixth joint of the articulated robot. The rotation of this joint does not affect the locations of either $pRCM$ or $ptool$. Thus, in the sixth column, $JRCM$ and $Jtool$ both have zero entries. If we allow the orientation of the image to be free, we can exclude $q˙6$ from the constraint. Then
$[p˙toolp˙RCMλ˙]=[J̃tool03×1J̃RCM01×51] [q˙1q˙2q˙3q˙4q˙5λ˙]=Jcombine[q˙λ˙]$
(5)

where $J̃tool$ and $J̃RCM$ are the 3 × 5 and 3 × 6 matrices formed by eliminating the sixth column from $Jtool$ and $JRCM$, respectively. Consequently, $Jcombined$ is a 6 × 6 matrix.

#### 2.1.2 Kinematics for the Bending Tip.

Because of budget constraints, we used a laparoscope with one bending degree-of-freedom. We adopted the model from Ref. [22] to describe the bending geometry and the pointing angle of the bending tip (Fig. 5).

In Fig. 5, $Fb$ is the base frame attached to the rigid cannula with the unit vector $kb$ pointing along the backbone, and $ib$ and $jb$ are pointing in the bending directions. This coordinate coincides with the coordinate on the sixth joint. $Ff$ is attached to the end of the flexible segment. The segment behind $Ff$ is the nonflexible section of the laparoscope module. $Fc$ is attached to the end of the camera module with $kc$ pointing normal to the image plane and $ic$ pointing toward the x-axis of the image. If $Π$ is the plane containing the bending backbone, then the angles $α$ and $β$ represent the swing and bending angles, respectively. Moreover, $α$ and $β$ denote the two degrees-of-freedom of the laparoscope. The sixth joint of the robot provides control for $α$, whereas the pulling wire controls $β$. $Lf$ is the length of the central backbone of the flexible section, and $R$ is the radius of the yaw curvature. $Ff$ can be reached from $Fb$ through rotation about the $kb$ axis through $α$ followed by banking about $if$ through $β$ with a shift of $pr$ and then by rotation back about $kf$ at an angle of $−$. Then, the homogeneous transformation matrix between is
$Tfb=[Rz(α)001] [Ry(β)pr01] [R(−α)001]=[s2α+cβc2α−sαcα(1−cβ)cαsβLfβ(1−cβ)cα−sαcα(1−cβ)c2α+cβs2αsαsβLfβ(1−cβ)sα−cαsβ−sαsβcβLfβsβ0001]$
(6)
where $s(·)$ and $c(·)$ represent the sine and cosine functions, respectively, and $pr$ is the displacement between $Fb$ and $Ff$
$pr=[Lfβ(1−cβ)0Lfβsβ]T$
(7)
Note that we are interested in the pointing direction of the line of sight. The exact location of the end of the tip is not of concern here.
$pr=[Lfβ(1−cβ)0Lfβsβ]T$
(8)

Note that we are interested in the pointing direction of the line of sight. The exact location of the end of the tip is not of concern here.

In Fig. 6, the relationship between the bending angle and cable travel is $Lf=Rβ$ and $Lf+δl=(R+δr)β$, where $δl$ is the additional arc length at the cable location and $δr$ is the distance between the center of the bending tip and the cable location. By subtracting $Lf$ from the second expression, we obtain
$β=δrδl$
(9)
where $δr$, as shown in Fig. 6, is the distance from the backbone to the cable duct. Recall that a set of rack and pinion drives the $β$ angle of the bending tip. Thus, the angular displacement of the pinion becomes
$θD=βrpδr$
(10)

where $rp$ is the radius of the pinion and $θD$ the angular displacement of the pinion required to achieve bending.

#### 2.1.3 Relationship of the Line of Sight With the Robotic Arm.

The flexible laparoscope is mounted on the end of the sixth joint of the robot, and the basic configuration is shown in Fig. 7, where $rbending$ stands for the additional length induced by the bending tip.

Suppose the bending tip sweeps across an angle $θ$ away from the original orientation, the section of the cannula length after the trocar is $r$, and the distance between the trocar and the target point is $d$. Geometrically, the bending tip must bend back by the angle $θ′$, if we ignore the dimension of the flexible part
$θ′=sin−1[rsinθd2−2drcosθ+r2]$
(11)
Here, $θ$ can be calculated through the direction of the end effector before ($kend$) and after ($kend′$) the swing as
$θ=cos−1(kend′⋅kend)$
(12)
However, considering the length of the bending tip, the inclusion of an additional correction term is required. In Fig. 6, the length of the bending tip is $Lf$, and the radius of curvature of the bending section is $R=Lf/β$. Geometrically, the additional extension from the bending tip $rbending$ can be calculated as shown in Fig. 8
$rbending=Lfβtanβ2$
(13)
The new effective cannula length $r′$ and can be approximated by
$r′=r+rbending$
(14)
respectively. As shown in Fig. 8, $r$ in Eq. (13) stands for the length of the cannula beyond trocar, $rvary$ is the extra cannula extension after the angular correction, and $rbending$ is from Eq. (12). The corrected bending angle becomes
$θc′=sin−1(r′sinθd2−2drcosθ+r′2)$
(15)
where $r′$ is the radius spanning $β$. The new bending angle for the bending tip is then
$β=θ+θc′(β)$
(16)

With Eq. (16), it is now possible to solve for $β$ iteratively.

## 3 Controlled Sign of the Laparoscope Holder

With the rigid cannula treated as part of the sixth joint, the system consists of two major components: the robotic arm and the bending tip. The goal is to implement impedance control and task control for the RCM constraint of the robotic arm [18] as well as kinematic control for image compensation of the bending tip.

### 3.1 Control of the Robotic Arm.

Considering the dynamic robot model (1), typical dynamic cancelation control would give
$τ=D(q)aq+C(q,q˙)q˙+Ĝ(q)$
(17)
Equations (1) and (17) give
$q¨=aq$
(18)
Then, $aq$ is designed as
$aq=q¨d+Kd(q˙d−q˙)+Kp(qd−q)$
(19)
where $q¨d$,$q˙d$, and $qd$ are the target joint space position, velocity, and acceleration, respectively. The closed-loop dynamic response of the joint tracking error is
$(q¨d−q¨)+Kd(q˙d−q˙)+Kp(qd−q)=0$
(20)

where $Kp$ and $Kd$ are the $n×n$ positive-definite gain matrices that can be tuned for joint space system stability. Next, we implemented robotic control on the kinematic level to coordinate with the lens pointing control.

### 3.2 RCM Task Control.

From Eq. (5), the position errors at the RCM and the end of the cannula are defined by $eRCM$ and $etool$, respectively,
$etool=ptask−ptool$
(21)
$eRCM=ptrocar−pRCM$
(22)
In Eq. (21), $ptask$ is the planning trajectory in the task space. Using a task control setup in conjunction with the joint space control in Eq. (19), we design the controller to be
$[q˙dλ˙]=Jcombine#[K1I3×3O3×30O3×3K2I3×30001] [etooleRCMλ˙]$
(23)

Note that $Jcombine#$ is the pseudo-inverse transformation matrix between $[q˙ λ˙]T$ and $[p˙tool p˙RCM λ˙]T$. Equations (23) and (19) then provide the necessary feedback to the controller (17), Fig. 9.

### 3.3 Surgeon Collaborative Task Control.

The strategy to react to the surgeon's push is to design the robot arm's behavior under certain external force. In this research, a force sensor is attached to the sixth axis of the robot, and we design the point where the force sensor is attached to move like a mass-spring-damper system, that is

$MX¨s+BX˙s+KXs=Fext$

where $Xs=ps,f−ps,d$ is the designed displacement resulted from the external force, $ps,f$ is the designed position with external force, and $ps,d$ is the desired position without external force. (M, B, K) is set as (1, 10, 25) to form a damped system with settling time around one second and the sensor position $ps$ is set to be coincident with $p6$ in this paper.

Instead of using null space control, a simpler way to deal with additional avoidance command is proposed here. Since both $p6$ and $ptool$ are on the same rigid body, $Xtool=ptool,f−ptask$ can be calculated under the constraint of the trocar, where $ptool,f$ is the designed position of the tool with external force, and $ptask$ is the desired position of the tool without external force. $Xtool$ is then added to the command of $ptask$ without changing the control structure. The total control block diagram is shown in Fig. 10.

### 3.4 Bending Tip Control.

After there is a movement in the cannula, the amount of pointing angle adjustment, $Δθ′$, can be obtained from Eq. (15), the bending angle, $β$, from Eq. (16), and the required angular displacement for the pinion, $θD$, from Eq. (9). The bending tip is very light compared with the robot links. Thus, the kinematic control suffices for image center compensation. Note that we implemented the RCM control also in the position loop. It is possible to cascade the bending compensation control to the position controller.

Proposition. Consider both the dynamic manipulator control (17) and the tip bending mechanism (16) have sufficient bandwidth. The robotic flexible laparoscope holder system consists of the robot arm dynamics (1) with an RCM constraint (5) and a flexible laparoscope bending kinematics described by Eq. (16). The control law (23) achieves a decoupled uniformly ultimate boundedness on the control errors$etool$and$eRCM$.

Proof . Because the dynamic response for the robotic manipulator has sufficient bandwidth, it is reasonable to assume decoupled dynamics $q=qd$ [17]. Also, the kinematics relationship, which describes the bending tip control, does not affect the trajectory feedback control loop for control law (23). Rewrite (23) as
$q̂˙=Jcombine#K̂ê$
with $q̂=[q˙dλ˙]$, $K̂=[K1I3×3O3×30O3×3K2I3×30001]$, and $ê=[etooleRCMλ˙]$.▪

Following the argument in Ref. [25] and defining the Lyapunov function

$Vc=12êTK̂ê$

Note that $êT=[ptask−ptoolptrocar−pRCMλ˙]T$

$V˙c=êTK̂(q˙d−Jcombine#K̂ê)$
$V˙c≤‖ê‖λM(K̂)‖q˙d‖−‖ê‖2[λm(K̂)]2λm(Jcombine#)$

We have $V˙c<0$ for $αc/‖ê‖<1$, and $V˙c≤0$ for $αc/‖ê‖=1$ with

$αc=λM(K̂)‖q˙d‖[λm(K̂)]2λm(Jcombine#)$

The constraint tracking error $ê$ is uniformly ultimately bounded within the ball of radius

$ρ>αc$

Figure 11 shows the resulting overall control system architecture.

### 3.5 Surgeon Avoidance.

Consider the trocar entrance position (RCM) is fixed. The distance between joints 2 and 3 is $a2$, and the distance between joints 3 and 5 is $d4$. And the distance from Joint 5 to the fixed location is $d6$. One can carry out the Geogebra kinematic analysis to obtain the robot attitude plot.

Figure 12 shows the admissible range of motion for the robot. The large circle represents the distance $a2+d4$ from the origin on the $xy$-plane. The projection ranges from 0 to $a2+d4$. The small circle stands for the projection of Joint 5 to the end effector with a range of 0–$d6$. Consider the case when the nudge points toward joint 5 ($θ5$ in Fig. 12). Joint 5 should move away from the nudge along the black line in the following figures. Figure 13 shows the avoidance motions when the nudge force is at 20$deg$, 50$deg$, 90$deg$, 130$deg$, and 150$deg$ with the RCM is at $yr=47.8$, where $(xr, yr,zr)$ indicates its location. The shaded regions show the area without collisions. The admissible regions shrink dramatically in Fig. 14 when on lower the RCM to $yr=20$. If the nudge force points toward joint 3 ($θ3$), the entire robot arm can turn away from the nudge along the direction of the force. In this case, one needs to turn Joint 1 to move the entire arm away from the nudge.

To evaluate the proposed algorithm, we implemented the combined RCM control and image compensation with the industrial robot RA605-710 and examined both the simulation results and experimental results.

## 4 System Hardware Setup

The robotic arm in the setup is a silver RA605-710 six-axis robotic arm from HIWIN Technologies Corp. (Taiwan). The robot uses alternating current servomotors of the Panasonic MSME series with Panasonic A5B Ethernet Control Automation Technology (EtherCAT) Servo-Drivers for the joint actuators. The controller, a NET3600E-ECM industrial computer, can communicate with each driver through the EtherCAT interface. Moreover, this controller is based on the “NexRobotEdu” manipulator education kit executed under the RTX 2012 real-time system from NEXCOM, Taiwan.

For the flexible endoscope, we used the inexpensive MitCorp F500 system (MitCorp; Fig. 15), which offers 640 × 480 resolution images on a 3.5″ thin-film-transistor liquid crystal display screen monitor. The bending tip is driven by a rack-and-pinion assembly to drive the bending cable. Only one rack-and-pinion set allows the tip to bend along one direction with a maximum bending angle of 110 deg. MitCorp F500 is designed for manual operation with one navigation wheel. Therefore, to motorize the bending control, we modified the rack-and-pinion mechanism (Fig. 3(b)) by attaching a driving motor. The motor needed to be strong enough to drive the rack-and-pinion assembly, which in turn drove the bending cable. Hence, we used an MG996R servomotor operating at 4.8–6 V with an output torque of 9.8 kg·cm (at 4.8 V) to 11 kg·cm (at 6 V).

The RCM constraint, together with the task control, requires five degrees-of-freedom of the robotic arm. The sixth joint of the robotic arm can provide the extra degree-of-freedom to perform the angle of swing required to restore the image. The experimental setup is shown in Fig. 16, where the red tape on the cannula indicates the trocar position for the RCM, and the cross on the box indicates the point of interest for the bending tip to point at.

## 5 Implementation Results and Discussion

### 5.1 Simulation Results.

To verify the control strategy, we constructed a matlab simulation model to reflect the proposed scenario. The model comprises the articulated robot modeled by RVCtool and the flexible part constructed ourselves. In Fig. 17, the red dot indicates the location of the RCM, and the blue dot shows the position of interest on the image. The series of pictures in Fig. 17 illustrates how the robotic arm moves around the RCM while maintaining the line of sight to the image point. Figures 18(a) and 18(b) illustrate the variation in $α$ and $β$, respectively. Figure 19(a) shows the tracking errors of the image center. The image center tracking error was maintained within 0.1 mm.

To demonstrate the effect of the task control under force from a surgeon, we introduced a small virtual force of $(10j−10k)$ N in the task space to $p6$ for 0.1 s. Figure 19(a) image center tracking error. Figure 19(b) shows the $ptool$ simulation result of collision avoidance algorithm. It turns out that even the force on x-direction is zero, through the constraint of the trocar, the x-position of $ptool$ is affected, and that the robotic arm returned to the original posture after a nudge.

### 5.2 Experimental Results.

The aforementioned simulation results are indicative of an ideal case scenario. As described in Sec. 3, the sixth axis of the robotic arm can provide the required tip rotation. However, because this method was affected by the upper limit of the joint speed of the real system, our method could not address large positive or negative variations in the angles. In the experiment with an actual operation, the sixth axis moved to the rotation angle $α$ of the fixed configuration once the end of the rigid cannula had reached the task point.

As described in Sec. 2, the endoscope was attached to the end of the sixth axis of the robotic arm. To verify that the manipulator system could complete the task under conditions similar to those in a human abdominal cavity, we used an acrylic box to represent an abdominal cavity prosthesis. The circular hole with a diameter of 15 mm on top of the box mimicked a ventral insertion point. The initial posture of the arm was $q0=[0,46.4 deg,−14 deg,0 25 deg,0]T$ in the joint space, and the endoscope trocar location coinciding the insertion point was $ptrocar$=$[804.9,0,949.0]T$ in the task space, with the unit being millimeters. The initial distance between the endoscope lens and target (marked with the black cross) was 85 mm. The experimental setup is shown in Fig. 19. There were three parts to the motion command: In maneuver A, the end of the trocar cannula moved +80 mm in the z-direction (assuming the doctor would like to observe the target from the top). In maneuver B, the end of the trocar cannula moved +90 mm in the y-direction (assuming the doctor now wanted to observe the target from the side). Finally, in maneuver C, the end of the cannula simultaneously moved +10 mm in the x-direction, +70 mm in the y-direction, and +20 mm in the z-direction. All commands are smoothed fifth-order polynomial with zero initial acceleration, velocity and zero final acceleration, velocity. All commands are done within 3 s. A movie of maneuver C is available as an attachment to this paper.

Figures 20 and 21 show the experimental results of maneuvers A and B, respectively. In Fig. 20, the first column shows the posture of the robot and the flexible endoscope at 0, 5, 11, and 14 s. The robot starts to lower its arm to raise the cannula in the fifth second. The image also begins to move away from the center (the second row). At the eleventh second, the robot completed the move; however, the image is still below the center. Finally, at the 14th second, the flexible endoscope bends back to restore the image to the center. One can see that the bending control took longer because the rack-and-pinion-cable mechanism suffers from delay and backlash.

Figure 21 shows the robot posture and the endoscopic image at 0, 5, 7, and 9th sec for maneuver B. The robot gradually moves sideways, as shown in the first column. The endoscope could not catch up with the movements and missed the target in the second row. On the third and fourth row, the flexible endoscope gradually bends back and points the lens to the point of interest. Figure 22 shows the bending of the tip for maneuver B. The images demonstrate how the tip bent after maneuver B to restore the cross to the center of the image. Figure 23 presents the experimental result for maneuver C. As the end of the cannula moves in all three directions in the task space, the image was considerably tilted, with unsatisfactory center restoration. The robotic arm, however, never violated the RCM constraint during the entire process. Because the endoscope only offers one degree-of-freedom bending, the sixth axis of the robot manipulator is used to drive the $α$ angle. One can see from Fig. 23 that the picture becomes tilted. This tilt can be minimized with a two-degree-of-freedom bending tip.

Among the three experiment results, the errors of maneuver C are the largest. The trajectory tracking errors for the end of the cannula in Fig. 24 are smaller than 10 mm in all three directions. The RCM error in Fig. 25 also remains within 0.8 mm in all three directions.

### 5.3 Discussion.

Although there are some errors in the simulation and experience, the function of the system still works as we want. First, both simulation and experience show that the RCM errors are smaller than 1 millimeter, which is allowable since the hole diameter of the trocar is about 15 millimeters wide. Second, the large errors of $ptool$ happen in the transient state of motion, and the errors converge to zero within 4 s, which is no longer than the time required to manually operate the endoscope. Finally, the point of interest remaining around the center of the image meets the requirement we setup (Fig. 26).

## 6 Conclusion

In this study, we designed a robotic flexible endoscope-holder system as follows. First, we used a kinematic formulation to establish the RCM constraint and translate it into an augmented control loop for implementation. Second, a combination of task control and impedance control was used to enable the endoscope holder to become partially compliant to the external force exerted by the surgeon. Third, we modified a flexible endoscope, connecting it to the robotic arm for automatic image relocating and installing a motor for driving the rack-and-pinion mechanism of the bending tip. We derived an expression for the tip bending angle required to restore the center of the image. By using the relationship between the bending angle and angular displacement of the driving pinion, we then proposed a kinematic model for the image center control. The overall flexible endoscope control finally involved combined kinematic–dynamic control.

In conclusion, the hardware and software architecture of the endoscope and robotic arm were combined, and both simulation and experimental verification were conducted. The simulation results demonstrated how the system responds to an external force while maintaining both the RCM and the line of sight. The experimental setup consisted of an RA605-710 robot with a MitCorp F500 flexible endoscope. The testing results demonstrated that the experimental endoscope holder could maintain a trajectory error of <10 mm for the end of the cannula and an RCM error of <0.8 mm in all three spatial directions.

There are quite a few issues that remain for future development. The budget limit prevented us from using a regular two-degree-of-freedom flexible laparoscope. The one-degree-of-freedom endoscope with the sixth axis leads to the image tilting when the arm moves. One possible remedy is to online image processing to maintain image orientation. Also, the backlash and tolerance in the experimental setup introduce undesirable oscillation and error. The problem may not remain an issue if the endoscope is guided through the incision trocar. This research used only a six-axes load cell to sense the surgeon's nudge. It is desirable to check into the use of artificial skin for this purpose.

## Funding Data

• Ministry of Science and Technologies, Taiwan (Grant No. 108-2221-E-002-149-MY3; Funder ID: 10.13039/100007225).

• National Taiwan University (Grant No. 109L890907; Funder ID: 10.13039/501100006477).

## References

References
1.
Berkelman
,
P.
,
Boidard
,
E.
,
Cinquin
,
P.
, and
Troccaz
,
J.
,
2003
, “
LER: The Light Endoscope Robot
,”
IEEE/RSJ International Conference on Intelligent Robots and Systems
(
IROS 2003
), Las Vegas, NV, Oct. 27–31, pp.
2835
2840
.10.1109/IROS.2003.1249300
2.
Long
,
J.-A.
,
Cinquin
,
P.
,
Troccaz
,
J.
,
Voros
,
S.
,
Berkelman
,
P.
,
Descotes
,
J.-L.
,
Letoublon
,
C.
, and
Rambeaud
,
J.-J.
,
2007
, “
Development of Miniaturized Light Endoscope-Holder Robot for Laparoscopic Surgery
,”
J. Endourology, Artic.
,
21
(
8
), pp.
911
914
.10.1089/end.2006.0328
3.
Kim
,
S. H.
,
Seo
,
J. T.
,
Woo
,
J.
, and
Yi
,
B. J.
,
2017
, “
Design of Endoscope Holder Mechanism and Controller for a Laryngeal Surgical Robotic System
,”
14th International Conference on Ubiquitous Robots and Ambient Intelligence, URAI
, Jeju, South Korea, June 28–July 1, pp.
815
817
.10.1109/URAI.2017.7992832
4.
Lefebvre
,
T.
,
Xiao
,
J.
,
Bruyninckx
,
H.
, and
De Gersem
,
G.
,
2005
, “
Active Compliant Motion: A Survey
,”
,
19
(
5
), pp.
479
499
.10.1163/156855305323383767
5.
Hannaford
,
B.
,
Rosen
,
J.
,
Friedman
,
D. W.
,
King
,
H.
,
Roan
,
P.
,
Cheng
,
L.
,
Glozman
,
D.
,
Ma
,
J.
,
Nia Kosari
,
S.
, and
Whiet
,
L.
,
2013
, “
Raven-II: An Open Platform for Surgical Robotics Research
,”
IEEE Trans. Biomed. Eng., Artic.
,
60
(
4
), pp.
954
959
. 10.1109/TBME.2012.2228858
6.
Larocca
,
V.
,
Marino
,
F.
,
Filippis
,
A.
,
Gidaro
,
S.
, and
Lococo
,
A.
,
Spirito Santo Hospital, Pescara, Italy
2014
, “
A New Operative Telesurgical System: Telelap ALF-X-Experimental Study on Animal Model
,”
J. Adv. Biotechnol. Bioeng.
,
2
(
1
), pp.
12
15
.10.12970/2311-1755.2014.02.01.2
7.
Guthart
,
G. S.
, and
Salisbury
,
K. J.
, Jr.
,
2000
, “
Intuitive Telesurgery System: Overview and Application
,”
Proceedings IEEE International Conference on Robotics and Automation
, Vol.
1
, San Francisco, CA, Apr. 24–28, pp.
618
621
.10.1109/ROBOT.2000.844121
8.
Sun
,
L. W.
,
Van Meer
,
F.
,
Bailly
,
Y.
, and
Yeung
,
C. K.
,
2007
, “
Design and Development of a da Vinci Surgical System Simulator
,”
Proceedings of the IEEE International Conference on Mechatronics and Automation, ICMA
, Harbin, China, Aug. 5–8, pp.
1050
1055
.10.1109/ICMA.2007.4303693
9.
Kuo
,
C.-H.
, and
Dai
,
J. S.
,
2009
,
Robotics for Minimally Invasive Surgery: A Historical Review From the Perspective of Kinematics
,
Springer
,
Dordrecht, The Netherlands
.
10.
Chan
,
J. Y. K.
,
Leung
,
I.
,
Navarro-Alarcon
,
D.
,
Lin
,
W.
,
Li
,
P.
,
Lee
,
D. L. Y.
,
Liu
,
Y-h.
, and
Tong
,
M. C. F.
,
2016
, “
Foot-Controlled Robotic-Enabled Endoscope Holder for Endoscopic Sinus Surgery: A Cadaveric Feasibility Study
,”
Laryngoscope, Artic.
,
126
(
3
), pp.
566
569
.10.1002/lary.25634
11.
He
,
Y.
,
Zhang
,
P.
,
Jin
,
H.
,
Hu
,
Y.
, and
Zhang
,
J.
,
2016
, “
Type Synthesis for Remote Center of Motion Mechanisms Based on Coupled Motion of Two Degrees-of-Freedom
,”
ASME J. Mech. Des.
,
138
(
12
), p. 122301.10.1115/1.4034301
12.
Wilson
,
J. T.
,
Tsao
,
T. C.
,
Hubschman
,
J. P.
, and
Schwartz
,
S.
,
2010
, “
Evaluating Remote Centers of Motion for Minimally Invasive Surgical Robots by Computer Vision
,”
IEEE/ASME International Conference on Advanced Intelligent Mechatronics, AIM
, Montreal, QC, Canada, July 6–9, pp.
1413
1418
.10.1109/AIM.2010.5695924
13.
Zorn
,
L.
,
Nageotte
,
F.
,
Zanne
,
P.
,
Legner
,
A.
,
Dallemagne
,
B.
,
Marescaux
,
J.
, and
de Mathelin
,
M.
,
2018
, “
A Novel Telemanipulated Robotic Assistant for Surgical Endoscopy: Preclinical Application to ESD
,”
IEEE Trans. Biomed. Eng.
,
65
(
4
), pp.
797
808
.10.1109/TBME.2017.2720739
14.
CMR Surgical
,
2020
, “Surgical Teams—Operate Your Way,” CMR Surgical Ltd., Cambridge, UK, accessed May 14, 2020, https://cmrsurgical.com/versius/surgical-teams/
15.
Marinho
,
M. M.
,
,
K.
, and
Mitsuishi
,
M.
,
2017
, “
Comparison of Remote Center-of-Motion Generation Algorithms
,”
SII 2017—2017 IEEE/SICE International Symposium on System Integration
, Taipei, Taiwan, Dec. 11–14, pp.
668
673
.10.1109/SII.2017.8279298
16.
Azimian
,
H.
,
Patel
,
R. V.
, and
Naish
,
M. D.
,
2010
, “
On Constrained Manipulation in Robotics-Assisted Minimally Invasive Surgery
,”
Third IEEE RAS and EMBS International Conference on Biomedical Robotics and Biomechatronics, BioRob 2010
, Tokyo, Japan, Sept. 26–29, pp.
650
655
.10.1109/BIOROB.2010.5627985
17.
Aghakhani
,
N.
,
Geravand
,
M.
,
Shahriari
,
N.
,
Vendittelli
,
M.
, and
Oriolo
,
G.
,
2013
, “
Task Control With Remote Center of Motion Constraint for Minimally Invasive Robotic Surgery
,”
Proceedings—IEEE International Conference on Robotics and Automation
, Karlsruhe, Germany, May 6–10, pp.
5807
5812
.10.1109/ICRA.2013.6631412
18.
Sandoval
,
J.
,
Poisson
,
G.
, and
Vieyres
,
P.
,
2017
, “
A New Kinematic Formulation of the RCM Constraint for Redundant Torque-Controlled Robots
,”
IEEE International Conference on Intelligent Robots and Systems
, Vancouver, BC, Canada, Sept. 24–28, pp.
4576
4581
.10.1109/IROS.2017.8206326
19.
Pham
,
C. D.
,
Coutinho
,
F.
,
Leite
,
A. C.
,
Lizarralde
,
F.
,
From
,
P. J.
, and
Johansson
,
R.
,
2015
, “
Analysis of a Moving Remote Center of Motion for Robotics-Assisted Minimally Invasive Surgery
,”
IEEE International Conference on Intelligent Robots and Systems
, Hamburg, Germany, Sept. 28–Oct. 2, pp.
1837
1842
.10.1109/IROS.2015.7353557
20.
Osa
,
T.
,
Staub
,
C.
, and
Knoll
,
A.
,
2010
, “
Framework of Automatic Robot Surgery System Using Visual Servoing
,”
IEEE/RSJ 2010 International Conference on Intelligent Robots and Systems, IROS—Conference Proceedings
, Taipei, Taiwan, Oct. 18–22, pp.
1837
1842
.10.1109/IROS.2010.5650301
21.
Marinho
,
M. M.
,
Bernardes
,
M. C.
, and
Bo
,
A. P. L.
,
2016
, “
Using General-Purpose Serial-Link Manipulators for Laparoscopic Surgery With Moving Remote Center of Motion
,”
J. Med. Rob. Res.
,
01
(
04
), p.
1650007
.10.1142/S2424905X16500070
22.
Ott
,
L.
,
Nageotte
,
F.
,
Zanne
,
P.
, and
Mathelin
,
M. D.
,
2011
, “
Robotic Assistance to Flexible Endoscopy by Physiological-Motion Tracking
,”
IEEE Trans. Rob.
,
27
(
2
), pp.
346
359
.10.1109/TRO.2010.2098623
23.
Webster
,
R. J.
, III.
, and
Jones
,
B. A.
,
2010
, “
Design and Kinematic Modeling of Constant Curvature Continuum Robots: A Review
,”
Int. J. Rob. Res.
,
29
(
13
), pp.
1661
1683
.10.1177/0278364910368147
24.
Degirmenci
,
A.
,
Loschak
,
P. M.
,
Tschabrunn
,
C.
,
Anter
,
M. E.
, and
Howe
,
R. D.
,
2016
, “
Compensation for Unconstrained Catheter Shaft Motion in Cardiac Catheters
,”
Proceedings—IEEE International Conference on Robotics and Automation
, Stockholm, Sweden, May 16–21, pp.
4436
4442
.10.1109/ICRA.2016.7487643
25.
Chiacchio
,
P.
,
Chiaverini
,
S.
,
Sciavicco
,
L.
, and
Siciliano
,
B.
,
1991
, “
Closed-Loop Inverse Kinematics Schemes for Constrained Redundant Manipulators With Task Space Augmentation and Task Priority Strategy
,”
Int. J. Rob. Res., Artic.
,
10
(
4
), pp.
410
425
.10.1177/027836499101000409