Dynamic virtual fixture on the Euclidean group for admittance-type manipulator in deforming environments

Abstract

Background

In a deforming anatomic environment, the motion of an instrument suffers from complex geometrical and dynamic constraints, robot assisted minimally invasive surgery therefore requires more sophisticated skills for surgeons. This paper proposes a novel dynamic virtual fixture (DVF) to enhance the surgical operation accuracy of admittance-type medical robotics in the deforming environment.

Methods

A framework for DVF on the Euclidean Group SE(3) is presented, which unites rotation and translation in a compact form. First, we constructed the holonomic/non-holonomic constraints, and then searched for the corresponded reference to make a distinction between preferred and non-preferred directions. Second, different control strategies are employed to deal with the tasks along the distinguished directions. The desired spatial compliance matrix is synthesized from an allowable motion screw set to filter out the task unrelated components from manual input, the operator has complete control over the preferred directions; while the relative motion between the surgical instrument and the anatomy structures is actively tracked and cancelled, the deviation relative to the reference is compensated jointly by the operator and DVF controllers. The operator, haptic device, admittance-type proxy and virtual deforming environment are involved in a hardware-in-the-loop experiment, human-robot cooperation with the assistance of DVF controller is carried out on a deforming sphere to simulate beating heart surgery, performance of the proposed DVF on admittance-type proxy is evaluated, and both human factors and control parameters are analyzed.

Results

The DVF can improve the dynamic properties of human-robot cooperation in a low-frequency (0 ~ 40 rad/sec) deforming environment, and maintain synergy of orientation and translation during the operation. Statistical analysis reveals that the operator has intuitive control over the preferred directions, human and the DVF controller jointly control the motion along the non-preferred directions, the target deformation is tracked actively.

Conclusions

The proposed DVF for an admittance-type manipulator is capable of assisting the operator to deal with skilled operations in a deforming environment.

Background

In a deforming anatomic environment, minimally invasive surgery (MIS) therefore requires more sophisticated skills for the surgeons, the motion of an instrument suffers from complex geometrical and dynamic constraints. One approach for enhancing the accuracy and safety of human operation is to use robot controller to regulate the instrument motion under either hands-on or teleoperation control. These are referred to as virtual fixtures (VF), which is kind of task-dependent and computer-generated mechanisms to limit movement into restricted regions [1–3] or influence movement along desired paths [4–8]. Extension of VF into deforming environment also refers to dynamic virtual fixture [9], where the geometric constraint moves continuously, as a result of changes in the physical environment or task being undertaken [10, 11].

An important case of VFs for surgical application is guidance VF, Bettini et al. [5, 6] used vision information to assist VF construction, and they discussed the application in vitreoretinal surgery. Marayong et al. [7] demonstrated a geometrical constraint with varying compliance, which is described for the general spatial case. Compliant control is significantly necessary to overcome the uncertainties associated with registration errors, variations in anatomy [12]. Li et al. [13] presented an anatomy generated VF for sinus surgery, which employed a constrained optimization framework to incorporate task goals, anatomy-based constraints, forbidden zones, etc. Castillo-Cruces et al. [14] presented an admittance controller with autonomous error compensation, a clear distribution of responsibilities between surgeon and robotic system is preserved. Becker et al. [15] derived a virtual fixture framework for active handheld micromanipulators, the forces were replaced with an actuated tool tip that is permitted by the high-bandwidth position measurements. These work all used admittance controller to implement VFs, the active robots comply with the manual force that applied on the end-effector or telemanipulated joystick. However, such first-order admittance controller with manual/autonomous error compensation is inadequate to adapt to dynamic environment. Bebek et al. proposed an intelligent control algorithm for robotic-assisted beating heart surgery, the robotic tools actively cancel the relative motion between the surgical instruments and the point of interest on the beating heart [16]. However, motions along the preferred directions are not regulated.

Traditional VFs deal with rotation and translation separately in ℝ³ space, the interconnection between them is not involved in VF design. Synergy of orientation and position is important for MIS, it requires that the orientation should be ready before the position is reached. Otherwise, a delay is needed to instrument motion when the orientation and position are out of synchronous, it could be awful for real-time operation, collisions could probably appears in the inner anatomic operation space. Bullo and Murray [17] proposed a generalized proportional derivative (PD) laws on Euclidean Group SE(3), which used logarithmic feedback control and dealt with rotation and translation simultaneously with a compact form. Besides, the geometric properties of Lie group and Lie algebra facilitate spatial compliance and stiffness matrix synthesis and decomposition. Shuguang Huang [18, 19] examined the structure of spatial stiffness by evaluating the eigenscrew featured rank-1 primitives stiffness matrices.

In our previous works, we studied the spatial compliance/stiffness matrices synthesis for admittance and impedance controlled devices respectively [20].

The proposed VFs could not effectively eliminate the tracking error drifts over the deforming frequency growth, thus we hope to improve the dynamic performance of the VF for admittance controlled device. In this paper, we study the application of VF in deforming environment, and propose a novel framework of DVF for admittance-type device on SE(3). We construct the holonomic constraint/non-holonomic constraints and then search for the corresponded reference tasks to make a distinction between preferred and non-preferred directions. Different control strategies are employed to deal with the tasks along the distinguished directions. The paper is organized as follows: the first section outlines the mathematical preliminaries of Lie group and Lie algebra for robot control, spatial compliance matrix synthesis and DVF on SE(3) for admittance-type device are proposed in Methods the last two sections describe a hardware-in-the-loop experiment and discussions.

Methods

Mathematical preliminaries

The configuration space of a robotic system is defined on the special Euclidean group SE(3) and its subgroups, position and orientation of the end-effector correspond to a element of matrix Lie group. The Lie algebra of SE(3) is denoted as se(3). We express a group element g = (R, p) ∊ SE(3) and velocity $\mathbf{V}=\left(\widehat{\mathit{\omega }},\mathit{v}\right)\in \mathit{se}\left(3\right)$ using homogeneous coordinates as

$$\mathit{g}=\left[\begin{array}{cc}\hfill \mathbf{R}\hfill & \hfill \mathit{p}\hfill \\ \hfill \mathit{0}\hfill & \hfill 1\hfill \end{array}\right],\mathbf{V}=\left[\begin{array}{cc}\hfill \widehat{\mathit{\omega }}\hfill & \hfill \mathit{v}\hfill \\ \hfill \mathit{0}\hfill & \hfill 0\hfill \end{array}\right]$$

(1)

the operator $\widehat{*}:{\mathrm{ℝ}}^3\to \mathit{so}\left(3\right)$ is a cross-product matrix that transforms cross-product operation into matrix multiplication, so that $\widehat{\mathit{x}}\mathit{y}=\mathit{x}\times \mathit{y}$ for all x, y ∈ R³. Elements of se(3) can also be represented as vector pair (ω, v), ω, v ∈ ℝ³.

For all g ∊ SE(3) and all X, Y ∊ se(3), the Adjoint map ${\mathrm{Ad}}_{\mathit{g}}:\mathit{se}\left(3\right)\to \mathit{se}\left(3\right)$ is a coordinate transformation on se(3), this allows us to transform spatial velocity from one coordinate to another. The Lie bracket is given by the matrix commutator ad_X, which generalizes the standard cross-product operation in se(3).

