Needle-Tissue Interaction Force Estimation using Residuals and Interaction Models

Developed by: M. Sodano, F. Roscia, G. Roggiolani (2020).

Supervisor: prof. M. Vendittelli.

Achievement: Medical Robotics exam.

The goal is to reconstruct the contact forces exerted on the needle mounted on the end-effector of a robot manipulator, in a sensorless fashion. This is been achieved considering that contact as a fault in the actuation system. The considered fault detection and isolation techniques are based on generalized momenta, which require proprioceptive sensors only. Then, a method for identifying rupture events is presented. Using the Recursive Least Squares (RLS) algorithm, the needle-tissue interaction force is estimated and the estimation error is analyzed to extract information about possible layer transitions.

Laboratory data of a KUKA LWR IV+ robot manipulator with a needle mounted on the end-effector penetrating multi-layered gel and meat piece have been used.

Implementation is done in MATLAB R2018b.

Sensorless Reconstruction of Interaction Forces via Residual Method

Consider the Lagrangian model of the robot

where is the vector of force and moment exerted by the environment on the tip of the needle and is the geometric Jacobian of the manipulator at the contact point. The joint torque caused by the contact is defined as .

This contact joint torque is reconstructed by means of the residuals , defined as

where is the generalized momentum. During free motion . When a contact happens, some entries of may vary (they will return to zero when the contact is lost). For large values of , the evolution of will reproduce the evolution of .

Finally, the force estimation:

The force estimation via residuals has been validated with the force measurement provided by the force/torque sensor mounted on the manipulator.

For the implementation, the residual equation has been discretized as

Detection of Layer Transition

Typical needle-tissue interaction force versus displacement can be seen in the figure below. Five phases can be noticed: no contact (1), contact (2), layer rupture (3), tip and shaft insertion (4), retraction (5).

Interaction Model

Call the position of the tip of the needle with respect to the base frame. The interaction force is described by means of the Kelvin-Voigt generalized model, that captures both the elastic force due to the resistance of the layer surface to the needle insertion and the damping force due to the viscous friction along the needle:

Interaction Force Prediction

The Recursive Least Squares (RLS) algorithm used to estimate the Kelvin-Voigt parameters and their covariance matrix is

leading to the estimate interaction force:

The estimation error can be defined as

Layer Transition

Inspecting the estimation error, informations about layer ruptures can be found. As a matter of fact, neglecting modelling errors, disturbances and noise, the estimated force should reconstruct well the measured force unless an abrupt change happens: typically, this is due to a layer rupture. The decision function

is introduced. If

then a layer ruptures is supposed to occur. The parameters and are the variance of the error function in the default and abrupt change case.

The main contribution of this work is an off-line method for identifying these variances based on a statistical analysis of the estimation error and its first-order time derivative.

1. From the plot of the measured force, identify the number of expected layer transitions. Notice that each transition introduces a peak in the error function (and therefore, two peaks in its derivative);
2. Compute the local maxima of and sort them in decreasing order. Pick the -th one: because of the above reasoning, it should not depend on any abrupt event, but only on modelling errors, disturbances and noise. Denote the value of this peak with .
3. Divide the time in two disjoint sets, and : the former will denote the times for which a puncturing event is expected to occur, the latter contains the others. For any abrupt event, denote with: the first time instant for which and the last time instant for which . Finally

   

   

4. Finally compute

Execution Requirements

The main.m runs the residual method for estimating the estimation force and the RLS algorithm to predict layer transitions consequently. At first, it calls the script data.m, that reads the data-set and the quantities that are necessary for the elaboration. The script is present as an example. The script datasets.m is not necessary for the elaboration, and is a collection of data-sets used during the project.

In the function forceReconstruction.m, lines 23-24, there is the possibility of including the friction torque. You should uncomment only one of the two lines. If you are willing to use a different robot from the KUKA LWR IV+, the friction torque vector at line 24 must be computed externally.

In the function layerDetection.m, some initializations must be carried out. In particular, the covariance matrix is initialized at lines 15-17, and also its threshold for enabling the covariance resetting method. Parameters have been chosen with experience.

References

Mattone, R., & De Luca, A. (2003). "Fault detection and isolation in robot manipulators". IFATIS, IRAR002R01.

Cacciotti, N., Cifonelli, A., Gaz, C., Paduano, V., Russo, A. V., Vendittelli, M. (2018). "Enhancing force feedback in teleoperated needle insertion through on-line identification of the needle-tissue interaction parameters". IEEE RAS and EMBS International Conference on Biomedical Robotics and Biomechatronics (BIOROB).