A Passivity Based Framework for Safe Physical Human Robot Interaction

In this paper, the problem of making a safe compliant contact between a human and an assistive robot is considered. Users with disabilities have a need to utilize their assistive robots for physical human-robot interaction (PHRI) during certain activities of daily living (ADLs). Specifically, we propose a hybrid force/velocity/attitude control for a PHRI system based on measurements from a 6-axis force/torque sensor mounted on the robot wrist. While automatically aligning the end-effector surface with the unknown environmental (human) surface, a desired commanded force is applied in the normal direction while following desired velocity commands in the tangential directions. A Lyapunov based stability analysis is provided to prove both convergence as well as passivity of the interaction to ensure both performance and safety. Simulation as well as experimental results verify the performance and robustness of the proposed hybrid controller in the presence of dynamic uncertainties as well as safe physical human-robot interactions for a kinematically redundant robotic manipulator.


I. INTRODUCTION
While assistive robotic devices such as Wheelchair Mounted Robotic Arms (WMRAs) [1]- [5] and Companion Robots [6]- [10] traditionally help users with object retrieval [11] or pick and place tasks [12], they are quite capable of physical humanrobot interaction (PHRI) with the user themselves.Users with disabilities have a need for assistance with activities of daily livings (ADLs) such hair-grooming, scratching, face-sponging etc.; all these daily activities require physical interaction with various surfaces on the human body.Under this need, the assistive robot has to be able to align with the unknown (human) surface, and also apply a desired force in the normal direction while following the surface based on desired velocity profiles that the user can command to the robot.It is critical that the assistive robot be able to execute a safe compliant contact with the human user.
Researchers have proposed various methods to achieve hybrid position/force control on a surface.In [13], the author used the exact CAD model for polishing position/force control.With the CAD/CAM model, they are able to track the desired trajectory, force, and contact direction.Since it requires an exact CAD model of the environment, this method will not work for the PHRI problem considered in this paper.In [14], the authors designed and implemented a compliant arm to perform bed bath for patient hygiene.A bang-bang controller Z. Ding and M. Baghbahari are graduate students with the Electrical and Computer Engineering Dept. at the University of Central Florida (UCF), Orlando, FL 32816.(E-mail: zhangchi@knights.ucf.edu;baghbahari@gmail.com)Aman Behal is with ECE and NanoScience Technology Center at UCF, Orlando, FL.Email: abehal@ucf.edu(corresponding author) was utilized to maintain the z-axis force against the body between 1-3N while a laser range finder was utilized to retrieve the skin surface point cloud of the skin followed by selection of wiping area by the operator.This method also required to obtain the point cloud of the environment.The authors of [15] proposed a contact force model for wiping and shaving tasks.They captured the face point cloud and the force profile of health participants performing daily living tasks such as wiping and shaving.Then, they built a threeparameter trapezoidal force model of each stroke and the force dependency on the facial area.Besides assuming previous knowledge of the environment, there are other approaches for unknown environments.In [16], the author proposed two methods for exploring unknown surfaces with discontinuities by using only a force/torque sensor.They rotate the direction of the desired motion/force instead of rotating the end-effector to keep moving and inserting force on unknown surface; however, this method cannot be used for certain PHRIs which need continuously variable alignment between the end-tool and human body, such as during shaving.In [17], the authors proposed a hybrid position-force sliding mode control for surface treatment such as polishing, grinding, finishing, and deburring -the end-effector can apply the desired pressure on the surface and also keep the end-effector orientation perpendicular to the surface; however, the orientation constraint is not considered for moving along a frictional environment.In [18], the authors proposed a deformation-tracking impedance control for interacting with unknown surfaces by using an extended Kalman filter to estimate the parameters of the environment, thereby controlling the interaction force indirectly by tracking the desired deformation without force sensing.However, this method cannot estimate the interaction torque; therefore, during the alignment phase of the assembly task, their desired interaction torque is just determined experimentally.In [19], the authors proposed an inverse differential kinematics-based position/force control for cleaning an unknown surface.They utilized a force/torque sensor to provide feedback for the force control part.However, this velocity control-based algorithm is not considered safe for human-robot interaction; furthermore, the evaluation of surface alignment is also missing.
In this paper, we propose a robust hybrid force/velocity/attitude controller which guarantees passivity of a robot manipulator with respect to unknown environments.By using measurements of robot states as well as interaction forces/torques at the end-effector, we are able to reshape the end-effector impedance (namely inertia, damping, and stiffness) to desired values needed for a safe and compliant interaction.Sliding mode control is utilized to compensate Preprint Submitted to IEEE TCST for Review for exogenous disturbances, model uncertainty, and friction effects which can otherwise degrade the performance of the impedance controller.Within the sliding mode controller, we are able to achieve the desired system dynamics with a general assistive robot which are lower cost and less precision than industry collaboration robot.The resulting controller is able to align the end-effector with the unknown environmental surface while simultaneously tracking desired force and velocity profiles, respectively, in the normal and tangential directions.This work is novel in terms of the guarantees of 6-DOF exponential stability and passivity of our hybrid controller while interacting with an unknown environment.
The remainder of this paper is paper is organized as follows.Sections II and III deal, respectively, with the problem statement and the modeling.In Section IV, we present the controller design, stability analysis, proof of passivity and simulation results for a frictionless environment.In Section V, we present the corresponding control design and stability analysis for a real environment with friction.Experimental results with a kinematically redundant collaborative robot are presented in Section VI.Section VII concludes the paper.

