Abstract-This work is aimed at studying the problem of controlling the moment and the orientation of a robot manipulator whose end effector is in contact with a.
Proceedings ofthe 1998 IEEE International Conference on Robotics & Automatlon Leuven, Belgium May 1998
-
Control of Moment and Orientation for a Robot Manipulator in Contact with a Compliant Environment Ciro Natale
Bruno Siciliano
Luigi Villani
PRISMA Lab Dipartimento di Informatica e Sistemistica Universiti degli Studi di Napoli Federico I1 Via Claudio 2 1, 80 125 Napoli, Italy
Abstract-This work is aimed at studying the problem of controlling the moment and the orientation of a robot manipulator whose end effector is in contact with a compliant environment. Differently from classical operational space formulations, a geometrical approach is pursued where orientation displacements are described in terms of unit quaternions. Regulation to a constant desired moment and tracking of a time-varying desired orientation trajectory is achieved, and the convergence of the closed-loop system is analyzed. Simulation results for a six-joint industrial robot are developed and an experimental test is carried out to demonstrate the effectiveness of the proposed approach in a practical contact task.
1. Introduction The problem of controlling a robot manipulator performing six-degree-of-freedom interaction tasks has been addressed in the literature by referring to classical operational space formulations [l]. The three components of conlact force are related to the end-effector position displacemeints, while the three components of contact moment are relaited to the end-effector orientation displacement which is described in terms of a minimum set of variables, e.g. Euler angles. The inconvenience of the above approach lies in the transformations needed to express the physical contact moment in terms of analytical quantities to be consistent with the particular representation of end-effector orientation. More recently, research has focused on seeking interaction controllers that might guarantee geometrical task consistency [2]. Along this line are the spatial compliance and impedance controllers proposed in [3,4] where an energy argument is used to derive the system equations dluring interaction with a precise physical meaning, which in turn simplifies the choice of the control parameters. In [5,6]this approach has been pursued to design impedance controllers where the end-effector orientation displacements are described in terms of unit quaternions; the advantages over an operational space approach have been clearly demonstrated both in theory and in practice.
0-7803-4300-~-5/98 $10.000 1998 lEEE
1755
In this paper a control scheme for a robot manipulator interacting with the environment is presented, where the focus is on the control of the contact moment and the end-effector orientation. The control is inspired by the innedouter [7] and the parallel [SI control techniques, where the goal is to regulate the moment to a constant desired value and to track a time-varying desired orientation trajectory. The inner control loop is of inverse dynamics type, where the orientation errors are described in terms of unit quaternions [9], and the outer control loop contains an integral action on the moment error as in [ 101. A model of elastically compliant environment is adopted [ 111to study the convergence of the closed-loop system. A six-joint industrial robot is considered to develop a case study where a disk-shaped end-effector tool is in contact with a frictionless compliant surface. The system is tested first in simulation, and the results confirm the conclusions of the theoretical analysis. Then a practical interaction task is set up for the real robot available in the lab, and the results of the experiment show a satisfactory performance in spite of the unmodelled effects, such as contact friction.
2. Modelling Without loss of generality, a six-joint rigid robot manipulator is considered performing a six-degree-of-freedom task. Its joint space dynamic model can be written in the wellknown form
where q is the (6 x 1)vector ofjoint variables, B is the (6 x 6) symmetric inertiamatrix, C4 is the (6 x 1)vector of Corioiis and centrifugal torques, d is the (6 x 1)vector of friction torques, g is the (6 x 1) vector of gravitational torques, U is the (6 x 1)vector of driving torques, he = [ : f p:lr is the (6 x 1)vector of contact forces and moments exerted by the end effector on the environment, and J is the (6 x 6) geometric Jacobian relating joint velocities q to the (6 x 1) vector of linear and angular velocities w e = [ j ~ ruTIT of the end-effector frame. i.e.
where a p and a, are to be designed to control the endeffector position and orientation, respectively. Let pd denote a constant desired position, then the position control law is
where J is assumed to be nonsingular. Note that both he and v, are referred to a common base frame; in the remainder, whenever a vector or a matrix is to be referred to a frame other than the base frame, a proper superscript will be added. Further, differentiation of (2) with respect to time yields the acceleration mapping
where k p p , kvp > 0 are the two feedback gains of a PD control action; in the case of a time-varying position, a complete resolved-acceleration control with desired acceleration and velocity feedforward can be considered [ 131. In the following, orientation displacements will be described in terms of unit quaternions. The reader is referred to Appendix A for some background on quaternion algebra.
Since the focus here is on the constraints imposed by the environment on the end-effector orientation, only the contact moment is of concern. By assuming a frictionless and elastic type of contact, the model of the compliant environment takes on the form [ 1 11
where c denotes the constraint frame attached to the environment; also, the quantity w,"dt denotes an infinitesimal angular displacement of frame c with respect to a frame U corresponding to the orientation of the undeformed environment at rest (null contact moment). The rotational stiffness K Ois a symmetric and positive semidefinite constant matrix; as such, its null subspace N(K,) characterizes the unconstrained motion. By assuming that the end effector is always in contact with the environment during the interaction, the unconstrained motion corresponds to a rotation of the end-effector frame with respect to the constraint frame, described by the relative angular velocity w & E N ( K , ) . Therefore, the end-effector angular velocity referred to the constraint frame can be decomposed as
where the angular velocity of the constraint frame w," E
J W K O ) .
3. Control An effective technique to cope with the nonlinear and coupled nature of the manipulator dynamics is the inverse dynamics control. By accounting for the model (1) and the mapping ( 3 ) ,the driving torques can be chosen as [ 121 U =
B ( q ) J - l( q ) ( a e
-
j ( q 9 +)+)
With reference to (A. l), let &, denote the unit quaternion associated with the orientation of a certain reference frame rotating by an angular velocity w,. Then, according to [9], the orientation control law is
where kp,, k v o > 0 and ere is the vector part of Qre = Q;' * Q, which gives the orientation error between the reference and the end-effector frame -see (A.2)-(A.5). Note that Q , and Q, describe orientations with respect to the base frame, and E , , is referred to the base frame via (A.4). The orientation control (9) becomes an interaction control by a suitable choice of U , and Q,.These shall be selected so as to achieve a constant desired contact moment and to track a time-varying desired orientation trajectory. The environment model (4) indicates that contact moments are generated only in the range subspace of the rotational stiffness matrix R(K,). Then, the desired moment is taken and the desired angular velocity is taken as pi E %?(KO), ; quantities have been conveniently as w& E " ( K O )both referred to the constraint frame and, in view of (5), a relative angular velocity between the desired and the constraint frame has been considered. At this point, the reference quantities are chosen as
(6)
+ C ( q ,4 ) +~d(q,4) + 9 ( q )+ J T ( q ) h e , where perfect compensation of the terms in the dynamic model and exact cancellation of contact forces and moments (employing a suitable forcekorque sensor at the end effector) have been assumed. The new control input a , is taken as
where w; and Qm are generated from a control loop acting on the moment error which turns out to be outer with respect to the inner control loop acting on the orientation error specified by (9). The moment control law is described by the equation
(7)
1756
TRAlECTORY
Fig. 1. Block scheme of momenVorientation control
where kv,, k n , > 0 and the vector quantities have been conveniently referred to the constraint frame. Note that, in view of the model (4) and the choice of the desired moment, it is w& E n/"(Ko)as long as w h ( t = 0) = 0. Then, the quaternion Q , is computed from w, by integrating the propagation equation -see (A.6)- with initial condition Q,(t = 0) = &, being Q, the quaternion describing the orientation of the undeformed frame with respect to the base frame. It is worth pointing out that the computation of 62, to be used in (1 1) gives rise to an integral action on the moment error; this fact, along with the observation that no selection mechanism is adopted in the orientation control law and the moment control law to discriminate between the constrained and the unconstrained motion, leads to casting the proposed control scheme into the parallel control framework [8]. In order to analyze the convergence of the overall1 closedloop system, it is reasonable to assume that the inner orientation control loop is much faster than the outer moment control loop, and thus w," = W E and Q,= QT. By comparing the expressions of w," in (5) and w : in (lo), and observing that w & , w & E N(Ko)and wE,w& E N L ( K 0 )it, can be stated that wz, = w& and w," = w h . Consider the composition
As far as the moment is concerned, taking the time derivative of (12) with constant pi gives
k~,Wh
+ kv,L;;& + li: = 0.
(14)
Then, taking the time derivative of (4) gives
Substituting (15) in (14) and accounting for w: = wh, yields ~A,WE kv,W: K,wE == (16)
+
+
implying that K 0 w : + 0 for aI1 strictly positive kvnL and k ~ , . Since KOis positive semidefinite and w," E " - ( K O ) ,it can be concluded that both w," and w," converge to zero, and thus from (12) p: + U , : i.e. regulation of the contact moment to the desired value is achieved. The convergence of the control scheme does not require the exact knowledge of the elements of tht: rotational stiffness, but it is necessary to know the stnicture (null and range subspaces) of matrix K O . A block scheme illustrating the connections of the various control actions for the momendorientation control part is sketched in Fig. 1. Notice that, in order to compute & in (12) from the available measurement pe,the fact that Q, practically coincides with Q, is exploited; this is also useful to coimpute w, from W&.
resulting from ( S ) , where Q,describes the orientation of the constraint frame with respect to the base frame and describes the mutual orientation between the endi-effector and the constraint frame. Since Q , is computed from w , via (A.6) with initial condition Q,(t = 0) = = 0) = Q,, from w , = w, it follows that Qc = & ., Then, comparing (13) with (11) and accounting for Q e = Q, leads to QeC = Qdc,i.e. tracking of the time-varying desired orientation trajectory is achieved.
e,,
4. Simulation A case study has been simulated by considering an industrial robot Comau SMART-3 S; this is a six-revolute-joint anthropomorphic manipulator with nonnull shoulder and elbow offsets and non-spherical wrist. The tool is a disk of 5.5 cm radius at the tip, and the end-effector frame has its origin at the center of the disk and its approach axis normal to the disk surface. The disk is in contact with a
1757
planar surface such that the approach axis is aligned with the normal to the surface. The contact is characterized by a rotational stiffness matrix K O= diag{30,30,0}, i.e. the unconstrained motion is described by any rotation about the approach axis. The constraint frame is chosen so that its z-axis is aligned with the normal to the surface. The interaction task is as follows. At t = 2 s, the moment is taken to [ 0 1.5 0IT Nm in the constraint frame, according to a 5th-order polynomial with null initial and final first and second time derivatives, and a duration of 0.5 s; the final value is kept constant for the remaining portion of the task. The desired end-effector position is kept constant. After a lapse of 6 s, the desired end-effector orientation is required to make a rotation of 0.5 rad about the approach axis; the trajectory is generated according to a 5th-order interpolating polynomial with null initial and final velocities and accelerations, and a duration of 4 s.
k
1
5
10
15
[SI
contact moment
5
2
O
'
I X
-0.5 I 0
In order to test the applicability of the proposed control scheme in a practical interaction task, the above case study has been experimented on the real industrial robot Comau SMART-3 S available in the PRISMA Lab. The joints are actuated by brushless motors via gear trains; shaft absolute resolvers provide motor position measurements. The robot is controlled by an open version of the C3G 9000 control unit which has a VME-based architecture with a bus-tobus communication link to a PC Pentium 133. This is in charge of computing the control algorithm and passing the references to the current servos through the communication link at 1ms sampling rate. Joint velocities are reconstructed through numerical differentiation of joint position readings.
/----I
0
2s
The simulation results are presented in Fig. 2 in terms of the time history of the rotation angle about the approach axis of the end-effector frame and of the three components of contact moment; desired quantities are dashed while actual quantities are solid. It can be recognized that virtually perfect tracking of the desired end-effector orientation trajectory is achieved, while the contact moment is successfully regulated to the desired value; notice, however, that a small deviation on the z- and y-components of the actual moment occurs during the transient of the orientation trajectory.
5. Experiment
rotation about approach axis 0.5
A discrete-time implementation of the control algorithm based on (6)-(12) at 1 ms sampling interval has been assumed, where the various feedback gains have been set to: k p p = 2500, k v p = 65, k p o = 4500, k v o = 65, IGvm = 24, k~~ = 1. The simulation has been performed in MATLAB/SIMULINK.
I 5
10
15
[SI
Fig. 2. Simulation results
The dynamic model of the robot manipulator has been identified in terms of a minimum number of parameters, where the dynamics of the outer three joints has been simply chosen as purely inertial and decoupled. Only joint viscous friction has been included, since other types of friction (e.g. Coulomb and dry friction) are difficult to model.
A 6-axis forceltorque sensor AT1 FT 130/10 with force range of *130 N and torque range of f 1 0 Nm is mounted at the wrist of the robot manipulator. The sensor is connected to the PC by aparallel interface board which provides readings of six components of generalized force at 1 ms. An end effector has been built as a steel stick with a wooden disk of 5.5 cm radius at the tip. The environment is constituted by a wooden surface and the resulting model is approximately the same as the compliant model of contact used in simulation. The experimental results in Fig. 3 reveal that the overall performance of the control scheme is satisfactory with respect to the idealized case in simulation. Among all the unmodelled effects and disturbances occurring for the real system, the contact friction is the most evident affecting the moment components and causing a steady-state deviation on the rotation angle from the desired value; this cannot be recovered since the integral action operates on all the components of the moment error, causing an orientation error also about the unconstrained motion axis.
1758
.
formulation,” IEEE J. of Robotics and Automation, vol. 3, pp. 43-53, 1987.
rotation about approach axis
0.4
I
-
__
H. Bruyninckx and J. De Schutfer, “Specifica-
_-
tion of force-controlled actions in the “task frame formalism”-A synthesis,” IEEE Trans. on Robotics andAutomation, vol. 12, pp. 581-589, 1996.
3 0.3
2
0.1 -
E.D. Fasse, “On the spatial compliance of robotic manipulators,”ASME J. of Dynamic Systems, Measurement, and Control, in press, 1997.
0
E.D. Fasse and J.F. Broenink, “A spatial impedance controller for robotic manipulation,” IEEE Trans. on Robotics and Automation, vol. 13, pp. 546-556, 1997.
Y
0.2
5
10
15
[SI
B. Siciliano and L. Villani, “Six-degree-of-freedom impedance robot control,” Pmc. 8th Int. Con$ on Advanced Robotics, Monterey, CA, 1997, pp. 387-392.
contact moment
E Caccavale, C. Natale, B. Siciliano, and L. Villani “Experiments of spatial impedance control,” Prepl: 5th Int. Symp. on Experimentcl Robotics, Barcelona, Spain, 1997, pp. 61-72.
-0.5
‘ 0
J. De Schutter and H. Van Brussel, “Compliant robot motion 11. A control approach based on external control loops,” Int. J. of Robotics Research, vol. 7, no. 4, pp. 18-33,1988. I
I
.
5
10
S. Chiaverini and L. Sciavicco, “The parallel approach to forcelposition control of robotic manipulators,” IEEE Trans. on Robotics anddutomation,vol. 9, pp. 361-373,1993. J.S.-C. Yuan, “Closed-loop manipulator contr,ol using quaternion feedback,” IEEE J. of Robotics and Automation, vol. 4, pp. 434-440, 1988.
15
[SI
Fig. 3. Experimental results
6. Conclusion A scheme for controlling the end-effector orientation and moment of a robot manipulator in contact with a compliant environment has been presented in this work. The key feature is the use of unit quaternions to represent orientation displacements which can be defined in a consistent way with the interaction task geometry. A theoretical ;analysis has been developed to prove that the contact moment is regulated to a constant desired value while the end-effector orientation tracks a time-varying desired trajectory. The results have been validated both in a simulation and in an experiment for a case study developed on a six-joint industrial robot available in the lab.
J.T. Gravdahl, 0. Egeland, B. Siciliano, and S. Chiaverini, “Stability analysis of 6-dof forcelposition control for robot manipulators,”Proc. 33rd IEEE Con$ on Decision and Control, Lake Buena Vista, FL, 1994, pp. 2408-2409.
Acknowledgments
J.Y.S. Luh, M.W. Walker, andR.P.C. Paul, “Resolvedacceleration control of mechanical manipulators,” IEEE Trans. on Automatic Control, VQI.25, pp. 468474,1980.
J. LonEariC, “Normal forms of stiffness and compliance matrices,” IEEE J. of Robotics arid Automation, vol. 3, pp. 567-572, 1987. L. Sciavicco and B. Siciliano, Modeling and Control of Robot Manipulators, McGraw-Hill, New York, NY, 1996.
This work was supported partly by Minister0 dell’ Universit&e dellu Ricerca Scientijica e Tecnologicuand partly by Consiglio Nuzionale delle Ricerche.
References [ 11 0. Khatib, “A unified approach for motion arid force
control of robot manipulators: The operational space
A. Appendix The most common tool to describe the orientation of a rigid body with respect to a base frame is given by the rotation
1759
matrix R of a frame attached to the body. An alternative four-parameter singularity-free representation of orientation is given by the unit quaternion Q = {q,E } :
0 q = cos2
E
0 = sin-r, 2
where S( .) is the matrix operator performing the cross product between two (3 x 1)vectors. It is worth considering the composition rule of two successive changes of orientation with respect to axes of current frame:
(A. 1)
where 0 and r are,respectively the rotation and the (3 x 1) unit vector of an equivalent anglehxis description of orientation. Thus, in general, the relative orientation of a frame 2 with respect to a frame 1can be expressed equivalently as
JG
+-)
Q21
1
= {721,E21};
(A.2)
notice that the vector part of the quaternion is the same = E&. The following when referred to frame 2, i.e. properties hold:
Rt = (JG)-' =
(JG)T *
The differential relationships expressing the relative motion of frame 2 with respect to frame 1 are characterized by the angular velocity vector w 2 1 :
= Qy: = { 7 ) 2 1 , - ~ : 1 } . 64.3) Consider a free vector w. Its expressions with respect to frames 1 and 2 are rclated as: w 1
Q12
=
1 w1 = w2
+ 27)21s(€;,)w2 + 2S(€;,)S(E;1)w2
(A.4)
1760
where I is the (3 x 3) identity matrix; the latter differential relationship is also known as quaternion propagation.
