Open AccessArticle

6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter

Istituto Dalle Molle di studi sull’Intelligenza Artificiale (IDSIA), Scuola Universitaria Professionale della Svizzera Italiana (SUPSI), Università della Svizzera Italiana (USI), 6928 Manno, Switzerland

Department of Mechanical Engineering, Politecnico di Milano, 20156 Milano, Italy

Author to whom correspondence should be addressed.

Machines 2020, 8(4), 67; https://doi.org/10.3390/machines8040067

Submission received: 29 September 2020 / Revised: 19 October 2020 / Accepted: 25 October 2020 / Published: 27 October 2020

(This article belongs to the Special Issue Advanced Autonomous Machines and Designs)

Download

Browse Figures

Review Reports Versions Notes

Abstract

Industrial robots are commonly used to perform interaction tasks (such as assemblies or polishing), requiring the robot to be in contact with the surrounding environment. Such environments are (partially) unknown to the robot controller. Therefore, there is the need to implement interaction controllers capable of suitably reacting to the established contacts. Although standard force controllers require force/torque measurements to close the loop, most of the industrial manipulators do not have installed force/torque sensor(s). In addition, the integration of external sensors results in additional costs and implementation effort, not affordable in many contexts/applications. To extend the use of compliant controllers to sensorless interaction control, a model-based methodology is presented in this paper for the online estimation of the interaction wrench, implementing a 6D virtual sensor. Relying on sensorless Cartesian impedance control, an Extended Kalman Filter (EKF) is proposed for the interaction wrench estimation. The described approach has been validated in simulations, taking into account four different scenarios. In addition, experimental validation has been performed employing a Franka EMIKA panda robot. A human–robot interaction scenario and an assembly task have been considered to show the capabilities of the developed EKF, which is able to perform the estimation with high bandwidth, achieving convergence with limited errors.

Keywords:

extended kalman filter; wrench estimation; 6D virtual sensor; sensorless cartesian impedance control; industrial robots; interaction robotized tasks

1. Introduction

1.1. Context

Robots are increasingly involved in daily life activities, which no longer consist of only repetitive simple tasks, but rather require interaction with an ever-changing environment, while performing a multitude of different tasks [1,2]. Considering the manufacturing context, the efforts that have to be deployed to pre-program all the possible tasks and scenarios are excessive; therefore, robots have to provide a flexible solution, adapting to new tasks/production while guaranteeing target performance [3,4]. In these complex scenarios the robot is required to learn and suitably modify its behavior on the basis of the operating conditions [5] and considering interaction tasks (i.e., robot exchanging forces/torques with the environment) the capability to adapt becomes even more critical [6]. To avoid any unwanted/unstable behavior, the interaction force must be controlled [7,8]. Common interaction control strategies, however, make use of expensive sensors [9,10,11], increasing hardware costs and implementation efforts not affordable in many contexts/applications. To avoid the use of such devices, with the robot able to adapt to uncertain interaction, many works are investigating external wrench estimation algorithms and sensorless control methodologies.

1.2. Related Works

The research community pays much attention to the achievement of a stable interaction between sensorless robots and the environment, employing efforts in developing force–sensorless methodologies to estimate the interaction between the robot and its environment. Such research area is strongly connected with the main aim of this paper, since the proper estimation of the established interaction between the robot and its environment is required to design a stable and high-performance interaction controller for a sensorless robot. Some approaches [12] focus on the derivation of high-accuracy models which are then used to estimate the interaction wrench during task execution. In the literature, to prevent the use of force sensors, disturbance observers-based solutions are developed, where the external wrench applied to the manipulator is observed exploiting the inverse of the robot model. In [13] a nonlinear disturbance observer is proposed to estimate the external interaction, allowing for the first time the possibility to consider the intrinsic nonlinearities of systems such as robot manipulators. This method guarantees the stability of the disturbance observer by properly tuning its design parameters while taking into account the physical parameters and constraints (e.g., maximum joint velocities) of the robot. A more general approach is developed in [14] where a disturbance state observer is coupled with a machine learning techniques, which are implement to identify a task-oriented dynamic model. The use of learning-based approach prevents the modeling of the robot dynamics terms (such as joints’ friction or Coriolis effects). To eliminate modeling errors, in [15] a parametric dynamics robot model coupled with machine learning techniques is designed. In this work a two-layer modeling is implemented with the combination of rigid-body dynamics (RBD) and a compensator trained with multilayer perception (MLP), ensuring a better model accuracy than each of the two model taken individually. To perform the interaction force estimation, such a modeled dynamics is then exploited in a disturbance Kalman filter based on a time-invariant composite robot model, providing robust estimation against uncertainty. To avoid the use of acceleration measurements and the computation of the inverse of the robot mass matrix that amplifies measurements noise, in [16] a sensorless admittance control scheme exploiting a generalized momentum-based disturbance observer to model a linear environment dynamics is proposed. A radial basis neural networks approach (RBNN) is used to compensate model uncertainties and, in order to properly manage the control inputs, actuation saturation is considered. Some state-of-the-art works structure the identification as an optimization problem. A different methodology that shows a deep connection with the generalized momentum-based approach, highlighted by simulation and experimental results, is developed in [17] where the filtered dynamic equations are combined with a recursive least-square estimation algorithm to provide a smooth external force estimation. Friction models, such as Coulomb friction, show uncertainties connected with the joint velocity, especially when velocities are close to zero. Solving in real time a convex optimization problem, the method proposed in [18] estimates the reaction force taking into account the aforementioned Coulomb friction uncertainties. Different types of virtual sensors based on high-performance dynamic model calibration have been proposed. A task-oriented calibrated robot dynamic model, which also includes the thermal state of the robot manipulator, is designed in [19]. The proposed dynamic model is calibrated by means of a two-stage optimization which provides suitable paths later combined in exciting trajectories. The estimation of the external force is obtained, using the residual method, as difference between the modeled and measured torques. In other works, Artificial Intelligence has been deployed to map the interaction between the robot and the environment. The designed controller is then based on the learned dynamics. Considering the scenario of working with soft tissue, in [20] the interaction is modeled as a visco-elastic system and the design of the force observer used to estimate the interaction force is based on Lyapunov time-varying equation. The force estimation is then used to develop a robot position controller. Considering a bilateral tele-operation system, in order to estimate the interaction forces between the slave manipulator and the environment, in [21] an online sparse Gaussian process regression (OSGPR) approach is proposed. The described work does not need any previous knowledge of the slave manipulator dynamic model and it avoids the use of the inverse of Jacobian transpose, but the generalized model is obtained through offline training with previously acquired dataset. The interaction force is obtained in real time by means of the design estimator. Other external sensors can be used instead of force ones to acquire more data useful to assess the interaction force. In [22] exteroceptive sensing (i.e., a depth camera) is used for the detection of contacts while the residual method is deployed to evaluate the external joint torques. This approach provides a reliable estimation of the exchanged force at the contact point even in the scenario of multiple contact points. Optical Coherence Tomography (OCT) images are classified with a Neural Network in [23]. The network is trained on images from a Finite Element Method (FEM) simulation of the deformed sclera, while a Bayesian filter is used to parameterize the model. In [24] Convolutional Neural Networks and Long-Short Term Memory networks are used to process the spatio-temporal information included in video sequences and tool data to assess the interaction force.

