Sensorless Optimal Switching Impact/Force Controller

Intelligent interaction control is required in many fields of application, in which different operative situations have to be faced with different controllers. Being able to switch between optimized controllers is, indeed, of extreme importance to maximize the task performance in the different operative conditions (i.e., free-space motion and contact), especially when considering sensorless robots. To deal with the proposed context, a sensorless optimal switching impact/force (OSIF) controller is proposed. The low-level robot control is composed of an inner joint position controller, fed by an outer Cartesian impedance controller with a reference position. The estimation of the external wrench is implemented by means of an Extended Kalman Filter (EKF). The high-level controller (feeding the Cartesian impedance controller with the setpoint) is composed of an optimized impact controller (LQR-based controller), an optimized force controller (SDRE-based controller), and a continuous switching mechanism (Fuzzy Logic-based). In addition, the output of the switching mechanism is used to adapt the Cartesian impedance control parameters (i.e., stiffness and damping parameters). Experimental tests have been performed on a Franka EMIKA panda robot to validate the proposed controller. Obtained results show the capabilities of the OSIF controller, being able to detect task phase transitions while satisfying the target performance.


I. INTRODUCTION
Having the robot able to interact with the surrounding environment is highly required in many fields of application [1], [2], [3], [4]. The robot has to properly select its behavior (i.e., the adopted controller) in order to deal with the different task phases. In fact, the selection of the wrong controller might result in reduced performance or, even worse, in task failures (e.g., instabilities). Within the considered application scenario, two main task phases can be identified for interaction tasks ( Figure 1): 1) free-space approaching motion, and 2) contact. To maximize the task performance, it is important to optimize the robot controller for each task phase, and to employ a switching mechanism to select the proper controller to be used on the basis of the interaction state. The main aim of this paper is, therefore, to develop an intelligent control framework, capable to switch between high-level optimized impact and force controllers as needed.
To be implemented on any robot, the here proposed methodology considers sensorless (i.e., without force/torque sensors) position-controlled robots (a common situation for most of the robots on the market [5]). The low-level controller employs an outer Cartesian impedance controller [6] feeding the inner joint position controller with a reference signal. The outer Cartesian impedance controller makes use of the estimation of the interaction wrench, computed exploiting the estimation of the external joint torques provided by an Extended Kalman Filter (EKF).

1) External interaction estimation
Being able to establish a safe and stable interaction between the robot and its target environment is attracting huge attention from the research community. Such a need is particularly VOLUME 4, 2016 of interest in the case of sensorless robots: manipulators that are not equipped by force/torque sensors to directly measure the external interaction. Therefore, a lot of effort has been put in recent years into the design of interaction estimation techniques for the development of sensorless interaction controllers. High-accuracy robot models have been developed, to be exploited for the estimation of the interaction while executing a target task [7]. Such approaches have been enhanced by means of disturbance observers, employing the available robot modeling to improve the interaction estimation performance. A nonlinear disturbance observer based on the nonlinear dynamics of the robotic arm is designed in [8]. The stability of the method is guaranteed by the tuning of its design gains, based on the physical modeling parameters of the robot and its software/hardware constraints (e.g., maximum joint accelerations and velocities). [9] proposed a disturbance state observer enhanced by machine learning (ML) algorithms for the identification of a task-oriented dynamic model. The adoption of ML allowed to avoid any analytical modeling of the robot dynamics, exploiting a purely data-driven approach. [10] derived a parametric robot dynamics modeling coupled with ML to overcome uncertainties and errors in the proposed analytical modeling. The described method combines rigid-body dynamics (RBD) modeling with a model-uncertainties compensator based on a multilayer perception (MLP) approach. The interaction estimation is then implemented exploiting the defined robot dynamics modeling into a disturbance Kalman filter. To avoid the use of acceleration measurements (i.e., due to the noise contained in such measurements) and the computation of the robot inertia matrix inverse (additionally amplifying the measurements noise), [11] developed a sensorless admittance control scheme on the basis of the definition of a generalized momentum-based disturbance observer for the modeling of a linear environment dynamics. A radial basis neural networks approach (RBNN) is employed to compensate for the modeling uncertainties. Other works, instead, tackle the interaction estimation as an optimization problem. In [12] a recursive least-square estimation algorithm enhanc-FIGURE 1: Considered application scenario. The interaction task is composed of two task phases: free-space approaching motion phase, and contact phase. The impact has to be controller to avoid dangerous situations.
ing the defined filtered dynamic equations to smooth the estimation of the interaction has been developed. In [13], a methodology to address in real-time a convex optimization problem for the estimation of the reaction forces considering Coulomb friction uncertainties has been proposed. Virtual sensors (exploiting high-performance dynamic modeling and calibration) have been also investigated for the estimation of the external interaction. A task-oriented calibrated robot dynamic model has been studied in [14], also considering the thermal state of the robot joints. The derived dynamic model is calibrated by making use of a two-stage optimization algorithm, generating joint trajectories maximizing the robot dynamics excitation. Exploiting the residual method, the estimation of the external interaction between the robot and the environment is then obtained as the difference between the estimated and measured torques. Artificial Intelligence has been also applied to map the interaction between the robot and the environment. In [15], the interaction between the robot and a soft tissue has been modeled as a visco-elastic system, allowing for the design of the external interaction observer based on a Lyapunov time-varying equation. The estimation of the interaction forces has been implemented in [16] by designing an online sparse Gaussian process regression (OSGPR) approach for a bilateral teleoperation system, that doesn't require any previous knowledge of the slave manipulator dynamic model. The derived method, in fact, only relies on acquired datasets. Additional external sensors have been exploited as well, in order to acquire useful data that can be employed to enhance the estimation of the external interaction. An exteroceptive device (i.e., a depth camera) has been used in [17]. In this work, the detection of contacts is performed by means of such an external sensing system, combining it with a residual method for the evaluation of the interaction joint torques. Optical Coherence Tomography (OCT) images have been farther exploited in [18]. A Neural Network approach has been implemented for the classification of the OCT images. In [19], instead, Convolutional Neural Networks and Long-Short Term Memory networks have been employed to process the spatiotemporal information included in video sequences, providing an estimation of the interaction force.

