Human–Robot Cooperative Control Based on Virtual Fixture in Robot-Assisted Endoscopic Sinus Surgery

Abstract

In endoscopic sinus surgery, the robot assists the surgeon in holding the endoscope and acts as the surgeon’s third hand, which helps to reduce the surgeon’s operating burden and improve the quality of the operation. This paper proposes a human–robot cooperative control method based on virtual fixture to realize accurate and safe human–robot interaction in endoscopic sinus surgery. Firstly, through endoscopic trajectory analysis, the endoscopic motion constraint requirements of different surgical stages are obtained, and three typical virtual fixtures suitable for endoscopic sinus surgery are designed and implemented. Based on the typical virtual fixtures, a composite virtual fixture is constructed, and then the overall robot motion constraint model is obtained. Secondly, based on the obtained robot motion constraint model, a human–robot cooperative control method based on virtual fixture is proposed. The method adopts admittance control to realize efficient human–robot interaction between the surgeon and robot during the surgery; the virtual fixture is used to restrain and guide the motion of the robot, thereby ensuring motion safety of the robot. Finally, the proposed method is evaluated through a robot-assisted nasal endoscopy experiment, and the result shows that the proposed method can improve the accuracy and safety of operation during endoscopic sinus surgery.

1. Introduction

Traditional endoscopic sinus surgery is usually performed by a surgeon with a left-handed nasal endoscope and right-handed surgical instruments. The surgeon’s hands are prone to fatigue and shaking due to the long operation, which may cause misoperation. Additionally, for some complicated operations, the assistant surgeon is required to hold the endoscope while the chief surgeon operates with both hands. Surgical operations between surgeons are easily interfered, and coordination is difficult. Introducing a robot to assist the surgeon to hold the endoscope, acting as the surgeon’s third hand, can combine the advantages of the robot with the surgeon’s experience, so that the surgeon can change the one-hand operation mode to a two-hand operation mode, thus the stability of the operation can be enhanced and the quality and safety of the operation can be improved.

Currently available robots for endoscopic sinus surgery can be divided into two categories: Passive surgical robots and active surgical robots. Active surgical robots include single-arm surgical robots, multi-arm surgical robots, and single-port type surgical robots. From the perspective of control, there are mainly three control modes. The first mode is the passive control mode, in which the surgeon locks or loosens the robot joints by operating buttons, pedals, etc., and constantly drags and adjusts the posture of the robot. This passive control mode is simple and reliable, and can meet the requirements of minimally invasive surgery to a certain extent, for example, the surgical robot Tiska [1] developed by Karl Storz, the Point Setter and UniARM [2,3] developed by Mitaka, and the EndoArm [4,5,6] developed by Olympus. Although the above passive control mode can basically realize the function of assisting surgeon in holding the endoscope in surgery, the robot still needs the surgeon to manually operate the button, pedal, etc. to loosen or lock all the joints, and the control has low efficiency and low intelligence. The second mode is the active control mode, in which the surgeon’s head movement and voice are used to control the motion of the robot. For example, the active surgical robot, Aesop [7,8,9,10], can use the voice control mode to drive the endoscope at the end of the robot to the desired spatial pose; the surgical robot, Endo Assist [11,12], can control the motion of the endoscope at the end of the robot with the movement of the surgeon’s head. The third mode is the master-slave control mode, which is often used in multi-arm surgical robot systems, such as Laprotek [13,14], Zeus [15,16,17], Da Vinci [18,19], Miro Surge [20,21,22,23], and Raven [24,25]. The surgeon controls the slave robot arms at the main console, and switches the control of each slave arm by pedals. The ratio of the surgeon’s input motion to the output motion of the robot can be adjusted in the master-slave control mode, achieving finer operation than the human hand. At the same time, the signal-filtering algorithm can eliminate the influence of the surgeon’s hand tremor, improving the quality of surgery. According to the above analysis, the passive control mode is simple and reliable, but the surgeon needs to manually adjust the posture of the nasal endoscope during the operation; the adjustment is awkward and has a low level of automation. The master-slave control mode is generally applicable on active surgical robots in the form of a multiple-arm, such as the Da Vinci robot. Considering that the end-effector of the multi-arm form robot is generally large in size, it can only be applied to the types of surgery that have a large operation space, such as abdominal cavity surgery, and it cannot be applied to types of surgery with a small operation space, such as the nasal cavity. For the active control mode based on the voice and head movement, the human–robot interaction efficiency is low, and the training time is long, which requires further research.

