A General Framework for Shared Control in Robot Teleoperation with Force and Visual Feedback

In the last decade, the topic of human robot interaction has received increasing interest from research and industry, as robots must now interface with human users to accomplish complex tasks. In this scenario, robotics engineers are required to take the human component into account in the robot design and control. This is especially true in telerobotics, where interaction with the user plays an important role in the controlled system stability. By means of a thorough analysis and practical experiments, this contribution aims at giving a concrete idea of the aspects that need to be considered in the design of a complete control framework for teleoperated systems, that are able to seamlessly integrate with a human operator through shared control.


Introduction
Although many branches of robotics are currently striving for complete autonomy, such as self-driving cars and drones, a parallel research line is instead trying to tie together robots and humans in a integrated way. Indeed, on the one hand, robots are often called to work together with humans as they are inherently designed to interact with their environment, on the other hand, the purpose of robots is that of being useful to its users. Therefore "closing the loop" around this human component is paramount for an optimal system design.
In certain applications demanding high-level reasoning, human presence is not only advised, but it is also mandatory. Nonetheless, the harshness and inaccessibility of the environments where they are called to operate makes the use of remotely controlled robotic systems, essential to complete the required tasks. With its first modern applications in the '50s in the field of nuclear material handling, robot teleoperation is nowadays commonly associated by the general public with surgical robots [19].
Even so, its use has spread to multiple domains, and it has sought the interest not only of the research community, but also industries. In the frame of the H2020 RoMaNS project, innovative teleoperation interfaces and visual servoing are proposed for the sorting of radioactive material [2], while the WALK-MAN project has aimed at building a humanoid teleoperated platform for inspection in dangerous unstructured environments damaged by natural events [11].
Massive attention is being given to telerobotics by industries producing pharmaceuticals, to handle toxic substances and customize drug production [18]. Clean room applications of teleoperated robots are now being investigated by ESA and NASA, in the scope of the Mars Sample Return exploration campaign, which aims at bringing back Mars samples for the analysis of life traces. Whereas classical glovebox systems are inadequate for planetary protection protocols, a remotely controlled robot can solve this problem by ensuring a high degree of isolation between the robotized cell and the external environment [21]. Nevertheless, space telerobotics is already a reality for in-orbit servicing, and at an advanced research stage in terms of exploration, with both directly teleoperated systems, such as the ones used in the Kontur project, and embedding the concept of supervised autonomy in the scope of METERON [17], with experiments already conducted from aboard the International Space Station.

Research Challenges
Historically, research on bilateral teleoperation has considered the closed loop system stability in presence of communication delays and with uncertain dynamics of the environment and the user [8]. The foundations of modern teleoperation were outlined by Anderson and Spong [4] with the scattering approach, highlighting the trade-off between stability and transparency, i.e. the system displayable impedance range. More recent approaches have shifted towards less conservative techniques such as time-domain passivity. However, to ensure practical stability, some feeling of presence at the remote location has to be sacrificed, degrading the user perception of the remote environment [10].
In the unstructured environments where teleoperation is required, the robust control of master and slave robots interaction dynamics are paramount, in order to ensure the accurate execution of the desired task and its stability. The problem of impedance control in teleoperation has been investigated in [15], which proposed tuning guidelines for the intercontinental control of a humanoid robot subject to communication delays.
The development of interfaces and control algorithms for kinesthetic, tactile, and visual feedback in teleoperation has found fertile grounds in research, for applications where it is critical that the user has a strong sense of presence, while exploiting these cues to improve the system usability. In [7] classical visual servoing aids a physician in the execution of a tele-ecography, by exploiting image features to automatically turn the robot tool while optimizing the ultrasound image quality. Overall, these topics outline a research interest that is increasingly focusing on the interaction between user and control system in the form of shared autonomy architectures. Splitting the burden of operation between user and robot becomes the main design objective, in order to reduce operator fatigue and offer an intuitive experience when using the platform for tasks that are physically and cognitively demanding [3]. Shared human-robot controllers can be the answer for an efficient and seamless human-robot cooperation, with estimation and prediction of user behavior being able to further improve the user experience.
The present paper aims at briefly illustrating a comprehensive control framework for teleoperated robot systems, from the low level motion control aspects, interaction control, and system stability analysis with human-in-the-loop, to the higher level visual-aided control algorithms reducing the operator cognitive load. In Sect. 9.2, a robust controller exploiting sliding mode control techniques is proposed for redundant robot manipulators to arbitrarily and reliably assign the robot impedance in the task space and track the user input in a teleoperation scenario. The enclosing optimization-based high level model predictive controller ensures robustness of the sliding mode to actuation delays and unmodeled system dynamics. System stability is proved using absolute stability criteria, with guidelines to tune the controller parameters. In Sect. 9.3, this architecture is exploited to allow the straightforward definition of virtual fixtures and their rendering to the operator via force feedback. Finally, in Sect. 9.4, the framework is extended to include visual feedback from a camera mounted on a dual-arm slave robot, and visual servoing is employed to improve user interaction with the system, by introducing some autonomy into the slave robot in terms of camera control. Section 9.5 concludes this brief.

