1 Introduction

1.1 Context

Robots are nowadays involved in various tasks and domains Ben-Ari and Mondada (2018), Yang et al. (2018). Due to the high variability of the operative conditions and target tasks, it is not possible to pre-program all the possible situations. Therefore, the robot is required to learn/adapt itself to the target perceived context Dattaprasad and Rao (2018), embedding intelligence for decision-making purposes. Within the manufacturing scenario, robots have to improve flexibility (i.e., adapting to new production) while guaranteeing target performance Polverini et al. (2016a), Mohamed (2018). Taking into account interaction tasks (i.e., robot physically in contact with an external environment), the capability to adapt to new situations becomes critical Hogan (1984). To avoid any undesired or unstable behavior, the interaction has to be controlled Vukobratovic (2010). Common interaction control strategies make use of external sensors Roveda et al. (2016), Roveda et al. (2018), Roveda (2018), Polverini et al. (2019). However, such a solution increases the hardware costs and the setup time of the robotic cell. To enhance interaction applications for sensorless robots, many works are investigating external wrench estimation algorithms and sensorless control methodologies.

1.2 Related works

Being able to achieve a stable interaction between sensorless industrial robots and the environment is one of the hot-topics in the robotics research community. Force-sensorless methodologies to estimate the interaction have been proposed. Developed methodologies relying on robot dynamic identification Janot et al. (2013) have been addressed, aiming at deriving an accurate robot dynamical model. Some of the state of the art contributions exploit disturbance observers to perform the identification. In Chen et al. (2000) a nonlinear disturbance observer is proposed to estimate the external interaction. In Colomé et al. (2013) a task-oriented dynamic model learning and a robust disturbance state observer are proposed to estimate the external interaction. In Jin and Xiong (2017) a parametric robot dynamic model based on rigid-body dynamics is combined with a non-parametric model to compensate for uncertainties. A disturbance Kalman filter is then developed to perform the interaction force estimation. In Peng et al. (2020) a sensorless admittance control scheme, exploiting a disturbance observer (to model the linear environment dynamics) together with a radial basis Neural Networks approach (to compensate for modeling uncertainties), are proposed. Some state of the art works structure the identification as an optimization problem. In Van Damme et al. (2011) the filtered dynamic equations are combined with a recursive least-square estimation algorithm to provide a smooth external force estimation. In Linderoth et al. (2013) a convex optimization problem is solved in real time to estimate the interaction force accounting for velocity-dependent uncertainties of the Coulomb friction. Some state of the art methodologies exploit the definition of virtual sensors based on high-performance dynamic model calibration. In Villagrossi et al. (2018) a virtual force sensor to estimate the interaction force is proposed by exploiting a task-oriented robot dynamic model calibration. Such a dynamic model is then used to estimate the external joint torques as a difference between measured motor torques and modelled torques (i.e., residual method). Other methodologies have been developed in order to map the interaction between the robot and the environment on the basis of Artificial Intelligence approaches, exploiting such learned dynamics for control purposes. In Sharifi et al. (2015) a robot position controller is implemented by exploiting the interaction force estimation between the robotic tool and a soft tissue. In Abeywardena et al. (2019) a Neural Networks approach is proposed to map the interaction forces between the robot and a soft environment, exploiting motor current measurements. In Mendizabal et al. (2019) a Neural Network approach to classify force ranges from optical coherence tomography (OCT) images is proposed. In Marban et al. (2019) a force estimation model based on Convolutional Neural Networks and Long-Short Term Memory networks is proposed. In Dong et al. (2020) an online sparse Gaussian process regression (OSGPR) approach is proposed to estimate the interaction forces between the slave manipulator and its surrounding environment for a bilateral tele-operation system. Additionally, external sensors have been also used in order to improve the accuracy of the external force estimation. In Magrini et al. (2014) external contacts between the robot and the environment are detected using a depth camera, while determining in parallel the external joint torques using the residual method.

Sensorless methodologies to control the interaction between the robot and the environment have been also proposed. In Alonge et al. (2010) a control law to track a target force without its measurements has been proposed based on Lyapunov techniques. In Huang et al. (2013) a nonlinear matrix mapping function between each joint motor control input and end-effector actuating force/torques vector is achieved, exploiting such a mapping into a model-free fuzzy sliding mode control for interaction control. In Polverini et al. (2016b) admittance control is implemented to perform an insertion task. A real-time trajectory generator, exploiting a model-based sensorless observer of the interaction forces is proposed to perform the assembly. In Zhou et al. (2017) a force/position decentralized robust control problem for constrained reconfigurable manipulator system with parameter perturbation and unmodeled dynamics is presented. In Phuong et al. (2018) a force estimation approach based on a dither periodic component elimination Kalman filter and on a disturbance observer for realization of a fine sensorless force control system under the existence of static friction is proposed. In Nakamura et al. (2018) a notch-type friction free disturbance observer is used for sensorless force control.

1.3 Paper contribution

The proposed contribution aims to design a sensorless robust model-based interaction control methodology. Extending the work in Roveda and Piga (2020) and relying on sensorless Cartesian impedance control, the estimation of the interaction force exchanged between the robot and the environment is performed making use of an Extended Kalman Filter (EKF). The force estimation provided by the EKF is then used by the controller to: compensate for the non-linearities in the sensorless impedance control behavior; tune the impedance control parameters (i.e., stiffness and damping parameters); and to derive the State Depended Riccati Equation (SDRE) controller to achieve high-performance force-tracking. The described approach has been validated in simulations. A Franka EMIKA panda robot has been considered as a test platform. A probing task involving different materials (i.e., with different - unknown - stiffness properties) has been considered to show the capabilities of the developed control methodology. The proposed controller has been compared with the constant impedance LQR force controller in Roveda et al. (2015), highlighting the achieved improved force-tracking performance.

1.4 Paper outline

The paper is structured as follows. Section 2 provides the proposed control framework overview. Section 3 describes the implemented sensorless Cartesian impedance controller with redundancy management. Section 4 describes the derivation of the proposed Extended Kalman Filter for interaction force estimation. Section 5 describes the derivation of the proposed force controller based on SDRE approach. Section 6 provides simulation results. Conclusions and directions for future works are given in Sect. 7.

2 Methodology

Fig. 1
figure 1

Proposed control schema highlighting the EKF, the sensorless Cartesian variable impedance control and the SDRE controller

Full size image

The main objective of the proposed contribution is the definition of a robust sensorless force controller. Exploiting variable impedance control, a State Dependent Riccati Equation (SDRE) controller is proposed for force-tracking purposes. The force estimation is provided by an Extended Kalman Filter (EKF), making possible to close a force loop without the use of external sensors. In addition, the compensation of the coupled dynamics resulting from the sensorless impedance controller is also performed on the basis of the interaction force estimation. The proposed control architecture is shown in Fig. 1, highlighting the EKF and the control schema.