II. PROBLEM STATEMENT
The research objective is to align the robot end-effector with the unknown environment and apply a desired force in the normal direction while following a commanded velocity profile along the tangential directions.In order to guarantee safe human-robot interaction, another research objective is to ensure that the robot acts as a passive system while transmitting user intent to and during interaction with the environment.To design and implement our robust impedance control framework, we assume knowledge of the joint position/velocity measurements as well as the interaction force at the endeffector using a wrist mounted 6-axis force/torque sensor.We assume uncertainty in the robot dynamics and no prior knowledge of the location/orientation of the environmental surface with respect to the robot coordinate system.While we assume that the surface presents damping and stiffness in the normal direction and pure damping along the surface, we assume no prior knowledge of the parameters.

A. Manipulator Model
The dynamics of an n degree-of-freedom robot are given by M (q)• q + C(q; _ q) _ q + G(q) = + env f where M (q) 2 R n n is the symmetric positive definite inertia matrix, C(q; _ q) 2 R n n is the matrix of Coriolis and centrifugal torques, G(q) 2 R n is the vector of gravitational torques, q; _ q; • q 2 R n denote, respectively, the joint angle, joint velocity and joint acceleration vectors, 2 R 6 is the control input vector of joint torques, env = J T F e;e 2 R 6 is the external torque registered at the robot joints, the wrist, J 2 R 6 n is the Jacobian matrix, while f 2 R 6 denotes joint friction.The joint velocity and acceleration for a redundant robot (i.e., n > 6) can be written as follows where end-effector velocity and acceleration vectors, respectively, v b and !b are the end-effector translation and angular velocity expressed in the base frame, J + , J T (JJ T ) 1 denotes the right pseudoinverse of the Jacobian matrix, while b 2 R n is an arbitrary vector utilized to accomplish secondary objectives such as joint limit, collision avoidance, etc.For ease of presentation, we choose b = 0 for the remainder of the paper.
After replacing the joint acceleration and velocity by ( 2) and (3), we can obtain the task space robot dynamics as follows To accomplish our velocity tracking objective, we define error e , e v e !T 2 R 6 in the end-effector frame as follows where T 2 R 3 denotes the desired translational velocity in the end-effector frame, v e , v e;x v e;y v e;z T = (R) T v b and !e , ! e;x !e;y !e;z T = (R) T !b are, respectively, the endeffector translation and angular velocity expressed in the endeffector frame, while R (t) 2 SO(3) denotes the rotation matrix between the robot base frame and the end-effector frame.By utilizing the velocity tracking error expressed in the end-effector frame, e (t) can be related to the end-effector velocity variables in the base frame as follows By rearranging (5) and taking its time derivative, one can obtain the following expressions for _ x (t) and • x (t) By substituting ( 6) and ( 7) in (4), we can obtain the open-loop error dynamics as follows where U , M J + _ JJ + CJ + M J + : RR T : We denote the best guess estimates of M; G; and U , respectively, as M ; Ĝ, and Û , while M , M M ; G , G Ĝ, and Ũ , U Û denote the corresponding uncertainties.Based on these definitions, we can rewrite the open-loop error dynamics as follows where is a lumped model uncertainty term.Before proceeding further, we are motivated by the structure of the robot dynamics and the ensuing control development and stability analysis to assume the existence of the following properties: Property 1 All kinematic singularities are always avoided and the pseudoinverse of the manipulator Jacobian, denoted by J + (q), is assumed to always exist.Property 2 The actual and best estimate values of M; G; U are assumed to be upper bounded as follows:

B. Environment Model
We model the environment as a spring-damper which provides the environmental force in object (environment) frame as follows where K e = diag 0 0 k e ; B e = diag b e b e 0 2 R 3 3 are diagonal matrices of environment stiffness and damping, respectively, ẑv = 0 0 1 T denotes the standard basis vector in the z-direction, while z n 2 R is the z-axis neutral position of the environment in the object frame and z o 2 R is the current z-axis position of the end-effector expressed in the object frame.Here, v o = R o e v e is the velocity of the end-effector with respect to the object expressed in the object frame, v e is the end-effector translational velocity as defined earlier, while R e o is the unknown rotation matrix between the object frame and end-effector frame.In the end-effector frame, the environmentally exerted torque on the end-effector can be defined as follows e = r e f e;e (13) where r e is the unknown position vector from the center of the sensor to the contact point while f e;e = R e o f e;o is the environmental force expressed in the end-effector frame.The model of the interaction between the end-effector and the environment is shown in Figure 1.We also model the endtool for the manipulator as a rigid partial sphere as specified in Figure 2. In the figure, x ee denotes the center of the robot wrist where the 6-axis force/torque sensor is mounted, x c denotes the center of the sphere, r R is the position vector from the sphere center of the end-tool to the contact point, while r of f is the position vector from the sphere center to the robot wrist center and is parallel with the end-effector z-axis denoted by ẑe .

A. Control Design
The proposed control design has an inner-and an outerloop.While the inner-loop is a robust controller to compensate for the system uncertainties and linearize the robot dynamics, the outer-loop reshapes the linearized dynamics to the desired dynamics.In what follows, we discuss the design of control strategies within the two loops that guarantees robust stability, convergence, as well as passivity.For lucidity of presentation, we first demonstrate PHRI stability and passivity when interacting with a non-dissipative (frictionless) environmental surface.
1) Design of the inner loop: Based on the structure of the open-loop error dynamics in (8) and our desire to obtain an impedance controller, we first design a computed torque inner loop controller to linearize the dynamics as follows By substituting ( 14) into ( 9), we can obtain In ( 14), a j is an auxiliary control term that is designed to compensate for the disturbance using a sliding mode controller as follows where a x , [ a x;v a x;! ] T is a yet to be designed auxiliary control term related to the desired dynamics.Motivated by the bound in (11) and in a manner similar to [22], we design the gain, Q, for the sign() function (the standard signum) as follows where is a positive design constant, b D0 ; b D1 , and b D2 have been introduced earlier in (11), while S denotes a sliding surface which is defined as follows such that Then, we have the following result for the inner loop: Lemma 1: Given the robot system in (9) under the control law of ( 14) and ( 16), the sliding surface S and its time derivative _ S will converge to zero in finite time t 1 , and remain there for all subsequent time.
Proof: We define a positive-definite function V S as follows After time differentiating (20) along (19) and utilizing (11) and ( 17), we can obtain Given that sliding mode control [21] has finite convergence time t 1 , it is easy to conclude that lim R _ e a x = 0.
2) Design of the Desired Dynamics: To meet our velocity and force control objectives while projecting desired impedance characteristics on the environment, we propose the following desired dynamics in the translational axes where denotes the force error with f d;z denoting the desired force in the z-direction.In order to achieve the desired dynamics of ( 23), the auxiliary control term a x;v can now be designed as follows For the angular axis, we design a quaternion based controller to align the end-effector with the unknown environment.The rotation between the end-effector z-axis ẑe and the environmental normal no can be represented by a unit quaternion q(q; q 0 ) where q 0 = cos( m 2 ); q = sin( m 2 )n, m and n being the angle and the rotation axis between ẑe and no .The quaternion can be extracted from the torque and force feedback as follows.Based on the definition of the environment torque from ( 13) and the geometry of the end-tool in Now, it is possible to calculate the rotation axis n as follows Since the rotation direction is already contained in the vector n, the range of the angle m between no and ẑe is [0; 2 ).From (26), we can calculate the angle m between no and ẑe as m = arcsin k e k kf e;e k kr of f k : The dynamics of the unit quaternion q(q; q 0 ) are as follows To reshape the angular dynamics in a desired mass/damper form, we begin by designing the desired rotational dynamics as follows where I d , diagfI d;x ; I d;y ; I d;z g is the desired rotational inertia, while B d and u are, respectively, a damping matrix and an auxiliary control input which are yet to be designed.By taking the time derivative of a filtered tracking error r , ! e +K 1 q (K 1 is diagonal positive definite) and premultiplying by I d , we obtain which, by substitution of ( 29) and (30), can be rewritten as Motivated by the ensuing stability analysis, we design u as follows such that where P is a positive definite matrix.Now, it is possible to rewrite the desired rotational dynamics in the following compact form where is a positive timevarying damping matrix, while a , (P K 1 + I)q = (P K 1 + I) 2 kf e;e k kr of f k q 0 e is an auxiliary torque vector which is well defined everywhere on the domain of the problem.Finally, we can design the auxiliary control term a x;! defined in (16) as follows The overall proposed controller is shown in Figure 3.