1.3. Paper Contribution

Extending the work in [25], a model-based methodology is presented in this paper for the online estimation of the interaction wrench, implementing a 6D virtual sensor. Relying on sensorless Cartesian impedance control (to give to the controlled robot a compliant behavior while interacting with an unknown environment), an Extended Kalman Filter (EKF) is proposed for the interaction wrench estimation. The proposed EKF is capable of estimating the forces and torques acting at the robot end-effector, making possible to implement a 6D virtual sensor.

The interaction wrench can be considered to be a deterministic variable (i.e., a model of the interaction between the robot and the environment can be derived and exploited for its estimation). Although other approaches can be used to model the interaction wrench dynamics (such as sequential Monte Carlo, unscented Kalman filter, and particle filtering methodologies [26,27,28,29,30]), they require the measurement (i.e., samples) of the interaction wrench for the training of the algorithm. In many practical cases, this is not possible or, if a force/torque sensor is available, the sensor is also used online, i.e., not requiring the implementation of an estimation algorithm. The here presented approach, instead, exploiting the well-known robot dynamics modeling, is capable of performing the estimation of the interaction wrench without any use of wrench data for the algorithm training.

The described approach has been validated in simulations, taking into account four different scenarios. In addition, experimental validation has been performed employing a Franka EMIKA panda robot. A human–robot interaction scenario and an assembly task have been considered to show the capabilities of the developed EKF, which is able to perform the estimation with high bandwidth, achieving convergence with limited errors.

2. Sensorless Cartesian Impedance Control

The Cartesian impedance controller guarantees a compliant behavior during interaction. Such controller must be implemented to design the proposed Extended Kalman Filter. The following manipulator dynamics is considered [31]:

B (q) \ddot{q} + C (q, \dot{q}) + g (q) + τ_{f} (\dot{q}) = τ - J {(q)}^{T} h_{e x t},

(1)

where

q

is the joint position vector,

B (q)

is the inertia matrix,

C (q, \dot{q})

is the Coriolis vector,

g (q)

is the gravitational vector,

τ_{f} (\dot{q})

is the joint friction vector,

J (q)

is the Jacobian matrix, and

h_{e x t} = {[f, C]}^{T}

is the external force/torque vector,

τ

is the joint torque vector. Based on Equation (1), the sensorless Cartesian impedance controller with dynamic compensation [31] is designed, defining the joint torque vector

τ

as:

τ = B (q) γ + C (q, \dot{q}) + g (q) + τ_{f} (\dot{q}),

(2)

where

γ

is the sensorless Cartesian impedance control law. Translational

\ddot{p}

and rotational

{\ddot{φ}}_{c d}

(described by the intrinsic Euler angles representation) accelerations of the sensorless Cartesian impedance controller

γ

can be written as:

\begin{matrix} \ddot{p} & = M_{t}^{- 1} (- D_{t} \dot{p} - K_{t} Δ p), \\ {\ddot{φ}}_{c d} & = M_{r}^{- 1} (- D_{r} {\dot{φ}}_{c d} - K_{r} φ_{c d}) . \end{matrix}

(3)

These equations describe, respectively, the translational part and the rotational part of the sensorless Cartesian impedance control. Regarding the translational part,

M_{t}

represents the mass matrix,

D_{t}

the damping matrix,

K_{t}

the stiffness matrix, the actual Cartesian positions vector and the target position vector are, respectively,

p

and

p^{d}

, while

Δ p = p - p^{d}

. Instead, concerning the rotational part,

M_{r}

represents the inertia matrix,

D_{r}

and

K_{r}

are again the damping matrix and the stiffness matrix respectively,

R_{c}

is the compliant frame at the end-effector,

R_{d}

is the target frame, and the mutual orientation between these two frames is represented by

R_{c}^{d} = R_{d}^{T} R_{c}

, from which the set of Euler angles

φ_{c d}

is extracted. Considering the rotational part of the sensorless Cartesian, it is possible to compute the angular accelerations

\dot{ω} c d