Robust Impedance Shaping of Redundant Teleoperators
At a local control level, robust control theory is used to guarantee a desired dynamic behavior of the robot during interaction. Sliding mode control [20] robustly shapes master and slave manipulators impedances, irrespectively of uncertainties. This formulation is addressed by directly specifying the task sliding manifold, while the manipulator redundancy is considered by successive null-space projections of sliding manifolds defined by lower priority tasks. A hierarchical optimization outer layer takes into account individual control and motion constraints, such as torque or joint limits, while its model predictive nature also guarantees robust compensation of actuation delays and unmodeled input filter dynamics. The overall stability analysis in presence of variable communication delays during contact is performed thanks to Llewellyn's absolute stability criterion.

Model Predictive Sliding Mode Control
The generic manipulator dynamics can be represented by the following model where q,q,q ∈ R n are the robot joint positions, velocities and accelerations respectively, B(q) ∈ R n×n is the symmetric positive definite inertia matrix, n(q,q) ∈ R n is a vector comprising Coriolis, gravitational, and friction terms, τ ∈ R n is the actuation torque, F e ∈ R m is the external generalized force due to interaction with the environment, and finally J(q) ∈ R m×n is the robot end effector Jacobian, with m ≤ n. The objective of impedance control is to change the displayed end effector dynamics according to the following equation where M, D, K > 0 ∈ R m×m are the desired inertia, damping and stiffness matrices respectively, whilex = x r − x ∈ R m is the end effector tracking error with x r being the reference trajectory, and( ·) indicating estimated quantities. Unfortunately, the simple application of a nominal control law [9] results in a system that is heavily affected by uncertainties and disturbances (the second term on the right-hand side of (9.3)), and that is not able to satisfy the dynamics requirement.
Therefore, the following Second Order sliding mode control law [6] is proposed in order to compensate the uncertainties, while maintaining a negligible control chattering where v is the auxiliary control to be selected after applying a feedback linearizing controller to (9.1), v 0 the nominal control used to obtain the desired impedance, and q the so-called sliding variable. By appropriately choosing q , we can prove the following Theorem 9.1 Consider the partially feedback linearized version of the robot dynamics (9.1) and the control (9.4) with sufficiently high gains. Let . On the sliding manifold q = 0, the system evolves with dynamics free of disturbances and uncertainties. Moreover, this holds beginning from the initial time instant t 0 (integral sliding mode).
Therefore, thanks to (9.4), we can completely disregard the uncertainties, and select the nominal control v 0 to ensure the desired end effector impedance as in the nominal case. For redundant manipulators (m < n), however, other tasks can be accomplished simultaneously. Hence, we enclose the sliding mode controller with a hierarchical formulation of model predictive control: • To enforce the desired task hierarchy • To consider unilateral motion and torque constraints • To compensate actuation delays and unmodeled input dynamics The proposed controller results in a cascade of optimizations starting from the higher priority one, enforcing the impedance (9.6a), to the tasks with lower priority (9.6b) whereσ i represents the derivative of the desired task sliding manifold, and Q i cost weighting factors. By taking the predictions of the state and the sliding manifolds, robustness up to d steps of actuation delay is guaranteed by applying the following control law at time k, where (·) * indicates predicted quantities.