$${\mathrm{Ad}}_{\mathit{g}}\left(\mathbf{Y}\right)=\mathit{g}\mathbf{Y}{\mathit{g}}^{-1}$$

(2)

$${\mathrm{ad}}_{\mathit{X}}\left(\mathbf{Y}\right)=\left[\mathbf{X},\mathbf{Y}\right]=\mathbf{XY}-\mathbf{YX}$$

(3)

Writing X as column vector pair (ω, v), the 6 × 6 matrix representation of adjoint transformation and Lie bracket are

$${\mathrm{Ad}}_{\mathit{g}}=\left[\begin{array}{cc}\hfill \mathbf{R}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \widehat{\mathit{p}}\mathbf{R}\hfill & \hfill \mathbf{R}\hfill \end{array}\right],{\mathrm{ad}}_{\mathit{X}}=\left[\begin{array}{cc}\hfill \widehat{\mathit{\omega }}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \widehat{\mathit{v}}\hfill & \hfill \widehat{\mathit{\omega }}\hfill \end{array}\right]$$

(4)

Spatial moment-force pair $\left(\mathit{m},\mathit{f}\right)\mathit{m},\mathit{f}\in {\mathrm{R}}^3$ are elements of the dual space of se(3), which is denoted as se(3)*. The dual Adjoint map ${\mathrm{Ad}}_{\mathit{g}}^{\ast }:\mathit{se}\left(3\right)*\to \mathit{se}\left(3\right)*$ and Lie bracket ${\mathrm{ad}}_{\mathit{X}}^{\ast }$ are given by

$${\mathrm{Ad}}_{\mathit{g}}^{\ast }=\left[\begin{array}{cc}\hfill {\mathbf{R}}^{\mathit{T}}\hfill & \hfill -\widehat{\mathit{p}}{\mathbf{R}}^{\mathit{T}}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathbf{R}}^{\mathit{T}}\hfill \end{array}\right]{\mathrm{ad}}_{\mathit{X}}^{\ast }=\left[\begin{array}{cc}\hfill -\widehat{\mathit{\omega }}\hfill & \hfill -\widehat{\mathit{v}}\hfill \\ \hfill \mathbf{0}\hfill & \hfill -\widehat{\mathit{\omega }}\hfill \end{array}\right]$$

(5)

Let V ₂ = (ω ₂, v ₂) ∊ se(3) denote the spatial velocity of a rigid body in frame M ₂, g₁₂ ∊ SE(3) represents the right coordinate transformation from frame M ₁ to frame M ₂, and then the spatial velocity relative to frame M ₁, denoted by V ₁ = (ω ₁, v ₁), is given by ${\mathit{V}}_1=\mathit{A}{\mathit{d}}_{{\mathit{g}}_{12}}{\mathit{V}}_2$. Let F ₂ = (m ₂, f ₂) ∊ se(3) * denotes the moment-force pair acting on a rigid body with respect to frame M ₂, g ₁₂ ∊ SE(3) represents the right coordinate transformation from frame M ₁ to frame M ₂, and then the spatial force in frame M ₁ is given by ${\mathit{F}}_2=\mathit{A}{\mathit{d}}_{{\mathit{g}}_{12}}^{*}{\mathit{F}}_1$.

On SE(3) and its proper subgroups, the exponential map exp: se(3) → SE(3) is a surjective map and a local diffeomorphism [21]. Given $\widehat{\mathit{\psi }}\in \mathrm{so}\left(3\right)$ and $\mathbf{X}=\left(\widehat{\mathit{\psi }},\mathit{q}\right)\in \mathrm{se}\left(3\right)$, we have

$${exp}_{\mathit{SO}\left(3\right)}\left(\widehat{\mathit{\psi }}\right)=\mathbf{I}+sin∥ \mathit{\psi }∥ \frac{\widehat{\mathit{\psi }}}{∥ \mathit{\psi }∥ }+\left(1-cos∥ \mathit{\psi }∥ \right)\frac{{\widehat{\mathit{\psi }}}^2}{{∥ \mathit{\psi }∥ }^2}$$

(6)

$${exp}_{\mathit{SE}\left(3\right)}\left(\mathit{X}\right)=\left[\begin{array}{cc}\hfill {exp}_{\mathit{SO}\left(3\right)}\left(\widehat{\mathit{\psi }}\right)\hfill & \hfill \mathit{A}\left(\mathit{\psi }\right)\mathit{q}\hfill \\ \hfill \mathit{0}\hfill & \hfill 1\hfill \end{array}\right]$$

(7)

$$\mathbf{A}\left(\mathit{\psi }\right)=\mathbf{I}+\left(\frac{1-cos∥ \mathit{\psi }∥ }{∥ \mathit{\psi }∥ }\right)\frac{\widehat{\mathit{\psi }}}{∥ \mathit{\psi }∥ }+\left(1-\frac{sin∥ \mathit{\psi }∥ }{∥ \mathit{\psi }∥ }\right)\frac{{\widehat{\mathit{\psi }}}^2}{{∥ \mathit{\psi }∥ }^2}$$

(8)

Equation (6) is also known as Rodrigues formula. The logarithmic map log: SE(3) → se(3) is the inverse operation of exponential map. Let (R, p) ∊ SE(3) be such that tr(R) ≠ −1, then

$${log}_{\mathit{SO}\left(3\right)}\left(\mathbf{R}\right)=\frac{\mathit{\varphi }}{sin\mathit{\varphi }}\left(\mathbf{R}-{\mathbf{R}}^{\mathit{T}}\right)\in \mathrm{so}\left(3\right)$$

(9)

in which ϕ satisfies $\mathit{\varphi }=\frac{1}{2}\left(\mathit{tr}\left(\mathbf{R}\right)-1\right)$ and |ϕ| < π. Also

$${log}_{\mathit{S}\mathrm{E}\left(3\right)}\left(\mathbf{R},\mathit{p}\right)=\left[\begin{array}{cc}\hfill \widehat{\mathit{\psi }}\hfill & \hfill {\mathbf{A}}^{-1}\left(\mathit{\psi }\right)\mathit{p}\hfill \\ \hfill \mathit{0}\hfill & \hfill 0\hfill \end{array}\right]$$

(10)

where $\widehat{\mathit{\psi }}={log}_{\mathit{SO}\left(3\right)}\left(\mathbf{R}\right)$.

Note that element of the Lie algebra can be expressed as a velocity as in Eq.(1) or the logarithm coordinates in Eq. (10). We denote them with $\mathbf{V}=\left(\widehat{\mathit{\omega }},\mathit{v}\right)$ in the first case, and with $\mathbf{X}=\left(\widehat{\mathit{\psi }},\mathit{q}\right)$ in the second case. According Ball’s screws theory, every rigid coordinate transformation corresponds to a finite screw motion [22]. More precisely, the twist corresponds to the spatial velocity $\mathbf{V}=\left(\widehat{\mathit{\omega }},\mathit{v}\right)$ of the Lie algebra, a screw refers to the exponential coordinate $\mathbf{X}=\left(\widehat{\mathit{\psi }},\mathit{q}\right)$. The screw is an element of the 1–dimensional projective space of se(3), while a screw system with k degree of freedom (DOF) is a k–dimensional subspace [23]. The distance between the configuration state g and the identity e_G = I∈SE(3) is given by the norm of the logarithmic coordinate, which is an Ad-invariant metric on the matrix Lie group G by either left or right transformation [24].