2) Impact control
Impact control (i.e., the capability to establish a stable interaction from the free motion condition to the contact condition, avoiding excessive force overshoots) is an important topic to enhance the interaction tasks execution involving uncertain environments (e.g., uncertainties related to the environment positioning). In fact, in the case that a too high impact force is established, the task can be compromised (e.g., damaging the environment, losing the grasping of the manipulated part, etc.). Common simplified solutions to deal with such an issue implement a reduced approaching velocity to limit the impact forces. However, this procedure significantly slows down the performance of the robotic system, in particular considering (partially) unknown environment positioning. To improve the performance of interaction tasks in real applications, impact control is investigated in the literature. In [20], an impact controller to reduce the (possible) damaging effects resulting from the interaction is presented. Considering a redundant manipulator, its null space motion was optimized on the basis of instantaneous collision dynamics modeling to minimize the resulting impulsive forces. In [21], a method for the analysis of the initial impact dynamics for a robotized peg-in-hole task has been proposed. A general formulation of the impact equation is achieved by means of the Lagrange impact modeling. [22] investigated the postimpact behavior to model the impact dynamics, taking into consideration a variety of pre-impact conditions (e.g., robot configuration, impact inclination, impact velocity, etc.). An adaptive nonlinear controller has been designed in [23] to modulate the states of two colliding systems. A Lyapunovbased controller has been designed to implement an adaptive impact controller for a planar robot impacting on a passive mass-spring system. In [24], an impact model has been defined to numerically compute the impact control gains and the reference impact velocity to establish a stable and controlled contact with the target environment, reducing the impact force. A task-space controller has been proposed in [25]. Quadratic programming (QP) has been exploited to generate modular and reactive motions for a wide variety of tasks. A discrete impact dynamics modeling has been explicitly introduced for the design of the QP-based controllers. In such a way, the generated robot motions are robust to impact-induced state jumps in the joint velocities and joint torques. In [26], post-impact states prediction and impact-aware inequality constraints have been integrated into a general-purpose whole-body controller, being able to establish controlled impact dynamics. [27] considered impedance-controlled locomotion tasks for legged robots, involving complex contact interactions. In such a context, the impedance control behavior has been optimized by means of a stochastic optimal control algorithm, tuning the control gains and generating reference tasks trajectories.

3) Sensorless force control
Interaction control methodologies exploiting the estimation of the external forces/torques have been investigated. [28] proposed a sensorless Lyapunov controller for force-tracking purposes. [29] derived a nonlinear mapping between the joint actions and the force/torques applied at the robot endeffector. A model-free Fuzzy sliding mode control has been implemented in order to regulate the interaction force at the robot end-effector, integrating and exploiting the defined mapping. [30] designed an admittance controller to face insertion tasks, proposing a real-time trajectory generator based on a model-based sensorless observer of the interaction wrench. [31] proposed a force/position decentralized robust controller, taking into account a constrained reconfigurable robot. [32] formulated a dither periodic component elimi-nation Kalman filter, enhanced by a disturbance observer. By means of the developed framework, it is possible to perform the estimation of the interaction force, building a high accuracy sensorless force-tracking controller capable to compensate for static friction effects. [33] made use of a notch-type friction-free disturbance observer for sensorless force control purposes. [34] proposed a sensorless control framework to implement a high-performance force-tracking controller. The estimation of the interaction environment stiffness, together with the estimation of the interaction force, have been used to design the control framework. The adaptation of the Cartesian impedance control parameters has been also implemented in order to modulate the coupled robotenvironment interaction dynamics. Such a control approach has been extended in [35], proposing an optimized interaction controller exploiting the coupled robot-environment dynamics modeling.

