Manipulation of a Whole Surgical Tool Within Safe Regions Utilizing Barrier Artificial Potentials

Abstract

Active constraint enforcement in robotic-assisted surgery is critical for reducing the intra-operative risk of unintentionally damaging sensitive tissues by the surgical instrument. This work considers surgical instruments which can be circumscribed by a geometric capsule and forbidden regions which can be approximated by point clouds in order to produce a repulsive wrench by the control action to guarantee manipulation within safe regions. This work details the control scheme which is based on barrier artificial potentials when considering the whole tool extending our previous results on the tool point. A proof of the control system’s passivity and non constraint violation is provided together with experimental results using a 7-dof KUKA LWR4+ manipulator as a master device in a virtual surgical scene in order to demonstrate the effectiveness of the proposed scheme.

We certify that there is no actual or potential conflict of interest in relation to this article.

Appendix

System’s Stability

Let us consider a 6-DOF revolute joint manipulator with gravity compensation and an additive dissipation term under the kinesthetic guidance of a human generalized force $\mathbf F_h \in \mathbb {R}^6$.

The dynamic model of the robot in the Cartesian space can be written in the task space as follows:

$$\begin{aligned} \mathbf {\Lambda }_x \dot{\mathbf {v}}+\left( \mathbf {C}_x+\mathrm {\mathbf D}_d\right) {\mathbf {v}}- \mathbf {u}_c =\mathbf {F}_{h} , \end{aligned}$$

(11)

where

$$\begin{aligned} \mathbf {\Lambda }_x =[\mathbf {J}(\mathbf {q})\mathbf {\Lambda }^{-1}(\mathbf {q})\mathbf {J}^T(\mathbf {q})]^{-1}\text {,} \end{aligned}$$

(12)

$$\begin{aligned} \mathbf {C}_x{\mathbf {v}} =\mathbf {\Lambda }_x \left( \mathbf {J}(\mathbf {q})\mathbf {\Lambda }^{-1}(\mathbf {q}) \mathbf {C}(\mathbf {q},\dot{\mathbf {q}}) - \dot{\mathbf {J}}(\mathbf {q})\right) \dot{\mathbf {q}}, \end{aligned}$$

(13)

with $\mathbf {q},\mathbf {\dot{q}} \in \mathbb {R}^6$ being the robot joint position and velocity, $\mathbf {x}=[\mathbf {p}_w^T \, \mathbf {Q}_w^T]^T\in \{\mathbb {R}^3 \times \mathbb {S}^3\}$ the pose of the wrist, where $\mathbf p_w$ is its position and $\mathbf Q_w$ its orientation expressed as a unit quaternion, $\mathbf {v}=[\dot{\mathbf {p}_w}^T \, \varvec{\omega }_w^T]^T\in \mathbb {R}^\mathrm {6}$ the generalized velocity of the wrist with $\dot{\mathbf {p}}_w , \, \varvec{\omega }_w$ the translational and angular velocities, $\mathbf {D}_d\in \mathbb {R}^{\mathrm {6} \times \mathrm {6}}$ being a positive definite matrix of the desired damping, $\mathbf {\Lambda }(\mathbf {q}) \in \mathbb {R}^{\mathrm {6} \times \mathrm {6}}$ being the manipulator’s inertia matrix, $\mathbf {C}(\mathbf {q},\dot{\mathbf {q}})\in \mathbb {R}^{\mathrm {6} \times \mathrm {6}}$ being the Coriolis and centripetal matrix, $\mathbf J(\mathbf q) \in \mathbb {R}^{\mathrm {6} \times \mathrm {6}}$ being the Jacobian of the manipulator which maps the velocity of the joints to the wrist velocity and $\mathbf u_c \in \mathbb {R}^{6}$ the virtual constraint control input. Notice that the unit Quaternion $\mathbf Q_w$ can be deduced given the rotation matrix $\mathbf R_w$, $\mathbf {\Lambda }_x$ is positive definite and $\dot{\mathbf {\Lambda } }_x-2\mathbf {C}_x$ is skew symmetric.

To proceed with the stability analysis, we initially write system (11) in state-space. Utilizing the state vector:

$$\begin{aligned} \mathbf s=[{\mathbf v}^T \ \mathbf x^T\ \xi ]^T \in \mathbb {R}^\mathrm {6}\times (\mathbb {R}^\mathrm {3}\times \mathbb {S}^\mathrm {3})\times \mathbb {R} \end{aligned}$$

(14)

with $\xi =\sum _{\mathbf p_i \in \mathcal {C}}{} V_{i}(\mathbf p{^*_i})$, the system (11) can be written as:

$$\begin{aligned} \dot{\mathbf {s}}=\mathbf {g}(\mathbf {s},\mathbf {F}_h ), \mathbf {s}_0=\mathbf {s}(t_0) \in D \end{aligned}$$

(15)

where