$${∥ \mathit{g}∥ }_{\mathrm{G}}={〈log\left(\mathit{g}\right),log\left(\mathit{g}\right)〉}^{1/2}=\frac{1}{2}\sqrt{\mathit{tr}\left[log{\left(\mathit{g}\right)}^{\mathit{T}}log\left(\mathit{g}\right)\right]}$$

(11)

Dynamic constrains and reference search

In deforming anatomic environment, the motion of instrument suffers from complex geometrical and dynamic constraints, their translational and rotational degree of freedom are restricted. MIS operation requires more sophisticated skills for the surgeons, making a distinction between and preferred and non-preferred directions and employed different control strategy helps reduce operation burden for surgeons. This can be achieved by construction of the holonomic/non-holonomic constraints and searching for the corresponded reference trajectories. The surgeons have complete control over the preferred directions, which are featured by a set of unit motion screws; while the deviation relative to the reference can be compensated autonomously. For a fully actuated control systems (the number of independent control inputs equals to the number of position variables), the reference trajectory falls into the holonomic and non-holonomic according to the constraints applied to the end-effector.

(i). Holonomic constraint and the corresponded reference

In holonomic constraints, the DOF of reference equals to the dimension of SE(3), motion of the end-effector is strictly constrained. The task sequence is defined as a generalized time-varying curve g _r (λ, t) ∊ G, λ ∊ R. Translation and rotation are continuous in λ − t plane, the first-order partial derivatives for t and λ exist. The temporal velocity with respect to inertia frame refers to

$${\mathbf{V}}_{\mathit{d}}\left(\mathit{\lambda },\mathit{t}\right)=\frac{\partial {\mathit{g}}_{\mathit{r}}}{\partial \mathit{t}}{\mathit{g}}_{\mathit{r}}^{-1},{\mathbf{V}}_{\mathit{r}}\in \mathrm{se}\left(3\right)$$

(12)

The tangent screw

$${\mathbf{V}}_{\mathit{t}}\left(\mathit{\lambda },\mathit{t}\right)=\frac{\partial {\mathit{g}}_{\mathit{r}}}{\partial \mathit{\lambda }}{\mathit{g}}_{\mathit{r}}^{-1},{\mathbf{V}}_{\mathit{t}}\in \mathrm{se}\left(3\right)$$

(13)

in inertia frame represents the preferred direction of instrument motion, which is independent of the choice of parameter λ. Given the current configuration g of the end-effector, the instantaneous desired configuration g _d on the task sequence g _r (λ, t) refers to the one with minimum norm

$$\begin{array}{l}\mathit{e}\left(\mathit{\lambda }\right)={\left.{\mathit{g}}_{\mathit{r}}{\left(\mathit{\lambda },\mathit{t}\right)}^{-1}\mathit{g}\left(\mathit{t}\right)\right|}_{\mathit{t}}\\ {\mathit{g}}_{\mathit{d}}=arg\underset{\mathit{\lambda }}{min}{∥ \mathit{e}\left(\mathit{\lambda }\right)∥ }_{\mathrm{G}}=arg\underset{\mathit{\lambda }}{min}\frac{1}{2}\sqrt{2{\mathit{\psi }}^{\mathit{T}}\mathit{\psi }+{\mathit{q}}^{\mathit{T}}\mathit{q}}\end{array}$$

(14)

in which ${log}_{\mathrm{SE}\left(3\right)}\left(\mathit{e}\right)=\left(\widehat{\mathit{\psi }},\mathit{q}\right)\in \mathit{se}\left(3\right)$, the norm of logarithmic error is a weighted distance of pure rotation and translation. Searching for the nearest reference along the task sequence can be generalized as the traversal searching the minimum point along the whole task sequence for initialization, local searching along the direction of norm reduction and the minimum is always reachable due to continuous restriction on rotation and translation.

(ii). Non-holonomic constraint and the corresponded reference

In non-Holonomic constraints, the DOF of reference is less than the dimension of Lie group, such as pure translation R³, rotation SO(3), or other displacement subgroups, motion of the end-effector is partially constrained. For example, instrument motion on a deforming surface while the tool shaft along the norm direction, which refers to the displacements in SE(2). Given the reference g _r  = ([m _x , m _y , m _z ], p _r ), f(p _r ) = 0 and current state g = ([n _x , n _y , n _z ], p), the task is to make n _z  → m _z , p → p _r . Any rotation about axis z cannot change the direction of axis-z, the instantaneous reference g _d is the one with minimum norm in all possible frames g _r g _z(θ),

$$\begin{array}{l}{\mathit{g}}_{\mathrm{z}}\left(\mathit{\theta }\right)=\left({\mathbf{R}}_{\mathit{z}}\right(\mathit{\theta }),\mathit{0}),\mathit{\theta }\in [0,\mathit{\pi })\\ {\mathit{g}}_{\mathit{r}}\left(\mathit{\theta }\right)={\mathit{g}}_{\mathit{r}}{\mathit{g}}_{\mathrm{z}}\left(\mathit{\theta }\right)\\ \mathit{e}\left(\mathit{\theta }\right)={\mathit{g}}_{\mathit{r}}{\left(\mathit{\theta }\right)}^{-1}\mathit{g}\\ {\mathit{g}}_{\mathit{d}}=arg\underset{\mathit{\theta }}{min}{∥ \mathit{e}\left(\mathit{\theta }\right)∥ }_{\mathrm{G}}={\left.{\mathit{g}}_{\mathit{r}}\left(\mathit{\theta }\right)\right|}_{tr\left(\mathit{e}\left(\mathit{\theta }\right)\right)=4}\end{array}$$

(15)

When the axis vectors n _z and m _z coincides, the rotation matrix of reference g _d equals to that of robot state g according to Eq.(15), that means the user has complete control over the robot along the tangent plane and directs the task flow intuitively, the deviations relative to reference can be compensated manually or autonomously during human-robot cooperation.

Spatial compliance for virtual fixture on SE(3)

Admittance control facilitates VF by allowing the end-effectors to comply with the manual force anisotropically. The compliant behavior of admittance VF is featured by a 6 × 6 symmetric positive semidefinite (PSD) compliance matrix C _VF : se(3) * → se(3), which maps spatial force into small displacement [25, 26]. The preferred and non-preferred directions construct the geometry constraints, both can be represented by unit screws in se(3). Given a set of linearly independent allowable screw set S in body-fixed frame, the task dependent compliant matrix ${\mathbf{C}}_{\mathrm{VF}}^{\mathit{b}}$ can be decomposed into sum of m rank-1 PSD matrices,