B. PAPER CONTRIBUTION
The here presented paper aims to propose a sensorless optimal switching impact/force (OSIF) controller, properly switching from impact control to force control (and vice versa) on the basis of the current task phase (i.e., free-space approaching motion or contact). The proposed control framework is designed to be implemented on sensorless positioncontrolled robots. The low-level control is composed of an inner joint position controller, fed by an outer Cartesian impedance controller with the reference signals. The outer Cartesian impedance control exploits the estimation of the external wrench, obtained from the estimation of the external joint torques provided by an Extended Kalman Filter (EKF). Such an EKF is designed exploiting the position-controlled robot dynamics, as proposed in [36]. The high-level controller is composed of an optimized impact controller, an optimized force controller, and a continuous switching mechanism. The optimized impact controller aims to limit the impact force during the free-space approaching motion phase. The impact modeling is employed by an LQR controller to track a reference impact velocity satisfying the specified maximum impact force. The optimized force controller aims to track a reference force during the contact phase. The coupled robot-environment modeling is employed by a State-Dependent Riccati Equation (SDRE) controller. The switching mechanism aims to (continuously, i.e., at each control step, differently w.r.t. event-based discrete controllers [37]) select the proper controller to be employed in the current task phase (i.e., determining which task phase is currently faced). A Fuzzy Logic approach is used in order to classify the current task phase (and, therefore, the controller to be selected). In addition, the output of the switching mechanism is used to adapt the Cartesian impedance control parameter (i.e., stiffness and damping parameters) to ensure continuous and smooth transitions between the optimal impact control and the optimal force control in the high-level controller. The relations between all the proposed components (i.e., the lowlevel controller -composed by the inner position control, the VOLUME 4, 2016 outer Cartesian impedance control, and the EKF -, the highlevel controller -composed by the optimal impact control, the optimal force control, and the switching mechanism -, and the impedance parameters adaptation strategy) are highlighted in Figure 2.
Indeed, the main contributions of the here presented paper are: • the design of an optimal switching impact/force controller for sensorless position-controlled robots; • the design of a switching mechanism making it possible to (continuously) select the proper high-level controller on the basis of the current task phase; • the design of an optimal impact controller to limit the impact force, while maximizing the free-space approaching motion velocity; • the design of a (continuous) adaptive mechanism for the impedance control parameters to achieve a smooth and stable transition between the optimal impact control and the optimal force control. W.r.t. the algorithm earlier developed in [24] by some of the authors, the here presented approach advances the state of the art in the field. In fact, in the previous approach, the impact controller was optimized offline in a simulation environment. In addition, the simple switching mechanism doesn't allow continuous switching between the optimal impact control and the optimal force control. W.r.t. the contribution in [36], the here presented methodology exploits the developed EKF for the estimation of the external joint torques in order to implement the proposed control approach for a sensorless industrial manipulator. In fact, in [36], no combined or single impact/force control has been proposed. Therefore, the here presented contribution is novel considering the proposed optimal switching impact/force controller.
Experimental tests have been performed to validate the proposed controller. A Franka EMIKA panda robot has been employed as a test platform. Its internal joint torque sensors have been exploited to validate the estimation of the external joint torques provided by the proposed EKF, together with the force impact and force tracking performance. An ablation study (i.e., a systematic incremental validation) has been pursued to validate the capabilities of each component of the proposed control framework (i.e., EKF, optimal impact control, OSIF control). Achieved results show the capabilities of the OSIF control to properly switch (i.e., correctly identifying the current task phase) from impact control to force control (and vice versa), guaranteeing to satisfy the target performance (i.e., limiting the impact force during the freespace approaching motion phase, and tracking the reference force avoiding force overshoots during the contact phase).
Remark 1. It has to be underlined that, to the best knowledge of the authors, no existing state-of-the-art method is implementing a switching controller to deal with impact/force control. The here presented paper, therefore, proposes a novel controller capable to continuously switching between optimal impact control and optimal force control on the basis of the current interaction state, i.e., identifying the current task phase and selecting the proper controller to address it.

C. PAPER OUTLINE
The paper is structured as follows: Section II describes the proposed control framework, highlighting all the developed components; Section III details the low-level controller, highlighting the design of the outer Cartesian impedance control and the design of the inner joint position control with robot dynamics compensation; Section IV derives the proposed EKF for the estimation of the external joint torques, as described in [36]; Section V faces the design of the proposed switching mechanism for the high-level controller exploiting the Fuzzy Logic approach; Section VI proposes the design of the optimal impact controller; Section VII deals with the design of the optimal force controller; Section VIII proposes the Cartesian impedance control parameters adaptation low; Section IX shows the achieved results; Section X states the conclusions and the future work.

II. OPTIMAL SWITCHING IMPACT/FORCE CONTROLLER
The main aim of the proposed paper is the definition of the OSIF controller, capable to switch from optimal impact control to optimal force control (and vice versa) on the basis of the current task phase. To this aim, the control framework shown in Figure 2 has been implemented. The Figure  highlights all the developed components defining the OSIF controller. The low-level control (composed of the inner joint position control and the outer Cartesian impedance control) is highlighted. The EKF for the estimation of the external joint torques is highlighted, showing the computation of the external interaction wrench used by the Cartesian impedance control. The high-level controller (composed of the optimal impact control, the optimal force control, the switching mechanism, and the stiffness and damping regulation block) is highlighted. In the next Sections, each main component defining the OSIF controller is analyzed.

III. LOW-LEVEL CONTROLLER
In this Section, the low-level robot controller is defined. Such a low-level controller aims to implement a compliant controller on top of a joint position controller, making it possible for the position-controlled robot to perform interaction tasks.