$$\begin{aligned} D\triangleq \mathbb {R}^\mathrm {6}\times \varOmega \times \mathbb {R}, \end{aligned}$$

$$\begin{aligned} \varOmega = \bigcup _{\mathbf p_i \in \mathcal {O}_s}\{\mathbf {x}\in \mathbb {R}^3\times \mathbb {S}^3 : d(\mathbf p_i,\mathbf p{^*_i})>d_c \}, \end{aligned}$$

$$\begin{aligned} \mathbf {g}(\mathbf {s},\mathbf {F}_{h} )=\begin{bmatrix} \mathbf {\Lambda }_x^{-1}(-(\mathbf {C}_x+\mathbf {D}_d) {\mathbf {v}}+\mathbf {F}_{h} +\mathbf {u}_c) \\ \dfrac{1}{2}\mathbf {J}_x{\mathbf {v}} \\ \ \sum _i\frac{\partial V_i(\mathbf {p}{^*_i})}{\partial \mathbf {p}{^*_i}}^T\dot{\mathbf {p}}{^*_i} \end{bmatrix} \end{aligned}$$

with $\mathbf J_x=\text {diag}(2\mathbf I_3, \, \, \mathbf J_Q^T)$ where $\mathbf J_Q$ is the mapping between the angular velocity of the frame $\{W\}$ and the unit quaternion rates. Notice that $\varOmega$ is the union of all wrist configurations in which the instrument does not touch the spheres $S(\mathbf p_i,d_{c_1})$.

Theorem 1

For system (15) under the exertion of the external human force $\mathbf {F}_h$ the forbidden area is not violated and the system is strictly output passive with respect to the wrist velocity ${\mathbf v}$.

Proof

Let the initial state be such that $\mathbf {u_c}\ne \mathbf 0$. Utilizing the following candidate Lyapunov-like function:

$$\begin{aligned} V=\dfrac{1}{2}{\mathbf {v}}^T\mathbf {\Lambda }_x {\mathbf {v}}+k\sum _{i}{ V_{i}(\mathbf {p}{^*_i})}, \end{aligned}$$

(16)

its time derivative, substituting $\mathbf {\Lambda }_x \dot{\mathbf {v}}$ from (11) and utilizing the skew-symmetry of matrix $\left( \dot{\mathbf {\Lambda } }_x-2\mathbf {C}_x\right)$, yields:

$$\begin{aligned} \dot{V}=-{\mathbf {v}}^T\mathbf {D}_d{ \mathbf {v}}+\mathbf {v}^T\mathbf {F}_h+ \mathbf v^T\mathbf {u}_c -k\sum _{i}{ \dot{\mathbf {p}}{^*_i}^T \mathbf f_i}. \end{aligned}$$

(17)

The time derivative of the nearest point, using the time derivative of (1) for $\sigma =\sigma *$, given by:

$$\begin{aligned} \dot{\mathbf p}_i^*=\dfrac{\partial \mathbf {p}_s( \sigma ,t)}{\partial t}|_{\ \sigma = {\sigma }{^*_i}}+\dfrac{\partial \mathbf {p}_s( {\sigma }{^*_i},t)}{\partial \sigma ^*_i}\dot{{\sigma }{^*_i}} = \dot{\mathbf p}_w (t)+\hat{\varvec{\omega }}_w(t) \mathbf n L {\sigma }{^*_i}+ \mathbf n L \dot{{\sigma }{^*_i}} \end{aligned}$$

with $\hat{\varvec{\omega }}_w(t) = \dot{\mathbf R}_w(t)\mathbf R_w(t)^T$ the wrist’s angular velocity expressed in the inertia frame and operator denoting the skew symmetric matrix of a vector. Substituting $\dot{\mathbf p}_i^*$ in (17) yields:

$$\begin{aligned} \begin{aligned} \dot{V}=&-{\mathbf {v}}^T\mathbf {D}_d{ \mathbf {v}}+\mathbf {v}^T\mathbf {F}_h+\mathbf v ^T \mathbf u_c - k\sum _{i}{( \dot{\mathbf p}_w (t)+\hat{\varvec{\omega }}_w(t) \mathbf n L {\sigma }{^*_i}+ \mathbf n L \dot{{\sigma }{^*_i}})^T\mathbf f_i} \end{aligned}. \end{aligned}$$

(18)

Utilizing (9) in (8) yields:

$$\begin{aligned} \begin{aligned} \dot{V}=&-{\mathbf {v}}^T\mathbf {D}_d{ \mathbf {v}}+\mathbf {v}^T\mathbf {F}_h+k \dot{\mathbf p}_w ^T \sum _{i}\mathbf f_i+ L k \varvec{\omega }_w^T\sum _{i}\mathbf \sigma {^*_i} \mathbf n \times \mathbf f_\mathrm {i}\\\ -&k\sum _{i}{( \dot{\mathbf p}_w (t)+\hat{\varvec{\omega }}_w(t) \mathbf n L {\sigma }{^*_i}+ \mathbf n L \dot{{\sigma }{^*_i}})^T\mathbf f_i}. \end{aligned} \end{aligned}$$

