Hybrid Position/Force Control of Manipulators1
http://fab.cba.mit.edu/classes/865.15/classes/measurement/hybrid-position-force.pdf
A new conceptually simple approach to controlling compliant motions of a robot manipulator is presented. The "hybrid" technique described combines force and torque information with positional data to satisfy simultaneous position and force trajectory constraints specified in a convenient task related coordinate system. Analysis, simulation, and experiments are used to evaluate the controller's ability to execute trajectories using feedback from a force sensing wrist and from position sensors found in the manipulator joints. The results show that the method achieves stable, accurate control of force and position trajectories for a variety of test conditions.
Introduction Precise control of manipulators in the face of uncertainties and variations in their environments is a prerequisite to feasible application of robot manipulators to complex handling and assembly problems in industry and space. An important step toward achieving such control can be taken by providing manipulator hands with sensors that provide information about the progress of interactions with the environment. Though recent advances in robotics technology have led to the application of computer controlled manipulators to industrial handling and simple assembly operations, advances in the use of hand sensors have been very slow to appear. Therefore manual dexterity remains quite low and continues to limit application opportunities and growth. The slow progress is due partly to a lack of rugged, reliable sensors of sufficient precision and versatility. But perhaps more important is the lack of adequate controller architectures and computing techniques needed to take advantage of such sensory information, where it available. Such techniques are just now being developed. The use of manipulators for assembly tasks requires that the precision with which parts are positioned with respect to one another be quite high—higher in fact than that available from joint-mounted position sensors attached to imperfectly rigid manipulators driven by imperfectly meshed gears. Manipulators of greater precision can be achieved only at the expense of size, weight, and cost. The ability to measure and control contact forces generated at the hand, however, offers a low cost alternative for extending effective precision. Since relative measurements are used, absolute errors in the position of the manipulator and manipulated objects no longer inThis paper presents the results of one phase of research carried out at the Jet Propulsion Laboratory, California Institute of Technology, under Contract No. NAS-7-100, sponsored by the National Aeronautics and Space Administration. Author is now a graduate student at Stanford University. Contributed by the Dynamic Systems and Control Divison for publication in the JOURNAL OF DYNAMIC SYSTEMS, MEASUREMENT, AND CONTROL. Manuscript received at ASME Headquarters, February 25, 1981. fluence control. Since small variations in relative position generate large contact forces when parts of moderate stiffness interact, knowledge and control of these forces can lead to a tremendous increase in efective positional accuracy. A number of methods for obtaining force information exist: motor currents may be measured or programmed, [6, 11], motor output torques may be measured [7], and wrist or hand mounted sensors may be used [9, 12]. The first two of these techniques are limited by the accuracy and availability of manipulator models that compensate for the complicated inertial, frictional, and gravitational effects that influence force measurements. The dynamic range of force information obtained at the joints may also be restricted when large gravitational terms exist, even for the static case. Though wrist-mounted force sensors pose challenging problems in mechanical design, eletronics, communications, and reliability, they are more sensitive and easier to use than joint sensors. Since the mass between a manipulator's wrist and its fingers is small and there are no articulations, a forcesensor mounted there requires only minor corrections for dynamic effects and has a larger potential dynamic range. For these reasons this paper concentrates on use of a hand sensor. In any case, the basic control formulation given here could easily be adapted for use with the other force measurement schemes mentioned. The goal here is to present a conceptually simple method for controlling both the position of a manipulator and the contact forces generated at the hand that does not suffer from the approximate nature of previous schemes [6]. We also present the results of experiments that explore use of the hybrid technique in conjunction with data provided by a wrist-mounted force sensor. Note that the method we propose here does not prescribe particular feedback control laws for the regulation of errors. Rather it suggests a control architecture within which such laws can be applied.
Background
The approach taken here is based on a theory of compliant force and position3 manipulator control that has its roots in the work of Whitney [5, 12], and Paul and Shimano [6, 10], and that was recently formalized by Mason [4]. The following briefly describes Mason's theoretical framework that supports, and is supported by the present work. Every manipulation task can be broken down into elemental components that are defined by a particular set of contacting surfaces. With each elemental component is associated a set of constraints, called the natural constraints, that result from the particular mechanical and geometric characteristics of the task configuration. For instance, a hand in contact with a stationary rigid surface is not free to move through that surface (position constraint), and, if the surface is frictionless, it is not free to apply arbitrary forces tangent to the surface (force constraint). Figure 1 shows two task configurations for which compliant control is useful along with the associated natural constraints. In general, for each task configuration a generalized surface can be defined in a constraint space having N degrees of freedom, with position constraints along the normals to this surface and force constraints along the tangents. These two types of constraint, force and position, partition the degrees of freedom of possible hand motions into two orthogonal sets that must be controlled according to different criteria. Additional constraints, called artificial constraints, are introduced in accordance with these criteria to specify desired motions or force patterns in the task configuration. That is, each time the user specifies a desired trajectory in either position or force, an artificial constraint is defined. These constraints also occur along the tangents and normals to the generalized surface, but, unlike natural constraints, artificial force constraints are specified along surface normals, and artificial position constraints along tangents - consistency with the natural constraints is preserved. (See Fig. 1.)
Fig. 1 Examples of force control tasks showing the constraint frame |C| , natural constraints, and artificial constraints. In these examples [vx,Vy,vz, uxuy , uz ] T is the hand's velocity vector, 3 translational and 3 angular components, given in [C\. [fx, fy, 1Z, TX> ry, T Z ] T is the force vector acting on the hand, 3 forces and 3 torques, also given in [ C |. The a's are constants, (a) Turning a crank at a constant rate, u1 . (b) Turning a screw with constant rate, «2 . Note that screw is frictionless. Reference [4] gives more details and examples of this formalism. Note that the coordinate system within which constraints and trajectories are specified is not that of the joints of the manipulator, nor necessarily that of the manipulator hand or sensor. It is an N degree of freedom Cartesian system defined with respect to the task geometry. Takase [11] first introduced specification of constraints in a Cartesian system, and Paul has called this system the constraint frame, (C) [6]. In this frame N natural constraints and N orthogonal artificial constraints can be specified. Though selection of the constraint frame's position and orientation is a matter of discretion, a suitable choice can make the task of determining the natural constraints particularly simple. For instance, for the case depicted in Fig. 1(a), the force constraints in the coordinate system shown attached to the crank do not change as the handle revolves about the center of the crank. Methods for choosing the constraints for a given assembly operation await further research. For the present work it is assumed that a valid set of constraints is specified along with position and force trajectories. Eventually the natural constraints that correspond to a particular task may be determined automatically by an algorithm that makes use of knowledge about the task geometry. Mason has developed a substantial portion of the conceptual machinery necessary for such an algorithm. Once the natural constraints are used to partition the degrees of freedom into a position-controlled subset and a force-controlled subset, and desired position and force trajectories are specified through artificial constraints, it remains to control the manipulator. The present control methodology was designed to address this low-level control problem.
The Hybrid Controller The basic hybrid control idea is an architectural concept that links the constraints of a task requiring force feedback to the controller design. The transformation form (C) to the joints of the manipulator is such that, for the general case, control of one manipulator joint involves every dimension in (C): Qi =Qi(Xi,X2 XN) (1) where: q, = position of/th joint of manipulator fi, = inverse kinematic function Xj = position of /'th degree of freedom in {C) Therefore in hybrid control the actuator drive signal for each joint represents that particular joint's instantaneous contribution to satisfying each positional and each force constraint. The actuator control signal for the /'th joint has N components - one for each force controlled degree of freedom in [C], and one for each position controlled degree of freedom: (2) where: Tj = torque applied by the rth actuator A/y = force error in/th DOF of {C] &Xj = position error in/th DOF of {C) Tjj and i/-,-, = force and ..position compensation functions, respectively, for the /'th input and this rth output Sj = component of compliance selection vector. The compliance selection vector, S, is a binary iV-tuple that specifies which degrees of freedom in [C\ are under force control (indicated by Sj = 1), and which are under position control (Sj = 0). (In this paper it is assumed that the number of manipulator joints equals N < 6.) For example: if S = [0,0, 1,0, l,l] r
then T, = +n (Ax,) + fe(Ax2) + r/3(A/3) + iMA*4) +r,,(A/ 5 )+r/ 6 (A/ 6 ) Though the total number of active control loops is always N, the type mix will vary as the task geometry and natural constraints change. We also define the compliance selection matrix [S]: ~S, 0 " S2 [S] = diag(S) = S3 0 "" SN_ Figure 2 illustrates a hybrid control system that incorporates these ideas. The two complementary sets of feedback loops (upper-position, lower-force), each with its own sensory system and control law, are shown here controlling a common plant, the manipulator. Notice that sensory signals must be transformed from the coordinate system of the transducer, [q] for position and [H] for force, into (C) before errors are found and the compliance selection vector is applied: where: cX = A(q) c F=[gr]«F X = position of manipulator hand A = kinematic transform from [q] to (C) F = force on manipulator hand (3a) (3b) [V] N 0 [vx ] [&R] I [%R] force transformation matrix from [H] to [ C]
Fig. 3 Physical layout of manipulator and reaction surface used in experiments. Joints 1 and 3 of the Scheinman arm were used to provide motion in plane normal to gravity vector. Hand is in contact with numerically controlled X-Y table. N [Vx] rotation matrix from [H] to {C) 0 -V, o -v[ v, o V = vector from the origin of (C) to the origin of {H}, expressed in (C) Error signals in position and force are found once equations (3o) and (3b) have been applied: AX(0 = c Xd{0 -A(q( 0 ) = c Xd«) - c X(/ ) (4a) AF(f)=c Fd (f)- M r F(r)= c F r f (r)- c F ( 0 (4b) In addition to these error-driven control signals, an ideal manipulator trajectory controller, whether controlling position or force, can include feed-forward compensation for the nonlinear dynamics that characterize the manipulator control problem [8], Such signals take into account the configuration dependent inertia and gravity forces, state dependent Coriolis forces, velocity dependent friction forces, and externally generated hand contact forces. Ideally, when a wrist-mounted force sensor is used there should also be adjustments for accelerations of the hand mass present between a wrist sensor and contact surfaces of interest (including mass of hand held objects or tools). While each degree of freedom in (C) is controlled by only one loop, both sets of loops act cooperatively to control each manipulator joint. This is the central idea of hybrid control. As is usually the case when sensing, control, and actuation each take place in different coordinate systems, the same sensors and actuators participate in each "separate" control loop. In certain respects, hybrid control is a modification and extension of Paul and Shimano's compliant control [6]. Both approaches employ a task-related coordinate system C, both partition [C] into position controlled and force controlled subspaces, and both given freedom in specifyng position and force trajectories. However, because they pair individual force controlled joints with individual force constraints in [Cj on each servo cycle, position and force errors result. These errors are corrected on subsequent cycles by adjusting the position setpoints differentially. (See [6] for more details.) These adjustments are not necessary for the hybrid approach because each joint always contributes to control of force and position.
Experiments In order to examine the behavior of the proposed hybrid control method we conducted simple experiments involving simulation and physical implementation. Our goals were to examine the feasibility of the hybrid method with regard to accuracy, interactions between force and position control and stability. The two dimensional layout shown in Fig. 3 was used to test the hybrid controller with simulations and the physical experiments reported in Figs. 7-10. Joints 1 and 3 of the Scheinman arm were used to control two degrees of freedom in a plane whose normal aligns with gravity vector. The constraint coordinate system, (C), was chosen to lie in this plane with the CX direction perpendicular to the reaction surface. A General Automation SPC-16/85 minicomputer was used to perform all control and simulation computations. The manipulator was a modified Scheinman Stanford arm equipped with a Scheinman force-sensing wrist. This wrist is a 'Maltese Cross' design, with one strain gauge mounted on each of the 16 faces of the cross webbings (Fig. 4). The gauges are operated as 8 voltage-divider pairs to measure distortions, and therefore forces, in 6 degrees of freedom in the hand coordinate system [H]. For more details see [10]. A rigid X-Y table under precise numeric control was used to provide reaction forces and disturbance motions to the manipulator hand during testing. In keeping with the natural constraints for this task configuration, force control was exercised in the CX direction, and position control in CY, (S = [1,0]T ). Note that these coordinates do not align themselves with the action of any single manipulator joint. Modeling The mechanical system of manipulator, force-sensing wrist, manipulator hand, and reaction surface was modelled as shown schematically in Fig. 5. The manipulator linkage has one revolute and one sliding joint, driven by pure torque and force sources, respectively. The hand of mass M3 is supported by webbing of the force-sensing wrist of elasticity Kw. Also acting on the hand is reaction force fx produced through contact with an environmental surface. The state equations for this system are as follows: 1 Ate) [Tl+Bi(q{) + q2Kww2+lKww1]
Fig. 5 Model used for simulation of hybrid control task 1 1 l-Kwwt -Acosfa,)] w 2 = TT \-~K ^ w 7. +Asin(
simulated and then physically controlled. The hybrid controller implementation that was used for experimentation is now given. Since the transfer function from actuator to sensor is different for force and position control, separate control laws are required. The position control loop used a constant gain, proportional-integral-derivative (PID) control law. Force control was achieved by combining proportional-integral (PI) control with a saturation-type feedback limiter and a simple feed forward term. Using equations (3) and (4) to find position and force errors in(CJ: AX(0= c X r f (0-A(q(0 ) AX(0= c X r f (0-[/]q( 0 (12a) (126) AF(/)= c F r f (0-[S7 , ] W F ( 0 (12c) where: [AA(q) =
ll damping was provided mechanically. However, since Coulomb friction dominates, increased manipulator speed results in decreased effective damping. To compensate for this effect a saturation type non-linearity was included which reduced the effective integral gain for large values of force error. This measure helped to make the controller stable despite interactions between Coulomb friction and integral gain. Without this term the system was stable only when heavily overdamped. With it an approximately critically damped response was obtained (see Fig. 7). The force feed-forward term used here is quite simple: it merely drives the manipulator joints with a transformed version of the artificial force constraints, assuming quasistatic operation. Comparison with previous results [1 and unpublished] shows that use of force feed-forward gives faithful trajectory control with relatively low force feedback gains. Therefore, stability is more easily achieved. The detailed control system is shown in Fig. 6. All feedback gains and limiter values were chosen empirically to provide stable, responsive, accurate behavior. No formal optimality criteria were employed. These gains are given in Appendix A. Notice that no attempt was made to incorporate correction for the manipulator's nonlinear, time varying dynamics, nor were corrections made for acceleration forces on the hand mass. An automatic calibration procedure, similar to those of [2] and [9, 10], was used before an experimental session to find the relationship between strain-gauge readings and applied forces. During experimental movements strain-gauge signals were sampled at regular intervals and transformed into measurements of force given in the hand coordinate system, {H}. Except where noted, all data shown below were obtained with a sampling rate and servo rate of 16.7 ms (60 Hz). Force trajectories were filtered with a 12 Hz cutoff before plotting. Results and Discussion Figure 7 shows the responses of the system to 1, 5, and 10 Nt step changes in force in CX, while CY was servoed to maintain a fixed position. Rise times were about 0.15 s with little or no overshoot. The small amplitude limit cycle oscillations observable in these data were caused by interaction between the integral term in the force controller and the manipulator's Coulomb friction. The maximum force error in the steady state was < 1.0 Nt. A simulated response for the 5.0 Nt step is also shown in the figure. There is a good agreement between the simulated and physical responses. This stable, accurate behavior is quite good. Figure 8 demonstrates the control of force in the presence of position disturbances. The controller attempts to maintain a constant 5.0 Nt force against the X- Y table while the table is ramped away from the manipulator in the CX direction at a constant rate of 0.65 cm/s. The lower curve in Fig. 8 shows the position of the reaction surface and the hand during recording. The force response is shown in the center curve. Preceding motion the force servo had reached steady state. As motion begins force control degrades somewhat, although contact with the reaction surface is never lost. When the surface stops moving, control returns to a stable steady state. Force errors do not exceed 1.75 Nt except at the ends of the ramp where there are accelerations. The upper plot shows response in the position-servoed CY direction. These data indicate that reasonably accurate force control with respect to a moving object (e.g., one moving down an assembly line), is possible using these methods. Figure 9 depicts the response to a sinusoidal position trajectory and a simultaneous step in force: cfx(t) =10+10 U(t-l.O)Nt cy(t) =30~30cos(0.75-rrt) cm where U is the unit step. This procedure was designed to examine coupling between the position and force loops. Although some error in position occurs along the position trajectory, the force step produces no noticeable disturbance in position. Furthermore, there is no clear relationship between errors in force and the position trajectory. In order to assess the controller's dependence on servo rate, the trajectories of equation (17) were repeated with servo periods of 12.5 and 8.3 ms (80 and 120 Hz). These changes resulted in improved force response -less overshoot and better stability - but did not affect the position servo perceptibly. The force response of the 8.3 msec condition is shown by the solid curve in Fig. 9. While Fig. 8 shows that maintenance of a constant force against a moving reaction surface is possible, the data of Fig. 10 show a ramp in force can be produced with constant position. This experiment was the only attempt to control forces in a continuously changing manner. The quality of force control shown here is quite good - it is satisfactory for many assembly applications. Using a high-level strategy first developed by Inoue [3] the hybrid controller presented here (augmented to 3 degrees of freedom) was used successfully to perform a peg in hole insertion operation (diameter = 15.9 mm, clearance = 0.025 mm (1 mil), no chamfer). See Fig. 11. To do this operation joint of the manipulator was controlled to give 3 degree-offreedom planar motion (X, Y, 0) The system's ability to apply a constant force to a moving environment is perhaps more important than its ability to closely follow reference input changes. Consider a manipulator moving its hand across a surface while force servoing in the direction normal to the surface. As the manipulator moves, irregularities in the reaction surface and small errors in the accuracy of the position servo will look like surface motion to the force controller. Our ramp disturbance data suggest adequate force control is possible under such circumstances. These same data suggest the feasibility of manipulating objects as they move down an assembly line, or through some other handling process. Forces applied by the finger tips during contact differ slightly from transduced forces, due to acceleration forces of the hand mass. Simulations showed, however, that forces due to accelerations of the hand mass were small, so that the transduced force is a very good approximation to contact force. In the simulation as well as the physical system the transduced force was the controlled variable. Examination of force-sensor response revealed the presence of a small amplitude, 286 Hz oscillation superimposed on the force signal's other behavior. Because the manipulator hand is supported between the spring-like force sensor and the compliance of the reaction surface, the wrist-hand-surface system acts as an oscillator. This interpretation was verified by calculating the natural frequency of hand oscillations predicted by the model: j, = ~.JKr+Kw n 2-rr M 3 This evaluates to 280 Hz for the parameters given earlier - very close to the observed value. These oscillations could not be controlled through the software loop because of their high frequency relative to the controller's bandwidth. Therefore an oscillation of approximately 0.5 Nt is superimposed on all force records. To make other behavior easier to observe, all plots of force shown in this paper have been low pass filtered with a 12 Hz cutoff:
Paul and Shimano [6] have described a hybrid controller that uses the so called "free-joint" method to provide compliant control. Their approach is approximate since only a subset of manipulator joint are force controlled, with correction for resulting position errors on subsequent servo cycles. However, because it requires less computing—fewer coordinate transformations—the free-joint method has been implemented to control six degrees of freedom with existing minicomputers. We feel that the method presented here is a more straightforward (perhaps obvious) approach to the problem of controlling forces and positions simultaneously. The experimental results show that the technique is feasible. Although this technique is straightforward and feasible, it is not cheap in the computational sense—this disadvantage may have prevented previous workers from exploring and embracing it. Recent advances in computing technology, mostly in the form of faster, smaler, and cheaper microelectronics, however, now justify exploration of computationally expensive but conceptually elegant methods, such as the one given here.
Summary
A hybrid position/force controller is proposed that uses a wrist mounted force sensor to control manipulator trajectories in a task oriented Cartesian coordinate system. The controller was implemented to control two axes of the Scheinman manipulator as well as a two axis simulation. Data are presented that show the controller's stable, accurate response to steps and ramps in force, to position ramp disturbances, and to simultaneous force and position trajectory control. Manipulations of servo rate (from 60 Hz to 80 and 120 Hz) revealed only modest improvements in force response with no detectable changes in position accuracy. Some problems with the practical use of the Scheinman force sensing wrist are explained with a two axis model.