Informática 18. (1994) 81-91 81 FORCE CONTROL OF AN INDUSTRIAL ROBOT WITH ADAPTIVE COMPENSATION OF THE ENVIRONMENT STIFFNESS Bojan Nemec and Leon Zlajpah Jožef Stefan Institute, University of Ljubljana Ljubljana, Jamova 39, Slovenia bojan.nemec@ijs.si Keywords: Force control, robot control, adaptive control Edited by: Jadran Lenarčič Received: September 15, 1993 Revised: March 20, 1994 Accepted: March 30, 1994 The paper describes the implementation of the adaptive force control of an industrial robot. The implemented algorithm is a position based hybrid control scheme with adaptation to the environment stiffness. Control scheme is sensitive to the changes in environment stiffness. We solved this problem by the adaptive controller. Implementation problems on the robot controller arc also discussed. The proposed control method is easy to implement and can be applied to existing industrial robots lit ted with a conventional position controJJer. The performance of the force controlled manipu/ator with the proposed control law was tested with the computer simulation and by using the real robot. 1 Introduction Many of robot industrial applications, such as automated assembly, deburring, teleoperation, etc., require exact control of interaction forces with the environment. The problem of controlling interaction forces has been investigated by many authors. According to Kazerooni [7], active force control strategy can be classified into two major approaches. The first approach force or torque is commanded along those directions constrained by the environment, while position or orientation is commanded in the direction unconstrained by the environment. The above approach was formalized by Mason [9].. Craig and Raibert [12] introduced a hybrid force/position controller by controlling the actuator torque. Whitney [14] proposed damping control where sensed force error is transformed into the commanded velocity of the actuator. A similar approach was used by Paul and Shimano [11]. Some advantages can be obtained if the decoupling of the manipulator is done in the task space, like in the operational space approach introduced by Kathib [6], The second approach is based on establishing a rela- tionship between the position of the manipulator and interaction forces. Error in position, velocity and force generates joint torque commands. Salisbury [13] introduced the stiffness control approach which acts like a six-dimensional active spring in the tool coordinates. Impedance control which combines stiffness and damping control was introduced by llogan [5]. Our approach is modified hybrid/position force controller where force error is converted to the position offset. This method is easy to implement and requires no modification at the servo level of the robot controller. The stability and response of the proposed force controller depend on the sensor and environment stiffness. For applications on unknown or changing environment stiffness we propose a simple adaptive controller which adapts the gain of the force control loop to the environment and sensor stiffness. 2 Force control The problem of compliant control can be broken down into pure position and pure force control. In a direction where the robot task is unconstrained 62 Informática 18. (1994) 81-91 2 by the environment, pure position control can be used, while in the direction constrained by the environment pure force control is used. The constrains imposed by the environment are callcd natural constrains. In order to specify the desired task, artificial constraints are introduced. Natural and artificial constraints together form N-dimensional constrained spacc C, where N is the number of the Cartesian degrees of freedom. The task of the controller is to map the C space into the manipulator joint movement. The hybrid force control method controls motor torque directly. Here, another approach was used due to the hardware limitation of the controller of our robot. The force control is implemented in the outer loop of the existing position/velocity control J and generates new N dimensional input vector yd in tool coordinate system yd = Sx/ + (I - S)xp (1) xf - K^F, - F) (2) where K/ is the force controller transfer function and F and F^ are the measured force and the desired force, respectively. The compliance selection matrix is defined as a, binary NxN-tuple which specifies which degrees of freedom in C are under force control and which are under position control. The first term in the Eq.l correspdnds to the force control loop where the last term is the position (orientation) command vcctor. The position (orientation) command vector is transformed from the tool coordinate system to the robot base coordinate system and then to the joint coordinates. Joint coordinates q^ are passed to the position/velocity controller. This transformation can be described by the equation qd = *-l(A(yd)) (3) where 'i'"1 describes the transformation from the Cartesian space to the joint angles and A denotes the transformation from the tool coordinates to the robot base coordinates. A simple PI controller with the discrete transfer function Kj 1 was used for the force controller transfer function. In order to improve the stability, first order anti alias filter was used in the force feedback loop. 2.1 Design of the force controller : Single—joint case We will first design the closed loop system for the single joint case. Stability analyses will be done in the S domain by a root locus design. A model of the one-joint robot system with DC (AC) motors is presented on Fig. 1. The parameters of the transfer functions were estimated by using the test signals and LS estimation procedure and compared with the known parameters of the system to validate results. For the third joint of our robot the transfer function parameters arc as follows: where xp is the desired displacement vcctor of the robot (translations and orientations), I is the identity matrix and S is the compliance selection matrix [12], and X/ is Ki - 0,06 rad/N gain of the force control i = 0.997 damping of the force control Kr = 1-100 1/s gain of the position control K. = 15900 gain of the velocity comp. gain of the 1' velocity control = 1000 Vs/rad K = 5000 V/rad gain of the I velocity control K, = 0.031831 V/rad* tachometer gain K, = 0.23 N'm/A torque constant = 0.101 Vs/rad back EMF constant II = : 0.91 il motor résistance «elf = 0.0003 Nms/rail = 0 000-16 Nms'-'/rad effective clamping It<„ effective inertia = 350 N/rad sensor/environment stiffness n — 1/70 gear ratio a — 50 anti alias filter pole Tt •■ = 0.01 s sampling time The position controller consists of simple Kp gain with feed-forward velocity compensation realized by a digital computer. Since the sampling time of the position control loop is much smaller than the sampling time of the force feedback loop, it is assumed that the position controller is realized with an analog feedback. The force control loop is realized by a digital computer, therefore we will assume zero-order sample/hold clement at the input of the position controller. We assume simple model environment, described by the Fq. 4, where qc and q is the environment contact position and measured position in joint coordinates respectevely. F = Kse(q - qc) (4) From Fig. 1 we can computc the open loop 2 3 'this method is referred to as position based force control 2open loop with respect to the force loop 3we will omit the subsystem index > in the equations for the single joint case Informática 18. (1994) 81-91 83 FORCE POSITION CONTROL CONTROL VELOCITY CONTROL ROBOT ARM n I q EiFrEh . j. 1 ii 1 s-t-a ANTl-ALIAS FILTER FORCE SENSOR -ENVIRONMENT Figure 1: Model of one robot joint with velocity and position control transfer function for the single joint case in the form <1 = Gi(s)qd + Giis)/ (5) where Gi(s) G2(s) nKj(Kp + Kus)(Kvt + A» n2Rs W(s) (6) (7) and W(s) = Hej/S*R+ (8) (BeJfR + KiKvKt + KiKb)s2 + Ki(KviKt + KpKvn)s + KiKpKvin Discrete PI force control law for the single joint case is ?<*(*) = " 1) + Kf{e{k) - te(k - 1)) (9) where e = {fd{k) - /(&)), f and /<* are the measured and the desired joint forces respectively. Factor £ was chosen to meet the desired dynamic performance of the closed loop system. The overall discrete transfer function of the reduced 4 open loop system with one sample delay in the control loop and first order anti alias filter for the sampling time Ts = 0.01 s is thus 0.02211-1 -o.oiaoi-1 - O.O021}*-3 -0.00007I-4 G(z) = --- I - 1.44M*-1 + CMT3Bi-2 -a.OOOSi-3 -0.00004I-* The above model was used to determine suitable gain for the force control loop via discrete root locus analyses. The root locus for the 3rd joint discrete model of our robot is presented in Fig.2. The gain A'/ where system becomes unstable is 0.132 rad/N and suitable gain at dominant damping factor ( = 0.5 is 0.06 rad/N. 2.2 Design of the force controller : Multi-joint case Robot dynamics is described by using the La-grangean formulation, with the Eq. 5 r = H(q)q + d(q, q) + JTF (10) 4The non-reduced system is of 5th order. The system was reduced by canceling non-dominate poles and non-dominate zeroes where r is the N-dimensional actuator force (torque) vector, H(q) is the NxN dimensional manipulator and actuator inertia matrix, d(q, q) is the N-dimensional vector of Coriolis, centrifugal, gravity and friction forces, J is the manipulator Jacobian and F is the compliant force in Cartesian coordinates. 'for the sake of simplicity we will omit time dependence in the equations that follow 84 Informalica 18 (1994) 8I-!)I -()."> 0 0.5 Figure "2: Discrete root locus of the system In a genera] case Eq.10 describes a highly nonlinear and strongly coupled mu 111 variable system. A stability analysis with the proposed control law is very difficult, if not impossible for such' a system. The following is assumed for the stability analyses - The gravity is compensated either by mechanical construction of the robot or by online calculation and compensation with a controller. - the manipulator is operating at low speed, centrifugal and Coriolis forces are therefore negligible. - the deflections in the position around I lie desired force are low, because force sensors have high stiffness. This assumption will allow linearisation of the system around the set point. Furthermore the majority of existing industrial robots have a high gear ratio between the drive motors and the joint. With the above assumptions the matrix of inertia H(q) can be approximated by a diagonal matrix with constant'terms Hj and d(q.q) can be approximated with constant damping Bq. l or a noncornpliant motion Eq.ll describes a decoupled system, which is generally not t rue in case of compliant motion. First we will analyse the open loop transfer function (Eq.5) for multi joint case. Matrices Gj and Gj are diagonal matrices consisting of the subsystem transfer functions described by Kq.fj and Kq.8 respectively. q — Giqj + G2JtF = Giqi/ + G2JTKsc(x - xt) (12) where x is the position and xc is the contact position in the Cartesian coordinates. VVe will define position as position for the desired force plus the deflection from that set point x = XJ +Ax, Then Eq.12 can be rewritten in the form q = G,.qfi +G2JrK,c((xt/-x for the each subsystem is calculated using RLS Nvc will omit, the subsystem index i in the equations tli at follow 86 Informática 18. (1994) 81-91 86 1-S A * ~—# position velocity control robot arm A f Kf Wz1 1- z" r'r state Kf filler estimation H.a. filter state filter force sensor environment Figure 4: Adaptive control scheme estimation procedure in the form Kj(k) = Kj(k~ 1) + P{k-2)j>{k-\) l + ^(Jfc-l)P(Jfc-2)V'(Jfe-l) (19) em{k- 1) (20) (21) P(k - 1) = (P(k - 2) - (22) 1 + ip(k - l)P(k - 2)ipk - 1 A is the appropriate chosen forgetting factor. Normally, the forgetting factor is set between A = 0.95 and A = 0.99. However, a large forgetting factor slows down the convergence of the estimation, but the algorithm is more resistant to a sudden change in the estimated parameters due to noise . In our case we get the best results with a forgetting factor of about A = 0.7. In this case the behaviour of the RLS estimation will approach the projection estimation algorithm. With a low forgetting factor. A, there is a danger of covari-ance blowup during the period when the system is insufficiently excited [3]. In our case we solved the problem by identifying only during the period when filtered derivative of the force was above the noise limit. If the contact position remains unchanged during the adaptation, the adaptive system is lin- ear in the unknown parameter and the stability of such a system can be easily verified under the assumption of a persistently excited system [4]. However, if the contact position changes during the adaptation, it is impossible to estimate both contact position and stiffness of environment. Namely, changes in contact position have the same effect as changes in environment stiffness. To solve this problem we propose to estimate Kj only a few samples after the sensor reaches the obstacle, i.e. to identify only few samples after the filtered force derivative changes from 0 to the £, where i is a suitably chosen constant according to the sensor reading resolution and noise in the measurement. The adaptive control scheme is presented in Fig.4. Note that at low speed calculation of the robot position in the task space can be replaced by the desired position in the task space. The closed loop behaviour was simulated by using continuous time simulation of the DC motor, gears, velocity controller and sensor, discrete simulation for the position control and trajectory interpolation at a sampling interval of 0.0016 s and discrete simulation of the force control loop at a sampling interval of 0.01 s. The simulation results for the step response of the proposed control scheme are presented in Fig.5 for the non-adaptive and adaptive controller with filtered signals respectively. Both, the adaptive and non-adaptive controller were tuned for the environment stiffness 1 N/mra, while ac- Informática 18. (1994) 81-91 87 tual environment stiffness was 4 N/mm. From the simulation results we tan see that the non-adaptive controller starts to bounce when environment stiffness increases and goes to a limit cycle. In contrary, the adaptive controller quickly adjusts to the new environment stiffness. The simulation results were compared to the measurement obtained on the reai robot. The step response of the adaptive and non adaptive controller for the stiff environment are presented in Fig.6. The adaptive controller estimates correct gain and is stable, but some oscillation can be noticed during the impact, which are not obtained in the simulation. This is mainly due to the nonlinear friction and backlash in the gears, which were not included in the simulation. 3 Implementation on the robot controller The proposed compliance control scheme was implemented on a 6. d.o.f. industrial robot RIKO 106. The architecture of the control system is presented in Fjg.7. The main CPU of the robot controller is dedicated to trajectory generation, kinematic transformation and man-machine interface, The axis computer is used for the digital position controller with feed-forward speed compensation and for interfacing with the controller periphery. Because of hardware limitations, force feedback was realized via the main CPU. The sampling interval of the force controller, as well as the sampling interval for the trajectory generation module was set to 0.01 s. The desired trajectory is passed to the axis CPU by a shared VME RAM. The axis computer generates trajectory with sampling time 0.0016 s by polynomial interpolation. Due to the interpolation algorithm and data exchange between the main and the axis CPU a delay of 0.02 s appears in the force feedback loop. RRL robot programming language is implemented on the robot controller [10]. Three additional commands were added to RRL for the compliant motion definition. Natural constrains are defined with command ForceSELect, FSEL si s2 , . s6. Nonzero parameters si .. s6 corresponds to the pure force control in the direction x y z roll pitch yaw, while the zero parameter specifies the pure position control in the tool coordinates. The value of the parameters si ..sG selects the A/D channel where the corresponding force signal appears. The negative parameter reverses the signal input sign. Artificial constraints are defined with command ForceTRACK, FTRAGK ci c6, where cl .. c6 is the desired velocity (angular velocity) or force (torque) vector according to the artificial constraints definition. The offset of the sensor and A/D converter, as well as the effect of gravity on the sensor and tool is removed using command CALIBRATE FS. Of course, during the calibration, the force sensor should not be in contact with the environment. Calibration activates also adaptation procedure. 4 Example Force control was tested oii the deburring process of an irregulary shaped workpiece. The task of the robot was to apply constant force 70 N in the orthogonal direction of the free movement of the robot and to maintain zero torque at the tool during movement at constant speed 10 mm/s along the X axis of the workpiece. We used a three-dimensional wrist mounted force sensor, developed at our institute. The RRL program for the required task is listed in Fig.8. The response of the robot is presented in Fig.9 for orthogonal force and wrist torques respectively. In the Fig.9 plot between (t=5scc) and (t=9sec) shows the tracking of the sensor when change in the shape of workpiece occur. We can see that the signals arc rather chattering. It was found that this is caused mainly by poor resolution, cross-coupling and noise of the sensor. A higher sampling frequency improves the transient response, but does not eliminate chattering from the response. 5 Conclusion A force control algorithm based on a hybrid control scheme was presented. The main difference between the original method and our approach is that force is controlled by changing the desired position. This approach allows implementation on existing robot controllers with a position and velocity control loop. The limitations of our approach are the following: - position resolution of the robot controller affects the force resolution of the system. In 88 Iiiformalica 18 {1994) 81-91 100 50 0 -50 non-adaptive adaptive 0.5 force j x 10 N ] : ■ ■------:■: ' —H-r ;'; I!: •rp——H i ! i 1 '_> Ll Li ij i J 1.5 2.5 lime js| Figure 5: Step response of the simulated adaptive and noti-adaptive system 100 50 -50 0.1 0.;! 0.2 0.1 0 0 lion-adaptive adaptive* force jxlO \] 0.5 As illiliiliiii I ; [ ; • «'!' i f ■ 1 ! ' : i i ■ <' i'!') 0.5 I 1.5 2 time fa] estimated force control loop gain 1.5 time Is J P. 5 Figure 6: Step response of the adaptive and non-adaptive system and estimated gain of the adaptive controller In fori»»;» lie;» 18 (MM) 81-91 PS - Cc-mpu:c" Li pariii-l MiIII CPU I.I 6z0?0 I.I 'f SSSI I Mb R--U 71 [¡uuuoa DOQOCQ sco□□a UDC3DO 9DBQOO O QLCJ OO'^'G OODQ unauzc 15000 AXIS Oi-iJ «-««log M tsoiu [/0 • i 1 • "T c!i':itci ¡no eiT'ienl/:! « ---w ■_-n CO ^ I/O r:. ..: -. 1,1.'-« -W.-.' ■ : -:■! : : ■ i - i 1 * prjnri >?l cJcdG j □ DDR □ a □□ i----- T I Figure 7: Architecture of the lUKO 106 robot controller 1 * RRL sample program for the deburring process 2 * 3 * define maximal', actual speed and toll center point 4 MAXSP = 1000 40 5 SPEED =50.0 6 TCP 1 = 0 350 0 0 0 0 7 * approach start point of the deburring and calibrate sensor 8 APPRO TG 1 FOR 0 -10 0 9 CALIBRATE FS 10 * natural constraints 11 FSEL 0 10 0 2 3 12 * artificial constraints , start deburring, stop on external signal 13 FTRACK 10.0 70.0 0 0 0 0 UNTIL SIG 0 14 DEPART FOR 0 -10 0 15 HOME Figure 8: RRL program for the deburring process time [s] time [s] Figure 9: Maintained force of the tool (in the direction normal to the surface) and torques during deburring (in the direction of the movement and orthogonal to the direction of tljc movement). Note that the shape of the object changes (Fig. 8) other words, the proposed control scheme will not work with sensors with high stiffness and robots with poor position resolution. - the sampling interval of the force control loop is the same as the sampling frequency of the trajectory generation module. Therefore, the proposed method is suitable for compliant tasks at low speed. In the proposed control law the environment stiffness is directly multiplied by the force control gain. Additionally, time delays introduced by the interpolation algorithms and communications between main and axis processor affect the stability of the control algorithm. To avoid this problem we proposed simple direct model reference adaptive controller. A discrete root locus was used for the force controller design. The results were verified with a simulation and compared with the response of the actual robot. The paper shows that the root locus design is also suitable for a multi-joint case in the case of the high gear robot and low speed robot movements. References [1] S.D. Eppinger and W.P Seering. On dynamic models of robot force control. In Proceed-trigs of IEEE International Conference on Robotics and Automation, pages 29-34,1986. [2] V. Hay ward L. Danesh emend and M. Pelletier. Adaptation to environment stiffness in the control of manipulators. Lecture Notes in Control and Information Science, Experimental Robotics /., Vol. 1, No. 1, pages 150164, 1989. [3] L.S. Kersenbaum T.R Fortescue and B.E. Ydstie. Implementation of self-tuning regulators with variable forgetting factors. Automatica, Vol. 17, No. 6, 1981. [4] C.G. Goodwin and K.S. Sin. Adaptive Prediction, Filtering and Control. Prentice-Hall, 1984. [5] N. Hogan. Impedance control of industrial robots. Journal of Robotics and Computer-Integrated Manufacturing, Vol. 1, No. 1, pages 97-113, 1984. Informática 18. (1994) 81-91 91 [6] O. Kathib. A unified approach for the motion and force control of robot manipulators: The operational space formulation. IEEE Journal of Robotics and Automation, Vol. 2, No. 2, pages 83-106, 1987. [7] H. Kazerooni, T.B. Sheridan, and P.K. Houpt. Robust compliant motion for manipulators. IEEE Journal of Robotics and Automation, Vol. 2, No. 2, pages 83-106, 1986. [8] Y. Landau. Adaptive Control. Marcel Dekket, 1979. [9] M.T. Mason. Compliance and force control for computer controlled manipulators. IEEE irons, on System Man and Cybernetics, Vol. 11, No. 6, 1981. [10] B. Nemec, A. Ruzic, V. lie. RRL — an integrated envirnonment for robot programming. Informática, Vol. 16, No. 1, pages 27-33,1992 [11] R.P. Paul and B. Shimano. Compliance and control. In Proc. of the Joint Automatic Control Conference, pages 694-699, July 1976. [12] M.II. Raibert and J.J. Craig. Hybrid position/force control of manipulators. Journal of Energy Resources Technology, Vol. 102, pages 126-133, 1981. [13] J.K. Salisbury. Active stiffness control of a manipulator in cartesian coordinates. In Proceedings of 19th Conference on Decision and Control, pages 95-100, 1980. [14] D.E. Whitney. Force feedback control of manipulator fine motions. Trans, on ASME and Journals of Dynamics Systems Measurement and Control, pages 91-97, 1977.