(19)

Using the distributive property of the dot product and the property $\mathbf a (\mathbf b \times \mathbf c ) =(\mathbf a \times \mathbf b) \mathbf c$ the above equation becomes:

$$\begin{aligned} \dot{V}=-{\mathbf {v}}^T\mathbf {D}_d{ \mathbf {v}}+\mathbf {v}^T\mathbf {F}_h-k L\sum _{i}{ \dot{{\sigma }{^*_i}}\mathbf n^T \mathbf f_\mathrm {i} }. \end{aligned}$$

(20)

For the first case of (3) utilizing (1) for $\sigma =\sigma {^*_i}$ yields that $\mathbf n^T(\mathbf {p}{^*_i}-\mathbf p_i)=0$ and for the other two cases $\dot{{\sigma }{^*_i}}=0$. Therefore:

$$\begin{aligned} \mathbf n^T( \mathbf {p}{^*_i}-\mathbf p_i)\dot{\sigma }{^*_i}=0. \end{aligned}$$

As $\mathbf f_i$ is in the direction of $\mathbf {p}{^*_i}-\mathbf {p}{_i}$ and using the above property in (20) the time derivative of V becomes:

$$\begin{aligned} \dot{V}=-{\mathbf {v}}^T\mathbf {D}_d{ \mathbf {v}} +\mathbf {F}_h^T{ \mathbf {v}}\le \mathbf {F}_h^T{ \mathbf {v}}. \end{aligned}$$

(21)

Hence, system (15) is strictly output passive (see Definition 6.3 in [9]).

Rewriting (21) by completing the squares, yields:

$$\begin{aligned} \begin{aligned} \dot{V}=&-||\sqrt{\mathbf {D}_d}{\mathbf {v}}-\dfrac{1}{2}\sqrt{\mathbf {D}_d}^{-1}\mathbf {F}_h||^2+\dfrac{1}{4}\mathbf {F}_h^T \mathbf {D}_d^{-1}\mathbf {F}_h \\\le&\dfrac{1}{4}\mathbf {F}_h^T \mathbf {D}_d^{-1}\mathbf {F}_h . \end{aligned} \end{aligned}$$

(22)

Notice that $\mathbf {F}_h$ represents the force applied by the human to guide the robot. Thus, $\mathbf {F}_h$ is bounded and therefore $\mathbf {F}_h^T\mathbf {D}_d^{-1}\mathbf {F}_h$ is also a bounded function of time. Additionally the human forces have bounded energy so by integrating equation (22) we get:

$$\begin{aligned} V\le V(t_0) +\int _{t_0}^{t}\dfrac{1}{4} \mathbf {F}_h^T{\mathbf {D}_d}^{-1}\mathbf {F}_h< \infty . \end{aligned}$$

(23)

Thus, states $\xi$ and ${\mathbf {v}}$ are bounded under the exertion of human force or otherwise there exist compact sets $\varOmega _{\xi }, \ \varOmega _{v}$ such that $\xi \in \varOmega _{\xi }\subset \mathbb {R}$ and ${\mathbf v} \in \varOmega _{v}\subset \mathbb {R}^6$. As a consequence, there exists a positive constant $\overline{\varepsilon }_i$ such that $V_i \le \overline{\varepsilon }_i$, which for the logarithmic function defined in (4) yields:

$$\begin{aligned} 0 \le \psi _i \le 1-\dfrac{1}{e^{2\sqrt{ \overline{\varepsilon }_i }}}<1, i=1,...,M \text {,} \end{aligned}$$

and this means that $\Vert \mathbf {p}{^*_i} - \mathbf {p}_i\Vert >d_c$ i.e. the instrument will never touch the sensitive area.

In the case that all $\mathbf p_i$ in $\mathcal {O}_s$ are such as $d(\mathbf p_i,\mathbf p{^*_i})\ge d_c +d_0$ i.e. $\mathbf u_c=\mathbf 0$ the Lyapunov function take the form $V=\dfrac{1}{2}{\mathbf {v}}^T\mathbf {\Lambda }_x {\mathbf {v}}$ and the time derivative $\dot{V}=-{\mathbf {v}}^T\mathrm {\mathbf D}_d \mathbf {v}+{\mathbf {v}}^T\mathbf {F}_h\le \dfrac{1}{4}\mathbf {F}_h^T \mathbf {D}_d^{-1}\mathbf {F}_h$. Thus, the system is strictly output passive w.r.t ${\mathbf {v}}$. Notice that when $\mathbf {F}_h=\mathbf 0$, $\mathbf {v}$ converges exponentially to zero. Hence, using Lemma 1 of the [5], we can conclude that $\mathbf {x}$ converges to a constant.