$$\begin{array}{l}\mathbf{S}=\left\{{\mathit{S}}_{\mathit{i}}\right\},{\mathit{S}}_{\mathit{i}}={\left[{\mathit{\psi }}_{\mathit{i}}^{\mathit{T}},{\mathit{q}}_{\mathit{i}}^{\mathit{T}}\right]}^{\mathit{T}},\mathit{i}=1,\dots \mathit{m}\le 6\\ {\mathbf{C}}_{\mathrm{VF}}^{\mathit{b}}={\mathit{c}}_1{\mathit{S}}_1{\mathit{S}}_1^{\mathit{T}}+{\mathit{c}}_2{\mathit{S}}_2{\mathit{S}}_2^{\mathit{T}}+\cdot \cdot \cdot +{\mathit{c}}_{\mathit{m}}{\mathit{S}}_{\mathit{m}}{\mathit{S}}_{\mathit{m}}^{\mathit{T}}\mathbf{,}{\mathit{c}}_{\mathit{i}}>0\\ ={\mathbf{C}}_1+{\mathbf{C}}_2+\cdot \cdot \cdot +{\mathbf{C}}_{\mathit{m}}\end{array}$$

(16)

positive scalar c _i defines the compliance for each primitive compliance matrix C _i . A wrench applied about the screw yields a twist deformation along the same screw, high compliance is encountered along the preferred directions, while low compliance along the non-preferred directions. Making a distinction between and non-preferred directions helps reduce operation burden of surgeons, they only focus on operations along the preferred direction. Any deformation δX^b = (δφ^b, δq^b) complying with the manual force F _h ^b = (m^b, f^b) can be linearly represented by motion screws of S

$$\left[\begin{array}{c}\hfill \mathit{\delta }{\mathit{\phi }}^{\mathit{b}}\hfill \\ \hfill \mathit{\delta }{\mathit{q}}^{\mathit{b}}\hfill \end{array}\right]={\mathit{C}}_{\mathrm{VF}}^{\mathit{b}}\left[\begin{array}{c}\hfill {\mathit{m}}^{\mathit{b}}\hfill \\ \hfill {\mathit{f}}^{\mathit{b}}\hfill \end{array}\right].$$

(17)

The term F _h ^b is manual force acting on the end-effector with respect to body frame, δ X^b refers to the dual vectors of translational deformation and rotational deformation in body frame. A complete constraint corresponds to a full-rank compliance matrix C, a deficient-rank matrix indicates an incomplete constraint. The compliance matrix ${\mathbf{C}}_{\mathrm{VF}}^{\mathit{b}}$ is defined in body-fixed frame ‘b’, if g ∈ SE(3) represents the right coordinate transformation from frame ‘b’ to frame ‘a’, then the compliance matrix ${\mathbf{C}}_{\mathrm{VF}}^{\mathit{a}}$ in the new coordinate frame ‘a’ is given by

$${\mathbf{C}}_{\mathrm{VF}}^{\mathit{a}}=\mathit{A}{\mathit{d}}_{{\mathit{g}}^{‒1}}{\mathbf{C}}_{\mathrm{VF}}^{\mathit{b}}\mathit{A}{\mathit{d}}_{{\mathit{g}}^{‒1}}^{\ast }$$

(18)

A customized virtual fixture for complicated surgical tasks can be treated as the combination of one or more primitives [27]. In Table 1, we lists the preferred motion screws for four static primitive tasks. The featured screw system could be redundant, twist matrices intersection is a method to reduce the redundancy of multiple allowable screw sets [28, 29].

Table 1 Four primitive tasks and the corresponding preferred motion screw

Dynamic virtual fixture for admittance-type device

Considering that the end-effector is of admittance type, which can be modeled as non-backdrivable kinematic system, and all joints are equipped with velocity-source actuators. The VF on admittance type device acts as anisotropic compliant wall, the end-effector complies with the manual force along preferred the directions. Bettini and Marayong [5, 7] combined the deviation error into the allowable motion subspace and redefined a new VF that makes it possible to reduce the deviations manually. However, it has been shown that manual compensation does not necessarily compensate for all deviations, especially when the VF is defined for translation and the deviation error is of orientation. Castillo-Cruces et al. [14] added linear error feedback term into the admittance controller to compensate rotation and translation deviations separately and autonomously along the non-preferred directions.

However, the first-order admittance controller with manual or autonomous error compensation is inadequate to adapt to dynamic environment. This section extends these VFs with dynamic tracking into a deforming environment on SE(3). The admittance-type robot can be modelled as a left invariant first-order fully actuated systems on SE(3), and satisfies the quasi-equilibrium condition ${\dot{\mathbf{V}}}^{\mathit{b}}=\mathbf{0}$.

$$\begin{array}{l}\dot{\mathit{g}}=\mathit{g}{\mathbf{V}}^{\mathit{b}}\\ {\mathit{V}}^{\mathit{b}}=\left({\mathit{\omega }}^{\mathit{b}},{\mathit{v}}^{\mathit{b}}\right)\in \mathit{se}\left(3\right)\\ \left[\begin{array}{c}\hfill {\mathit{\omega }}^{\mathit{b}}\hfill \\ \hfill {\mathit{v}}^{\mathit{b}}\hfill \end{array}\right]={\mathbf{K}}_{\mathit{c}}{\mathbf{C}}_{\mathrm{VF}}\left(\mathit{g},{\mathit{g}}_{\mathit{d}}\right)\left[\begin{array}{c}\hfill {\mathit{m}}^{\mathit{b}}\hfill \\ \hfill {\mathit{f}}^{\mathit{b}}\hfill \end{array}\right]+{\mathit{U}}_{\mathit{DVF}}^{\mathit{Ad}}(\mathit{g},{\mathit{g}}_{\mathit{d}})\end{array}$$

(19)

The VF is defined as task-dependent and time-varying compliance matrix ${\mathbf{C}}_{\mathrm{VF}}\left(\mathit{g},{\mathit{g}}_{\mathit{d}}\right)$, ${\mathbf{C}}_{\mathrm{VF}}^{\mathit{b}}$ , velocity V^b, and manual force ${\mathit{F}}^{\mathit{b}}=\left({\mathit{m}}^{\mathit{b}},{\mathit{f}}^{\mathit{b}}\right)\in \mathfrak{se}\left(3\right)*$ acting on the end-effector are represented in body-fixed frame. Admittance gain K _c =diag(c _r I _3×3, c _p I _3×3) controls the compliance of the robot to user input, choosing c _r , c _p low imposes additional resistance along the preferred directions, high compliance could result in fast robot motion. Velocity of end-effector is controlled jointly by the admittance controller and dynamic tracking controller U _DVF ^Ad to make the robot state g track the reference g _d , which is described by ġ _d  = V _d g _d . The deviation of robot state g relative to the reference g _d with respect to the reference frame is defined as e = g _d ⁻¹ g, the matrix form is given by

$$\mathit{e}=\left[\begin{array}{cc}\hfill {\mathbf{R}}_{\mathit{d}}^{\mathit{T}}\mathbf{R}\hfill & \hfill {\mathbf{R}}_{\mathit{d}}^{\mathit{T}}\left(\mathit{p}-{\mathit{p}}_{\mathit{d}}\right)\hfill \\ \hfill \mathbf{0}\hfill & \hfill 1\hfill \end{array}\right]$$

(20)