A. OUTER IMPEDANCE CONTROL
As described in [38], an impedance controller can be designed to perform a compliant task, providing with a reference the inner position controller. On the basis of the interaction force acting on the manipulator, impedance control allows to calculate the robot accelerationsẍ imp = [p;φ cd ] (wherep are related to the traslational degrees of freedom -DoFs -, andφ cd are related to the rotational DoFs described FIGURE 2: Proposed control schema. The OSIF controller (composed by the Fuzzy Logic switching mechanism, the optimal impact controller, and the optimal force controller, the outer impedance controller, the control block for the adaptation of the stiffness and damping parameters, and the inner position controller with robot dynamics compensation are highlighted. The proposed EKF providing the estimate of the external joint torques τ ext is also shown in the control schema. by the intrinsic Euler angles representation): Considering the traslational part of the impedance control, M t is the target mass matrix, D t is the target damping matrix, K t is the target stiffness matrix, f t is the external forces vector. p is the actual Cartesian positions vector, while ∆p = p−p d and ∆ṗ =ṗ−ṗ d , where p d is the target positions vector andṗ d is the target velocity vector. Considering the rotational part of the impedance control, M ϕ is the target inertia matrix, D ϕ is the target damping matrix, K ϕ is the target stiffness matrix. φ cd is the set of Euler angles extracted from R d c = R T d R c , describing the mutual orientation between the compliant frame R c (coincident with the robot end-effector reference frame) and the target frame R d . τ ϕ is the external torques vector referred to the target frame. Matrix S ω (φ cd ) defines the transformation from Euler angles derivatives to angular velocities ω = S ω (φ cd )φ cd [39]. The six DoFs impedance control results, therefore, in: where M, D, K are the impedance diagonal matrices composed by both the traslational and rotational parts, It has to be underlined that the damping matrix can be computed as follows: D = 2ζ √ KM, where ζ is the damping ratio diagonal matrix. On the basis of the initial conditions on the impedance control position (x Ci (t = 0) = x 0 Ci ) and velocityẋ Ci (t = 0) =ẋ 0 Ci ), it is possible to compute the impedance control accelerationẍ Ci from (2). Such acceleration can be integrated in order to compute the Cartesian position x Ci and velocity (considering angular velocities for the rotational components)ẋ Ci to be used as a reference to the inner position controller.
It has to be noted that, in the proposed paper, no force/torque sensor is used in order to measure the interaction wrench vector h ext . Instead, an observer (described in Section IV) has been implemented in order to estimate the interaction joint torques τ ext . The interaction wrench vector can then be computed as follows: where J(q) is the Jacobian matrix and q is the joint position vector. The estimated interaction wrench h ext can therefore be substituted into 2 to achieved the designed compliant robot controlled behavior.

B. INNER JOINT POSITION CONTROL
The Cartesian position x Ci and velocityẋ Ci computed by the impedance control in the previous Section are employed to compute the joint reference signals to be provided to a PID position controller. The joint reference velocity vectorq d is computed as follows [40]: where e p,c = x Ci − x is the Cartesian error, x is the robot end effector pose, and K p,c is a diagonal proportional gain matrix. The PID position controller, providing the control torque τ PID , can be written as: where K p is the proportional gain matrix, K i is the integral gain matrix, K d is the derivative gain matrix, and e q = q d − q is the joint position error. q d is obtained integratingq d . The gain matrices K p , K i , K d are tuned on the basis of the methodology in [41]. Exploiting the Cartesian position x Ci and velocityẋ Ci computed by the impedance controller it is, therefore, possible to impose to the position-controlled robot a compliant behavior.

C. ROBOT DYNAMICS COMPENSATION
The following manipulator dynamics can be considered [42]: (6) where B(q) is the robot inertia matrix, C(q,q) is the robot Coriolis vector, g(q) is the robot gravitational vector, τ f (q,q) is the robot joint friction vector, and τ is the robot joint torque vector.
Based on (6), it is possible to design a position controller with robot dynamics compensation: Therefore, the resulting controlled robot dynamics results in:

IV. EXTERNAL JOINT TORQUES ESTIMATION
This Section describes the derivation of the EKF for the estimation of the external joint torques, as described in [36]. In order to provide to the reader a comprehensive description of the methodology in Figure 2, the employed EKF is reported in this paper.
To derive the proposed EKF, an augmented filter state is defined, which comprehends the robot joint velocitiesq, the robot joint positions q, the integral of the robot joint positions q, and the external joint torques τ ext : The augmented filter state x a is then substituted in the robot dynamics equation (8) to write the state-space interaction dynamics: where To account for the uncertainties in the model, a variable ν a = [ν q , νq, ν q ν τ ext ] is included in the filter dynamics. The resulting equations represent the filter dynamics: Therefore, calling q a the augmented state estimate, C a the observation matrix for the robot joint velocityq and the robot joint position q, and K EKF the gain matrix, the EKF is defined as: where the gain matrix K EKF is computed as follows: R represents the measurements noise covariance matrix: The observation function h linearly maps the sample inaccuracies, due to measurement noise w, through the matrix H: The covariance matrix P and its rate: are based on the dynamics of the state and on the model uncertainties. Matrices A a and G a are defined, respectively, as: Matrix Q, used for the estimation of the parameters, is defined as: It has to be mentioned that it is possible to neglect the evaluation of the derivative B(q)q in (17). This assumption is justified for low-dynamics systems such as compliantcontrolled robots (as in the case of this paper). In fact, the modification of B(q) is negligible since the robot motion has a reduced dynamics. The proposed EKF has been discretized for its implementation and online usage [43].
Remark 2. It has to be highlighted that an observer for the estimation of the external interaction is proposed due to the fact that compliance control is employed. Therefore, this observer can be also adopted in order to close the control loop [44], [45], [34].