{\dot{ω}}_{c d} = T (φ_{c d}) (M_{r}^{- 1} (- D_{r} {\dot{φ}}_{c d} - K_{r} φ_{c d})) + \dot{T} (φ_{c d}) {\dot{φ}}_{c d},

(4)

where matrix

T (φ_{c d})

defines the transformation from Euler angles derivatives to angular velocities

ω_{c d} = T (φ_{c d}) {\dot{φ}}_{c d}

, and

ω = R_{e e} ω_{c d}

(with

R_{e e}

the rotation matrix from the robot base to its end-effector) [31]. By defining

{\tilde{M}}_{r} = {(R_{e e} T (φ_{c d}))}^{- 1} M_{r}

and

{\tilde{D}}_{r} = D_{r} - {\tilde{M}}_{r} R_{e e} \dot{T} (φ_{c d})

, Equation (4) can be written as:

\dot{ω} = {\tilde{M}}_{r}^{- 1} (- {\tilde{D}}_{r} {\dot{φ}}_{c d} - K_{r} φ_{c d}) .

(5)

The formulation resulting from Equations (3)–(5), can be written in a compact form as follows:

{\ddot{x}}^{i m p} = - M^{- 1} (D \dot{x} + K Δ x),

(6)

where

{\ddot{x}}^{i m p} = [{\ddot{x}}_{t}; {\ddot{x}}_{r}] = [\ddot{p}; \dot{ω}]

is the target acceleration computed by the sensorless Cartesian impedance control.

M = [M_{t} 0; 0 {\tilde{M}}_{r}]

D = [D_{t} 0; 0 {\tilde{D}}_{r}]

K = [K_{t} 0; 0 K_{r}]

are the sensorless Cartesian impedance mass, damping and stiffness matrices composed by both the translational and rotational parts, and

Δ x = x - x^{d} = [Δ p; φ_{c d}]

x

is the current robot end-effector pose vector including both translational and rotational components, while

x^{d}

is the reference robot end-effector pose vector including both translational and rotational components. The sensorless Cartesian impedance control law

γ

can then be written as follows:

γ = J {(q)}^{- 1} ({\ddot{x}}^{i m p} - \dot{J} (q, \dot{q}) \dot{q}) .

(7)

In general, matrix

J {(q)}^{- 1}

can be substituted with the pseudo-inverse of the Jacobian matrix

