On Robust Impedance Force Control of Robot Manipulators Seul Jung, T C Hsia and R G Bonitz y Robotics Research Laboratory Department of Electrical and Computer Engineering University of California, Davis Davis, CA 95616 Abstract In this paper, a new impedance function is proposed to achieve accurate force tracking under the presence of uncertainties in robot dynamics and environment models The new impedance function is formulated on the basis of PID control of the force tracking error which compensates for the unknown environment stiness and position The robot dynamics uncertainties are compensated by a simple time-delayed robust control algorithm Stability and convergence of the control scheme are analyzed Simulation studies with three link rotary robot manipulator are shown Furthermore, experimental results on a PUMA 56 arm are carried out to conrm the proposed impedance controller's performance 1 Introduction After the pioneering work of impedance control by Hogan [1] there have been two major research issues to be solved on the impedance control: one is to give the force tracking capability to impedance control and another is to compensate for the uncertainties occurred in both robot dynamics and the environment(position and stiness) Within this impedance force control framework, many control algorithms have been proposed Lasky and Hsia have proposed the inner/outer loop control scheme where the robot dynamics uncertainties are compensated by a robust position control algorithm in the inner loop [2] and estimated environment position is modied using an integral control of the force tracking error in the outer loop [3] The generalized impedance control of considering a general dynamic relation between a position error and a force error to deal with the unknown environment stiness has been proposed [4] The adaptive control techniues of us- The author is currently at AHMCT in California y The author is currently at JPL in California ing force tracking errors have been proposed to estimate environment stiness or adjust controller gains to compensate for unknown environment stiness [5] A simple trajectory modication scheme has been proposed using the robot controller to compensate for robot dynamics uncertainties and environment stiness is replaced by force information [6] The purpose of this paper is to provide a simple practical solution to force tracking impedance control that is robust with respect to uncertainties in both robot dynamic model and environment position and stiness The main idea is to set the stiness gain in the impedance function to zero in force controlled direction as soon as to the environment is made, and the nominal environment position is chosen in such a way that the robot will be assured to make with the environment from any position in free space The force tracking error is compensated by an integral control This results in an impedance function having a PID control structure for the force tracking error Environment stiness knowledge is not reuired by the algorithm Stability analysis of the controller is presented Simulation studies of a three link rotary robot manipulator are carried out to demonstrate the robustness under uncertainties in robot dynamics, environment position, and environment stiness Experimental results using a PUMA 56 robot arm are presented to conrm the performance of the control scheme 2 The Proposed Force Control Scheme 21 Control law We start from the ideal impedance function as follows: F e = M E B _E KE (1) where E = X r?x and X r is a 3 x 1 vector of reference position (Orientation control is not considered) For
simplicity, we consider that force is applied to only one direction of x that is X r = [x e x d1 x d2 ] T where x e is an environment position and x di are desired trajectories in position control directions The corresponding force is F e = [f e ] T : Subtracting the desired force F d from F e yields the generalized impedance function F e? F d = M E B _E KE (2) Let f d ; f e ; m; b; k be elements of F d ; F e ; M; B; K respectively in the force control direction x e Then, euation (2) becomes me b _e ke? f e f d = (3) where e = x e? x In free space control, the control law for the force control direction can be obtained from (3) for f e = as follows: me b _e ke =?f d (4) We note that if the desired force f d is set to zero in (4) when the environment position is known exactly, then the robot would stop at x = x e which would just make with the environment The presence of f d in (4) serves as the driving force to enable the robot to exert a force on the environment In space, careful investigation of the impedance function (3) suggests that setting the stiness gain k to zero will satisfy the ideal steady state condition of f e = f d for any k e So the proposed law is to set the stiness gain k = in the force control direction at the time of, that is (3) becomes me b _e? f e f d = (5) Substituting f e = k e (x? x e ) =?k e e in (5) yields the space impedance law : me b _e k e e =?f d (6) We see that (6) is asymptotically stable Even though k e is not known exactly in practice, the proper gains m and b based on the approximation of k e can be chosen to achieve good transient response of (6) Therefore, our proposed impedance function (5) is simple, stable and robust in force tracking under unknown environment stiness condition 22 Inaccurate environment position Consider for the practical case that we do not have the exact knowledge of the environment position x e, but we do have an estimate x e and x e = x e? x e is the uncertainty of x e Dene e = e x e, then e will replace e in both (4) and (5) yielding me b _e ke =?f d (free space) (7) me b _e = f e? f d ( space) (8) For free space motion (7), our previous analysis has shown that when x e < the robot manipulator may not always make with the environment [7] Therefore, we should always overestimate the environment position x e such that x e > to guaranteed that will always occur For space control (8), we can easily show that exact force tracking can be achieved if the environment is a at surface where x e is a constant and x e is also a constant satisfying x e? x e > In this case (8) becomes mx b _x = f d? f e (9) Hence f e = f d in steady state However when f d ; x e and x e are time varying, force tracking error will occur For example, there will be nite steady state force tracking error when f d and/or x e are ramp functions, and the steady state force error would become innite if f d and/or x e are parabolic functions To improve the performance of (8), we propose the inclusion of an integral control as follows: me b _e where is the integral gain (f d? f e )dt = f e? f d (1) 23 Stability and Convergence of the integral control law Here, we like to reformulate (1) on the basis of force tracking error From the relationship f e = k e (x? x e ), we can express x = x e fe f k e ; _x = _x e _ e k e, and x = f x e e k e Substituting x; _x; x into (1) yields the second order force error euation as m( f d? f e ) b( _ f d? _ f e ) k e (f d? f e ) (f d? f e )dt = m f d b _ f d? mk e x e? bk e _x e (11) Dene force tracking error = f d?f e and rewrite (11) as m b_ k e dt = m f d b _ f d? mk e x e? bk e _x e (12) 2
Dene the F as the force due to uncertainty of the environment position x e as f e = k e x e Then (12) becomes m b_ k e dt = m" b _" (13) where " = f d? f e Laplace transform of (13) and multiply by s yields (s) E(s) = ms 3 bs 2 ms 3 bs 2 k e s (14) The stability of the system (14) by examining the characteristic euation of (14) ms 3 bs 2 k e s = (15) For stability, m; b; should be < m; b and < < bk e m (16) so when the input E is a step or a ramp the output (t) goes to zero in steady state so that Therefore, when t! 1, f e! f d lim t!1 (t) = (17) 3 Compensation for Robot Model Uncertainty The dynamic euation of an n degrees-of-freedom manipulator is given by : D h f =? e (18) where D is an nxn inertia matrix, h is an n x 1 Coriolis,centrifugal torue and gravity, f is an n x 1 joint friction torue, is the joint torue, and e is the external applied joint torue For the purpose of the subseuent presentation, we will write the robot dynamic euation (2) as D(t) w(t) = (t) (19) where D = I and w = h(t) f (t) e (t) (D? D)(t) For stability, it is reuired that the scalar constant be bounded by < < 2 where is the minimum eigenvalue of D [2] To implement the position and force tracking controllers presented above, the control law is (t) = DJ?1(V? _ J _) w (2) where V = [v f v p1 v p2 ] T and x v f = e 1 m (f d b _e R ke ) free x e 1 m ( b _e dt) (21) v pi = x di 1 m (b_ (x di? x pi ) k(x di? x pi ) (22) In (2), w is function of the robot model which is not only complex to compute but is also subjected to various model uncertainties In this paper, we apply a previously well established techniue to estimate w using the time delayed measurements of and [2, 8] We rst express w as Then we estimate w(t) by w(t) = (t)? D(t) (23) ^w(t) = (t? )? D(t? ) (24) where is the sampling period of the controller We see that ^w(t) is independent of the robot model This yields the following complete robust force/position robot controller (t) = J?1 [V? _ J _] (t? )? ^D(t? ) (25) The stability of controller (25) has been analyzed extensively It has been shown that the necessary condition for stability is that should be chosen within the bounds < < 2 Experimental results have veried the validity of the design techniue [6, 8] 4 Simulations The nominal system parameters are used as the basis in forming the robot model ^D() and ^h(; _) Model uncertainties included a 1 Kg mechanical tool attached to the third link, Coulomb friction and viscous friction forces f ( _) added to each joint where f ( _) = 5:sgn( _) 8: _ In order to show the robustness to unknown environment stiness of the proposed scheme, we tested the system performance for abruptly changing environment stinesses with the stiness prole as k e = 4(N=m) t < 3 8(N=m) 3 t < 6 (26) which is considered as a sti environment Sample task is tested for the robot to follow the at wall with a large triangle shaped crack in the middle as shown in Figure 4 The initial controller gains are selected as M = I; B = diag[44; 4; 44] and K = diag[3; 1; 3] At, K at the force 3
controllable direction is set to zero The environment position is not correctly estimated but specied inside the environment to guarantee the Since the deepest point of the crack is :2m from the surface the user specied x e is selected at :3m such that the estimated environment position x e = x e :3m Since we assume that the environment prole is not available, x e is constant and _x e ; x e are assumed to be zero In this case, the control behavior is controlled by (9) and the plots are shown in Figure 2 Even though the force tracking is excellent when the environment is at, the force tracking is not eective with notable errors when the robot follows the crack position The deviation in force tracking in Figure 2 results from not specifying _x e and x e in the control law In most practical case, the environment is not assumed to be known so that we adopt the integral control idea (1) to compensate for _x e which is a major contribution term to the force deviation The force tracking plot using the control law is shown in Figure 3 The update rate is selected as = :3 The corresponding position tracking plot is shown in Figure 4 is excellent Another simulation is carried out for the sinusoidally shaped environment The force and position tracking are shown in Figure 5 and 6, respectively In this case, the update rate is set to :2 and the controller gains B = diag[6; 2; 2]; K = diag[5; 1; 1] are used in free space and K = diag[; 1; 1] is used in space Again, the tracking performance is excellent 5 Experimental Results To demonstrate the performance of the proposed algorithm, experiments were conducted using the UCD Robotics Research Laboratory (RRL) experimental testbed consisting of a PUMA 56 industrial manipulator tted with a JR3 wrist force sensor Single arm experiments were rst conducted on the PUMA manipulator operating in both free space and in with a rigid surface using conventional impedance control which resulted in the impedance chosen as follows: M = diag f5 5 5 1 1 1g, B = diag f3142 3142 1571 31:4 31:4 31:4g, and K = diag f31583 31583 11844 237 237 237g The inertia matrix was chosen to satisfy a lower bound reuirement which results from digital implementation during constrained operation The preceding impedance yields good trajectory tracking and force step response performance Damping in the z direction was reduced to achieve faster response in the force tracking direction Two experiments were conducted each consisting of moving in the negative z direction onto a surface, and then moving 25cm along the surface in the x direction in 15 seconds with f d = [?2N ] The rst surface is a at hard surface and the second surface is sti curved sheet of galvanized steel The exact location and stiness of each surface is unknown For the at hard surface, was chosen as 1 and for the curved sti surface was increased to 2 since the surface is more compliant The results of the experiments are shown in Figures 8 and 9 Initially, the end eector is commanded to move to a point beyond the surface Upon with the surface, the initial overshoot is approximately 6% and the force settles to -2N in approximately two seconds During the motion the force is controlled to within 2% The experiments demonstrate that the algorithm exhibits good force tracking in the presence of environmental uncertainty 6 Conclusions A simple force control scheme of robot manipulator is presented in this paper The new impedance controller realized by setting the stiness gain to zero performs very well under the condition of inaccurate estimation of the environment position and stiness In order to deal with unknown environment position we employed an integral control that minimizes a force tracking error The conventional impedance euation are newly formed as a second order force error euation It allows us to analyze the stability of the proposed control that show the bounds for the controller parameters Simulation / experimental results prove that the proposed controller is excellent to achieve the best force/position tracking under the situation that the environment is not known References [1] N Hogan, \Impedance control : An approach to manipulator, part i, ii, iii", ASME Journal of Dynamic Systems, Measurement, and Control, vol 3, pp 1{24, 1985 [2] TC Hsia, \Simple robust schemes for cartesian space control of robot manipulators", International Journal of Robotics and Automation, pp 167{174, 1994 [3] T Lasky and TC Hsia, \On force-tracking impedance control of robot manipulators", Proc 4
X Forward Kinematics X J D ^ s s Desired force() and Actual force( ) X (t) e X e (t) E M -1 K J Delay _ V τ J -1 U h ^ D Robot F e 25 2 Xe (t) Fd E M -1 B M -1 Adaptive law F e τ e J T Force (N) 15 Figure 1: Robust Force Control Structure 1 of the IEEE International Conference on Robotics and Automation, pp 274{28, 1991 [4] Sukhan Lee and Hahk Sung Lee, \Intelligent control of manipulators interfacing with an uncertain environment based on generalized impedance", Proc of IEEE Symposium on Intelligent Control, pp 61{66, 1991 5 1 2 3 4 5 6 Figure 2: Force Tracking for a Triangle Crack when _x e = [5] H Seraji and R Colbaugh, \Force tracking in impedance control", Proc of the IEEE International Conference on Robotics and Automation, pp 499{56, 1993 [6] S Jung, T C Hsia, and R G Bonitz, \On force tracking impedance control with unknown environment stiness", Proc of IASTED International Conference on Robotics and Manufacturing, pp 181{184, Cancun, June, 1995 [7] S Jung and T C Hsia, \Neural network techniues for robust force control of robot manipulators", Proc of IEEE Symposium on Intelligent Control, pp 111{116, Monterey, August, 1995 [8] RG Bonitz and TC Hsia, \Robust internal-force based impedance control for coordinating manipulators - theory and experiments", IEEE International Conference on Robotics and Automation, 1996 Force (N) 25 2 15 1 5 Desired Force() and Actual Force( ) 1 2 3 4 5 6 Figure 3: Force Tracking for a Triangle Crack with the integral control 5
65 Real() and estimated () environment, Actual position( ) Real () and estimated () environment, Actual position ( ) 9 6 85 x axis (m) 55 z axis (m) 8 75 5 1 2 3 4 5 6 7 Figure 6: Position Tracking for a Sinusoidal Surface 1 Position z 65 45 5 55 6 65 7 x axis (m) Figure 4: Position Tracking for a Flat Wall mm 8 6 4 2 estimated 5 1 15 2 25 3 Force z 1 Desired force () and Actual force ( ) Newtons 2 3 desired motion 4 5 1 15 2 25 3 Force (N) 2 18 16 14 12 1 8 6 4 2 1 2 3 4 5 6 Figure 5: Force Tracking for a Sinusoidal Surface Figure 7: Experiment - Flat Environment Surface mm Newtons 8 7 6 5 4 3 Position z estimated 2 5 1 15 2 25 3 1 2 3 desired Force z motion 4 5 1 15 2 25 3 Figure 8: Experiment - Curved Environment Surface 6