For robot-assisted endoscopic sinus surgery, due to the narrow and complex anatomy of the nasal cavity, the endoscope easily collides and interferes with the nasal tissue. To ensure the safety of the operation, it is necessary to restrain the movement of the endoscope at the end of the robot. Presently, there are two main motion constraint methods for robots: Hardware constraint based on robot structure design and software constraint based on algorithms. Hardware-based constraint is generally considered to be more safe, but often less flexible, such as RCM (remote center of motion) constraint. VF (virtual fixture) represents one of the most important software-based constraints, which was proposed by Professor Rosenberg [26,27] in 1993. VF is a constraint method that uses an algorithm to generate invisible spatial motion constraint, thereby limiting robot motion. Current VF can be divided into GVF (guiding virtual fixture) and FRVF (forbidden regions virtual fixture) [28]. A GVF is generally used to guide the end-effector of the robot to move along a predefined trajectory to avoid interference; an FRVF is generally used to prevent the end-effector of the robot from entering a specific area and can be used to protect critical tissues and organs during surgery. Park et al. [29] prevented the robot end-effector from entering a specific area by constructing a VF "virtual wall" in robot-assisted cardiac surgery and verified the effectiveness of the VF through simulation. Prada et al. [30] proposed a VF library design method by introducing VF into the virtual reality system. Ren et al. [31] proposed a dynamic VF design method for minimally invasive cardiac surgery, in which the interpolation algorithm was used to calculate the dynamic VF at different moments in the heartbeat cycle to avoid the probe entering the dangerous area during operation. Hu et al. [32] restricted the movement of the end-effector of a spinal surgical robot by introducing a GVF (straight type) and an FRVF (cone type) to improve the safety of the operation. Li et al. [33,34,35] used an optimization constraint algorithm to generate VFs to solve the problem of a narrow operating space in endoscopic sinus surgery, but the algorithm is too complicated and does not consider nasal soft tissue. In summary, the existing VF research is mainly based on regular geometric shapes. Due to the complex and narrow anatomical structure of the human nasal cavity, the use of regular geometric shapes as a motion constraint will result in the loss of substantial valuable operation space, which is not allowed in the surgical operation. Therefore, it is urgent that the problem regarding how to properly fit the VF model with a 3D anatomical structure of the human nasal cavity is solved and the safety constraint of the nasal endoscope’s movement is realized.

Based on the above control mode and safety constraint analysis, considering the current clinical requirements of endoscopic sinus surgery, this paper proposes a human–robot cooperative control method based on VFs in robot-assisted endoscopic sinus surgery. This method uses admittance-based cooperative control to realize human–robot interaction between the surgeon and robot during surgery; the VFs are used to restrain and guide the movement of the robot, thereby ensuring the movement safety of the nasal endoscope. The experimental results showed that the robot can assist the surgeon in holding the endoscope in the endoscopic sinus surgery with the proposed method, and the surgical operation is more accurate and safer during surgery.

The remainder of this paper is organized as follows. The robot motion constraint model based on VF is presented in Section 2. The human–robot cooperative control method is detailed in Section 3. The experiment and discussion are presented in Section 4, and the conclusions are summarized in Section 5.

2. Robot Motion Constraint Model Based on Virtual Fixture

The narrow and complex anatomical structure of the nasal sinus makes it easy for the endoscope at the end of the robot to interfere with the nasal tissue, and the entire surgical operation space is very crowded. To ensure the safety of the surgical operation, it is necessary to restrain the movement of the endoscope. The VF can construct invisible "clamps" (e.g., spatial curves and surfaces) to assist the operator to improve operational efficiency and accuracy. In this section, the robot motion constraint requirements for each motion stage during endoscopic sinus surgery are determined based on the endoscope’s trajectory analysis. Next, based on the obtained motion constraint requirements, three typical VF models suitable for endoscopic sinus surgery are constructed. Finally, based on the three typical VF models, a composite VF model is constructed to achieve the robot constraint for the whole endoscopic sinus surgery.

2.1. Motion Constraint Requirements Analysis

Generally, the trajectory of the endoscope at the end of the robot in endoscopic sinus surgery is shown in Figure 1.

It can be seen from Figure 1 that the movement of the endoscope can be mainly divided into four stages:

The first stage is from point A to point B: The endoscope moves from a safe area away from the patient (point A) to the vicinity of the patient’s nasal inlet (point B). In this stage, since the endoscope is far away from the patient, it is not easy to collide with the patient. Therefore, it is only necessary to construct a plane-based FRVF to prevent the endoscope from colliding with the patient.

The second stage is from point B to point C: When the endoscope moves further from point B to point C, the endoscope gradually approaches point C at the patient’s nasal inlet. To prevent the endoscope from colliding with the patient during this stage, a single-leaf hyperboloid based FRVF is constructed to guide and constrain the movement of the endoscope to prevent it from colliding with the patient’s nasal inlet.