B. Stability Analysis
Before presenting the main results, we state the following lemmas which will be invoked later.
Lemma 2: The desired dynamics of the angular axis in (35) are locally exponentially stable at the equilibrium point (! e = 0; q = 0; q 0 = 1) in the sense that kqk kq(0)k e t , k! e k k! e (0)k e t when kq(0)k 2 0; 1= p 2 which implies that ! e ; q 2 L 1 when the robot is interacting with a frictionless environment.
Proof: Inspired by [20], we define a nonnegative function V ! as follows which can be upperbounded as follows by utilizing the fact that q T q + q 2 0 = 1.After taking the derivative of (37) along (34) and (29) and canceling common terms, we can obtain It is easy to see from (37) and (39) that q; r 2 L 2 \ L 1 : From this and previous definitions, it follows that ! e ; _ q; _ r 2 L 1 .Then, one can utilize Barbalat's Lemma [23] to prove that lim it is easy to see from ( 38) and (40) that _ V !V !where = minf min (K1); min (P )g maxf2; max (I d ) 2 g : Thus we can conclude that the equilibrium point (q 0 = 1; q = 0; ! e = 0) is exponentially stable in the sense specified in the statement of the Lemma.
Since we are assuming no environmental friction, we can simplify (12) as the time derivative of which can be written as To facilitate the analysis, we take the time derivative of (42) and obtain the dynamics as which is a second-order linear system with perturbation G 1 defined in (73) in the Appendix.By utilizing (74), (75), and (76), we can upperbound G 1 as follows where x , _ v e;z v e;z 1 T is a state vector.Given this definition, we rewrite (45) as follows where xv , 1 0 0 T denotes the standard basis vector in the x-direction while A is a Hurwitz matrix defined in the Appendix.We also define a positive definite function V = 1 2 x T P x whose derivative along (47) is where P and Q are defined in the Appendix.After utilizing the growth bound of (46), we can upperbound (48) as follows which implies exponential stability of V (t) in the following sense Remark 4: From Lemma 1, it can be seen that the closedloop dynamics converge to the desired dynamics in ( 23) and (35) such that the desired impedance characteristics can be projected on the frictionless environment.Based on Lemma 2, the quaternion q (which represents the tool-environment misalignment) converges to zero which implies that the robot end-effector aligns with the frictionless environment.Furthermore, according to Lemma 3, the end-effector can reproduce the commanded end-effector velocities on the environmental tangential plane while applying the desired amount of force in a direction normal to the environment.
From the above Lemmas, we can also have the main passivity result for the proposed controller in the following theorem: Theorem 5: The proposed control law can ensure the work done by the robot on the frictionless environment (human) is bounded in the sense that where F env , f T e;e T e;e T is the force/torque applied by the robot on the environment.Proof: We split the work done by the robot into two phases: (Part a) Work done before reaching the desired dynamics: Since the sliding mode control has finite-time convergence, it will drive the system dynamics to the desired dynamics in finite time t 1 .So the work done by the robot from t = 0 to t = t 1 is as follows Based on the stability analysis of the sliding mode controller, we know that Therefore, we can write Given the definition of (24), we can rewrite (51) as follows which is nothing but a perturbed version of the desired translational dynamics driven by the additional input M d R T J _ S v which remains bounded between [0; t 1 ] and is zero thereafter.Since we know from previous analysis that the solution for the desired dynamics of ( 23) stays bounded, the solution for the perturbed version driven by a bounded input over a finite time period will also stay bounded.A similar argument can be made for the angular velocity desired dynamics.Thus, (k2q 0 q 1 + 2q 2 q 3 k + k2q 1 q 3 2q 0 q 2 k) dt given that f e;o 2 L 1 and where A xy , 1 0 0 0 1 0 .Since jjqjj 1 and q 2 L 1 , it is easy to see that R