3 Robot control

3.1 Sensorless Cartesian impedance control

To design the proposed sensorless Extended Kalman Filter for the estimation of the environment stiffness, the sensorless Cartesian impedance controller has to be implemented. The following manipulator dynamics is considered Siciliano and Villani (2000):

$$\begin{aligned} \mathbf {B}(\mathbf {q})\ddot{\mathbf {q}}+ \mathbf {C}(\mathbf {q},\dot{\mathbf {q}})+ \mathbf {g}(\mathbf {q})+ \varvec{\tau }_f(\dot{\mathbf {q}})= \varvec{\tau }- \mathbf {J}(\mathbf {q})^T \mathbf {h}_{ext}, \end{aligned}$$
(1)

where \(\mathbf {B}(\mathbf {q})\) is the inertia matrix, \(\mathbf {C}(\mathbf {q},\dot{\mathbf {q}})\) is the Coriolis vector, \(\mathbf {g}(\mathbf {q})\) is the gravitational vector, \(\varvec{\tau }_f(\dot{\mathbf {q}})\) is the joint friction vector, \(\mathbf {q}\) is the joint position vector, \(\mathbf {J}(\mathbf {q})\) is the Jacobian matrix, and \(\mathbf {h}_{ext}\) is the external force/torque vector, \(\varvec{\tau }\) is the joint torque vector. Based on (1), it is possible to design the sensorless Cartesian impedance controller with dynamic compensation Siciliano and Villani (2000), defining the joint torque vector \(\varvec{\tau }\) as:

$$\begin{aligned} \varvec{\tau }= \mathbf {B}(\mathbf {q})\varvec{\gamma } + \mathbf {C}(\mathbf {q},\dot{\mathbf {q}})+ \mathbf {g}(\mathbf {q})+ \varvec{\tau }_f(\dot{\mathbf {q}}), \end{aligned}$$
(2)

where \(\varvec{\gamma }\) is the sensorless Cartesian impedance control law. Translational \(\ddot{\mathbf {p}}\) and rotational \(\varvec{\ddot{\varphi }}_{cd}\) (described by the intrinsic Euler angles representation) accelerations of the sensorless Cartesian impedance controller \(\varvec{\gamma }\) can be written as:

$$\begin{aligned} \ddot{\mathbf {p}}&= \mathbf {M}_t^{-1} \, \left( - \mathbf {D} \, _t \dot{\mathbf {p}} - \mathbf {K}_t \, \Delta \mathbf {p} \right) , \nonumber \\ \ddot{\varvec{\varphi }}_{cd}&= \mathbf {M}_r^{-1} \, \left( - \mathbf {D}_r \, \dot{\varvec{\varphi }}_{cd} - \mathbf {K}_r \, \varvec{\varphi }_{cd} \right) . \end{aligned}$$
(3)

Considering the translational part of the sensorless Cartesian impedance control, \(\mathbf {M}_t\) is the mass matrix, \(\mathbf {D}_t\) is the damping matrix, and \(\mathbf {K}_t\) is the stiffness matrix. \(\mathbf {p}\) is the actual Cartesian positions vector, while \(\Delta \mathbf {p} = \mathbf {p} - \mathbf {p}^d\), where \( \mathbf {p}^d \) is the target position vector. Concerning the rotational part of the sensorless Cartesian impedance control, \(\mathbf {M}_r\) is the inertia matrix, \(\mathbf {D}_r\) is the damping matrix, \(\mathbf {K}_r\) is the stiffness matrix. \(\varvec{\varphi }_{cd}\) is the set of Euler angles extracted from \( \mathbf {R}_c^d = \mathbf {R}_d^T \mathbf {R}_c \), describing the mutual orientation between the compliant frame \(\mathbf {R}_c\) (at the end-effector) and the target frame \(\mathbf {R}_d\). Angular accelerations \(\dot{\varvec{\omega }}_{cd}\) can be computed considering the rotational part of the sensorless Cartesian:

$$\begin{aligned} \dot{\varvec{\omega }}_{cd} = \mathbf {T}(\varvec{\varphi }_{cd}) \left( \mathbf {M}_r^{-1} \, \left( - \mathbf {D}_r \, \dot{\varvec{\varphi }}_{cd} - \mathbf {K}_r \, \varvec{\varphi }_{cd} \right) \right) + \dot{\mathbf {T}}(\varvec{\varphi }_{cd}) \dot{\varvec{\varphi }}_{cd}, \end{aligned}$$
(4)

where matrix \(\mathbf {T}(\varvec{\varphi }_{cd})\) defines the transformation from Euler angles derivatives to angular velocities \(\varvec{\omega }_{cd} = \mathbf {T}(\, \varvec{\varphi }_{cd}) \dot{\varvec{\varphi }}_{cd}\), and \(\varvec{\omega } = \mathbf {R}_{ee} \varvec{\omega }_{cd}\) (with \(\mathbf {R}_{ee}\) the rotation matrix from the robot base to its end-effector) Siciliano and Villani (2000). By defining \(\widetilde{\mathbf {M}}_r=\left( \mathbf {R}_{ee} \mathbf {T}(\varvec{\varphi }_{cd})\right) ^{-1} \mathbf {M}_r\) and \(\widetilde{\mathbf {D}}_r=\mathbf {D}_r - \widetilde{\mathbf {M}}_r \mathbf {R}_{ee} \dot{\mathbf {T}}(\varvec{\varphi }_{cd})\), (4) can be written as:

$$\begin{aligned} \dot{\varvec{\omega }} = \widetilde{\mathbf {M}}_r^{-1} \left( - \widetilde{\mathbf {D}}_r \, \dot{\varvec{\varphi }}_{cd} - \mathbf {K}_r \, \varvec{\varphi }_{cd} \right) . \end{aligned}$$
(5)

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

$$\begin{aligned} \ddot{\mathbf {x}}^{imp} = - \mathbf {M}^{-1} \left( \mathbf {D} \, \dot{\mathbf {x}} + \mathbf {K} \, \Delta \mathbf {x} \right) , \end{aligned}$$
(6)

where the target acceleration computed by the sensorless Cartesian impedance control is \(\ddot{\mathbf {x}}^{imp} = [\ddot{\mathbf {x}}_t; \ddot{\mathbf {x}}_r] = [\ddot{\mathbf {p}}; \dot{\varvec{\omega }}]\). \(\mathbf {M} = [\mathbf {M}_t \, \mathbf {0}; \mathbf {0} \, \widetilde{\mathbf {M}}_r]\), \(\mathbf {D} = [\mathbf {D}_t \, \mathbf {0}; \mathbf {0} \, \widetilde{\mathbf {D}}_r]\), \(\mathbf {K} = [\mathbf {K}_t \, \mathbf {0}; \mathbf {0} \, \mathbf {K}_r]\) are the sensorless Cartesian impedance mass, damping and stiffness matrices composed by both the translational and rotational parts, and \(\Delta \mathbf {x} = \mathbf {x} - \mathbf {x}^d = [\Delta \mathbf {p}; \varvec{\varphi }_{cd}]\). \(\mathbf {x}\) is the current robot end-effector pose vector including both translational and rotational components, while \(\mathbf {x}^d\) is the reference robot end-effector pose vector including both translational and rotational components. The sensorless Cartesian impedance control law \(\varvec{\gamma }\) can then be written as follows:

$$\begin{aligned} \varvec{\gamma } = \mathbf {J}(\mathbf {q})^{-1} \left( \ddot{\mathbf {x}}^{imp} - \dot{\mathbf {J}}(\mathbf {q},\dot{\mathbf {q}})\dot{\mathbf {q}}\right) . \end{aligned}$$
(7)

In general, matrix \(\mathbf {J}(\mathbf {q})^{-1}\) can be substituted with the pseudo-inverse of the Jacobian matrix \(\mathbf {J}(\mathbf {q})^{\#}\) Chang and Lee (1988). Substituting (2) in (1), under the hypothesis that the manipulator dynamics is known (such identification can be performed with state-of-the-art techniques Pedrocchi et al. 2013), the controlled robot dynamics results in:

$$\begin{aligned} \ddot{\mathbf {q}}= \varvec{\gamma } - \mathbf {B}(\mathbf {q})^{-1} \mathbf {J}(\mathbf {q})^T \mathbf {h}_{ext}, \end{aligned}$$
(8)

where \(\mathbf {h}_{ext}= [ \mathbf {f}, \mathbf {T}^T(\varvec{\varphi }_{cd}) \, \varvec{\mu }^d ]\) (considering the external forces \(\mathbf {f}\) and torques \(\varvec{\mu }^d\) - referred to the target frame \(\mathbf {R}_d\) - acting on the robot related to the interaction with the environment). Substitution of (7) into (8) leads to:

$$\begin{aligned} \mathbf {J}(\mathbf {q})\ddot{\mathbf {q}}+ \dot{\mathbf {J}}(\mathbf {q},\dot{\mathbf {q}})\dot{\mathbf {q}}= \ddot{\mathbf {x}} = \ddot{\mathbf {x}}^{imp} - \mathbf {J}(\mathbf {q})\mathbf {B}(\mathbf {q})^{-1} \mathbf {J}(\mathbf {q})^T \mathbf {h}_{ext}, \end{aligned}$$
(9)

having \(\ddot{\mathbf {x}} = \mathbf {J}(\mathbf {q})\ddot{\mathbf {q}}+ \dot{\mathbf {J}}(\mathbf {q},\dot{\mathbf {q}})\dot{\mathbf {q}}\) the resulting Cartesian acceleration of the robot end-effector resulting from the implementation of the proposed sensorless Cartesian impedance controller. Finally, substituting (6) into (9), the controlled robot dynamics resulting from the design of the sensorless Cartesian impedance control is described by the following equation:

$$\begin{aligned} \mathbf {M} \ddot{\mathbf {x}} + \mathbf {D} \dot{\mathbf {x}} + \mathbf {K} \Delta \mathbf {x} = - \overline{\mathbf {L}}(\mathbf {q}) \mathbf {h}_{ext}, \end{aligned}$$
(10)

where \(\overline{\mathbf {L}}(\mathbf {q})= \mathbf {M} \mathbf {J}(\mathbf {q})\mathbf {B}(\mathbf {q})^{-1} \mathbf {J}(\mathbf {q})^T\). The resulting dynamic equation is therefore coupled in the Cartesian degrees of freedom (DoFs) by the matrix \(\overline{\mathbf {L}}(\mathbf {q})\).

Remark 1

The sensorless Cartesian impedance control is therefore resulting in a coupled controlled robot dynamics. Matrix \(\overline{\mathbf {L}}(\mathbf {q})\) redistributes interaction forces along all the Cartesian DoFs. While the decoupled robot behavior cannot be achieved implementing such controller, the sensorless Cartesian impedance control strategy allows to implement a tunable compliant robot behavior, ensuring a safe and stable interaction with the target environment.

3.2 Redundancy management

The Franka EMIKA panda manipulator has been considered as a test platform. Such a robot is redundant, requiring to manage its null-space configuration while performing the main task. In this paper, a pure damping behavior is proposed for the null-space configuration control, aiming to damp the null-space motion:

$$\begin{aligned} \varvec{\tau }_{R} = \mathbf {B}(\mathbf {q})\left( \left( \mathbf {I} - \mathbf {J}(\mathbf {q})^{\#} \mathbf {J}(\mathbf {q})\right) \left( - \mathbf {D}_{n} \dot{\mathbf {q}}\right) \right) , \end{aligned}$$
(11)

where \(\varvec{\tau }_{R}\) is the null-space control torque, \(\mathbf {I}\) is the identity matrix, \(\mathbf {J}(\mathbf {q})^{\#}\) is the pseudo-inverse of the Jacobian matrix, and \(\mathbf {D}_{n}\) is the null-space damping diagonal matrix. The term \(\left( \mathbf {I} - \mathbf {J}(\mathbf {q})^{\#} \mathbf {J}(\mathbf {q})\right) \) is the null-space projection matrix. The term \(- \mathbf {D}_{n} \dot{\mathbf {q}}\) allows to damp the null-space motion. The control law (2) is, therefore, modified as follows:

$$\begin{aligned} \varvec{\tau }= \mathbf {B}(\mathbf {q})\varvec{\gamma } + \mathbf {C}(\mathbf {q},\dot{\mathbf {q}})+ \mathbf {g}(\mathbf {q})+ \varvec{\tau }_f(\dot{\mathbf {q}})+ \varvec{\tau }_{R}. \end{aligned}$$
(12)

The control torque \(\varvec{\tau }_{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.

4 Sensorless extended Kalman Filter for interaction forces estimation

The aim of this Section is to propose a sensorless model-based methodology to estimate the interaction force between the robot and the environment along translational directions. Therefore, in the following Extended Kalman Filter (EKF) design, only the translational Cartesian DoFs will be considered.

Exploiting the coupled robot-environment dynamic equation (10), it is possible to design the EKF to be used for the estimation of the interaction force \(\mathbf {f}\).

The robot-environment interaction dynamics can be defined by the following filter state \(\mathbf {x}_{a}\) composed by the robot velocity \(\dot{\mathbf {x}}_t\) and position \(\mathbf {x}_t\) states (along translational directions), and augmented with the interaction force \(\mathbf {f}\):

$$\begin{aligned} \mathbf {x}_{a} = [ \dot{\mathbf {x}}_t, \mathbf {x}_t, \mathbf {f}]^T. \end{aligned}$$
(13)

Substituting the augmented state \(\mathbf {x}_{a}\) (13) in the interaction dynamics model (10), the filter dynamics results in:

$$\begin{aligned} \mathbf {f}(\mathbf {x}_{\mathbf {a}},\mathbf {\varvec{\nu }})= \begin{bmatrix} \ddot{\mathbf {x}}_t \\ \dot{\mathbf {x}}_t \\ \dot{\mathbf {f}} \end{bmatrix} = \begin{bmatrix} \mathbf {M}_t^{-1}\left( - \mathbf {D}_t \dot{\mathbf {x}}_t - \mathbf {K}_t \mathbf {x}_t - \overline{\mathbf {L}}(\mathbf {q}) \mathbf {f} + \mathbf {K}_t \mathbf {x}_t^d + \varvec{\nu }_{\mathbf {x}_t} \right) \\ \dot{\mathbf {x}}_t + \mathbf {M}_t^{-1} \varvec{\nu }_{\dot{\mathbf {x}}_t} \\ \varvec{\nu }_{f} \end{bmatrix}, \end{aligned}$$
(14)

where the vector \(\varvec{\nu }_a = [\varvec{\nu }_{\mathbf {x}_t}, \varvec{\nu }_{\dot{\mathbf {x}}_t}, \varvec{\nu }_{\mathbf {f}}]^T\) accounts for uncertainties in models parameters/estimates. The observer of the augmented state is therefore defined as:

$$\begin{aligned} \left\{ \begin{aligned} \dot{\widehat{\mathbf {x}}}_{a}&= \mathbf {f(\mathbf {x}_a,\varvec{\nu }_a)} + \mathbf {K}_{EKF} (\mathbf {y} - \mathbf {C}_{a}\widehat{\mathbf {x}}_{a}), \\ \widehat{\mathbf {y}}&=\mathbf {h(\mathbf {x}_a,\mathbf {w})}, \end{aligned} \right. \end{aligned}$$
(15)

with \(\widehat{\mathbf {x}}_a\) the augmented state estimate, \(\mathbf {C}_{a}\) the observation matrix for the robot velocity \(\dot{\mathbf {x}}_t\) and the robot position \(\mathbf {x}_t\), and \(\mathbf {K}_{EKF}\) the gain matrix:

$$\begin{aligned} \mathbf {K}_{EKF} = \mathbf {P} \mathbf {C}_{a} \mathbf {R}^{-1}. \end{aligned}$$
(16)

\(\mathbf {R}\) is the measurement noise matrix defined as:

$$\begin{aligned} \mathbf {R} = \mathbf {H} E\lbrace \mathbf {w} \mathbf {w}^T\rbrace \mathbf {H}^T = \mathbf {H} \mathbf {W} \mathbf {H}^T, \end{aligned}$$
(17)

where the observation function \(\mathbf {h}\) linearly maps the sample inaccuracies, due to measurement noise \(\mathbf {w}\), through the matrix \(\mathbf {H}\):

$$\begin{aligned} \mathbf {H}=\left. \frac{\partial \mathbf {h}}{\partial \mathbf {w}}\right| _{\widehat{\mathbf {x}}_{a}}. \end{aligned}$$
(18)

The covariance matrix \(\mathbf {P}\) and its rate, as in:

$$\begin{aligned} \dot{\mathbf {P}} = \mathbf {A}_a\mathbf {P}-\mathbf {P} \mathbf {C}_{a}^T \mathbf {R}^{-1}\mathbf {C}_{a} \mathbf {P} + \mathbf {Q} +\mathbf {P}\mathbf {A}_a^T, \end{aligned}$$
(19)

are based on the dynamics of the state and the model uncertainties. Matrices \(\mathbf {A}_a\) and \(\mathbf {G}_a\) are defined respectively as:

$$\begin{aligned} \mathbf {A}_a=\left. \frac{\partial \mathbf {f}}{\partial \mathbf {x}_{a}}\right| _{\widehat{\mathbf {x}}_{a}};&\quad&\mathbf {G}_a=\left. \frac{\partial \mathbf {f}}{\partial \varvec{\nu }_{a}}\right| _{\widehat{\mathbf {x}}_{a}}. \end{aligned}$$
(20)

Matrix \(\mathbf {Q}\), used for the estimation of the parameters, is defined as:

$$\begin{aligned} \mathbf {Q} = \mathbf {G}_a E\lbrace \varvec{\nu }_a \varvec{\nu }_a^T\rbrace \mathbf {G}_a^T = \mathbf {G}_a \mathbf {V} \mathbf {G}_a^T. \end{aligned}$$
(21)

It has to be underlined that in the filter dynamics (14) the matrix \(\overline{\mathbf {L}}(\mathbf {q})\) appears as a function of the joint position \(\mathbf {q}\). Analyzing the definition of \(\overline{\mathbf {L}}(\mathbf {q})\), the joint position dependence is due to the matrices \(\mathbf {J}(\mathbf {q})\) and \(\mathbf {B}(\mathbf {q})\). It has to be noted that, when the robot is in interaction with an environment while executing a task (e.g., an assembly task), its joint configuration is not excessively modifying, or at least such modification is happening with a dynamics much slower than the interaction dynamics. It is therefore possible to neglect the time-derivative \(\dot{\overline{\mathbf {L}}}(\mathbf {q})\) in (20), considering the constant matrix \(\overline{\mathbf {L}}(\overline{\mathbf {q}})\), updating \(\overline{\mathbf {q}}=\mathbf {q}\) as soon as the robot joint configuration modifications are affecting the values of \(\mathbf {J}(\mathbf {q})\) and \(\mathbf {B}(\mathbf {q})\). In such a way, (14) can be written as follows:

$$\begin{aligned} \mathbf {f}(\mathbf {x}_{\mathbf {a}},\mathbf {\varvec{\nu }})= \begin{bmatrix} \ddot{\mathbf {x}}_t \\ \dot{\mathbf {x}}_t \\ \dot{\mathbf {f}} \end{bmatrix} = \begin{bmatrix} \mathbf {M}_t^{-1}\left( - \mathbf {D}_t \dot{\mathbf {x}}_t - \mathbf {K}_t \mathbf {x}_t - \overline{\mathbf {L}}(\overline{\mathbf {q}}) \mathbf {f} + \mathbf {K}_t \mathbf {x}_t^d + \varvec{\nu }_{\mathbf {x}_t} \right) \\ \dot{\mathbf {x}}_t + \mathbf {M}_t^{-1} \varvec{\nu }_{\dot{\mathbf {x}}_t} \\ \varvec{\nu }_{f} \end{bmatrix}, \end{aligned}$$
(22)

Remark 2

The approximation of \(\overline{\mathbf {L}}(\mathbf {q})\) in \(\overline{\mathbf {L}}(\overline{\mathbf {q}})\) introduces negligible modeling errors, that are of orders of magnitude smaller than common modeling errors resulting from the robot dynamics identification procedures, therefore not affecting the target estimation.

5 Robust state depended Riccati equation variable impedance controller

The aim of this Section is to propose the sensorless model-based robust SDRE variable impedance controller for force-tracking purposes. Since the proposed approach is model-based, the dynamic model of the interaction environment is derived in the following to characterize the coupled robot-environment interaction dynamics. Exploiting the developed EKF in Sect. 4, the estimation of the interaction force \(\widehat{\mathbf {f}}\) is performed. \(\widehat{\mathbf {f}}\) can be, therefore, used as a feedback into the controller to close the force loop. The proposed control strategy is described in the following subsections, highlighting the three main contributions: the compensation of the sensorless Cartesian impedance control non-linearities in (10), the variable impedance control parameters laws, and the implementation of the robust SDRE variable impedance force controller. While the coupled non-linear dynamics compensator and robust SDRE controller define a deformation term (respectively, \(\mathbf {x}_{t,c}\) and \(\mathbf {x}_{t,SDRE}\)) for the impedance control setpoint, the variable impedance controller defines the adaptation of the stiffness and damping matrices. The sensorless Cartesian impedance control setpoint \(\mathbf {x}_t^d\) (i.e., along translational DoFs) can be, therefore, defined as follows:

$$\begin{aligned} \mathbf {x}_t^d = \mathbf {x}_t + \mathbf {x}_{t,c} + \mathbf {x}_{t,SDRE}. \end{aligned}$$
(23)

5.1 Interaction environment dynamics modeling

In order to design the proposed SDRE variable impedance force controller, the interaction environment dynamics has to be modeled. Based on Roveda et al. (2013), the simplest way to describe the interaction environment dynamics is the linear spring-damper model (with environment stiffness matrix \(\mathbf {K}_e\), environment damping matrix \(\mathbf {D}_e\) - both diagonal and positive definite -, and environment position vector \(\mathbf {x}_e\)):

$$\begin{aligned} \sum _i ( \mathbf {D}_e^i \dot{\mathbf {x}}_{e}^i + \mathbf {K}_e^i \Delta \mathbf {x}_{e}^i ) = \mathbf {f} , \ \ \ \forall i = 1, \cdot \cdot , N, \end{aligned}$$
(24)

for all the finite number N of interaction ports. Considering the environment translational DoFs \(\mathbf {x}_{e,t}\) and under the hypothesis of a stable single contact point (i.e., the robot and the environment are always in contact with \(\mathbf {x}_{e,t}=\mathbf {x}_t\), where \(\mathbf {x}_{t}\) is the robot end-effector translational DoFs vector), (24) can be written as follows:

$$\begin{aligned} \mathbf {D}_e \dot{\mathbf {x}}_t + \mathbf {K}_e \Delta \mathbf {x}_t = \mathbf {f}. \end{aligned}$$
(25)

The identification of the environment damping \(\mathbf {D}_e\) is not possible without a persistent excitation Dehghan et al. (2015) (option not applicable in the context of the proposed contribution). The environment damping does not affect the stiffness estimation at steady-state (being related to the robot velocity in (25)), while affecting the transitory dynamics. However, since the robot end-effector velocity is limited along the interaction directions (due to the contact with the environment), the damping can be neglected in the environment dynamics (25), considering a pure elastic dynamics:

$$\begin{aligned} \mathbf {K}_e \left( \mathbf {x}_t - \mathbf {x}_{e,t}^0 \right) = \mathbf {f}, \end{aligned}$$
(26)

where \(\mathbf {x}_{e,t}^0\) is the equilibrium position of the environment. For simplicity, \(\mathbf {x}_{e,t}^0=\mathbf {0}\) is considered.

Remark 3

It has to be underlined that the proposed EKF for external force estimation in Sect. 4 is capable to detect the interaction establishment between the robot and the environment (i.e., identifying free-motion and contact situations). Therefore, the proposed robot-environment modeling can be enabled only if a contact force is detected.

5.2 Robust variable impedance force-tracking controller definition

The here proposed sensorless force controller aims at both tune online the impedance control setpoint and the impedance control parameters (i.e., stiffness and damping parameters) to achieve high performance in force-tracking (i.e., zero steady-state force error and force overshoots avoidance, achieving a high closed loop bandwidth). Three main contributions are defined into the controller structure: a compensator for the coupled non-linear dynamics resulting from the implementation of the sensorless Cartesian impedance control in Sect. 3, the variable impedance control parameters laws, and the robust (w.r.t. the environment stiffness parameter) SDRE variable impedance controller for force-tracking.

5.2.1 Coupling non-linearities compensation

As shown in (10), the non-linearities resulting from the implementation of the sensorless Cartesian impedance controller are coupling the controlled robot dynamics, redistributing the interaction wrench along the Cartesian DoFs. Taking into account the translational DoFs, (10) results in:

$$\begin{aligned} \mathbf {M}_t \ddot{\mathbf {x}}_t + \mathbf {D}_t \dot{\mathbf {x}}_t + \mathbf {K}_t \Delta \mathbf {x}_t = - \overline{\mathbf {L}}_t(\mathbf {q}) \mathbf {f}. \end{aligned}$$
(27)

Exploiting the estimation of the interaction force \(\widehat{\mathbf {f}}\) provided in Sect. 4, it is possible to compensate such coupling non-linearities including a compensation term \(\mathbf {x}_{t,c}\) into the impedance setpoint \(\mathbf {x}^d_t\):

$$\begin{aligned} \mathbf {x}_{t,c} = \overline{\mathbf {L}}_t(\mathbf {q}) \widehat{\mathbf {f}} + \widehat{\mathbf {f}}. \end{aligned}$$
(28)

Substituting (28) into (27), the linearization of the controlled dynamics is achieved:

$$\begin{aligned} \mathbf {M}_t \ddot{\mathbf {x}}_t + \mathbf {D}_t \dot{\mathbf {x}}_t + \mathbf {K}_t \Delta \mathbf {x}_t = - \widehat{\mathbf {f}}. \end{aligned}$$
(29)

Remark 4

The compensation of the non-linearities in (10) is effective if the provided estimated interaction force \(\widehat{\mathbf {f}}\) is correctly performed, i.e., \(\mathbf {f} \simeq \widehat{\mathbf {f}}\).

5.2.2 Variable impedance control

Adaptation laws for the impedance control stiffness \(\mathbf {K}_t\) and damping \(\mathbf {D}_t\) parameters (on the basis of the current state of the interaction) are here proposed.

Considering the achieved linear and decoupled dynamics in (29), the robust SDRE variable impedance controller can be derived for each translational Cartesian DoF separately, i.e., scalar equations will be considered in the following taking into consideration the translational Cartesian DoF i.

Considering the stiffness parameter, the following adaptation law is proposed:

$$\begin{aligned} K_{t,i} = K_{t,i}^0 - m_{k,i} \left( \frac{f^d_i - \widehat{f}}{f^d_i}\right) ^2. \end{aligned}$$
(30)

In such a way, the controlled robot behavior is softer starting the interaction task, while becoming stiffer as soon as the reference force \(f^d_i\) is reached. \(K_{t,i}^0>0\) and \(m_{k,i}>0\) (with \(K_{t,i}^0>m_{k,i}\)) are design parameters guiding the adaptation of the stiffness.

Considering the damping parameter, the following adaptation law is proposed for the damping ratio:

$$\begin{aligned} h_{t,i} = h_{t,i}^0 - m_{h,i} \left( \frac{f^d_i - \widehat{f}}{f^d_i}\right) ^2. \end{aligned}$$
(31)

The damping parameter can then be computed as \(D_{t,i}=2 h_{t,i} \sqrt{K_{t,i} M_{t,i}}\). In such a way, the controlled robot behavior is less damped starting the interaction task (to achieve a faster closed loop bandwidth), while becoming more damped as soon as the reference force \(f^d_i\) is reached (to avoid force overshoots). \(h_{t,i}^0>0\) and \(m_{h,i}>0\) (with \(h_{t,i}^0>m_{h,i}\)) are design parameters guiding the adaptation of the damping ratio.

5.2.3 Robust state depended Riccati equation controller

The proposed robust SDRE controller aims to achieve high-performance in force-tracking applications. In particular, the proposed controller should have a fast dynamics, while avoiding force overshoots and achieving zero steady-state force error. In addition, the robustness of the controller is considered w.r.t. the uncertainties on the environment parameters (i.e., environment stiffness \(\mathbf {K}_e\)).

Considering the achieved linear and decoupled dynamics in (29), the robust SDRE variable impedance controller can be derived for each translational Cartesian DoF separately, as performed in Sect. 5.2.2. Taking into consideration the translational Cartesian DoF i, the following structure has been proposed for the outer interaction controller \(x_{t,SDRE,i}\):

$$\begin{aligned} x_{t,SDRE,i} = G_{opt1,i} \, e_{\dot{x},t,i} + G_{opt,2,i} \, e_{x,t,i} - G_{opt,3,i} \, \int \widehat{e}_{f,i}, \end{aligned}$$
(32)

where \(e_{\dot{x},t,i} = \dot{x}_{t,i}^d - \dot{x}_{t,i}\) is the velocity error (having \(\dot{x}_{t,i}^d\) the reference velocity and \(\dot{x}_{t,i}\) the measured velocity), \(e_{x,t,i} = x_{t,i}^d - x_{t,i}\) is the position error (having \(x_{t,i}^d\) the reference position and \(x_{t,i}\) the measured position), \(\widehat{e}_{f,i} = f^d_i - \widehat{f}_i\) is the force error (having \(f^d_i\) the reference force and \(\widehat{f}\) the estimated interaction force provided by the EKF described in Sect. 4). The control loop displays, therefore, three regulators: \(G_{opt,1,i}\) is the control gain at the velocity level, \(G_{opt,2,i}\) is the control gain at the position level, and \(G_{opt,3,i}\) is the control gain of the integral of the force-tracking error \(\widehat{e}_{f,i}\).

By substituting the environment modeled dynamics (26) in (29), the corresponding state-expression describing the robot-environment interaction dynamics results in:

$$\begin{aligned} \begin{bmatrix} \ddot{x}_{t,i}\\ \dot{x}_{t,i} \\ \widehat{f}_i \end{bmatrix} = {\scriptstyle \begin{bmatrix} -\frac{D_{t,i}(\varvec{\eta } _i)}{M_{t,i}} &{} -\frac{K_{e,i}}{M_{t,i}} &{} 0 \\ 1 &{} 0 &{} 0 \\ 0 &{} -K_{e,i} &{} 0 \end{bmatrix}} \begin{bmatrix} \dot{x}_{t,i}\\ x_{t,i} \\ \int \widehat{f}_i \end{bmatrix} + \begin{bmatrix} \frac{K_{t,i}(\varvec{\eta } _i)}{M_{t,i}}\\ 0 \\ 0 \end{bmatrix} x_{t,SDRE,i}, \end{aligned}$$
(33)

where the stiffness parameter \(K_{t,i}(\varvec{\eta } _i)\) and the damping parameter \(D_{t,i}(\varvec{\eta } _i)\) are a function of the state \(\varvec{\eta } _i = [\dot{x}_{t,i}, \, x_{t,i}, \, \int \widehat{f}_i]^T\). Such parameters are, in fact, adapted on the basis of the current state of the interaction, as described in Sect. 5.2.2.

The state-expression (33) can be re-written in the following matrix form:

$$\begin{aligned} \dot{\varvec{\eta }}_i = \mathbf {A}_i(\varvec{\eta } _i) \varvec{\eta } _i + \mathbf {b}_i(\varvec{\eta } _i) \,x_{t,SDRE,i}. \end{aligned}$$
(34)

In order to implement a robust controller w.r.t. the environment dynamics (i.e., w.r.t. the stiffness parameter \(K_{e,i}\) of the environment), it is possible to include such uncertainties into the proposed modeling, modifying (34) to:

$$\begin{aligned} \dot{\varvec{\eta }}_i = \mathbf {A}_i(U_{K_{e,i}},\varvec{\eta } _i) \varvec{\eta } _i + \mathbf {b}_i(\varvec{\eta } _i) \,x_{t,SDRE,i}, \end{aligned}$$
(35)

where \(U_{K_{e,i}}\) denotes the modeled uncertainties (i.e., the uncertainties bounds) on the environment stiffness \(K_{e,i}\) and:

$$\begin{aligned} \mathbf {A}_i(U_{K_{e,i}},\varvec{\eta } _i) = \begin{bmatrix} -\frac{D_{t,i}(\varvec{\eta } _i)}{M_{t,i}} &{} -\frac{K_{e,i}+U_{K_{e,i}}}{M_{t,i}} &{} 0 \\ 1 &{} 0 &{} 0 \\ 0 &{} -K_{e,i}-U_{K_{e,i}} &{} 0 \end{bmatrix}. \end{aligned}$$
(36)

Since the state-expression (35) is non-linear in the state variable \(\varvec{\eta } _i\), SDRE control approach can be exploited in order to compute the optimized control gains \(G_{opt,1,i}(t)\), \(G_{opt,2,i}(t)\), and \(G_{opt,3,i}(t)\) for each control step t. As described in Çimen (2007), the SDRE control methodology approximates the infinite-horizon nonlinear optimal tracking control problem and it can be trated similarly to the solution of an LQR optimal tracking control problem.

In order to design the SDRE controller, shall \(J_{cost}\) be the controller performance index:

$$\begin{aligned} J_{cost,i}(\varvec{\eta } _i):= \int _{0}^{\infty } \left( \varvec{\eta } _i^T \mathbf {Q}_{i}(\varvec{\eta } _i) \varvec{\eta } _i + \left( x_{t,SDRE,i}\right) ^2\right) dt, \end{aligned}$$
(37)

where \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) is a diagonal control matrix. In this paper, \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) is defined as follows:

$$\begin{aligned} \mathbf {Q}_{i}(\varvec{\eta } _i) = \begin{bmatrix} q_x \left( 1 - g_{\%_x}\left( \dfrac{\widehat{f}_i-f^d_i}{f^d_i}\right) ^2\right) &{} 0 &{} 0 \\ 0 &{} q_{\dot{x}} \left( \dfrac{\dot{x}_{t,i}}{\dot{x}_{t,i}^{max}} \right) ^2 &{} 0 \\ 0 &{} 0 &{} q_{\int f} \left( 1 - g_{\%_{\int f}}\left( \dfrac{\widehat{f}_i-f^d_i}{f^d_i}\right) ^2\right) \\ \end{bmatrix}. \end{aligned}$$
(38)

The first and the third diagonal elements of \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) are defined on the basis of the force error \(\widehat{f}_i-f^d_i\), normalized by the reference force \(f^d_i\). When the estimated force \(\widehat{f}_i\) is small (i.e., establishing the contact), the first and diagonal terms of \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) are bigger, giving to the controller higher bandwidth. When the estimated force \(\widehat{f}_i \simeq f^d_i\) (i.e., reference force is tracked), the first and diagonal terms of \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) are smaller, limiting the control action. \(q_x\) and \(q_{\int f}\) are constant gains defined to regulate the control action, together with \(q_{x}\) and \(q_{\int f}\) that are useful gains to tune the controller behavior on the basis of the force-tracking error evolution. The second diagonal element of \(\mathbf {Q}_{i}(\varvec{\eta } _i)\) is defined on the basis of the robot velocity \(\dot{x}_{t,i}\), normalized by a maximum allowed velocity \(\dot{x}_{t,i}^{max}\). In such a way, the controller’s damping behavior is increased if the measured velocity increases. \(q_{\dot{x}}\) is a constant gain defined to regulate the control action.

The optimal controller with the action \(x_{t,SDRE,i}\) is obtained by solving the related minimization problem:

$$\begin{aligned} J_i^*(\varvec{\eta } _i):=\min _{x_{t,opt,i}} \left\{ J_{cost,i}(\varvec{\eta } _i) \right\} . \end{aligned}$$
(39)

Resolving (39) corresponds to solve the related Riccati matrix equation Çimen (2007):

$$\begin{aligned} \mathbf {0}=\mathbf {S}_i \,\mathbf {A}_i(U_{K_{e,i}},\varvec{\eta } _i) + \mathbf {A}_i^T(U_{K_{e,i}},\varvec{\eta } _i) \mathbf {S}_i+ \mathbf {Q}_i(\varvec{\eta } _i) - \mathbf {S}_i \,\mathbf {b}_i(\varvec{\eta } _i)\, \mathbf {b}_i^T(\varvec{\eta } _i) \mathbf {S}_i + \mathbf {F}_{z,i} + \beta _i^2 \mathbf {I}, \end{aligned}$$
(40)

where \(\mathbf {S}_i=\mathbf {S}_i^T\) is the solution of (40) (symmetric and positive semi-definite constant 3x3 matrix), \(\mathbf {I}\) is the 3x3 identity matrix, \(\beta _i\) is a design parameter, and \(\mathbf {F}_{z,i}\) includes the modeling uncertainties. \(\mathbf {F}_{z,i}\) is defined as:

$$\begin{aligned} \mathbf {F}_{z,i} = \Delta \mathbf {A}_i^T \left( \mathbf {b}_i(\varvec{\eta } _i)^{-1}\right) ^T \mathbf {b}_i(\varvec{\eta } _i)^{-1} \Delta \mathbf {A}_i, \end{aligned}$$
(41)

with:

$$\begin{aligned} \Delta \mathbf {A}_i = \begin{bmatrix} 0 &{} -\frac{U_{K_{e,i}}}{M_{t,i}} &{} 0 \\ 0 &{} 0 &{} 0 \\ 0 &{} -U_{K_{e,i}} &{} 0 \\ \end{bmatrix}. \end{aligned}$$
(42)

Considering the reference vector \(\varvec{\eta } _{i}^d = [\dot{x}_{t,i}^d, x_{t,i}^d, \int f_{i}^d]^T\) for the reference tracking control problem Çimen (2007), the control law in (32) becomes:

$$\begin{aligned} x_{t,SDRE,i} = -\mathbf {b}_i^T \left( \mathbf {S}_i \varvec{\eta } _i - \varvec{\eta } _{i}^d \right) . \end{aligned}$$
(43)

Remark 5

It has to be underlined that (40) has to be solved for each control step t. In fact, having the state-depended state-expression (35), the control gains are not constant and they cannot be computed in advance as in Roveda et al. (2015).

The solution \(\mathbf {S}_i\) can be analytically computed as a function of the coupled robot-environment system parameters, and as a function of the environment parameter uncertainties. In such a way, the control gains can be computed and updated at each control step. The terms of \(\mathbf {S}_i\) can be derived as follows.

Considering the term \(S_i(1,1)\), it can be computed solving a fourth order equation resulting in the following solutions:

$$\begin{aligned} \left\{ \begin{aligned} S_i(1,1)^{\#1} =&-F_{2,i}/(4 \, F_{1,i}) - N_{i} \\&+ \sqrt{-4 \, N_{i}^2 - 2 \, p_{1,i} + q_{1,i}/N_{i}}/2, \\ S_i(1,1)^{\#2} =&-F_{2,i}/(4 \, F_{1,i}) - N_{i} \\&- \sqrt{-4 \, N_{i}^2 - 2 \, p_{1,i} + q_{1,i}/N_{i}}/2, \\ S_i(1,1)^{\#3} =&-F_{2,i}/(4 \, F_{1,i}) + N_{i} \\&+ \sqrt{-4 \, N_{i}^2 - 2 \, p_{1,i} - q_{1,i}/N_{i}}/2, \\ S_i(1,1)^{\#4} =&-F_{2,i}/(4 \, F_{1,i}) + N_{i} \\&- \sqrt{-4 \, N_{i}^2 - 2 \, p_{1,i} - q_{1,i}/N_{i}}/2, \end{aligned} \right. \end{aligned}$$
(44)

where:

figure a

The real and positive solution of (43) will be selected as \(S_i(1,1)\).

Considering the term \(S_i(3,1)\), it can be computed as follows:

$$\begin{aligned} S_i(3,1) = - \dfrac{\sqrt{\beta _i^2 + Q_i(3,3)}}{b_i(1,1)}. \end{aligned}$$
(46)

Considering the term \(S_i(2,1)\), it can be computed as follows:

$$\begin{aligned} S_i(2,1) = \dfrac{b_i(1,1)^2 S_i(1,1)^2}{2} - \dfrac{\beta _i^2}{2} - A_i(1,1) S_i(1,1) - \dfrac{Q_i(1,1)}{2}. \end{aligned}$$
(47)

Considering the term \(S_i(3,2)\), it can be computed as follows:

$$\begin{aligned} S_i(3,2) = \dfrac{A_i(1,1) \sqrt{\beta _i^2 + Q_i(3,3)}}{b_i(1,1)} - b_i(1,1) S_i(1,1) \sqrt{\beta _i^2 + Q_i(3,3)}. \end{aligned}$$
(48)

Considering the term \(S_i(3,3)\), it can be computed as follows:

$$\begin{aligned} S_i(3,3) = \sqrt{\beta _i^2 + Q_i(3,3)} \dfrac{A_i(1,2)/b_i(1,1) - b_i(1,1) S_i(2,1)}{A_i(3,2)}. \end{aligned}$$
(49)

Considering the term \(S_i(2,2)\), it can be computed as follows:

$$\begin{aligned} S_i(2,2)&= b_i(1,1)^2 S_i(1,1) S_i(2,1) - A_i(1,2) S_i(1,1) - A_i(1,1) S_i(2,1) \nonumber \\&+ A_i(3,2) \sqrt{\beta _i^2 + Q_i(3,3)}/b_i(1,1). \end{aligned}$$
(50)

Remark 6

It has to be underlined that the control gains computation can be easily implemented for on-line tuning. On the basis of the coupled dynamics parameters, the proposed formulas allow to update the control gains in (32) at each control step.

6 Results

Simulations have been performed in order to validate the proposed approach exploiting Matlab and its Robotics ToolboxCorke (2017). A Franka EMIKA panda manipulator has been simulated as a test platform. The robot torque controller has been also simulated, with a control frequency of 1 kHz. In order to introduce modeling errors into the robot dynamics modeling (thus making simulations realistic), an error of \(5\%\) on the robot dynamics parameters has been considered (both on the link-mass parameters and on the friction parameters, modeled as in Gaz et al. (2019)).

The control gains and reference signals have been initialized as follows (along z Cartesian DoF): mass parameters \(M_{t,z}\) equal to 10 kg; \(K_{t,z}^0\) equal to 5000 Nm; \(m_{k,z}=4500\) Nm; \(h_{t,z}^0\) equal to 10; \(m_{h,z}=9\); \(q_x\) equal to 0.1; \(q_{\dot{x}}\) equal to 10; \(q_{\int f}\) equal to 0.0005; \(\dot{x}^{max}_{t,z}\) equal to 0.1 m/s; \(q_{\%_x}\) equal to 0.9; \(q_{\%_{\int f}}\) equal to 0.9; SDRE control reference velocity \(\dot{x}_{t,z}^d\) in the reference vector \(\varvec{\eta } _{z}^d\) equal to 0 m/s; SDRE control reference position \(x_{t,z}^d\) in the reference vector \(\varvec{\eta } _{z}^d\) equal to \(x_{e,t,z}^0\) (i.e., the equilibrium position of the environment estimated during the task execution, in the correspondence of the first measured contact exploiting \(\widehat{f}_z\) from the proposed EKF); SDRE step reference force \(f^d_z\) in the reference vector \(\varvec{\eta } _{z}^d\) equal to 30 N; SDRE design parameter \(\beta _i\) equal to 0.0001.

A probing task has been considered as a reference task. Simulations have been performed establishing the interaction along the z Cartesian DoF. In the proposed simulations, the robot moves along the Cartesian DoF \(-z\), establishing the contact with the environment and controlling the interaction to track the reference force \(f^d_z\).

Two environments have been considered in the simulation: a soft environment (nominal stiffness \(K_{e,z}^{\#1,n} \simeq 5000\) Nm), and a hard environment (nominal stiffness \(K_{e,z}^{\#2,n} \simeq 50000\) Nm).

The proposed controller has been compared with the optimal controller in Roveda et al. (2015), to highlight the improved achieved performance w.r.t. the uncertainties related to the environment stiffness parameter \(K_{e,z}\). In the proposed simulations, the stiffness parameter \(K_{e,z}\) to be included in (35) has been set equal to \(60\%\) of the nominal environment stiffness value (i.e., \(K_{e,z}=0.6 K_{e,z}^{\#i,n}\)). The uncertainty bounds related to the stiffness environment parameter have been imposed equal to \(U_{K_{e,i}}=\pm 0.5 K_{e,z}\).

Achieved results for the soft environment \(\#1\) are shown in Fig. 2. The proposed EKF is able to estimate the interaction force \(f_z\). A small error is shown in the force estimation (\(<10\%\)), making the proposed EKF effective in comparison with the state of the art approaches. Considering the proposed controller, force overshoots are avoided and a fast dynamics is achieved. Comparing the proposed controller with the LQR approach in Roveda et al. (2015), it is possible to highlight the avoidance of the force overshoots, while achieving a fast closed loop dynamics. Achieved results for the hard environment \(\#2\) are shown in Fig. 3. Again, the proposed EKF is able to estimate the interaction force \(f_z\). A small error is shown in the force estimation (\(<10\%\)). Comparing the proposed controller with the LQR approach in Roveda et al. (2015), also in this case force overshoots are avoided and a fast closed loop dynamics is achieved.

Remark 7

The EKF gains have been experimentally tuned in order to achieve the fastest dynamics for the proposed observer.

Fig. 2
figure 2

Estimated interaction force \(\widehat{\mathbf {f}}\) (dot-dashed line) vs. real interaction force \(\mathbf {f}\) (continuous line) for the soft environment \(\#1\). Reference force \(f_z^d\) (dashed line) is highlighted

Full size image
Fig. 3
figure 3

Estimated interaction force \(\widehat{\mathbf {f}}\) (dot-dashed line) vs. real interaction force \(\mathbf {f}\) (continuous line) for the hard environment \(\#2\). Reference force \(f_z^d\) (dashed line) is highlighted

Full size image

7 Conclusions and future work

This paper proposed a sensorless model-based methodology for high-performance force control. Exploiting sensorless Cartesian impedance control, an Extended Kalman Filter has been designed to estimate the interaction force between the robot and the environment. The estimated interaction force is then used to: compensate for the non-linearities in the sensorless impedance control behavior, tune the impedance control parameters (i.e., stiffness and damping parameters), and to derive the State Depended Riccati Equation (SDRE) controller to achieve high-performance in force-tracking applications. The described approach has been simulated in a probing task, considering the Franka EMIKA panda manipulator as a test platform. The proposed approach has been compared with the LQR force controller in Roveda et al. (2015), highlighting the improved achieved performance.

Current and future works are devoted to the extension of the EKF for force estimation to the complete external wrench estimation (i.e., including torques estimation). In addition, artificial intelligence procedures for optimal tuning of the EKF gains are under investigations. Finally, the extension of the proposed controller to include free-motion to contact transitions control is under development.