Robust Impedance Shaping
The application of this Model Predictive Sliding Mode Control scheme ( Fig. 9.1) to a teleoperation system is straightforward, if we modify the desired impedance in order to reflect the slave environment force F e on the master (9.9), and ensure master reference trackingx = x s − k p x d m for the slave (9.10), where k f and k p are force and position scaling factors, and (·) d indicates a delayed quantity due to the communication delay between the two systems.
Tuning guidelines for the master and slave parameters can be obtained by analyzing the hybrid matrix H describing the interconnected system, while guaranteeing teleoperation stability in presence of delays.
where τ is the force feedback filter constant, and d 1 , d 2 the communication delays. From (9.11), Llewellyn's absolute stability condition requires that all impedance dynamic parameters are greater than zero, and that the following condition is satisfied, with d rt the round-trip communication delay.
Given the previous equation, we can show that the optimal tuning that maximizes teleoperation transparency is obtained by solving the optimization problem where ω 0 is the largest zero crossing frequency of (9.12) when the force feedback is unfiltered. This formulation and tuning of the teleoperation controller is able to provide better performance transparency without sacrificing stability, compared to modern time-domain passivity approaches [16].

Virtual Force Feedback
Force feedback information can be provided to the user not only through contact with a real environment, but also via an artificial force feedback generated by virtual fixtures [1]. The proposed architecture naturally gives itself to the specification of these robot motion constraints, with the force feedback rendered via the dual solution of the optimization to help and guide the operator. The robot autonomously can control some of the tasks and provides kinesthetic information on the status of the remotely controlled device [13].
In order to understand how the virtual force feedback can be computed, let us rewrite the hierarchical controller of the slave ((9.6a), (9.6b)) in an alternative way, by considering two priority stages and highlighting the slack variables w S in the second one v 0,0 = arg min v 0 where we indicate with subscripts H and S, hard and soft virtual fixtures for reasons that will be now explained.
Let us start by considering the first optimization, and writing down the Karush-Kuhn-Tucker (KKT) optimality conditions where λ H,1 is the dual optimum associated with the hard constraints of the first stage, and ∇ f (v 0,1 ) the gradient of the cost function in the optimum. Whenever a hard fixture is active, (9.15a) has to hold, and the corresponding Lagrange multiplier λ H,1 may be different from zero. Equation (9.15b) can then be rewritten as follows Therefore, with a Lagrangian mechanics interpretation, the second of the KKT conditions represent the slave dynamics with an additional virtual environment force F v,H , defined by the virtual fixture Lagrange multiplier. This additional contribution can then be added to the master manipulator force feedback in (9.9). For the soft fixture stage, application of the same principle gives the following for the second KKT condition where λ H,2 and λ S,2 are the dual optima associated with the constraints of the second stage, and K S = 2 Q S represents the soft fixture stiffness. Therefore, the soft fixtures contribution to the force feedback is easily computed With the proposed controller formulation, we can avoid the tuning of very large and very small weights to simulate rigid fixture surfaces, since hard constraints are separately defined in the first optimization stage and are never violated. While we can also prevent the appearance of unwanted master-slave coordination errors due to the penetration of soft fixtures in the second stage. Overall, from (9.9), by applying both environmental and virtual feedbacks, the desired master dynamics equation becomes the following.