C. Simulation Results
During the simulation, we choose 0-5 s as the initial alignment phase during which the commanded velocities in xand y-directions are both chosen to be 0 cm= s. Between 5-20 s and 20-35 s, we, respectively, command the desired velocity to be 1:5 cm= s along the end-effector y-direction.Finally, between 35-40 s, we command 0 cm= s velocity in the tangential direction of the end-effector.From Figures 4-8, we can see that the proposed controller can regulate the force error in end-effector z-direction to zero after the initial alignment process.The end-effector velocity along the tangential directions also tracks the desired velocity with nearly zero tracking error.As for the alignment, the norm of the quaternion kqk and the misalignment angle decreases to 0. To evaluate the alignment, we define the so-called equivalent lever r tan = q F 2 z =( 2 x + 2 y ) which represents the length of projection of the lever arm r e in end-effector x-y plane.When the end-effector is aligned with the environment, it is clear to see from Figure 8 that r tan also converges to zero.

A. Control Design
In the previous section, it is seen that the proposed controller is stable and passive during interaction with a frictionless environment.In this section, we will introduce the controller for a dissipative (frictional) environment which is a better model for the real world.For the case with friction, we can design the sliding mode control and torque input as similarly done in Section IV-A, but if friction is not compensated for, even though the equilibrium for the angular axis remains the same (q 0 = 1; q = 0; ! e = 0), the quaternion now represents the rotation between r e and f e;e ; therefore, the equilibrium represents r e k f e;e .Since f e;e = f o + f f is the resultant of the friction force f f 2 R 3 and the normal force f o 2 R 3 from the object, the angle between the environment normal and r e is o = arctan( kf f k kf o k ); per the geometry of the problem, there exists a non-zero misalignment angle m between the end-effector and the environment normal which is related to the surface friction and the environment normal force.The orientation between the end-effector and environment can be seen in Figure 9.
To compensate for the misalignment due to friction, we design a desired torque d , r d f e;e and extract the desired quaternion q d (q d ; q d0 ) which represents the rotation between the desired lever arm r d , (kr R k kr of f k)ẑ e and the environment force f e;e .Similar to the case without friction in Section IV-A.2, we can define q(q; q 0 ) and q d (q d ; q d0 ) as follows )n e (55) Fig. 9. Geometry of the end-tool.Left plot is the general misalignment case for the environment with friction.Middle plot is the equilibrium of the controller in last section applied to the frictional environment.The right plot is the desired equilibrium for the frictional environment.
where n e = e k e k ; sin( c ) , k e k kf e;e k kr e k ; c is the angle between r e and f e;e , while , where d is the angle between r d and f e;e .Then, we define the quaternion error e(e; e 0 ) based on q and q d e = q 1 d q (57) e 0 = q 0 q d0 + q T q d e = q d0 q q 0 q d + [q] q d The quaternion dynamics are as follows With a process similar to that in Section IV-A.2, the desired dynamics of the rotation axis are designed as follows where such that the dynamics of r , ! e + K 1 e can be written as Finally, we design the auxiliary control term a x;! as follows