The third stage is from point C to point D: When the endoscope moves from point C at the inlet of the nose to point D in the surgical area, the endoscope movement path is in the nasal cavity of the patient. Due to the narrow and complex anatomy of the nasal cavity, the endoscope easily collides with the nasal tissue during surgery; thus, a VF based on a spatial curve is constructed to guide the motion of the robot.

The fourth stage: After the endoscope reaches the surgical area, the surgeon constantly adjusts the endoscope posture to obtain a suitable surgical vision field according to the needs of the operation. To prevent collision and interference between the endoscope and nasal tissue, an FRVF based on the anatomical structure of the nasal cavity is constructed to constrain the motion of the endoscope.

2.2. Virtual Fixtures for Endoscopic Sinus Surgery

Based on the above robot motion constraint requirement analysis, the main types of VFs in endoscopic sinus surgery can be obtained as follows: GVF based on the spatial curve, FRVF based on the plane and hyperboloid, and FRVF based on the anatomical structure of the nasal cavity. Their action mechanisms and implementation processes are shown below.

2.2.1. Guiding Virtual Fixture Based on Spatial Curve

For the geometry of GVFs, there are mainly straight line, two-dimensional curve, three-dimensional curve, etc. However, for the robot system, considering the smoothness and computational complexity of the motion trajectory, the cubic polynomial spatial curve is mostly used.

As shown in Figure 2a, the schematic diagram of the GVF based on a spatial curve is shown. Assuming that the coordinates of the robot end are $p(x,y,z)$, the velocity of the robot end is $V_{in}$, and the curve, S, is the guiding spatial curve, the velocity of the robot end can be decomposed into the velocity along the tangential direction and velocity in the direction perpendicular to the tangential direction:

$$\{\begin{array}{c}{V}_{in}=V_{in}^{\parallel }+V_{in}^{\perp }\\ V_{in}^{\parallel }=T·V_{in}\\ V_{in}^{\perp }=V_{in}-V_{in}^{\parallel }\end{array}$$

(1)

Introducing a guiding stiffness coefficient, $k\in [0,1]$, to scale the velocity in the perpendicular tangential direction, the following is obtained:

$$V_{out}^{\perp }=k·V_{in}^{\perp }$$

(2)

Thus, the velocity of the robot end after the action of the GVF can be obtained as follows:

$$V_{out}=V_{in}^{\parallel }+k·V_{in}^{\perp }$$

(3)

For the above GVF, different guiding requirements can be achieved by setting different values of the guiding stiffness coefficient. When $k=1$, the velocity of the robot end remains unchanged before and after guidance, the VF does not work, and the robot is in a free motion state. When $0<k<1$, the velocity in the perpendicular tangential direction of the robot end is reduced, so the robot will be directed to the curve, S. When $k=0$, the velocity of the robot end in the perpendicular tangential direction is completely removed and the robot end will be forced to move in the tangential direction of the curve, S. From the above, the smaller the guiding stiffness coefficient is, the stronger the motion guidance to the robot end obtained.

The above situation is the velocity guidance under ideal conditions. In practical applications, the end of the robot is generally not on the spatial curve, so that the motion trajectory of the robot end will be a curve parallel to the guiding curve.

Figure 2b shows the action of the GVF when the robot end is not on the guiding curve. Assume $P^s$ is the closest point on the curve to the robot end, then:

$$P^s=\mathrm{Mindistance}(S,P)$$

(4)

Adding a regression coefficient, $\lambda$, then the regression velocity can be calculated as follows (direction is pointed to $P^s$ from $P$):

$$V_s=\lambda ·\overline{{\mathrm{PP}}^{\mathrm{s}}}$$

(5)

By adjusting the regression coefficient, the velocity of the robot end returning to the curve can be adjusted, and then the final velocity can be obtained:

$$V_{out}^s=V_{out}+V_s$$

(6)

From the above analysis, it can be seen that different guiding requirements of the GVF can be realized by adjusting the guiding stiffness coefficient, k, and the regression coefficient, $\lambda$.

The above derivation process of the GVF based on the spatial curve can be applied to the straight line, the two-dimensional curve, and so on because they can be regarded as a special case of the spatial curve.

2.2.2. Forbidden Virtual Fixture Based on Hyperboloid

The FRVF can limit the operating range of the robot and prevent the robot from entering a specific area, thereby avoiding collision between the instrument and tissue during operation. For the FRVF, how to construct appropriate geometries to constrain the workspace of the robot is an important issue to be considered in the application of the surgical robot. Considering that the endoscope and instrument swing around the nostril to form a workspace similar to the "funnel" shape [36], a single-leaf hyperboloid is used as the geometry of the FRVF.

The schematic diagram of the FRVF based on a single-leaf hyperboloid is shown in Figure 3. Assuming that the position of the robot end is $P(x,y,z)$, and the velocity of the robot end is $V_{in}$ (direction as $\overline{\mathrm{PD}}$), the equation of the single-leaf hyperboloid, S, is shown below:

$$\frac{x^2}{a^2}+\frac{y^2}{b^2}-\frac{z^2}{c^2}=d$$

(7)

The action process of the FRVF based on a single-leaf hyperboloid is as follows:

1) According to the position of the robot end and Equation (7), the spatial relationship between $P(x,y,z)$ and the single-leaf hyperboloid is judged.

