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.
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
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).
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:
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
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.
- 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); - 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 
. - 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 
- Finally compute
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.
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).