B. Stability Analysis
Lemma 6: The desired dynamics of the angular axis in (59) is locally exponentially stable at the equilibrium point (! e = 0; e = 0; e 0 = 1) when the robot is interacting with a frictional environment, in the sense that kek ke(0)k e t , k! e k k! e (0)k e t for ke(0)k 2 [0; 1= p 2) and a positive constant .Furthermore, the states of the angular axis ! e ; e 2 L 1 : Proof: For the environment with friction, our sliding mode control remains the same, but the desired dynamics in angular axis are different.Based on the desired dynamics in (59) and the dynamics of quaternion error in (58) we can define a positive function similar with (37) as follows where .After taking the time derivative of (62) along ( 58) and ( 60) and simplifying, we can have With a process akin to that followed in the proof of Lemma 2 in Section IV-B, it is easy to see that the angular axis is exponentially stable at the equilibrium point (e 0 = 1; e = 0; ! e = 0) such that we can have kek ke(0)k e t and k! e k k! e (0)k e t which clearly implies that ! e ; e 2 L 1 : Proof: The analysis for the x-and y-axes remains the same as in Section IV-B but the model of the environmental force has the damping term due to nonzero B e as shown in (12).The time derivative of the the desired dynamics of ( 23) in the z-axis can be written as follows Given that _ f e;o can be written as ] v e ) the second order dynamics can of (63) can be compactly written after algebraic manipulations as follows where G 1 has been previously defined while G 2 is an additional term defined and bounded, respectively, in (79) and (80).
It is now clear to see from ( 46) and (80) that G 1 + G 2 can be norm bounded as follows  42) and ( 12) that ẑT v R e o ẑv k e (z n z 0 ) 2 L 1 from which it is clear that k e (z n z 0 ) 2 L 1 since ẑT v R e o ẑv > 0 as shown in the Appendix.This implies from ( 12) and ( 13) that f e;o ; f e;e ; e 2 L 1 .It can now be seen from ( 23) that lim t!1 f e;z = f dz : Remark 8: To summarize the analysis for the dissipative case, Lemma 1 shows that we can project the desired impedance on the frictional environment which is then followed up by Lemmas 6 and 7 which show that the robot endeffector aligns with the environment and tracks the endeffector velocity along the environmental tangential axes while also applying the desired amount of force in the environmental normal direction.
Given the above Lemmas, the passivity result for the proposed controller can be stated in the following theorem: Theorem 9: The proposed control law can ensure the work done by the robot over and above the dissipation expected from relative motion at the desired surface velocity between the endeffector and the frictional environment(human) is limited in the sense that f T e;z v e;z dt Putting together the results from Part a and Part b proves the passivity result as stated in the statement of the theorem.
During implementation, we treat c and d defined in (55) and (56) as immeasurable since is impractical to assume that the lever arm r e (as shown in Figure 2) is measurable.Instead, we replace the unknown value of kr e k with a known upperbound kr m k and redefine q( q; q 0 ) and q d ( q d ; q d0 ) as follows where n e and n d have been previously defined, while 1 is a function of c .While the proof using the redefined quaternions is beyond the scope of this manuscript, it can be easily shown (see Appendix) that exponential convergence of c to d implies the exponential convergence of c to d .From Figures 10-14, we can see similar performance as obtained with the frictionless environment.The force error in z-axis is regulated within 0.5N.The force in y-axis is the friction force from the environment surface.The quaternion error is also regulated to within 0.01 which means that the relative attitude between the end-effector z-axis and environment normal converges to (e = 0; e 0 = 1).Alignment results from Figure 13 show that the misalignment angle converges to 0. The velocity and position tracking in the x-and y-axes also perform as expected and the tracking error is regulated to within 0.2 cm.From Figure 14, we can see that the sliding mode control signal compensates for the disturbance, thereby converging the system dynamics to the desired dynamics.

A. Implementation
For experimentation, the Baxter robot from Rethink Robotics was used as the testbed.The ATI Mini45 force/torque sensor was mounted on the wrist of the Baxter to sense the interaction force/torque.The sensor was covered with soft rubber to lower the stiffness of the end-tool.As for the signal processing, we utilized an averaging filter with 45-sample window on the 1000 Hz Baxter status publish node.For ease of implementation, we modified the controllers ( 16) and (61) as follows