2) If $\frac{x^2}{a^2}+\frac{y^2}{b^2}\le d+\frac{z^2}{c^2}$, the robot end, $P(x,y,z)$, is inside the single-leaf hyperboloid, the FRVF does not work, and the robot end is in the free motion state, its velocity, $V_{in}$, remains unchanged.

3) If $\frac{x^2}{a^2}+\frac{y^2}{b^2}>d+\frac{z^2}{c^2}$, the robot end is outside the single-leaf hyperboloid, the velocity of the robot end will be changed by the FRVF. If $V_{in}·\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}>0$, the velocity of the robot end is in the same direction as the normal vector, $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}$, of the tangent plane, $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{AC}}$, indicating that the robot end is leaving the prohibited area of the VF; if $V_{in}·\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}<0$, the velocity of the robot end is opposite to the normal vector, $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}$, of the tangent plane, $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{AC}}$, indicating that the robot is entering the prohibited area of the VF. In the above two cases, the velocity of the robot needs to be adjusted.

By projecting the velocity vector, $V_{in}$, onto the normal, $V_{in}{}^{\perp }$, and tangent planes, $V_{in}{}^{\parallel }$ (the $V_{in}{}^{\perp }$ direction is along the direction of $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}$, the $V_{in}{}^{\parallel }$ direction is along the direction of $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{D}\prime }$), the following can be obtained:

$$\{\begin{array}{l}{V}_{in}=V_{in}{}^{\parallel }+V_{in}{}^{\perp }\\ V_{in}{}^{\perp }=V_{in}·normal(\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}})\\ V_{in}{}^{\parallel }=V_{in}-V_{in}{}^{\perp }\end{array}$$

(8)

Introducing the forbidden stiffness coefficient, $k\in \left[-1,1\right]$, the input velocity can be expressed as follows:

$$V_{in}=V_{in}{}^{\parallel }+k·V_{in}{}^{\perp }$$

(9)

When $V_{in}{}^{\perp }$ and $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}$ is in the same direction, if $k<0$, the robot end will continue to enter the prohibited area; if $k\ge 0$, the robot end will leave the prohibited area. When $V_{in}{}^{\perp }$ and $\overline{{\mathrm{P}}^{\mathrm{s}}\mathrm{B}}$ is in the reverse direction, if $k<0$, the robot end will leave the prohibited area; if $k\ge 0$, the robot end will continue to enter the prohibited area. In the whole process, the velocity of the robot can be adjusted to leave the forbidden area by applying different values of $k$.

The above derivation process of the FRVF based on the single-leaf hyperboloid is applicable to other three-dimensional surface geometries, such as the plane, except that the method of solving the tangent plane of the nearest point is different.

2.2.3. Forbidden Virtual Fixture Based on the Anatomical Structure of the Nasal Cavity

When the robot end enters the inlet of the nasal cavity, due to the complex anatomy of the nasal cavity, the VF based on regular geometry (e.g., cylinder and cone) is used to constrain the movement of the robot, which will lose substantial precious operation space, and is not allowed during endoscopic sinus surgery. Therefore, in this section, a point cloud model is obtained based on the CT image sequence of the patient’s nasal tissue, based on which an FRVF suitable for the nasal cavity is constructed. The boundary of the FRVF coincides with the boundary of the nasal tissue, thereby maximally retaining the operation space while protecting the nasal tissue.

Figure 4 shows the schematic diagram of the FRVF based on the anatomical structure of the nasal cavity.

The FRVF works as follows:

1) Assuming that the position of the endoscope tip is $P(x,y,z)$, the corresponding pixel point, $Q(i,j,k)$, of the endoscope tip in the binarized point cloud model can be obtained as follows:

$$i=\mathrm{INT}((x-origin.x)/space.x)$$

(10)

$$j=\mathrm{INT}((y-origin.y)/space.y)$$

(11)

$$k=\mathrm{INT}((z-origin.z)/space.z)$$

(12)

where $origin.x$, $origin.y$, and $origin.z$ represent the origin coordinates of the binarized point cloud model of nasal tissue; $space.x$, $space.y$, and $space.z$ represent the pixel spacing in the x, y, and z coordinate directions of the nasal tissue binarized point cloud model; and INT() represents the rounding function.

2) Assuming that the search radius of the endoscope tip is R, it can be determined that the local search range of the collision detection is:

$$\overline{i}=[-\mathrm{INT}(R/space.x)+i,\mathrm{INT}(R/space.x)+i]$$

(13)

$$\overline{j}=[-\mathrm{INT}(R/space.y)+j,\mathrm{INT}(R/space.y)+j]$$

(14)

$$\overline{k}=[-\mathrm{INT}(R/space.z)+k,\mathrm{INT}(R/space.z)+k]$$

(15)

3) For the pixels in the local search range, the Euclidean distance from the pixels to the endoscope tip is calculated as follows:

$$Distance=sqrt({((\overline{i}-i)·space.x)}^2+{((\overline{j}-j)·space.y)}^2+{((\overline{k}-k)·space.z)}^2)$$

(16)

4) If the pixel value of the local pixel is 0, it indicates the pixel is a free spatial point and is shown with the black color in Figure 4. There is no collision in this case, the endoscope is in a free motion state, and the FRVF does not work. If the pixel value of the local pixel is 255, it indicates the pixel is a nasal tissue point and is shown with the white color in Figure 4. There is a collision in this case. Since the surface of the human nasal sinus is a thin layer of mucous membrane, considering the safety of the operation, the velocity of the endoscope is set to zero, which means the robot immediately stops moving, protecting the nasal tissue of the patient.

2.3. Overall Robot Motion Constraint Model Based on Virtual Fixture

Through the analysis in the above section, several typical VF types suitable for endoscopic sinus surgery have been obtained. Considering that it is difficult for a single VF to meet the diverse constraint requirements during different stages of robot-assisted endoscopic sinus surgery, a composite VF is constructed based on the above typical VFs, and the overall robot motion constraint model is obtained, as shown in Figure 5.

After multiple VFs are combined, the working condition and scope of each basic VF have been changed to avoid conflicts among the basic VFs. Based on the robot-assisted endoscopic sinus surgery procedure, the process of the composite VF system is shown in Figure 6.

The detailed process of the composite VF is as follows:

1) Enter the coordinates, $P(x,y,z)$, and velocity, $V_{in}$, of the endoscope tip in the composite VF system.

2) Calculate the directed distance, $D_{P2Plane}$, from the endoscope tip to the constraint plane (plane normal direction is positive). If $D_{P2Plane}\ge 0$, indicating the endoscope tip is above the constraint plane, the robot can move freely in all directions, and go to step (7); if $D_{P2Plane}<0$, indicating the endoscope tip is below the constraint plane, go to step (3).

3) Calculate the spatial relationship between the endoscope tip and nasal entrance point (RCM point) (the direction of the single-leaf hyperboloid opening is positive). If $D_{P2\mathrm{RCM}}\ge 0$, it means that the endoscope tip is above the nasal entrance point, go to step (4); if $D_{P2\mathrm{RCM}}<0$, it means that the endoscope tip is below the nasal entrance point, go to step (5).

4) Calculate the spatial relationship between the endoscope tip and single-leaf hyperboloid (if the endoscope tip is outside the single-leaf hyperboloid, the $D_{P2Surface}$ is positive). If $D_{P2Surface}\ge 0$, it means the endoscope tip is outside the single-leaf hyperboloid, go to step (6); if $D_{P2Surface}<0$, it means the endoscope tip is inside the single-leaf hyperboloid, the robot can move freely in all directions, go to step (7).

5) The endoscope enters the RCM restraint state, and the movement of the endoscope will be constrained to two rotational degrees of freedom around the RCM point, go to step (7).

6) Compare the distance from the endoscope tip to the constraint plane and distance from the endoscope tip to the single-leaf hyperboloid. If $D_{P2Surface}\ge D_{P2Plane}$, it means the endoscope tip is close to the constraint plane, and the plane constraint is used to deal with the velocity of the endoscope tip; if $D_{P2Surface}<D_{P2Plane}$, it means the endoscope tip is close to the single-leaf hyperboloid, and the velocity of the endoscope tip is processed by the single-leaf hyperboloid constraint. Go to step (7).

7) Calculate the distance between the endoscope tip and the point cloud model of the nasal tissue. If $D_{P2Cloud}>0$, it means that there is no interference between the endoscope tip and nasal tissue, and the robot can move freely in all directions; if $D_{P2Cloud}\le 0$, it means that the endoscope tip and nasal tissue are interfering. The robot stops moving.

3. Human–Robot Cooperative Control Based on Virtual Fixture

3.1. Interactive Force Acquisition and Filtering

Figure 7 shows the schematic diagram of human–robot cooperative control based on VFs. The surgeon adjusts the endoscope to the desired posture by dragging the operating handle at the end of the robot. During the dragging process, the robot is restrained and guided by the VFs to prevent the endoscope at the end of the robot from interfering with the nasal tissue. The human–robot interaction process of the control method is the same as that when no robot is involved in the surgical operation, but the advantage is that after the endoscope of the robot is dragged to the proper posture, the robot can be maintained in the current posture without operating buttons or pedals. With the control method, the surgeon can free his left hand and carry out complex surgical operations with both hands, effectively reducing the surgeon’s operational burden.