V. SWITCHING CONTROL MECHANISM
In order to implement the proposed OSIF controller, the current contact phase has to be classified, defining the switching mechanism to enable the impact controller or the force controller as needed. For such a purpose, a Fuzzy Logic methodology [46] is implemented. For each Cartesian DoF separately (since the Cartesian impedance control allows to decouple the robot DoFs), the Fuzzy Logic takes as an input the absolute value of the estimated interaction force from (3) (it is computed exploiting the estimated external joint torques τ ext ) and the absolute value of the measured Cartesian robot velocity along the specified DoF. The Fuzzy Logic gives as an output a continuous variable α, used for the current contact phase classification. Therefore, two input membership functions are defined. The first input membership function, based on the estimated interaction force, is defined by the following states: low, and high. The parameters f l,1 , f l,2 , f h,1 , and f h,2 defines the shape of the membership function. The second input membership function, based on the measured Cartesian velocity, is defined by the following states: low, and high. The parameters v l,1 , v l,2 , v h,1 , and v h,2 defines the shape of the membership function. The output membership is defined by the following two states: motion, and contact. The parameters α m,1 , α m,2 , α c,1 , and α c,2 defines the shape of the membership function. Figure 3 shows the defined input and output membership functions. The following rules are applied to correlate the inputs and the output of the Fuzzy Logic: The proposed Fuzzy rules allow taking into account the dynamics of the implemented EKF (might being affected by delay). Exploiting the measured Cartesian velocity it is possible to quickly detect transitions in the contact phase, improving the shaping of the Fuzzy Logic output α parameter.
The Fuzzy Logic output parameter α is then used in order to classify the current contact phase during the task execution, making it possible to switch from impact control to force control, and vice-versa. The following rule is employed in order to implement the switch between the controllers: I f α <= α min , then impact control enabled, I f α > α min , then f orce control enabled.
In addition, the α parameter is used to adapt the Cartesian impedance parameters (i.e., stiffness and damping) for the impact controller and for the force controller, avoiding abrupt parameters modification which might be resulting in instable  Remark 3. In the case that enough fast dynamics is achieved for the implemented EKF, only the input membership function based on the estimated interaction force can be exploited. The Fuzzy Logic rules are modified to: #1 I f f orce : low, then α : motion, #2 I f f orce : high, then α : contact.

VI. OPTIMAL IMPACT CONTROL
In this Section, the optimal impact control design is addressed. In particular, since the Cartesian impedance control is employed, it is possible to decouple the Cartesian DoFs. Therefore, scalar quantities are considered in this Section. Thus, considering the translational DoFs, (2) can be written as: where x t,i considers the i th translational Cartesian DoF.

A. IMPACT MODELING
In order to design the optimal impact controller, impact modeling has to be discussed. Following the modeling in [24], it is possible to assume that the impact occurs in a small-time ∆t, in which the system velocity remains finite. Indeed, there is no modification of the controlled robot position. Therefore, integrating both sides of (22) in ∆t from the impact time t s : As ∆t → 0 the magnitude of the acceleration variation is ≫ than the magnitude of the position and velocity variation, making the second term negligible [24].
Defining the impulse at the collision point Imp = lim ∆t→0 t s +∆t t s f t,i dt, the impact equation that defines the relation between Imp and the instantaneous modification of the robot Cartesian velocity, ∆ẋ t,i , results in: By considering the instantaneous collision between two bodies, it is possible to model the variation of the velocities of the two bodies after the collision: where v 1 and v 2 are their velocities before the collision, ∆v 1 and ∆v 2 are the changes in the velocities of the bodies after collision, and λ is the coefficient of restitution (0 ≤ λ ≤ 1). Under the assumption that the target interaction environment is not initially moving (i.e., v 1 = 0, ∆v 1 = 0) and the manipulator has to approach to it (i.e., v 2 =ẋ t,i , ∆v 2 = ∆ẋ t,i ), that is a common scenario in many interaction tasks [47], the dynamics of the collision can be expressed by: Finally, considering (24) and (26), the relation between the impact impulse Imp and the robot Cartesian velocityẋ t,i results in: The resulting impact force from the impact Imp can be expressed as: Therefore, substituting (28) in (27): (29) allows correlating the resulting impact force w.r.t. the current robot velocityẋ t,i , along the considered direction i.
Exploiting (29), it is, therefore, possible to define a maximum impact force f re f ,imp t,i and to compute the reference impact velocityẋ re f ,imp Such a reference velocity guarantees to establish an interaction characterized by an impact force do not trespassing the defined limit f re f ,imp t,i . On the basis of the derived impact modeling, it is possible to design an optimal impact controller guaranteeing to track the reference impact velocityẋ re f ,imp t,i .

B. OPTIMAL IMPACT CONTROL DESIGN
Exploiting the impact modeling in (29), it is possible to design an optimal impact controller capable to establish a controlled interaction between the robot and the environment. The main objective of this controller is to provide a reference to the Cartesian impedance controller that moves the robot towards the environment, resulting in an impact force f imp t,i that does not trespass a specified maximum impact force f re f ,impact t,i . For such a purpose, the Cartesian impedance control dynamics in (22) is modified to: The elastic term is indeed neglected (i.e., K t,i = 0 N/m is imposed), implementing a pure viscous behavior for the controlled robot. Therefore, the optimal impact control has to specify a reference velocityẋ re f ,imp t,i for the Cartesian impedance control, to establish a safe and controlled interaction between the robot and the environment. The following structure is proposed for the high-level optimal impact controller defining the Cartesian impedance setpointẋ d t,i =ẋ d,imp where e imṗ x,t,i =ẋ re f ,imp displays, therefore, two regulators: G imp 1,i is the control gain at the velocity level, and G imp 2,i is the control gain of the integral of the impact force-tracking error.
By considering (31) and (29), it is possible to define the following state expression modeling the impact dynamics: The above expression can be easily written in the compact form:η In order to design the optimal impact controller, shall J imp cost,i be the controller performance index: where Q imp,i is a diagonal control matrix. In this paper, Q imp,i is defined as follows: The optimal impact controller with the action x d,imp t,i is obtained by solving the related minimization problem: Resolving (37) corresponds to solve the related Riccati matrix equation [48]: where S imp,i = S T imp,i ≥ 0 is the solution (symmetric and positive semi-definite constant 2x2 matrix) of (38).