B. Experimental Results
The following experiments were conducted: 1) alignment of the end-effector with a yoga ball, 2) moving the end-effector along the yoga ball, and 3) moving the end-effector along the back of a mannequin.The first experiment focused on alignment and stabilization of the end-effector with respect to the environment in order to highlight the critical role of sliding-mode control (SMC).Here, we commanded 0 cm= s desired x-and y-velocities to the end-effector and 10(1 e 0:2t ) N as the desired force in the end-effector z-direction.From Figures 15 -19, it can be seen that use of SMC leads to the interaction force along the end-effector z-direction being regulated to around 10 N with 0:5 N error while the position error was less than 0:2 cm after the end-effector aligned with the surface.The figures also show that the system does not converge without application of SMC.The interaction torque was seen to decrease to less than 0:005 N m, and the equivalent lever converged to around 0.3 mm; both results clearly illustrate that the end-effector aligned along the yoga ball surface with high fidelity through the course of the experiment.In the second experiment, we commanded 10(1 e 0:2t ) N as the desired force in the end-effector z-direction and the desired velocity was set to 0 from 0-10s for the initial alignment and 1:5 cm= s along the end-effector x-direction during the movement.The movement of the manipulator is shown in Figure 20.
From Figures 21-25, the force along the end-effector zdirection was seen to be regulated around 10N while moving on the yoga ball with the desired velocity/position.It can be noticed that there was a minuscule torque error on the pitch axis of around 0:015 N m during movement.Since the controller design assumes a level environment, this torque error drives the end-effector to perform a constant angular velocity to align with the curvature of surface during the movement.
For the third experiment, we replaced the ball environment with a mannequin with an irregular surface typical of the back of a human torso.The movement of the manipulator is shown in Figure 26.From 0-10 s, we commanded the same force along the end-effector z-direction.Then, we set v dx = 0 cm= s; v dy = 0:15 cm= s for 10-30 s, v dx = 0:05 cm= s, v dy = 0 cm= s for 30-50 s, v dx = 0 cm= s, v dy = 0 cm= s for 50-55 s, and then commanded the same velocities in the opposite direction.From Figures 27-31, it can be seen that the overall position/velocity tracking and force regulation performance was close to expected.However, the performance was comparatively degraded with respect to the results in the second experiment due to the high stiffness and the sharp curvature changes in the surface of the mannequin.During the time periods when the end-effector moves over the sharp curvature regions on the mannequin back, such as 13 s-18 s, 40 s-45 s, 70 s-73 s, 88 s-93 s, the force tracking and position tracking and alignment were affected.The force in end-effector z-direction reduced to 4N for about 0.1s, the position error increased to 0.6cm, the equivalent lever also increased to between 1 and 1:5 cm, but the degradation was seen to happen for short periods and the force and position/velocity tracking profiles recover each time.

VII. CONCLUSIONS
In this paper, a robust hybrid controller has been implemented relying on the wrist force/torque and robot joint position/velocity feedback.A Lyapunov-based stability analysis is provided to prove both convergence as well as passivity of the interaction to ensure both performance and safety.Simulation as well as experimental results verify the performance and robustness of the proposed impedance controller in the presence of dynamic uncertainties as well as safety compliance of physical human-robot interactions for a redundant robot manipulator.Definition and Growth Bound of The time derivative of (42) can be written as  we can rewrite the perturbation system (73) as follows Definitions of A, P and Q The matrices A, P , and Q are defined as follows   Based on the relation between c ; d and c , d we can write e = q d0 q q 0 q d + [q] q d = cos(

T 2 R 6
F e;e , f T e;e T e;e T = f e;x f e;y f eis the interaction force measured by the force/torque sensor mounted on

1 i1b 2 ( 11 )
where k k i1 denotes the induced infinity norm of a matrix while b M ; b G ; b U0 ; b U1 ; b M ; b Ĝ; b Û0 ; b Û1 denote known positive bounding constants [22][23].Property 3 The inverse of the inertia matrix is assumed to be bounded by a known positive constant b Minv as M Minv where b Minv denotes a known positive bounding constant [23].Property 4 Based on Properties 2 and 3, the lumped model uncertainty term D defined above in (10) can be upper bounded by a function of the joint velocity as follows kDk < b D0 + b D1 k _ qk + b D2 k _ qk where b D0 ; b D1 , and b D2 denote known positive bounding constants.

Fig. 2 .
Fig. 2. End-tool geometry, where fo = Ke(xn xo), f f = Bevo are the spring and damper induced forces from the environment.
Figure (2), we can rewrite the environmental interaction torque as follows e = r e f e;e = (R e o r R r of f ) f e;e :(25)Since the environmental surface is assumed frictionless and based on the geometry of the end tool shown in Figure2, it is evident that R e o r R k f e;e k no and r of f k ẑe where the notation k denotes parallel vectors.Then, we can simplify the relationship expressed in (25) as e = f e;e r of f = kf e;e k kr of f k no ẑe :

Fig. 3 .
Fig. 3. Block Diagram of Proposed Physical Human Robot Interaction Controller

Lemma 3 :
The desired dynamics of the translational axes in(23) are globally exponentially stable (GES) at the equilibrium point (e v = 0; _ e v = 0) when the robot is interacting with a frictionless environment.Furthermore, v e;z , _ v e;z , _ e v;xy , e v;xy 2 L 1 \ L 1 while f e;o ; e 2 L 1 : Proof: We begin our analysis in the x-and y-axes.The desired dynamics in the x-and y-axes are as follows M d;xy _ e v;xy e v;xy = B d;xy K d;xy I 2 0 2 e v;xy R t 0 e v;xy ( ) d (41) where the notation is obvious from context given the previous definitions.Since (41) is a second-order LTI system with eigenvalues b d;xy p b 2 d;xy 4k d;xy m d;xy 2m d;xy in the strict LHP, the equilibrium point (e v;xy = 0; R t 0 e v;xy d = 0) is GES.As for the z-axis, the desired dynamics of (23) in that direction can be written as follows m d;z _ v e;z = b d;z v e;z + ẑT v R e o f e;o f d;z :

1 t1 kf e;xy k dt < 1 .
Since v e;z ; ! e 2 L 1 as previously shown, it is clear to see that W b c b < 1.Therefore, we can conclude that the total work done by the robot end-effector during Parts a and b is W = W a + W b c a + c b , c < 1 which proves passivity.

Lemma 7 :
The desired dynamics of the translational axis in (23) are globally exponentially stable at the equilibrium point (e v;e = 0; _ e v;e = 0) when the robot is interacting with a frictional environment.Furthermore v e;z ; _ v e;z ; _ e xy ; e xy 2 L 1 \ L 1 while f e;o ; e 2 L 1 :

c < 1
where B e v T d;xy v d;xy 0 is the essential power expended.Proof: As similarly done in the proof of Theorem 5, we split the work done by the robot into two phases: (Part a) Work done before reaching the desired dynamics: Proof is identical as for the frictionless environment shown earlier.(Part b) After reaching the desired dynamics: The work done by the robot on the frictional environment is denoted by W b (t), and can be bounded as follows (e v;xy + v d;xy ) dt + Z 1 t1 10 10 70 ] K d = diag[ 30 30 0 0 0 0 ] Q = diag[ 24 48 48 60 72 72 84 ] P = diag[ 4 4 4 ] K 1 = diag[ 10 10 10 ] Ke = diag[ 0 0 506 ] Be = diag[ 100 100 0 ]
) where e , e d ; d , r d f e;e ; K is a constant diagonal matrix, while I d , diagfi d ; i d ; i d g > 0, and B d! , diagfb d;!x ; b d;!y ; b d;!z g > 0 denote desired rotational inertia and damping matrices.