It is also called natural error and independent of the choice of inertia frame [17]. Another equivalent definition is e^b = g⁻¹ g _d , the deviation is seen in body frame. These two error definitions and their logarithmic coordinate are mutually invertible.

$$log\left(\mathit{e}\right)=-log\left({\mathit{e}}^{\mathit{b}}\right)$$

(21)

Define the configuration error ė = e V _e as the first case, the error velocity with respect to inertia frame is given by

$${\mathit{V}}_{\mathit{e}}=-\mathit{A}{\mathit{d}}_{{\mathit{g}}^{-1}}{\mathit{V}}_{\mathit{d}}+{\mathit{V}}^{\mathit{b}}$$

(22)

We design the dynamic tracking controller with logarithmic error feedback in matrix form

$${\mathit{U}}_{\mathit{DVF}}^{\mathit{Ad}}\left(\mathit{g},{\mathit{g}}_{\mathit{d}}\right)=\mathit{A}{\mathit{d}}_{{\mathit{g}}^{-1}}{\mathit{V}}_{\mathit{d}}-{\mathbf{K}}_{\mathit{p}}log\left(\mathit{e}\right)$$

(23)

where K _p = k _p D _e , D _e =diag(k _r I _3×3, I _3×3), k _p ,k _r  > 0, the linear coefficient k _p controls the speed of error convergence, maximum choice of k _p is restricted by the actuation power of motors, the rotation-translation (R-T) ratio k _r controls the synchronous the rotation and translation. We take this control law into the robot kinematics and cancel out manual effect, the error control function of the close loop system satisfies

$${\mathit{V}}_{\mathit{e}}+{\mathbf{K}}_{\mathit{p}}log\left(\mathit{e}\right)=0$$

(24)

The control law exponentially stabilizes the configuration error e at identity I from any initial configuration. Let X = log (e(t)) = (ψ, q) ∊ se(3) be the logarithmic coordinate of e(t), we can relate the column vector form Ẋ and V _e through

$$\dot{\mathit{X}}={\displaystyle \sum _{\mathit{n}=0}^{\infty }\frac{{\left(-1\right)}^{\mathit{n}}{\mathrm{B}}_{\mathit{n}}}{\mathit{n}!}}\mathrm{a}{\mathrm{d}}_{\mathit{X}}^{\mathit{n}}\left({\mathit{V}}_{\mathit{e}}\right)={\mathrm{B}}_{\mathrm{X}}{\mathit{V}}_{\mathit{e}}$$

(25)

where the {B_n} are Bernoulli numbers, the symbols B_x denote the Lie bracket series [17]. We have

B_X X = X and ${\mathrm{B}}_{\mathrm{X}}\left[\begin{array}{l}\mathit{0}\\ \mathit{q}\end{array}\right]=\left[\begin{array}{l}\mathit{0}\\ \mathit{A}{\left(\mathit{\psi }\right)}^{-\mathit{T}}\mathit{q}\end{array}\right]$

so