J {(q)}^{#}

[32]. Substituting Equation (2) in (1), under the hypothesis that the manipulator dynamics is known (such identification can be performed with state-of-the-art techniques [33]), the controlled robot dynamics results in:

\ddot{q} = γ - B {(q)}^{- 1} J {(q)}^{T} h_{e x t},

(8)

where

h_{e x t} = [f, T^{T} (φ_{c d}) μ^{d}]

(considering the external forces

f

and torques

μ^{d}

- referred to the target frame

R_{d}

—acting on the robot related to the interaction with the environment). The substitution of Equation (7) into (8) leads to:

J (q) \ddot{q} + \dot{J} (q, \dot{q}) \dot{q} = \ddot{x} = {\ddot{x}}^{i m p} - J (q) B {(q)}^{- 1} J {(q)}^{T} h_{e x t},

(9)

with

\ddot{x} = J (q) \ddot{q} + \dot{J} (q, \dot{q}) \dot{q}

the resulting Cartesian acceleration of the robot end-effector resulting from the implementation of the proposed sensorless Cartesian impedance controller. Finally, substituting Equation (6) into (9), the controlled robot dynamics resulting from the design of the sensorless Cartesian impedance control is described by the following equation:

M \ddot{x} + D \dot{x} + K Δ x = - \bar{L} (q) h_{e x t},

(10)

where

\bar{L} (q) = M J (q) B {(q)}^{- 1} J {(q)}^{T}

. The resulting dynamic equation is therefore coupled in the Cartesian degrees of freedom (DoFs) by the matrix

\bar{L} (q)

Remark 1.

The sensorless Cartesian impedance control is therefore resulting in a coupled controlled robot dynamics. Matrix

\bar{L} (q)

redistributes interaction forces along all the Cartesian DoFs. Although the decoupled robot behavior cannot be achieved implementing such controller, the sensorless Cartesian impedance control strategy allows implementation of a tunable compliant robot behavior, ensuring a safe and stable interaction with the target environment.

3. Extended Kalman Filter for External Wrench Estimation

In this Section, the Extended Kalman Filter (EKF) for interaction wrench estimation is designed. The authors defined an augmented filter state which comprehends translational and rotational components of position and velocities of the robot, respectively

x

and

\dot{x}

, and the external interaction wrench

h_{e x t}

x_{a} = {[\dot{x}, x, h_{e x t}]}^{T} .

(11)

The augmented filter state

x_{a}

is then substituted in the interaction dynamics Equation (10) to write the state-space interaction dynamics:

{\dot{x}}_{a} = [\begin{matrix} - M^{- 1} D & - M^{- 1} K & - M^{- 1} \bar{L} (q) \\ 1 & 0 & 0 \\ 0 & 0 & 0 \end{matrix}] x_{a} + [\begin{matrix} M^{- 1} K \\ 0 \\ 0 \end{matrix}] x^{d} = A_{s p} x_{a} + B_{s p} x^{d},

(12)

where

A_{s p}

is the state-space matrix and

B_{s p}

is the input matrix.

To account for the uncertainties in the model, a variable

ν_{a} = [ν_{x}, ν_{\dot{x}}, ν_{h_{e x t}}]

is included in the filter dynamics. The resulting equations represent the filter dynamics:

f (x_{a}, ν_{a}) = [\begin{matrix} \ddot{x} \\ \dot{x} \\ {\dot{h}}_{e x t} \end{matrix}] = [\begin{matrix} M^{- 1} (- D \dot{x} - K x - \bar{L} (q) h_{e x t} + K x^{d} + ν_{x}) \\ \dot{x} + M^{- 1} ν_{\dot{x}} \\ ν_{h_{e x t}} \end{matrix}],

(13)

Therefore, calling

{\hat{x}}_{a}

the augmented state estimate,

C_{a}

the observation matrix for the robot velocity

\dot{x}

and the robot position

x

, and

K_{E K F}

the gain matrix, the EKF is defined as:

\{\begin{matrix} {\dot{\hat{x}}}_{a} & = f (x_{a}, ν_{a}) + K_{E K F} (y - C_{a} {\hat{x}}_{a}), \\ \hat{y} & = h (x_{a}, w), \end{matrix}

(14)

where the gain matrix

K_{E K F}

is computed as follows:

K_{E K F} = P C_{a} R^{- 1} .

(15)

R

represents the measurements noise covariance matrix:

R = H E {w w^{T}} H^{T} = H W H^{T} .

(16)

The observation function

h

linearly maps the sample inaccuracies, due to measurement noise

w

, through the matrix

H

H = {\frac{\partial h}{\partial w}|}_{{\hat{x}}_{a}} .

(17)

The covariance matrix

P

and its rate:

\dot{P} = A_{a} P - P C_{a}^{T} R^{- 1} C_{a} P + Q + P A_{a}^{T},

(18)

are based on the dynamics of the state and on the model uncertainties. Matrices

A_{a}

and

G_{a}

are defined, respectively, as:

\begin{matrix} A_{a} = {\frac{\partial f}{\partial x_{a}}|}_{{\hat{x}}_{a}}; & G_{a} = {\frac{\partial f}{\partial ν_{a}}|}_{{\hat{x}}_{a}} . \end{matrix}

(19)

Matrix

Q

, used for the estimation of the parameters, is defined as:

Q = G_{a} E {ν_{a} ν_{a}^{T}} G_{a}^{T} = G_{a} V G_{a}^{T} .

(20)

It has to be mentioned that it is possible to neglect the evaluation of the time-derivative

\dot{\bar{L}} (q)

in Equation (19). Instead,

\bar{q} = q

is updated as soon as the values of the Jacobian

J (q)

and the inertia matrix

B (q)

are affected by the modifications of the joint configuration. This assumption is justified by the small change in the joint configuration when the robot interacts with the environment while performing a task, such as assembly, or at least such modification dynamics is much slower than the dynamics of the interaction. Therefore, Equation (13) can be modified accordingly:

f (x_{a}, ν_{a}) = [\begin{matrix} \ddot{x} \\ \dot{x} \\ {\dot{h}}_{e x t} \end{matrix}] = [\begin{matrix} M^{- 1} (- D \dot{x} - K x - \bar{L} (\bar{q}) h_{e x t} + K x^{d} + ν_{x}) \\ \dot{x} + M^{- 1} ν_{\dot{x}} \\ ν_{h_{e x t}} \end{matrix}] .

(21)

Remark 2.

The proposed EKF has been discretized for its implementation and online usage [34].

4. Simulation Results

In this Section, the results of the interaction wrench’s estimation are evaluated, deploying the proposed Extended Kalman Filter in different simulations. The Robotics Toolbox [35] is used to implement the kinematics and dynamics of Franka EMIKA panda.

The robot is controlled through the sensorless Cartesian impedance control described in Section 2. The impedance control matrices have been imposed as diagonals and the parameters are selects as follows: the mass parameters of the diagonal matrix

M

have been selected equal to 10 kg while the inertia parameters have been imposed equal to 10 kg/m

^{2}

; the translation and the rotational parameters of the diagonal stiffness matrix

K

have been selected respectively equal to 1000 N/m and 5000 Nm/rad; the diagonal matrix

h

is composed of damping ratio parameters equal to 2. The damping ratio can be exploited in order to compute the damping matrix as

D = 2 h \sqrt{M K}

Since the Franka EMIKA panda robot is redundant, its null-space configuration must be managed. In the proposed robot control implementation, a pure damping behavior is exploited for the null-space configuration management, damping the null-space motion:

τ_{R} = B (q) ((I - J {(q)}^{#} J (q)) (- D_{n} \dot{q})),

(22)

where

τ_{R}

is the null-space control torque,

I

is the identity matrix,

J {(q)}^{#}

is the pseudo-inverse of the Jacobian matrix, and

D_{n}

is the null-space damping diagonal matrix. The term

(I - J {(q)}^{#} J (q))

is the null-space projection matrix. The term

- D_{n} \dot{q}

allows the dampening of the null-space motion. The control law Equation (2) is, therefore, modified as follows:

τ = B (q) γ + C (q, \dot{q}) + g (q) + τ_{f} (\dot{q}) + τ_{R} .

(23)

The control torque

τ_{R}

acts in the null-space of the manipulator, i.e., not affecting the Cartesian motion of the robot. Indeed, the Cartesian controlled robot behavior in [10] is not affected by this term, together with the proposed estimation provided by the EKF in Section 3.

Four simulation scenarios have been implemented:

# 1

constant external wrench applied to the robot;

# 2

variable-sinusoidal external wrench applied to the robot;

# 3

probing task in a full-coupled robot-environment scenario;

# 4

sliding task on a stiff environment. In the following, such scenarios are analyzed.

4.1. $# 1$ Constant External Wrench

A constant external wrench is imposed after

0.5

s from the starting of the simulation, with a magnitude of 20 N for the interaction forces

f

, and 5 Nm for the interaction torques

C

. In Figure 1 shows the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 2, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

are shown. As it can be seen from the provided plots, a fast dynamics is achieved (bandwidth of the implemented EKF of 5 Hz, an order of magnitude higher than the one implemented by the sensorless Cartesian impedance controller). A zero steady-state estimation error is achieved. The obtained performance shows the capabilities of the algorithm to perform the estimation in the proposed scenario. The rms has been also computed for each force and torque error component to show the limited generalized mean estimation error.

4.2. $# 2$ Variable-Sinusoidal External Wrench

A variable-sinusoidal excitation is applied to the robot to verify the capabilities of the proposed EKF within a dynamic scenario. The applied external wrench is in the following form:

h_{ext} = A cos (2 π f_{1} (1 + \frac{1}{20} cos (2 π f_{2} t)) t + ϕ) .

(24)

The diagonal matrix

A

contains the magnitudes of the applied forces/torques (

[20, 25, 30]

N have been considered for the interaction forces

f

, and

[5, 7.5, 10]

Nm for the interaction torques

C

f_{1}

and

f_{2}

define the frequencies of the variable-sinusoidal profile, and the related parameters have been imposed to

0.25

Hz for

f_{1}

, and

0.85

Hz for

f_{2}

ϕ

contains the random phases of the variable-sinusoidal profile. In Figure 3 the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 4, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

are shown. As it can be seen from the provided plots, the estimation provided by the proposed EKF is capable of following the profile of the applied wrench. The obtained performance shows the capabilities of the algorithm to perform the estimation in the proposed scenario. The rms has been also computed for each force and torque error component to show the limited generalized mean estimation error.

4.3. $# 3$ Probing Task

A probing task has been simulated, with the robot approaching a target environment along the z vertical direction. Once the environment is reached, a full-coupled robot-environment scenario is simulated, modeling the target environment as a pure elastic system with a stiffness matrix with the following parameter values: stiffness along translational DoFs:

[10, 000, 15, 000, 40, 000]

N/m; stiffness along rotational DoFs:

[100, 100, 100]

Nm/rad. In Figure 5 the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 6, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

are shown. As it can be seen from the provided plots, a fast dynamic is achieved. A zero steady-state estimation error is achieved. Limited transition errors are shown. The obtained performance shows the capabilities of the algorithm to perform the estimation in the proposed scenario. The rms has been also computed for each force and torque error component to show the limited generalized mean estimation error.

4.4. $# 4$ Sliding Task

A sliding task has been simulated, with the robot in contact along the z vertical direction and sliding on the surface of the environment along x and y directions. Friction forces have been simulated (

f_{f, x} = - {\dot{x}}_{t, x} F_{x}

f_{f, y} = - {\dot{x}}_{t, y} F_{y}

, with

F_{x} = 30

Ns/m, and

F_{y} = 50

Ns/m). A stiffness parameter

K_{e, z} = 40, 000

N/m has imposed to model the elastic contact between the robot and the environment. In Figure 7 the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 8, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

5. Experimental Results

In this Section, experimental results related to the evaluation of the proposed EKF for the estimation of the interaction wrench are shown. A Franka EMIKA panda robot has been employed as a test platform. The wrench estimation provided by the EKF has been compared with the measured wrench obtained from the robot (exploiting its joint-level torque sensors).

The sensorless Cartesian impedance control in Section 2 has been employed to control the robot. The impedance control matrices have been imposed as diagonals and the parameters are selects as follows: the mass parameters of the diagonal matrix

M

have been selected equal to 10 kg while the inertia parameters have been imposed equal to 10 kg m

^{2}

; the translation and the rotational parameters of the diagonal stiffness matrix

K

have been selected respectively equal to 1000 N/m and 5000 Nm/rad; the diagonal matrix

h

is composed of damping ratio parameters equal to 1.

To manage the redundancy of the robot, the controller in Equation (23) has been implemented. The friction compensation has been performed as proposed in [36].

Two experimental scenarios have been tested:

# 1

a human–robot interaction scenario;

# 2

an assembly task. In the following, such scenarios are analyzed.

5.1. $# 1$ Human–Robot Interaction

In the here proposed scenario, the robot is controlled exploiting the proposed sensorless Cartesian impedance controller, maintaining a fix setpoint. The human interacts with the manipulator along its kinematic chain, applying forces and torques. In Figure 9 the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 10, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

are shown. As it can be seen from the provided plots, a fast dynamic is achieved. Limited errors are shown during the human–robot interaction. In particular, most of the estimation errors are shown around zero forces/torques. Such estimation errors are related to the non-perfect friction compensation, resulting in fictitious external wrench. The obtained performance shows the capabilities of the algorithm to perform the estimation in the proposed scenario. The rms has been also computed for each force and torque error component to show the limited generalized mean estimation error.

5.2. $# 2$ Assembly Task

The proposed task consists of an assembly of a gear into its shaft. The target task is shown in Figure 11. The main task direction is z and, therefore, a reference force

f_{z}^{d} = 30

N has been defined to perform the insertion task. A PI controller has been implemented in order to track such a reference force, exploiting the estimated force

{\hat{f}}_{z}

to close the control loop. In Figure 12 the estimated interaction forces

\hat{f}

and interaction torques

\hat{C}

vs. the applied interaction forces

f

and torques

C

are represented. In Figure 13, the force estimation error

{\hat{e}}_{f}

and the torque estimation error

{\hat{e}}_{C}

are shown. As it can be seen from the provided plots, a fast dynamic is achieved. A limited steady-state estimation error is achieved (around 3 N for forces, around

0.3

Nm for torques). Limited transition errors are shown. The obtained performance shows the capabilities of the algorithm to perform the estimation in the proposed scenario. In addition, the proposed experiment was able to show the possibility to exploit the wrench estimation for control purposes. The rms has been also computed for each force and torque error component to show the limited generalized mean estimation error.

6. Conclusions

The presented paper proposed a sensorless model-based methodology (exploiting sensorless Cartesian impedance control and Extended Kalman Filter) to estimate the interaction wrench. The applied methodology is therefore capable of implementing a 6D virtual sensor for the estimation of both interaction forces and torques. The described approach has been validated in both simulations and experiments, employing a Franka EMIKA panda manipulator. Simulation and experimental results show fast dynamics performing the proposed estimation and limited estimation errors. Estimation errors are shown mostly at zero interaction (i.e., where friction becomes critical) and at transitions (where the proposed filter dynamics is not able to track the real interaction). The proposed filter can therefore be applied to such robotics applications with dynamics slower than the achieved one, where the interaction forces/torques are needed to close a control loop (such as assembly tasks). Current/future work is devoted to improve the estimation accuracy of the proposed EKF developing local high-performance friction compensation algorithms based on learning techniques [37]. The design of a sensorless force control exploiting the proposed 6D virtual sensor is under investigation, exploiting SDRE control [38] for the tuning of both impedance matrices and setpoint. The optimization of the EKF gains is under investigation, making use of machine learning techniques.

Author Contributions

Conceptualization, L.R.; Formal analysis, L.R. and A.B.; Funding acquisition, L.R.; Methodology, L.R. and A.B.; Project administration, L.R.; Software, L.R. and A.B.; Supervision, F.B. and D.P.; Validation, L.R. Writing—original draft, L.R. and A.B.; Writing—review and editing, L.R. and A.B.; All authors have read and agreed to the published version of the manuscript.

Funding

The work has been developed within the project ASSASSINN, funded from H2020 CleanSky 2 under grant agreement n. 886977.

Conflicts of Interest

The authors declare no conflict of interest.

References

Ben-Ari, M.; Mondada, F. Robots and their applications. In Elements of Robotics; Springer: Berlin, Germany, 2018; pp. 1–20. [Google Scholar]
Yang, G.Z.; Bellingham, J.; Dupont, P.E.; Fischer, P.; Floridi, L.; Full, R.; Jacobstein, N.; Kumar, V.; McNutt, M.; Merrifield, R.; et al. The grand challenges of Science Robotics. Sci. Robot. 2018, 3, eaar7650. [Google Scholar] [CrossRef]
Polverini, M.P.; Rossi, R.; Morandi, G.; Bascetta, L.; Zanchettin, A.M.; Rocco, P. Performance improvement of implicit integral robot force control through constraint-based optimization. In Proceedings of the 2016 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Daejeon, South Korea, 9–14 October 2016; pp. 3368–3373. [Google Scholar]
Mohamed, Z.M. Flexible Manufacturing Systems: Planning Issues and Solutions; Taylor & Francis: Oxfordshire, UK, 2018. [Google Scholar]
Dattaprasad, S.; Rao, Y.V. A Survey of Various Robot Learning Techniques. Int. J. Pure Appl. Math. 2018, 118, 3823–3831. [Google Scholar]
Hogan, N. Impedance control: An approach to manipulation. In Proceedings of the 1984 American control conference, San Diego, CA, USA, 6–8 June 1984; pp. 304–313. [Google Scholar]
Vukobratovic, M. Robot-environment dynamic interaction survey and future trends. J. Comput. Syst. Sci. Int. 2010, 49, 329–342. [Google Scholar] [CrossRef]
Roveda, L.; Pedrocchi, N.; Tosatti, L.M. Exploiting impedance shaping approaches to overcome force overshoots in delicate interaction tasks. Int. J. Adv. Robot. Syst. 2016, 13, 1729881416662771. [Google Scholar] [CrossRef] [Green Version]
Roveda, L.; Pedrocchi, N.; Beschi, M.; Tosatti, L.M. High-accuracy robotized industrial assembly task control schema with force overshoots avoidance. Control. Eng. Pract. 2018, 71, 142–153. [Google Scholar] [CrossRef]
Roveda, L. Adaptive interaction controller for compliant robot base applications. IEEE Access 2018, 7, 6553–6561. [Google Scholar] [CrossRef]
Polverini, M.P.; Formentin, S.; Merzagora, L.; Rocco, P. Mixed Data-Driven and Model-Based Robot Implicit Force Control: A Hierarchical Approach. IEEE Trans. Control. Syst. Technol. 2019, 28, 1258–1271. [Google Scholar] [CrossRef]
Janot, A.; Vandanjon, P.O.; Gautier, M. A generic instrumental variable approach for industrial robot identification. IEEE Trans. Control. Syst. Technol. 2013, 22, 132–145. [Google Scholar] [CrossRef]
Chen, W.H.; Ballance, D.J.; Gawthrop, P.J.; O’Reilly, J. A nonlinear disturbance observer for robotic manipulators. IEEE Trans. Ind. Electron. 2000, 47, 932–938. [Google Scholar] [CrossRef] [Green Version]
Colomé, A.; Pardo, D.; Alenya, G.; Torras, C. External force estimation during compliant robot manipulation. In Proceedings of the 2013 IEEE International Conference on Robotics and Automation, Karlsruhe, Germany, 6–10 May 2013; pp. 3535–3540. [Google Scholar]
Hu, J.; Xiong, R. Contact force estimation for robot manipulator using semiparametric model and disturbance Kalman filter. IEEE Trans. Ind. Electron. 2017, 65, 3365–3375. [Google Scholar] [CrossRef]
Peng, G.; Yang, C.; He, W.; Chen, C.L.P. Force Sensorless Admittance Control With Neural Learning for Robots With Actuator Saturation. IEEE Trans. Ind. Electron. 2020, 67, 3138–3148. [Google Scholar] [CrossRef] [Green Version]
Van Damme, M.; Beyl, P.; Vanderborght, B.; Grosu, V.; Van Ham, R.; Vanderniepen, I.; Matthys, A.; Lefeber, D. Estimating robot end-effector force from noisy actuator torque measurements. In Proceedings of the 2011 IEEE International Conference on Robotics and Automation, Shanghai, Cina, 9–13 May 2011; pp. 1108–1113. [Google Scholar]
Linderoth, M.; Stolt, A.; Robertsson, A.; Johansson, R. Robotic force estimation using motor torques and modeling of low velocity friction disturbances. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems, Tokyo, Japan, 3–7 November 2013; pp. 3550–3556. [Google Scholar]
Villagrossi, E.; Simoni, L.; Beschi, M.; Pedrocchi, N.; Marini, A.; Tosatti, L.M.; Visioli, A. A virtual force sensor for interaction tasks with conventional industrial robots. Mechatronics 2018, 50, 78–86. [Google Scholar] [CrossRef]
Sharifi, M.; Talebi, H.; Shafiee, M. Adaptive estimation of robot environmental force interacting with soft tissues. In Proceedings of the 2015 3rd RSI International Conference on Robotics and Mechatronics (ICROM), Tehran, Iran, 7–9 October 2015; pp. 371–376. [Google Scholar]
Dong, A.; Du, Z.; Yan, Z. A sensorless interaction forces estimator for bilateral teleoperation system based on online sparse Gaussian process regression. Mech. Mach. Theory 2020, 143, 103620. [Google Scholar] [CrossRef]
Magrini, E.; Flacco, F.; De Luca, A. Estimation of contact forces using a virtual force sensor. In Proceedings of the 2014 IEEE/RSJ International Conference on Intelligent Robots and Systems, Chicago, IL, USA, 14–18 September 2014; pp. 2126–2133. [Google Scholar]
Mendizabal, A.; Sznitman, R.; Cotin, S. Force classification during robotic interventions through simulation-trained neural networks. Int. J. Comput. Assist. Radiol. Surg. 2019, 14, 1601–1610. [Google Scholar] [CrossRef] [Green Version]
Marban, A.; Srinivasan, V.; Samek, W.; Fernández, J.; Casals, A. A recurrent convolutional neural network approach for sensorless force estimation in robotic surgery. Biomed. Signal Process. Control. 2019, 50, 134–150. [Google Scholar] [CrossRef] [Green Version]
Roveda, L.; Piga, D. Interaction Force Computation Exploiting Environment Stiffness Estimation for Sensorless Robot Applications. In Proceedings of the 2020 IEEE International Workshop on Metrology for Industry 4.0 & IoT, Rome, Italy, 3–5 June 2020; pp. 360–363. [Google Scholar]
Wan, E.A.; Van Der Merwe, R. The unscented Kalman filter for nonlinear estimation. In Proceedings of the IEEE 2000 Adaptive Systems for Signal Processing, Communications, and Control Symposium (Cat. No. 00EX373), Lake Louise, AL, Canada, 4 October 2000; pp. 153–158. [Google Scholar]
Andrieu, C.; Doucet, A.; Holenstein, R. Particle markov chain monte carlo methods. J. R. Stat. Soc. Ser. 2010, 72, 269–342. [Google Scholar] [CrossRef] [Green Version]
Chopin, N.; Jacob, P.E.; Papaspiliopoulos, O. SMC2: An efficient algorithm for sequential analysis of state space models. J. R. Stat. Soc. Ser. 2013, 75, 397–426. [Google Scholar] [CrossRef] [Green Version]
Urteaga, I.; Bugallo, M.F.; Djurić, P.M. Sequential Monte Carlo methods under model uncertainty. In Proceedings of the 2016 IEEE Statistical Signal Processing Workshop (SSP), Palma de Mallorca, Spain, 26–29 June 2016; pp. 1–5. [Google Scholar]
Martino, L.; Read, J.; Elvira, V.; Louzada, F. Cooperative parallel particle filters for online model selection and applications to urban mobility. Digit. Signal Process. 2017, 60, 172–185. [Google Scholar] [CrossRef] [Green Version]
Siciliano, B.; Villani, L. Robot Force Control, 1st ed.; Kluwer Academic Publishers: Norwell, MA, USA, 2000. [Google Scholar]
Chang, P.R.; Lee, C.G. Residue arithmetic VLSI array architecture for manipulator pseudo-inverse Jacobian computation. In Proceedings of the 1988 IEEE International Conference on Robotics and Automation, Philadelphia, PA, USA, 24–29 April 1988; pp. 297–302. [Google Scholar]
Pedrocchi, N.; Villagrossi, E.; Vicentini, F.; Molinari Tosatti, L. On robot dynamic model identification through sub-workspace evolved trajectories for optimal torque estimation. In Proceedings of the 2013 IEEE/RSJ International Conference on Intelligent Robots and Systems (IROS), Tokyo, Japan, 3–7 November 2013; pp. 2370–2376. [Google Scholar]
Roveda, L.; Iannacci, N.; Tosatti, L.M. Discrete-time formulation for optimal impact control in interaction tasks. J. Intell. Robot. Syst. 2018, 90, 407–417. [Google Scholar] [CrossRef]
Corke, P. Robotics, Vision and Control: Fundamental Algorithms in MATLAB^® Second, Completely Revised; Springer: Berlin, Germany, 2017; Volume 118. [Google Scholar]
Gaz, C.; Cognetti, M.; Oliva, A.; Giordano, P.R.; De Luca, A. Dynamic identification of the franka emika panda robot with retrieval of feasible parameters using penalty-based optimization. IEEE Robot. Autom. Lett. 2019, 4, 4147–4154. [Google Scholar] [CrossRef] [Green Version]
Roveda, L.; Pallucca, G.; Pedrocchi, N.; Braghin, F.; Tosatti, L.M. Iterative learning procedure with reinforcement for high-accuracy force tracking in robotized tasks. IEEE Trans. Ind. Inform. 2017, 14, 1753–1763. [Google Scholar] [CrossRef]
Çimen, T. Approximate nonlinear optimal SDRE tracking control. In Proceedings of the 17th IFAC Symp. Automatic Control in Aerospace, Toulouse, France, 25–29 June 2007; pp. 147–152. [Google Scholar]

Figure 1. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 1

simulation scenario.

Figure 1. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 1

simulation scenario.

Figure 2. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 1

simulation scenario.

Figure 2. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 1

simulation scenario.

Figure 3. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 2

simulation scenario.

Figure 3. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 2

simulation scenario.

Figure 4. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 2

simulation scenario.

Figure 4. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 2

simulation scenario.

Figure 5. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 3

simulation scenario.

Figure 5. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 3

simulation scenario.

Figure 6. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 3

simulation scenario.

Figure 6. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 3

simulation scenario.

Figure 7. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 4

simulation scenario.

Figure 7. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. real interaction forces

f

and torques

C

(dashed line) for the

# 4

simulation scenario.

Figure 8. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 4

simulation scenario.

Figure 8. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 4

simulation scenario.

Figure 9. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. measured interaction forces

f

and torques

C

(dashed line) for the

# 1

experimental scenario.

Figure 9. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. measured interaction forces

f

and torques

C

(dashed line) for the

# 1

experimental scenario.

Figure 10. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 1

experimental scenario.

Figure 10. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 1

experimental scenario.

Figure 11. Experimental assembly task, including the Franka EMIKA panda manipulator and the target gear to be installed.

Figure 12. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. measured interaction forces

f

and torques

C

(dashed line) for the

# 2

experimental scenario.

Figure 12. Estimated interaction forces

\hat{f}

and torques

\hat{C}

(continuous line) vs. measured interaction forces

f

and torques

C

(dashed line) for the

# 2

experimental scenario.

Figure 13. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 2

experimental scenario.

Figure 13. Estimated interaction forces

{\hat{e}}_{f}

and torques

{\hat{e}}_{C}

errors for the

# 2

experimental scenario.

Publisher’s Note: MDPI stays neutral with regard to jurisdictional claims in published maps and institutional affiliations.

© 2020 by the authors. Licensee MDPI, Basel, Switzerland. This article is an open access article distributed under the terms and conditions of the Creative Commons Attribution (CC BY) license (http://creativecommons.org/licenses/by/4.0/).

Share and Cite

MDPI and ACS Style

Roveda, L.; Bussolan, A.; Braghin, F.; Piga, D. 6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter. Machines 2020, 8, 67. https://doi.org/10.3390/machines8040067

AMA Style

Roveda L, Bussolan A, Braghin F, Piga D. 6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter. Machines. 2020; 8(4):67. https://doi.org/10.3390/machines8040067

Chicago/Turabian Style

Roveda, Loris, Andrea Bussolan, Francesco Braghin, and Dario Piga. 2020. "6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter" Machines 8, no. 4: 67. https://doi.org/10.3390/machines8040067

APA Style

Roveda, L., Bussolan, A., Braghin, F., & Piga, D. (2020). 6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter. Machines, 8(4), 67. https://doi.org/10.3390/machines8040067

Note that from the first issue of 2016, this journal uses article numbers instead of page numbers. See further details here.

Article Menu

6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter

Abstract

1. Introduction

1.1. Context

1.2. Related Works

1.3. Paper Contribution

2. Sensorless Cartesian Impedance Control

3. Extended Kalman Filter for External Wrench Estimation

4. Simulation Results

4.1. $# 1$ Constant External Wrench

4.2. $# 2$ Variable-Sinusoidal External Wrench

4.3. $# 3$ Probing Task

4.4. $# 4$ Sliding Task

5. Experimental Results

5.1. $# 1$ Human–Robot Interaction

5.2. $# 2$ Assembly Task

6. Conclusions

Author Contributions

Funding

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI

Article Menu

6D Virtual Sensor for Wrench Estimation in Robotized Interaction Tasks Exploiting Extended Kalman Filter

Abstract

1. Introduction

1.1. Context

1.2. Related Works

1.3. Paper Contribution

2. Sensorless Cartesian Impedance Control

3. Extended Kalman Filter for External Wrench Estimation

4. Simulation Results

4.1. # 1 Constant External Wrench

4.2. # 2 Variable-Sinusoidal External Wrench

4.3. # 3 Probing Task

4.4. # 4 Sliding Task

5. Experimental Results

5.1. # 1 Human–Robot Interaction

5.2. # 2 Assembly Task

6. Conclusions

Author Contributions

Funding

Conflicts of Interest

References

Share and Cite

Article Metrics

Article Access Statistics

Further Information

Guidelines

MDPI Initiatives

Follow MDPI

4.1. $# 1$ Constant External Wrench

4.2. $# 2$ Variable-Sinusoidal External Wrench

4.3. $# 3$ Probing Task

4.4. $# 4$ Sliding Task

5.1. $# 1$ Human–Robot Interaction

5.2. $# 2$ Assembly Task