VII. OPTIMAL FORCE CONTROL
In this Section, the optimal force control design is addressed. As described in Section VI, the scalar Cartesian impedance control dynamic equation (22) is considered, imposingẋ d t,i = 0 m/s. The proposed optimal force controller aims to provide force-tracking capabilities (i.e., to achieve zero steady-state force error) to the controlled robot, implementing the highest possible bandwidth while limiting force overshoots. In order to take into consideration uncertainties in the coupled robotenvironment dynamics modeling (in particular w.r.t. the environment stiffness K e ), a robust design is considered for the optimal force controller.
A robust SDRE variable impedance controller can be, therefore, designed. As described in Section VI-B, each translational Cartesian DoF can be controlled separately, on the basis of the resulting linear and decoupled dynamics resulting from (22). Indeed, taking into consideration the i th translational Cartesian DoF, the high-level optimal force controller can be defined with the following formulation for the Cartesian impedance setpoint x d t,i = x d, f t,i : By defining x re f , f t,i as the reference position, and x t,i as the measured position, the position error is defined as e x,t,i = x re f , f t,i − x t,i . The velocity error is defined as eẋ ,t,i = −ẋ t,i since the reference velocityẋ re f , f t,i = 0 m/s is imposed. By defining f re f , f i as the reference force, and f t,i as the interaction force available from the estimation of the external joint torques provided by the implemented EKF (Section IV), the force error is defined as e f ,i = f re f , f t,i − f t,i . Three regulators are indeed implemented in the proposed controller: the velocity control gain G f 1,i , the position control gain G f 2,i , and the integral force-tracking error gain G f 3,i . In order to implement the proposed optimal controller, a coupled robot-environment interaction dynamics has to be defined. To this purpose, the interaction environment is modeled as an elastic system (i.e., with a stiffness parameter K e,i as in [44]), making it possible to write the following coupled robot-environment dynamics in the state-space: (40) The stiffness K t,i (η f ,i ) and the damping D t,i (η f ,i ) parameters of the Cartesian impedance control are written as a function of the system state η f ,i = [ẋ t,i , x t,i , f i,t ] T . Such parameters are, in fact, modulated based on the current interaction state in order to achieve a smooth and stable transition between the the optimal impact control and the optimal force control, as it will be described in Section VIII.
The state-expression (40) can be re-formulated into the compact matrix form: To design a robust implementation of the optimal force controller to take into account uncertainties in the environment dynamics modeling (i.e., considering the uncertainties in the estimation of the environment stiffness parameter K e,i ), such uncertainties are included into (41), resulting in the following updated state-expression: with U K e,i representing the bounded modeling uncertainties on the target parameter K e,i , and: Since the state-expression (42) including the modeling uncertainties is nonlinear w.r.t. the state η f ,i , a SDRE control approach is exploited to implement the optimal force control, making it possible the online (i.e., at each control step) computation of the control gains G f 1,i (t), G f 2,i (t), and G f 3,i (t) in (39). In fact, the proposed SDRE control methodology allows for the approximation of the infinite-horizon nonlinear optimal tracking control problem [48]. Therefore, the solution of the SDRE controller can be tackled in the same way as for an LQR control problem (i.e., similarly as described in Section VI).
Shall J f cost,i be the controller performance index for the design of the SDRE controller: being Q f ,i (η f ,i ) the diagonal control matrix. In this paper, the diagonal element of Q f ,i (η f ,i ) are non-zero, and they are defined as follows: Such a definition of the control matrix Q f ,i (η f ,i ) allows adapting its gains on the basis of the current interaction state, making it possible to accordingly modulate the control action. The position error-related gain Q f ,i (η f ,i )(1, 1) exploits a formulation involving the estimated interaction force f t,i . By normalizing it by means of the reference force f re f , f t,i , it is possible to decrease the considered control gain as soon as the f t,i approaches f re f , f t,i . In such a way, the resulting control action is reduced in order to limit force overshoots. Vice versa, for f t,i → 0, Q f ,i (η f ,i )(1, 1) increases, providing to the controlled system a higher bandwidth (i.e., a faster dynamics). q f x and g f % x are the constant gains modulating the adaptation of Q f ,i (η f ,i )(1, 1). The same considerations apply for the integral force error-related gain Q f ,i (η f ,i ) (3,3), with q f f and g f % f representing the constant gains modulating its adaptation. The velocity error-related gainQ f ,i (η f ,i )(2, 2) exploits a formulation involving the robot velocityẋ t,i . By normalizing it by means of a maximum allowed velocitẏ x max t,i , it is possible to increase the considered control gain as the measured velocity increases. q fẋ is a constant gain modulating the adaptation of Q f ,i (η f ,i ) (2,2).
In order to compute the optimal control action x d, f t,i , the following minimization problem has to be solved: Resolving (46) corresponds to solve the related Riccati matrix equation [48]: where S f ,i = S T f ,i is the solution of (47) (symmetric and positive semi-definite constant 3x3 matrix), I is the 3x3 identity matrix, β f ,i is a design parameter, and F f ,i includes the modeling uncertainties. F f ,i is defined as: with: Considering the reference vector η re f f ,i = [0, x t,i , f re f , f i ] T for the reference tracking control problem [48], the control law in (39) becomes: where: Remark 4. (47) has to be solved for each control step t due to the fact that the state expression (42) includes non-linearities on the state η f ,i . Therefore, the control gains G f 1,i (t), G f 2,i (t), and G f 3,i (t) in (39) are not constant. Indeed, such control gains cannot be offline computed on the basis of the robotenvironment modeling as in [24]. The solution S f ,i of (47) can be analytically computed as a function of the dynamic parameters (including the modeled uncertainties) in (42), as it is shown in the following.