Fig. 27 .Fig. 28 .
Fig. 27.External force profile for moving on mannequin back.During initial alignment (0-10s), force error is less than 0.5N.During movement, force error stays less than 2N except when end-effector moves on regions of sharp curvature where force error goes to 4N for less than 0.2s.

k 1 k 2
cos( c 2 ) cos(d2 ) Following the same process as in the proof for Lemma 2, it can be shown that v e;z ; _ v e;z ; _ e v;xy ; e v;xy 2 L 1 \ L 1 which implies according to (42) that f e;o ; f e;e 2 L 1 and that lim t!1 f e;z = f dz .Since e = f e;e r of f kf e;e k kr of f k supfkf e;e kg kr of f k < 1;it is evident that e 2 L 1 .
max( 1 ; 2 ; 3 ; 4 ) kxk where 4 ; 5 ; 6 are positive constants.It is now possible to following the analysis as pursued in Section IV-B for the nondissipative case to obtain exponential stability in the sense that kxk ; kv e;z k ; k _ v e;z k e 1 t where and 2 are positive constants of analysis.Since v e ; v o ; _ v e;z 2 L 1 , it can be seen from ( ; e v;xy ; v e;z ; ! e 2 L 1 , we can clearly see from (65) and (67 d B e v d dt + b e kv d;xy k R 1 t1 ke v;xy k dt + kv d;xy k kA xy k sup n kB e k kv e k + kf e;o k

TABLE I SIMULATION PARAMETERS
and q 0 converges exponentially to 1. Since ! e and H converge exponentially to the origin, we can upperbound G 11 ; G 12 ; G 13 as follows (m d;z ) ; 2 (k e ; b dz ) ; 3 (k e ) are system parameter dependent constants.
o ẑv m d;z _ v e;z ẑT v R e o ẑv v R e o ẑv in G 11 ; G 12 ; and G 13 can be B e R o e [! e ] v e + ẑT v HB e _ v e + ẑT v HB e H T _ v e +ẑ T v B e H T _ v e (79) Since ! e and H converge exponentially to the origin and v x ; v y 2 L 1 , G 2 can be norm bounded (in a manner akin to G 1 ) as follows kG 2 k 4 (B e ) exp ( t) kxk (80) where 4 (B e ) is a system parameter dependent constant.Bound for R 1 t1 f T e;xy v d;xy dt