$$\dot{\mathit{X}}=-{\mathrm{B}}_{\mathrm{X}}\left({\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\left[\begin{array}{l}\mathit{\psi }\\ \mathit{q}\end{array}\right]+\left({\mathit{k}}_{\mathit{p}}-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\right)\left[\begin{array}{l}\mathit{0}\\ \mathit{q}\end{array}\right]\right)=-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\mathit{X}-\left({\mathit{k}}_{\mathit{p}}-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\right)\left[\begin{array}{l}\mathit{0}\\ \mathit{A}{\left(\mathit{\psi }\right)}^{-\mathit{T}}\mathit{q}\end{array}\right]$$

Separating the rotational part and the translational parts

$$\dot{\mathit{\psi }}=-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\mathit{\psi }$$

(26)

$$\begin{array}{l}\dot{\mathit{q}}=-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\mathit{q}-{\mathit{k}}_{\mathit{p}}{\mathrm{R}}_{\mathit{\psi }}\mathit{q}\\ {\mathrm{R}}_{\mathit{\psi }}=\left(1-{\mathit{k}}_{\mathit{r}}\right)\mathit{A}{\left(\mathit{\psi }\right)}^{-\mathit{T}}\end{array}$$

(27)

the effect of rotation on the translation is featured by operator ℛ _ψ , which is complex and highly dependent on the rotation axis ψ. We can adjust the interconnection between rotation and translation by choosing R-T ratio k _r . The rotation part is exponentially stabilized for all initial rotation matrix R(0) such tr(R) ≠ −1. Regarding the translational part, consider the candidate Lyapunov function $\mathit{W}\mathit{=}\frac{1}{2}{∥ \mathit{q}∥ }^2$. Its time derivative satisfies

$$\begin{array}{l}\frac{\mathit{d}}{\mathit{dt}}\mathit{W}=-〈\mathit{q},{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\mathit{q}+\left({\mathit{k}}_{\mathit{p}}-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\right)\mathit{A}{\left(\mathit{\psi }\right)}^{-\mathit{T}}\mathit{q}〉\\ =-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}{∥ \mathit{q}∥ }^2-\left({\mathit{k}}_{\mathit{p}}-{\mathit{k}}_{\mathit{p}}{\mathit{k}}_{\mathit{r}}\right)\left({∥ {\mathit{q}}_{\left|\right|}∥ }^2+\mathit{\alpha }\left(∥ \mathit{\psi }∥ \right){∥ {\mathit{q}}_{⟂}∥ }^2\right)\\ <0\end{array}$$

where α(y) ≜ (y/2) cot (y/2), 0 < ‖ψ‖ < 1, q = q _|| + q _⟂ is the orthogonal decomposition of q along span {ψ} and {ψ}_⟂. Thus global exponential stability is proven also for translational part.

Any deviation of the end-effector from the reference is compensated autonomously. This error compensation is regarded as a target tracing acting on the virtual fixture [14]. The discrete realization of the VF controller usually introduces time delay and dynamic tracking errors, forecast of the reference velocity V _d helps improve the tracking precision. As seen in Figure 1, the instantaneous reference and allowable motion set are defined from multiple geometric constraints; manual operation is regulated by the compliance controller, which is synthesized by the allowable motion set. The operator has complete control along the preferred directions, fast motion along the non-preferred direction and deviations relative to the reference are compensated autonomously. The dynamic tracking term in the DVF controller helps the surgeon to deal with operations in deforming environment.

Figure 1

Control loop of DVF for admittance-type device.

Experiments, results and discussion

The proposed DVF is designed for fully actuated robotic system with force/torque sensor. However, both force/torque sensor and admittance controlled device are not available. We designed a virtual proxy based hardware-in-the-loop simulation test bed to evaluate the proposed DVF. The PHANToM Omni device, from SensAble Technologies, is equipped with 3 degree-of-freedom (DOF) actuations and 6 DOF position measurements. It serves as a force source rather than a haptic device to drive a virtual admittance-type proxy alternatively. The device is actuated passively by human force that applied on the end-effector, while there is no haptic feedback to the operator.

The kinematics equation of the admittance-type proxy refers to

$$\begin{array}{l}{\dot{\mathit{g}}}_{\mathit{P}}={\mathit{g}}_{\mathit{P}}{\mathbf{V}}_{\mathit{P}}^{\mathit{b}}\\ {\mathit{V}}_{\mathit{P}}^{\mathit{b}}={\mathbf{C}}_{\mathrm{VF}}\left(\mathit{g},{\mathit{g}}_{\mathit{d}}\right){\mathit{F}}^{\mathit{b}}+{\mathit{U}}_{\mathit{DVF}}^{\mathit{Ad}}(\mathit{g},{\mathit{g}}_{\mathit{d}})\end{array}$$

(28)

the proxy is controlled by the DVF controller in Eq. (19). The proxy complies with the actuation force F^b acting on the proxy with respect to body-fixed frame, which is estimated from the robot dynamics of the PHANToM device.

$$\begin{array}{l}{\mathbf{J}}^{\mathit{T}}\left(\mathbf{\Theta }\right){\mathit{F}}^{\mathit{s}}=\mathbf{M}\left(\mathbf{\Theta }\right)\ddot{\mathbf{\Theta }}-\mathbf{N}(\mathbf{\Theta },\dot{\mathbf{\Theta }})\dot{\mathbf{\Theta }}\\ {\mathit{F}}^{\mathit{b}}=\mathit{A}{\mathit{d}}_{\mathit{g}}^{*}{\mathit{F}}^{\mathit{s}}\end{array}$$

(29)

where M(Θ) is the inertia matrix, $\mathbf{N}\left(\mathit{\Theta },\dot{\mathit{\Theta }}\right)$ is the Coriolis matrix, the gravity term C(Θ) is eliminated from the actual manual force, kinematics and dynamics details of the Phantom Omni device can be found in [30]. F^s represents the manual force with respect to the inertia frame, J(Θ) is manipulator Jacobian matrix, U _DVF ^Ad refers to the dynamic tracking controller in Eq. (23).

The real haptic device and the virtual admittance-type proxy are integrated in the MATLAB/Simulink environment by the PHANSIM TOOLKIT. The overview of the hardware-in-the-loop test bed for DVF implementation on admittance-type proxy is illustrated in Figure 2. The control loop of the haptic device runs at 1 kHz, we use the PHANToM clock to down sample the joint measurements to 200Hz. The joint angles Θ of the PHANToM Omni are processed by a low-pass filter and a differentiator successively to estimate the joint velocity $\dot{\mathbf{\Theta }}$ and acceleration $\ddot{\mathbf{\Theta }}$, manual force F^b are estimated by the Phantom robot dynamics in Eq. (29). The proxy state g _P is updated by proxy body velocity V _P ^b discretely, which is jointly controlled by admittance controller and dynamic error compensator Eq. (28). The proxy motion is constrained on a deforming sphere to simulate the beating heart surgery environment. We use an external clock to update the reference state g _d , proxy state and the deforming radius. For fully apprehension, a 3D virtual environment is built up and visualized using the MATLAB Virtual Reality Toolbox. Both the proxy and the reference are modelled as rigid pens with unit isotropic inertia matrix.

Figure 2

Hardware-in-the-loop test bed for DVF simulation on admittance-type proxy.

In the simulation experiment, we simplify the admittance controller robot as a virtual proxy, manual force is estimated indirectly through robot dynamics rather than a real force sensor. The proxy workspace is not restricted compared to a real robot, measurement errors of joint angles are not involved, manual force is also inaccurate due to inertia parameters estimation. We will evaluate the proposed algorithm on a real admittance controlled device when all instruments are ready in the future.

Numerous surgical tasks require the surgeon to follow a predetermined path on a deformable anatomy tissue surface while maintaining the shaft orientation within a safe range, such as minimally invasive beating heart surgery, the area of interest deforms with heartbeat and respiratory motion, instrument motion should be regulated to satisfy the varying geometric constraints. We simplified this task as a non-holonomic constraint problem by moving a tool on a deforming ball, while the tool shaft along the radial direction, the axial rotation is not constrained. The radius r of the ball varies periodically, r = r ₀ + Δr sin ωt, r ₀ = 15 mm, Δr = 5 mm. The bandwidth of the position measurements is 26 Hz, which refers to the upper limit of tissue motion resulted from physiologic movements, such as heartbeat and respiration [16].

The instantaneous reference g _d refers to the one with minimum norm in Eq. (14). The allowable motions on the deforming sphere refer to translation along the tangent directions and rotation around the radius direction, the corresponded allowable motion set S is defined in the inertia frame,

$${\mathit{S}}_1=\left(\mathit{0},{\mathit{r}}_{\mathit{x}}\right),{\mathit{S}}_2=\left(\mathit{0},{\mathit{r}}_{\mathit{y}}\right),{\mathit{S}}_3=\left({\mathit{r}}_{\mathit{z}},{\mathit{p}}_{\mathit{d}}\times {\mathit{r}}_{\mathit{z}}\right),\mathit{S}=\left\{{\mathit{S}}_1,{\mathit{S}}_2,{\mathit{S}}_3\right\}$$

where r _x , r _y , r _z are unit axis vectors of g _d , p _d is the translation part. The compliance matrix C _VF is synthesized by the allowable motion screw set S. The workspace of the Phantom Omni device is 1:1 mapped into the proxy workspace, the corresponding coordinate axes are parallel and without rotational transformation. The initial state of the proxy tool is set to be the inkwell-calibrated configuration. The translation and orientation errors of the tool with respect to the reference are defined as

$$\begin{array}{l}\mathit{e}={\mathit{g}}_{\mathit{d}}^{-1}\mathit{g}=\left({\mathbf{R}}_{\mathit{d}}^{\mathit{T}}\mathbf{R},{\mathbf{R}}_{\mathit{d}}^{\mathit{T}}\left(\mathit{p}-{\mathit{p}}_{\mathit{d}}\right)\right)\\ {\mathit{e}}_{\mathit{p}}=∥ \mathit{p}-{\mathit{p}}_{\mathit{d}}∥ \\ {\mathit{e}}_{\mathit{r}}=∥ \mathit{\psi }∥ \end{array}$$

(30)

Rotational error e _r describes the angle of the rotation about the equivalent axis ψ, translational error e _p refers to the distance between tool tip p and reference p _d . We use tracking error and following index to evaluate the performance of the proposed dynamic DVF in deforming environment. Translational tracking error (TTE) and rotational tracking error (RTE) refer to the root-mean-square of translation and orientation errors respectively during the human-robot cooperation. The following index represents how quick and how well the proxy can follow manual operation, or how much manual input can be transferred to the task related commands. More specifically, the following index is defined as the cross-correlation coefficient of manual force F^b = (m^b, f^b) and corresponded velocity acted on the proxy V _C ^b = (ω _C ^b, v _C ^b), which consists of the admittance controller output and the logarithmic error feedback in Eq. (22). The rotational following index (RFI) and translational following index (TFI) for admittance-type device are defined as

$$\mathit{RFI}=corr\left({\mathit{\omega }}_{\mathit{C}}^{\mathit{b}},{\mathit{m}}^{\mathit{b}}\right)=\frac{{\displaystyle \sum {\left({\mathit{\omega }}_{\mathit{C}}^{\mathit{b}}-{\overline{\mathit{\omega }}}_{\mathit{C}}^{\mathit{b}}\right)}^{\mathit{T}}\left({\mathit{m}}^{\mathit{b}}-{\overline{\mathit{m}}}^{\mathit{b}}\right)}}{\sqrt{{\displaystyle \sum {∥ {\mathit{\omega }}_{\mathit{C}}^{\mathit{b}}-{\overline{\mathit{\omega }}}_{\mathit{C}}^{\mathit{b}}∥ }^2}{\displaystyle \sum {∥ {\mathit{m}}^{\mathit{b}}-{\overline{\mathit{m}}}^{\mathit{b}}∥ }^2}}}$$

(31)

$$\mathit{TFI}=corr\left({\mathit{v}}_{\mathit{C}}^{\mathit{b}},{\mathit{f}}^{\mathit{b}}\right)=\frac{{\displaystyle \sum {\left({\mathit{v}}_{\mathit{C}}^{\mathit{b}}-{\overline{\mathit{v}}}_{\mathit{C}}^{\mathit{b}}\right)}^{\mathit{T}}\left({\mathit{f}}^{\mathit{b}}-{\overline{\mathit{f}}}^{\mathit{b}}\right)}}{\sqrt{{\displaystyle \sum {∥ {\mathit{v}}_{\mathit{C}}^{\mathit{b}}-{\overline{\mathit{v}}}_{\mathit{C}}^{\mathit{b}}∥ }^2}{\displaystyle \sum {∥ {\mathit{f}}^{\mathit{b}}-{\overline{\mathit{f}}}^{\mathit{b}}∥ }^2}}}$$

(32)

Three experiments were conducted to validate the efficiency of proposed DVF for admittance-type proxy.

Proportional feedback control experiment

In this experiment, we compare the performance of three control laws to stabilizes the robot state g at fixed state g _d from any initial state g(0) = (R(0), p(0)). The deviation of the end-effector state relative to the reference is defined as e = g _d ⁻¹ g, the logarithmic coordinates refers to $log\left(\mathit{e}\right)=\left(\mathit{\psi },\mathit{q}\right)\in \mathrm{se}\left(3\right)$.

1). Separate feedback