VIII. CARTESIAN IMPEDANCE CONTROL PARAMETERS ADAPTATION
As stated in Section V, the output of the switching mechanism (i.e., the Fuzzy Logic approach) is employed to adapt the Cartesian impedance control parameters (i.e., the stiffness and damping parameters). In fact, as described in Section VI and in Section VII, a slightly different tuning of the Cartesian impedance control is considered in (22) and (31). By properly defining an adaptation law for the impedance control parameters, a smooth and continuous transitions between the optimal impact control and the optimal force control can be implemented.
The following law for the update of the stiffness parameter K t,i has been defined, exploiting the Fuzzy Logic output α: where α min (the same parameter defined in Section V) and α max defines the range of adaptation. K min t,i and K max t,i are the minimum and maximum stiffness values, respectively. The stiffness is increased in the contact phase to achieve the forcetracking objective. Similarly, the damping parameter D t,i can be updated exploiting the following law based on the Fuzzy Logic output α: (53) D min t,i and D max t,i are the minimum and maximum damping values, respectively. The damping is increased in the contact phase to absorb the impact and to damp oscillations that can arise due to the robot-environment coupled dynamics. Exploiting the Fuzzy Logic output α, (52), and (53), it is possible to implement a smooth and continuous transition for the Cartesian impedance control parameters when switching from optimal impact control to optimal force control (and vice versa).

IX. EXPERIMENTAL RESULTS
In this Section, experimental results are provided. The experimental setup is described, defining the target probing task for validation purposes. An ablation study has been performed to evaluate the performance of each defined component (i.e., EKF, optimal impact control, OSIF control), incrementally composing the OSIF controller. The proposed sensorless OSIF controller has been compared with the sensor-based OSIF controller (i.e., exploiting the measurements available from the employed robotic platform instead of the estimation provided by the proposed EKF). In addition, an assembly task has been tested to prove the possibility to implement the proposed OSIF controller in real applications.
A video showing the experimental results achieved by the OSIF controller is available at the link https://youtu.be/ 64oWtLloTSY.

A. SETUP AND TASK DESCRIPTION
To implement and validate the proposed OSIF controller, a Franka EMIKA panda manipulator has been employed. The adopted robot is equipped with torque sensors at jointlevel, making it possible to estimate the interaction forces at the robot's end-effector. These measurements are used as ground-truth to validate the EKF (Section IV) and the performance of the sensorless OSIF controller w.r.t. the performance of the sensor-based OSIF controller. The Franka EMIKA panda robot torque control (control frequency: 1000 Hz) has been exploited for the implementation of the proposed OSIF controller (Section III, Section VI, Section VII, Section VIII).
Two tasks have been considered for validation purposes. Firstly, a probing task has been used for the proposed ablation study (i.e., a systematic incremental validation). The robot has to move in the free-space to approach a stiff environment (exploiting the optimal impact controller in Section VI) along with the vertical task Cartesian direction z. As soon as the contact is established, a reference force has to be tracked (exploiting the optimal force control in Section VII). In this task, no tool has been attached to the robot, i.e., having a payload equal to 0 kg. An assembly task has been also considered in order to show the possibility to use the proposed controller in a real application. The robot has to assemble a gear into its shaft. The main task direction is the vertical Cartesian direction z. In this task, the Franka EMIKA panda robot gripper, grasping the target part to be assembled, has been attached to the robot, i.e., having a payload equal to 1 kg. For both the tasks, the OSIF controller allows to properly switch between optimal impact and optimal force control (and vice versa) as needed, exploiting the switching mechanism described in Section V.
Remark 5. While the Franka EMIKA panda robot has been used as a test platform, it has to be underlined that the proposed control method can be applied to any industrial robot. In fact, no specific safety features from the test platform robot have been used in order to implement the proposed controller. In addition, the proposed controller has been experimentally validated on the basis of a sensorless and sensor-based framework, showing a generalizable behavior. Remark 6. It has to be underlined that the proposed OSIF controller has been applied along the z translational task direction, while the other translational and rotational DoFs are controlled only exploiting the low-level controller in Section III (i.e., constant impedance control parameters and fixed setpoint). The OSIF controller can be easily applied to the other DoFs due to the decoupled behavior achieved by the Cartesian impedance control.

B. CONTROL PARAMETERS SETTINGS
In order to implement the proposed OSIF controller, the following parameters have been applied for the different controllers.

3) Switching control mechanism
the proposed EKF is capable to achieve high-accuracy estimation of the external joint torques with high bandwidth. Therefore, the rules in (20) have been applied (i.e., only the input membership function related to the force has been implemented). The following parameters have been imposed:

C. COMPUTATIONAL EFFORT
The Franka EMIKA panda robot torque controller is a realtime closed-loop control, requiring to provide a reference to it with a frequency of 1000 Hz. Indeed, the proposed control framework has to be capable to compute a reference signal within 1 ms. Based on their analytical derivation (both for the optimal impact control -Section VI -and for the optimal force control -Section VII), it is possible to online compute the control gains of the high-level controller within the specified control step. The FuzzyLite library has been used for the implementation of the Fuzzy Logic in Section V, making it possible to implement the high-level controller switching mechanism (https://www.fuzzylite.com/cpp/).

D. EXTERNAL JOINT TORQUES ESTIMATION RESULTS
The proposed EKF (Section IV) has been tested in order to verify its performance w.r.t. the estimation of the external joint torques. External forces/torques have been applied to the Cartesian impedance-controlled robot (as described in Section III, i.e., the high-level switching controller is not active) by a human operator. Exploiting the measurements provided by the Franka EMIKA panda robot, it has been possible to measure the external wrench vector h ext , to be compared with the estimated external wrench vector h ext . Figure 4 shows the measured external wrench h ext vs. the estimated external wrench h ext . As it can be seen from the provided plots, the proposed observer is capable to estimate the external wrench with high accuracy and reduced delay.

E. OPTIMAL IMPACT CONTROL RESULTS
The proposed optimal impact controller (Section VI) has been tested in order to verify its capabilities to limit the impact force w.r.t. a predefined maximum value f re f ,imp , as in (30). The impact reference velocityẋ re f ,imp t,i is then imposed equal to 0 m/s as soon as the interaction force overcomes a specified threshold of 4 N. The estimated impact force provided by the EKF has been compared with the measured impact force provided by the manipulator. Figure 5 shows the established impact force achieved with VOLUME 4, 2016 This work is licensed under a Creative Commons Attribution 4.0 License. For more information, see https://creativecommons.org/licenses/by/4.0/ This article has been accepted for publication in a future issue of this journal, but has not been fully edited. Content may change prior to final publication. (dotted line). The switching between the optimal impact control and the optimal force control (and viceversa) is highlighted by the change of f re f , f t,i . the implemented optimal impact controller. As it can be seen, the proposed controller allows to limit the impact velocity, satisfying the limit defined by f re f ,imp t,i .

F. OSIF CONTROLLER RESULTS
The proposed OSIF controller has been tested to verify the complete performance of the proposed framework. Figure 6 shows the measured interaction force f t,z vs. the estimated interaction force f t,z during the probing task execution. The optimal impact controller is exploited during the free-space approaching motion, while the optimal force controller is exploited during the contact phase. The sensorless OSIF controller has been compared with the sensor-based OSIF controller. As it can be seen, the sensorless OSIF controller is capable to achieve high-performance in force-tracking, even if the measured force shows limited overshoots. Due to its dynamics, in fact, the EKF is not capable to properly (dotted line). The switching between the optimal impact control and the optimal force control (and viceversa) is highlighted by the change of f re f , f t,i . estimate the interaction force at high frequency (i.e., during transitions). To reduce the overshoot in the measured force and to limit the controller bandwidth, the optimal force control gains q f x , q fẋ , and q f f can be reduced.

G. ADVANCED ASSEMBLY TASK EXPERIMENT
An assembly task (of a gear into its shaft) has been also considered to verify the applicability of the proposed approach in a real application. Figure 7 shows the measured interaction force f t,z vs. the estimated interaction force f t,z during the assembly task execution. Trial 1 shows a smooth insertion of the gear without pre-collisions. Trial 2, instead, shows a pre-collision between the shaft and the gear. In such a situation, the optimal force controller is switched on. After the collision, the gear slides engaging the shaft. The optimal impact control is switched on again until the gear is fully assembled and the optimal force control is again switched on. The proposed OSIF control is, therefore, able to manage the switching between controllers during the different task phases. The switching mechanism (as shown in Figure 8, having the α parameter varying as soon as the contact is established) allows switching from one controller to the other. Trial 1 shows only one switching event, while trial 2 shows multiple transitions. Figure 9 shows the Cartesian impedance control stiffness parameter adaptation on the basis of the Fuzzy Logic output α, as described in Section VIII, guaranteeing a continuous and smooth switching between the controllers. Similar results are achieved for the Cartesian impedance control damping parameter. It is important to highlight three main time instants related to the experiments to evaluate the control performance: the real impact time, the detected impact time, and the control reaction time. W.r.t. trial 1, the measured real impact time is 7.250 s, the detected impact time is 7.380 s, and the control reaction time is 7.381 s. Indeed, 0.131 s is the delay affecting the controller. Such a delay is mainly related to the EKF dynamics. In the case of the sensor-based OSIF controller (as shown in Section IX-F) such a delay is remarkably reduced to few control steps. Similar results are obtained for the trial 2. : The Cartesian impedance control stiffness K t,z parameter is adapted during the assembly task execution.

X. CONCLUSIONS AND FUTURE WORK
The presented paper proposes a sensorless optimal switching impact/force controller to deal with interaction tasks. The controller is developed to be employed by a sensorless position-controlled robot, developing a low-level controller (composed of an inner joint position controller, an outer Cartesian impedance controller, and of an EKF -developed in [36] -for the estimation of the external joint torques), and a high-level controller (composed of a switching mechanism to select an optimal impact controller and an optimal force controller). Experimental results (involving a Franka EMIKA panda robot) validated the proposed approach, highlighting the achieved performance: high performance in the estimation of the external joint torques (and interaction wrench), satisfying the limit on the maximum impact force, highaccuracy force-tracking capabilities, and stable switching mechanism managing the transitions of the high-level controllers. While the implemented optimal force controller relies on the approach in [41], the here presented experimental results show its implementation for real robotic applications within the proposed OSIF controller (i.e., only simulation results have been provided in [41]).
In this Appendix, the optimal force control gains calculation is shown, considering the controller designed in Section VI-B.
On the basis of 50, its control gains have to be updated at each control step to implement the proposed SDRE force controller. To do that, the terms of S f ,i can be derived as it follows.
Considering the term S f ,i (1, 1), it can be computed by solving a fourth-order equation resulting in the following solutions:   Intelligence in Lugano (Switzerland) and Lecturer at the SUPSI University of Applied Sciences and Arts of Southern Switzerland. His main research interests include system identification, robust control, Bayesian filtering and non-convex optimization, with applications to process control and smart manufacturing. VOLUME 4, 2016