Occlusion-Free Visual Feedback
Visual feedback by means of image-based visual servoing is integrated and experimentally validated on a dual-arm platform, where one arm is teleoperated and the other completely autonomous and equipped with a eye-in-hand camera sensor. The proposed extension to the framework helps the user in navigating cluttered environments and keep a clear line of sight with its target by autonomously avoiding occlusions, reducing the operator workload to complete a reaching task by delegating any camera reorientation to the autonomous arm.
The vision system requirements in a remote reaching task can be summarized as follows: clearly understand what is happening remotely, and how to influence the system behavior.
The occlusion-free behavior can be easily included in the slave controller as an inequality constraint, by using a minimum distance criterion [12]. Indeed, it is sufficient to notice in Fig. 9.2 that, for a generic point p t in the FoV, an occlusion only occurs if it enters the polytope area defined by the occluding object projection in the image. The constraint is defined as follows where the first term on the left represents the squared distance between the feature of interest p t and a point p s on the segment delimiting the polytope, while the second one is proportional to their relative velocity. t b is a design parameter that relates to the maximum time required by the robot to bring to a halt the features in the image. By making use of the pinhole camera model and the camera Jacobian, it is straightforward to obtain a dependence on the robot joint accelerations as required by the optimization procedure (9.6a).
The constraint can also be made robust to the features relative position and velocity uncertainties (9.21): p ∈ D p and ṗ ∈ Dṗ.
It can be shown that this approach has also the benefit of providing robustness against camera interaction matrix uncertainties as well as unmodeled dynamics of the environment.
Although the constraint itself guarantees the absence of occlusions, it does not ensure an optimal camera motion, nor a movement that is perceived as natural by the user. To achieve this, a state machine is defined, where in each state different quantities are regulated and added to the cost function to be minimized in (9.6a). The two main system phases are identified by an approach state (S A ), where there is no danger of occlusion occurring, and the camera simply follows the teleoperated arm, and an occlusion state (S O ), where an occlusion is likely to happen, and the camera has to promptly react.
In the approach phase, the main impedance and tracking task are pursued, while regulating the teleoperated arm and target goal to fixed points in the FoV (e t , e g,com ), and limiting the camera rotation around its optical axis (e φ ). When an occlusion constraint activates, a transition to S O occurs.
During the occlusion phase, instead, the teleoperated end effector and goal features velocities are minimized (ṗ t ,ṗ g,com ), along with the occluding object projected area (a o ) and the camera velocity along its optical axis (v c c,z ). This allows to create a camera motion that pushes the occluding object out of the FoV (e o,com ) and recovers nominal operation, transitioning back to S A .
The proposed framework has been experimentally validated on a platform consisting of a dual-arm ABB YuMi robot slave device, and a Novint Falcon master device ( Fig. 9.3). The controller is not only able to track the user input, but also to autonomously adjust the camera position to provide an intuitive visual feedback, as well as environmental and virtual force feedback, fully integrating robot control and user intention in a single shared autonomy architecture.

Conclusion
We have presented a complete robot teleoperation framework, encompassing robot dynamics control, as well as user interaction through force and visual feedback. We have also shown how operator and robot autonomy can coexist by means of a shared controller experimentally validated on a real industrial dual-arm robot.
Further studies should be conducted on how to improve the user-robot interaction. In human-robot collaboration for industrial applications, the use of machine learning techniques have been employed to infer the user intention during collaborative tasks [5]. Based on human motion limb studies and synthetic data, the model trained in [14] is able to predict the user motion and use this information to actively help the operator via the inclusion of an assistance control component. A similar approach could be investigated to further reduce the user cognitive burden and simplify task execution in telerobotics: [22] provides one possible implementation, with an assistive behavior learned via demonstration and then integrated with the user input. The images or other third party material in this chapter are included in the chapter's Creative Commons license, unless indicated otherwise in a credit line to the material. If material is not included in the chapter's Creative Commons license and your intended use is not permitted by statutory regulation or exceeds the permitted use, you will need to obtain permission directly from the copyright holder.