$$\left[\begin{array}{c}\hfill {\mathit{\omega }}^{\mathit{b}}\hfill \\ \hfill {\mathit{v}}^{\mathit{b}}\hfill \end{array}\right]=-\left[\begin{array}{cc}\hfill {\mathit{k}}_{\mathit{w}}{\mathbf{I}}_{3\times 3}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathit{k}}_{\mathit{v}}{\mathbf{I}}_{3\times 3}\hfill \end{array}\right]\left[\begin{array}{l}\mathit{\psi }\\ \mathit{p}-{\mathit{p}}_{\mathit{d}}\end{array}\right]$$

2). Double-geodesic feedback

$$\left[\begin{array}{c}\hfill {\mathit{\omega }}^{\mathit{b}}\hfill \\ \hfill {\mathit{v}}^{\mathit{b}}\hfill \end{array}\right]=-\left[\begin{array}{cc}\hfill {\mathit{k}}_{\mathit{w}}{\mathbf{I}}_{3\times 3}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathit{k}}_{\mathit{v}}{\mathbf{I}}_{3\times 3}\hfill \end{array}\right]\left[\begin{array}{l}\mathit{\psi }\\ {\mathbf{R}}^{\mathit{T}}\left(\mathit{p}-{\mathit{p}}_{\mathit{d}}\right)\end{array}\right]$$

3). Logarithmic feedback

$$\left[\begin{array}{c}\hfill {\mathit{\omega }}^{\mathit{b}}\hfill \\ \hfill {\mathit{v}}^{\mathit{b}}\hfill \end{array}\right]=-\left[\begin{array}{cc}\hfill {\mathit{k}}_{\mathit{w}}{\mathbf{I}}_{3\times 3}\hfill & \hfill \mathbf{0}\hfill \\ \hfill \mathbf{0}\hfill & \hfill {\mathit{k}}_{\mathit{v}}{\mathbf{I}}_{3\times 3}\hfill \end{array}\right]\left[\begin{array}{l}\mathit{\psi }\\ \mathit{q}\end{array}\right]$$

Law-1 takes the error definition and feedback control from Castillo-Cruces’s work [14], which treats translation and rotation separately. Law-2 applies proportional feedback actions along geodesic directions for both rotation and translation in SO(3) and R³ respectively, law-3 is the logarithmic feedback control. Law-1 is dependent on the choice of inertia frame, both law-2 and law-3 are independent of inertia frame choice. To compare the proportional feedback laws presented above, the same planar motion SE(2), initial state g ₀, destination state g _d and proportional parameters k _w , k _v are set for the three control laws in our experiment.

As seen in Table 2, there are not obvious difference in the residual errors of the three laws, expect for their behaviors during the convergence period. All three control laws converge at the same rate in rotational part, as seen in Figure 3, law-1 shows more curved behaviour and takes the longest path to approach the fixed state, law-2 follows the shortest path. The performance of Law-2 deteriorates for 2^nd order robotic systems, only the logarithmic feedback control could locally exponentially stabilized the robot state at I, more detail comparison can be found in [17]. We take the logarithmic feedback law in our DVF controller design, with which the robot performs steadily stable, both the rotational and translational part converged uniformly to zero, no oscillations appeared.

Table 2 Residual errors of three proportional feedback control laws

Figure 3

Three proportional actions on SE(2) for 1st kinematics system with k _w  = 2 , k _v  = 1, (a) translation error, (b) convergence trajectories, each point is depicted as a frame.

We also analyzed the influence of proportional parameter k _p of the logarithmic error feedback controller. The proxy is asked to approximate the nearest reference on the sphere autonomously and manual excitation is still. The initial state of the proxy is set to be the inkwell-calibrated position. As shown in Figure 4, k _p controls the speed of error convergence, maximum choice of k _p is restricted by the actuation power of motors

Figure 4

Influence of k _p on error convergence with deforming frequency 2 rad/s, R/T ratio =1, (a) translational error, (b) rotational error.

Dynamic tracking experiment

To discusses the dynamic properties, the operator dragged the gimbal pen of haptic device back and forth along x-axis in the inertia frame, the tangent components of manual force drove the proxy move along the circle of latitude on the deforming sphere with frequency varies from 0 to 40 rad/s, R-T ratio =1, c _r =0.1 rad/s, c _p =100 mm/s. As shown in Figure 5, the TTE increases linearly with deforming frequency growth, the impact of k _p on the dynamic error is not clear, but the impact of dynamic compensation (DC) is obvious. The RTE is suppressed by linear logarithm error compensation, and is independent of deforming frequency. This is due to the fact that orientation of reference remained constant while translation along the radial direction, translational deviation along the same direction is compensated autonomously; the proxy is actuated manually along tangent directions, the referred orientation varied accordingly. The discrete realization of the DVF controller usually introduces time delay and dynamic tracking error, forecast of the reference velocity V _d helps improve the tracking precision.

Figure 5

Results of dynamic tracking experiment with difference k _p settings and different tracking strategies (without dynamic compensation (DC), with dynamic compensation, with dynamic forecasting compensation (DFC)), (a) translational tracking error; (b) rotational tracking error.