Firstly, the surgeon drags the handle of the robot to move, and the control system obtains the surgeon’s drag force through the six-dimensional force sensor (ATI Industrial Automation Company, USA):

$$F={[f_x f_y f_z m_x m_y m_z]}^T$$

(17)

Considering the manual operation is easy to shake, the obtained force signal is generally not smooth and needs to be filtered. Considering the computational efficiency, the force signal can be smoothed with the moving average filtering as follows:

$$\overline{F}=f(F_1,F_2,\dots ,F_n)$$

(18)

where n is the length of the intercepted window; the larger the n is, the smoother the force signal obtained. However, excessive n is likely to cause a control response delay. Assuming that the system control period is $T_{\mathrm{control}}$, the force sensor sampling period is $T_{\mathrm{sample}}$, then n can be determined by $n=T_{\mathrm{control}}/T_{\mathrm{sample}}$.

3.2. Admittance Mapping Design Based on Piecewise Function

After obtaining the interaction force between the surgeon and robot end-effector, the admittance control method is used to map the interaction force to the velocity of the endoscope, thereby establishing the spatial admittance relationship between the surgeon’s drag force and the velocity of the robot:

$$V_F=\left[\begin{array}{c}v\\ w\end{array}\right]=K·\overline{F}$$

(19)

$$v={[v_x,v_y,v_z]}^T$$

(20)

$$w={[w_x,w_y,w_z]}^T$$

(21)

Since the drag force contains spatial force and moment, the admittance coefficient, K, is a square matrix of 6 × 6. For admittance control, the design of admittance mapping is key. In general, there are two types of admittance mappings: Linear admittance mapping and nonlinear admittance mapping. Linear admittance mapping is simple to calculate, but it is easy to produce a sudden change in velocity at the boundary point, affecting the drag performance and resulting in unstable robot motion. Nonlinear admittance mapping can easily adjust the admittance coefficient at different regions, but the calculation is relatively complicated. Considering the motion stability requirements of medical robots and computational efficiency, a mixed admittance mapping based on the piecewise function is adopted in this paper. The mapping combines the advantages of linear admittance mapping and nonlinear admittance mapping, and smooths the inflection point by the cubic polynomial function. The detailed mapping function is shown below:

$$V(F)=\{\begin{array}{ll}-V_{\max }\hfill & , F<-k_4\hfill \\ -V_{\max }+C{(F+k_4)}^3\hfill & , -k_4\le F<-k_3\hfill \\ 3C{(k_2-k_1)}^2(F+k_2)-C{(k_2-k_1)}^3\hfill & , -k_3\le F<-k_2\hfill \\ C{(F+k_1)}^3\hfill & , -k_2\le F<-k_1\hfill \\ 0\hfill & , -k_1\le F\le k_1\hfill \\ C{(F-k_1)}^3\hfill & , k_1<F\le k_2\hfill \\ 3C{(k_2-k_1)}^2(F-k_2)+C{(k_2-k_1)}^3\hfill & , k_2<F\le k_3\hfill \\ V_{\max }+C{(F-k_4)}^3\hfill & , k_3<F\le k_4\hfill \\ V_{\max }\hfill & , F>k_4\hfill \end{array}$$

(22)

Figure 8 shows the admittance mapping relationship based on the piecewise function, and the coefficients, $k_1,k_2,k_3,k_4$, are positive. For the drag force at the end of the robot, when $-k_1\le F\le k_1$, the drag force is in the stopping zone. The setting of the stopping zone can effectively prevent the robot from moving under the action of a small input force (such as mechanical vibration and sensor zero drift), so that the robot can smoothly maintain the original posture after the drag of the robot is stopped. When $k_1<F\le k_2$ or $-k_2\le F<-k_1$, the drag force is in the transition zone from the stopping zone to the working zone. The transition zone allows the robot motion admittance mapping to smoothly change from the stopping zone to the working zone. When $k_2<F\le k_3$ or $-k_3<F\le -k_2$, the drag force is in the main working zone. The sensitivity of the robot’s dragging motion can be dynamically adjusted by adjusting the slope of the zone. When $k_3<F\le k_4$ or $-k_4<F\le -k_3$, the drag force is in the transition zone from the working zone to the saturation zone. The transition zone allows the robot motion admittance mapping to smoothly change from the working zone to the saturation zone. When $F>k_4$ or $F<-k_4$, the drag force is in the saturation zone. In this zone, even if the drag force continues to increase, the response velocity of the robot remains unchanged. The saturation zone is used to limit the maximum drag response velocity of the robot, preventing the robot from moving too fast and losing control.

3.3. Human–Robot Cooperative Control Based on Virtual Fixture

The velocity, ${}^F{V}_F={[v_x v_y v_z {\omega }_x {\omega }_y {\omega }_z]}^T$, obtained based on Equation (22) is the velocity vector in the force sensor coordinate system. Considering the VF is generally designed in the image coordinate system, it is necessary to transform the velocity of the endoscope from the force sensor coordinate system to the image coordinate system before performing the VF calculation. Assuming that after the navigation system is registered, the expression of the force sensor coordinate system in the image coordinate system is:

$${}_{\mathrm{Robot}}{}^{\mathrm{Image}}T=\left[\begin{array}{cc}{}_{\mathrm{Robot}}{}^{\mathrm{Image}}R& {}_{\mathrm{Robot}}{}^{\mathrm{Image}}P{}_{Org}\\ 0& 1\end{array}\right]$$

(23)

The expression of the endoscope velocity in the image coordinate system can be obtained as follows:

$${}^{\mathrm{I}}{V}_F=\left[\begin{array}{cc}{}_{\mathrm{Robot}}{}^{\mathrm{Image}}R& 0\\ 0& {}_{\mathrm{Robot}}{}^{\mathrm{Image}}R\end{array}\right]{}^{\mathrm{F}}{V}_F$$

(24)

For ${}^{\mathrm{I}}{V}_F$, using the composite VFs in Figure 6 for constraint calculation, the endoscope velocity after VF restraint can be obtained in the image coordinate system as ${}^{\mathrm{I}}{V}_{out}$. Then, the endoscope velocity is transformed back from the image coordinate system to the robot base coordinate system:

$${}^{\mathrm{Robot}}{V}_{out}=\left[\begin{array}{cc}{}_{\mathrm{Robot}}{}^{\mathrm{Image}}R{}^{-1}& 0\\ 0& {}_{\mathrm{Robot}}{}^{\mathrm{Image}}R{}^{-1}\end{array}\right]{}^{\mathrm{I}}{V}_{out}$$

(25)

According to the robot Jacobian matrix, $J_0$, the robot joint velocity, $\dot{q}$, can be obtained as follows:

$$\dot{q}=J_0^{-1}·{}^{Robot}{V}_{out}$$

(26)

4. Experiments and Discussion

4.1. Experiment Setup

The experimental platform is shown below in Figure 9, which includes a nasal endoscope subsystem (Shenda Endoscope Company, China), a self-developed image navigation subsystem based on an optical measurement device, Polaris Vicra (Northern Digital Company, Canada), a human head model (Phacon Company, Germany), and an endoscopic sinus surgery assistant robot. The robot is developed by our team on the basis of its previous version [37]; it has 7 degrees of freedom and a ring-shaped workspace, which can cover the operation range of endoscopic sinus surgery of 200 × 200 × 150 mm (size of a typical human head).

4.2. Guiding Virtual Fixture Experiment

Figure 10 shows the experiment scene of the GVF, which is a cubic polynomial curve. The purpose of this experiment is to evaluate the constraint performance of the GVF. The starting and ending coordinates of the curve in the figure are (147.3, 49.2, –2.5), (–117.8, 90.6, –30.3). The robot works in the admittance-based drag control mode, and is dragged from the starting point along the curve to the ending point, without GVF constraint and with GVF constraint. During the experiment, the trajectory of the endoscope tip is obtained through the navigation system.

The experimental results are shown in Figure 11. It can be seen that the maximum deviation of the trajectory of the endoscope tip from the ideal trajectory is approximately 5.2 mm without the GVF constraint. After loading the GVF constraint, the maximum deviation from the ideal trajectory is approximately 1.0 mm. The comparison results show that the GVF can effectively reduce the trajectory tracking error of the robot and improve the drag operation precision.

4.3. Forbidden Virtual Fixture Experiment

Figure 12 shows the experiment scene of the FRVF. Considering FRVF generally contains multiple VF geometries, the spatial plane-based FRVF and the single-leaf hyperboloid-based FRVF are composites to be used in this experiment. While evaluating the constraint performance of the single FRVF, the constraint performance of the composite FRVF is also evaluated. In the experiment, the robot works in the admittance-based drag control mode. The endoscope of the robot is dragged from the starting point to the ending point, without FRVF constraint and with composite FRVF constraint. During the experiment, the trajectory of the endoscope tip is obtained through the navigation system.

The experimental results are shown in Figure 13. It can be seen that without the FRVF constraint, the trajectory of the endoscope tip fluctuates drastically, and the maximum fluctuation distance is approximately 4.9 mm. To avoid collision, the drag velocity is slowed down when the endoscope tip is close to the nasal entrance (the density of the sampling points is increased). With the composite FRVF constraint, the trajectory of the endoscope tip is basically above the plane and inside the hyperboloid, the maximum constraint surface penetration distance is approximately 1.3 mm. At the intersection of the plane and the hyperboloid, the trajectory of the endoscope tip is smooth, verifying the effectiveness of the composite FRVF constraint.

4.4. Robot-Assisted Endoscopic Nasal Examination Experiment

In endoscopic nasal examination surgery, the surgeon inserts the endoscope into the small, narrow nasal cavity and carefully examines the anatomy, such as the shape of the turbinate and the opening of the sinus. This surgery is a typical surgical procedure in nasal surgery, so it is selected in this experiment.

Experiment model. Considering that a real human corpse head is very rare and an animal’s nasal cavity is not similar to a human’s cavity, a high-imitation head model with fine anatomical structure (Phacon Company, Germany) is adopted in this experiment, as shown in Figure 14.

Surgical path planning. A three-dimensional model of the nasal cavity is obtained by reconstruction of the nasal CT image sequence, and then the surgical path from the entrance point of the nasal cavity to the opening of each sinus (taking maxillary sinus as an example) is obtained by the surgical planning module.

Interactive endoscopic nasal examination. The surgical scene of the interactive endoscopic nasal examination is shown in Figure 15. The upper left corner of the figure shows the real-time image of the endoscope during examination, and the upper right corner of the figure shows the control system interface and the virtual robot system.

Based on the proposed human–robot cooperative control method, the operator drags the endoscope at the end of the robot and performs interactive nasal sinus examination, browsing along the planned path that is a GVF based on a spatial curve. During this process, the motion of the endoscope tip is constrained by the robot motion constraint model, and the operator constantly adjusts the pose of the endoscope to see the small and deep anatomy inside the nasal cavity from different angles of view. Figure 16 shows the endoscope tip trajectory together with the preoperative planned trajectory during interactive endoscopic nasal examination. It can be seen that due to the effect of the VF constraint, the trajectory of the endoscope tip is substantially coincident with the preoperative planned trajectory, and the maximum deviation is approximately 1.1 mm. Figure 17 shows the endoscopic image during the interactive endoscopic nasal examination. As shown in the figure, the endoscopic field of view is clear throughout the whole examination.

Through the above experiment, it can be seen that the proposed human–robot cooperation control method based on VFs can effectively realize the motion guidance and constraint of the robot, improving the accuracy and safety of surgical operation during surgery.

5. Conclusions

This paper proposed a human–robot cooperative control method based on VF to improve the accuracy and safety of surgical operation in robot-assisted endoscopic sinus surgery. Firstly, based on the analysis of endoscopic motion constraint requirements in different surgical stages, three typical VFs suitable for endoscopic sinus surgery were designed and implemented. The GVF based on a spatial curve was designed to achieve motion guidance from the nasal cavity entrance to the operating area. Considering the funnel-shaped workspace of the endoscope, the FRVF based on the single-leaf hyperboloid was designed to achieve motion constraints at the entrance to nasal cavity. The FRVF based on the anatomical structure of the nasal cavity was designed to constrain the motion of the endoscope inside the nasal cavity, the boundary of which coincides with the boundary of the nasal tissue, thereby maximally retaining the precious operation space while protecting the nasal tissue. Thereafter, a composite VF was constructed based on the above typical VFs, and an overall robot motion constraint model was obtained. Secondly, based on the obtained robot motion constraint model, a human–robot cooperative control method based on VF was proposed. The method adopts admittance-based control to realize efficient human–robot interaction between the surgeon and robot during surgery, and the surgeon adjusts the endoscope to the desired posture by dragging the end of the robot. During the dragging process, the motion of the robot is restrained and guided by the VFs to prevent the endoscope at the end of the robot from interfering with the nasal tissue. With the proposed control method, the surgeon can free his left hand and carry out complex surgical operations with both hands, thus improving operation efficiency. Finally, validation experiments were conducted. The GVF experiment based on the cubic polynomial curve was performed and the results showed that the proposed GVF can effectively reduce the trajectory tracking error of the robot and improve the drag operation precision. The composite FRVF based on the spatial plane and a single-leaf hyperboloid was tested and the results showed that the proposed FRVFs can prevent the endoscope from colliding with the nasal tissue and improve the safety of the surgical operation. Thereafter, robot-assisted endoscopic nasal examination experiment was performed using a high-imitation head model, and the results showed that the proposed human–robot cooperative control method based on VF can effectively realize the motion guidance and constraint of the robot, and help to improve the accuracy and safety of operation during endoscopic sinus surgery. Considering clinical applicability, the project team is now working on clinical experiments to further optimize the robot system.