As seen in Figure 5, the TTE of the VF with DC is smaller than the VF without DC; furthermore, the VF with dynamic forecasting compensation (DFT) performs much better than the VF with DT. The DVF can improve the dynamic properties in low-frequency range, its dynamic performance deteriorated slowly with deforming frequency growth, this is due to the first-order kinematics property of the admittance-type device. R-T ratio can affect the synchronous of rotation and translation as well as the tracking error convergence speed because of interconnection between rotation and translation on SE(3). We recorded the TTE and settling time with deforming frequency 40 rad/s, k _p =20, admittance gains c _r =0.1 rad/s, c _p =100 mm/s, R-T ratio was set as 4,3,2,1,0.5 respectively, the results are listed in Table 3. Appropriate settings of R-T ratio help speed up the error convergence and reduce error level.

Table 3 Impact of R/T ratio on dynamic tracking error

Human-robot collaboration experiment

This experiment is designed to analyze human factors on human-robot collaboration with assistance of the DVF controller. To analyze the influence of manual force on the tracking error, three people took part in this experiment. The operator applied time-varied force along x-axis with respect to inertia frame, manual force and tracking error are depicted in Figure 6(a-b); the operator pivoted the gimbal pen around arbitrary axis, the tool shaft orientation deviated from the reference accordingly, manual torque and the tracking error are plotted in Figure (c-d).

Figure 6

The tracking error of multiple manual operations with k _p  = 2, ω = 5 rad/s, R-T ratio = 1, c _r  = 0.1 rad/s, c _p  = 100 mm/s; (a) manual force when moving along axis-x; (b) resulted error of operation (a), (c) manual torque when pivoting around arbitrary axis; (d) resulted error of operation (c).

As seen in Figure 6, changes in the magnitude and direction of manual force had influence on rotational and translational errors. The operator dragged the tool along 8-shaped curve with three intensities of manual force{low, medium, high}, k _p =10, deforming frequency 5 rad/s, the following index RFI and TFI, dynamic tracking error TTE and RTE are recorded in Table 3.

As seen in Table 3, the dynamic tracking error is linearly related to manual force intensity and R-T ratio, the compliances c _r , c _p play the same role as they scaled the manual force intensity that acted on the proxy. Faster manual operation resulted in larger dynamic tracking error. The proportional gain of the rotational part is multiplied by R-T ratio. The VF controller dominates the rotation over manual operation when large R-T ratio is set, the angular velocity resulted from human input is less than that from the logarithmic error feedback controller, and then the rotational following index decreases accordingly. A fair distribution on rotation control between the autonomous controller and human operation is achieved when the R-T ratio is low; the rotational following index has little change. The translational following index is negatively related to the R-T ratio, this is because the impact of rotation part on the translation is weakened when small R-T ratio is set. As seen from Tables 3 and 4, we can also conclude that higher R-T ratio helps suppress the dynamic tracking error, however it could decrease the rotational following index during human-robot cooperation, choosing R-T ratio small can increase the tracking error and the translational following index, the choice of R-T ratio should compromise between user experience and the tracking error. More specifically, the inter-relationship between the components of manual moment-force pair (m^b, f^b) and the output velocity (ω _C ^b, v _C ^b) are featured by the correlation coefficients of F^b and V _C ^b. The operator is asked to drag the tool randomly on the deforming sphere for five minutes with deforming frequency 5 rad/s, k _p =10. All the data are standardized to eliminate the variable dimension difference. The Pearson product–moment correlation coefficients are calculated for all one-to-one element combinations in F^b and V _C ^b with confidence interval 99% [31].

Table 4 Results of shape-8 operation with assistance of DVF on admittance proxy

As seen in Table 5, the difference of correlation coefficient reveals that the DVF controller has stronger control over the manual operation in tool shaft orientation. Rotation ω_z  round the tool shaft and translation v_x, v_y  along tangent directions are strictly linear correlative to manual input m_z, f_x, f_y  respectively, the operator has completely control over these direction. The correlation between manual moment m_x, m_y  and tangent velocity v_x, v_y are notable due to the coupling between rotation and translation. Velocity v_z is independent of the manual input; the DVF controller takes charge of the deviation compensation along the deforming direction.The trajectories of the operator driving the proxy along straight line and circle on the deforming sphere are illustrated in Figure 7, the complicated behaviors of dynamic task is beyond human control, making a distinction between the preferred and non-preferred directions and employed different control strategy helps reduce operation burden of surgeons and improve their skills in dynamic tasks.

Table 5 The correlation between manual force and proxy velocity

Figure 7

Trajectories of the proxy along straight line (a) and circle (b) on the deforming sphere with k _p  = 5, ω = 5 rad/s, R-T ratio = 1, c _r  = 0.1 rad/s, c _p  = 100 mm/s.

Conclusions

This paper described a novel framework of DVF for admittance-type manipulators on the Euclidean Group SE(3) to assist the surgeons to deal with the dynamic tasks, which unites rotation and translation in a compact form We constructed the holonomic/ non-holonomic constraints, and then searched for the corresponded references to make a distinction between preferred and non-preferred directions. Different control strategies are employed to deal with the task along these directions. The DVF can improve the dynamic properties of human-robot cooperation in low-frequency deforming environment, and maintain synergy of orientation and translation during the operation. The experiments show that the DVF implemented on the virtual admittance-type proxy can assist the user to deal with the skilled operations in deforming environment. We will evaluate the proposed algorithm on a real admittance controlled device when all instruments are ready in the future.

Notation and nomenclature

a; scalar

f; force vector

m; moment vector

ω; angular velocity vector

v; translational velocity vector

R; rotation matrix

0; null vector

0; null matrix

q; Vector

$\widehat{\mathit{q}}$; cross-product matrix of vector q

SE(3); Lie group

SE(2); Displacement subgroup of Lie group

SO(3); Rotation group

se (3); Lie algebra

so(3); Lie algebra of rotation group

se(3)*; dual space of Lie algebra

so(3)*; dual space of so(3)

g; elements in SE(3)

$\mathbf{V}=\left(\widehat{\mathit{\omega }},\mathit{v}\right)$; matrix form velocity in se(3)

V = (ω, v); column vector form velocity in se(3)

$\mathbf{X}=\left(\widehat{\mathit{\psi }},\mathit{q}\right)$; the matrix form logarithmic coordinates of g

X = (ψ, q); column vector form logarithmic coordinates of g

* ^b; element relative to the body frame

* ^s; element relative to the spatial frame

${\mathrm{Ad}}_{\mathit{g}},{\mathrm{Ad}}_{\mathit{g}}^{\ast }$; adjoint transformation

${\mathrm{ad}}_{\mathit{X}},{\mathrm{ad}}_{\mathit{X}}^{\ast }$; Lie bracket

exp  _SE(3)( * ); exponential map on SE(3)

log _{S E(3)}(∗); logarithmic map on SE(3)

‖ ∗ ‖_G; norm on the matrix Lie Group SE(3)

Generally, we use italic small characters for scalars, italic bold for vectors , bold capitals for matrices, italic bold capitals for vectors of Lie algebra.