IEEE TRANSACTIONS ON ROBOTICS A N D AUTOMA r i m . VOL 6. N O 1.

A P R I L IWO

159

The Use of Multisensor Data for Robotic Applications

performed usually in manual mode, sometimes in teleoperated mode but seldom in autonomous mode. This philosophy increases the level of involvement of astronauts, further raising their exposure to danger. Autonomous operation should be the norm in hazardous environments. Total or partial autonomy of space systems will enhance safety, reliability, productivity, and adaptability and will reduce the overall cost of space missions. Each of these factors alone justifies automation [3]-[5]. 1) Safety: Although a human-machine mix addresses many of the safety issues, specifically those associated with the safety of equipment, this strategy is hazardous to humans when placed in space in either intravehicular or extravehicular activities. Additional risk is run during extravehicular activities, particularly those performed in high orbits where the levels of radiation are harmful to humans. Other kinds of risks are associated with maneuvering large payloads. Consequently, I. INTRODUCTION optimal safety is achieved when humans are used exclusively as a last resort for dealing with unforeseen circumstances that TH the recent advances in the areas of vision and sensare beyond the capabilities of current autonomous technoloing, robots have become a major element of todays ingies. dustrial world [ 11, [ 2 ] . They have been beneficial in replacing 2 ) Reliability: When a definite line of dichotomy can be humans not only in tasks at which robots are more efficient established between normal and abnormal situations, but also in those that humans find undesirable because they are strenuous, boring, difficult, or hazardous. Robotic systems machine-driven decisions can be more reliable than human are being used currently in hazardous environments such as decisions. The latter can be affected negatively by omissions those encountered in chemical, nuclear, military, underwater, and illusions that are often induced by stringent conditions in manual operations or a lack of awareness in teleoperated mining, and space applications. Examples of space applications are found in areas such as servicing and repair facilities activities. Therefore, when circumstances allow for full task for spacecraft and manufacturing and assembling of space lab- automation, reliability is expected to be greatly enhanced. 3) Adaptability: Autonomous systems can and should be oratories [3]. Space applications can be greatly enhanced by the use of designed in a modular fashion to accommodate future growth robotics and automation. Activities such as collision-free nav- and to allow subsystems to monitor their own operation as igation, rendezvous and docking, satellite retrieval and ser- well as the operation of other subsystems. This leads to safer and more reliable overall systems requiring less reliance on vicing, orbital inspection, maintenance, refurbishment and human and ground support. repair, and navigation of remotely piloted vehicles now re4) Cost: Improvements in safety, reliability, and adaptabilquire human assistance, often in environments that are hostile. ity result in higher efficiency, hence a reduction in the overall Presently, because of the actual state of the art of artificial intelligence, robotics, and automation, these space activities are cost of space missions. It is noted, however, that automation does not Dreclude the implementation of alternative strategies Manuscript received December 2 1. 1988; revised July 24, 1989. This work for manual override by humans, which would provide smooth was supported by the DOE University Program in Robotics for Advanced the from One to the Other with passage Of Reactors (Universities of Florida, Michigan, Tennessee, Texas, and the Oak Ridge National Laboratory) under Grant D O E - D ~ - ~ ~ 0 2 - 8 6 ~ ~ 3 The6 8 . cations associated with a robust and flexible human-machine 79 FIS and MIS experiments were carried out under Contract NAG-8.630interface. BO1 199962 with NASA, Marshall Space Flight Center. and low are the main Safety, M, A , Abidi is with the Depanment of Electrical and Computer Engineer. advantages to be gained from autonomous space operations ing, University of Tennessee, Knoxville, TN 37996-2 100. R. c.Gonzatez is with Perceptics Corporation, Knoxville, TN 379334991. [ 3 ] , [6]. In order to achieve autonomous systems with these and the Department of Electrical and Computer Engineering, University of features to be implemented at low cost, the human presence Tennessee, Knoxville, TN 37996-2100. or telepresence should be replaced/extended to some degree IEEE Log Number 8933037.
Abstract-Space applications can be greatly enhanced by the use of robotics and automation in activities such as orbital inspection and maintenance. Partial or full autonomy capabilities in space systems will enhance safety, reliability, productivity, adaptability, and will reduce overall cost. At the core of a robotic system i s the ability to acquire, fuse, and interpret multisensory data to generate appropriate actions in the performance of a given task. The feasibility of realistic autonomous space manipulation tasks using multisensory information is presented. This i s shown through two experiments involving a fluid interchange system and a module interchange system. In both cases, autonomous location of the mating element, autonomous location of a guiding light target, mating, and demating of the system are performed. Specifically, vision-driven techniques were implemented that determine the arbitrary two-dimensional position and orientation of the mating elements as well as the arbitrary three-dimensional position and orientation of the light targets. The robotic system i s also equipped with a forcehorque sensor that continuously monitors the six components of force and torque exerted on the end effector. Using vision, force, torque, proximity, and touch sensors, the fluid interchange system and the module interchange system experiments were accomplished autonomously and successfully.

lO42-296X/9O/O400-0159$01 .OO 0 1990 IEEE

160

IEEE TRANSACTIONS O N ROBOTICS A N D AUTOMATION. VOL 6. NO 2. APRIL 1990

by an array of sensors capable of mapping the environment in which they operate. This sensory system should be able to acquire, fuse, and interpret multisensory data to generate actions based on sensed data. The need for several sensors is evident from the complexity of the inspection and manipulation tasks necessary to carry out space missions. The use of sensors to perform inspection and manipulation tasks in a variably structured or unstructured workspace is often based on the following three steps: 1) On the basis of collected sensory data, a model of the workspace is built and continuously updated thereafter. (This model integrates information about the scene collected by various sensors. Knowledge in this model relates to position, orientation, size, and other features of the objects in the work space.) 2) Using the model built in 1) along with a list of tasks to be performed, the system negotiates a list of feasible tasks and generates moves to implement each of them. 3) Each of the tasks selected in 2) is implemented after planning and verification. The experiments described in this paper are performed within the scope of full task autonomy. In a teleoperation mode, the human operator sets up the robotic system to perform the desired task. Using its sensory cues, the system maps the workspace and performs its operations in a fully autonomous mode. Finally, the system reports back to the human operator on the success or failure of this task and resumes its teleoperation mode. 11. OBJECTIVE The intent here is to demonstrate the feasibility of realistic autonomous robotic manipulations using multisensory information cues. The environment is variably structured. Only the relative position of the manipulator/end effector with respect to the workpieces being manipulated is unknown when data are being sensed. This does not preclude relative motion of objects once an experiment has started. This task is performed using integrated information from vision, forceltorque, proximity, and touch sensing. The cooperative use of sensors is illustrated through the following two experiments, which are of basic importance in numerous NASA operations. 1) Fluid Interchange System (FIS): The purpose of this experiment is to autonomously mate and demate a fluid interchange system using multisensory data. A mock-up of the FIS has been designed and built. It is composed of a mating nozzle element, a receptacle, and a number of light targets (Fig. 1). 2) Module Interchange System (MIS): The purpose of this experiment is to autonomously identify, remove, and replace a faulty or spent module (from an orbiter) by a new module (from a servicer). A mock-up of the MIS system was designed and built (Fig. 2). It is composed of a mating module element, a mounting rack, and light targets. Here, again, the system is manipulated using cues from the vision, forcekorque, proximity, and touch sensors. Because of the similarity between the FIS and MIS experiments, in this paper, we will describe the FIS operation in

Fig. 1

Fluid interchange system (FIS) setup nozzle, receptacle, and light target

-----.Fig. 2 . Module interchange system (MIS) setup: exchange module, mounting rack, and light target.

detail and mention the results of the MIS experiment only briefly. A . Experimental Setup The mock-up of the FIS is composed of an alignment/locking mechanism that consists of a nozzle and a receptacle mounted in a module rack (Fig. 1). This module also holds the four-light guiding target used by the vision system. The nozzle and receptacle are essentially a large male and female BNC-like connector pair. The nozzle is cylindrical and has two stainless steel pins located 180" apart, perpendicular to its outer surface, extending 0.50 in. It has a flat, padded, parallelepipedic end that serves as an attachment point to the robot end effector. The nozzle is 4.75 in long. It consists of a cylindrical portion, which is 2.25 in in height. Its inner and outer diameters are 1.25 and 1.75 in, respectively. The attachment handle is 2.50 x 1.75 x 1.25 in and is centered on the opposite end of the cylindrical portion from the nozzle opening. The receptacle is mounted on a rectangular plate of 6.75 x 8.75 in, which is in turn mounted on a module of 1.25 x 1.25 x 7.50 in in dimension. The receptacle module system is 12.75 in deep. It is mounted in a standard nuclear instrumentation module (NIM) rack. The receptacle has a flared rim and inner and outer diameters of 2.00 and 2.90 in, respectively. At a depth of 1.40 in, a spring-loaded pressure plate is mounted; its travel distance is 0.25 in. The receptacle also has two V notches located 180" apart in its rim leading into two grooves that lock the nozzle into position once it is inserted.

ABlDl AND GONZALEZ MULTISENSOR DATA FOR ROBOTIC MATING AND DEMATING IN SPACF

161

The receptacle also holds the four-light guiding target, which is used by the vision system to locate the position and orientation of the receptacle. The positions of the light target, with respect to the lower left-hand corner of the receptacle, are in inches: (6.25, 7.50), (6.25, 1.20), (0.50, 1.20), and (7.50. 0.50). The first dimension is the distance across the rack, and the second is the height. The rack is 8.50 x 20.00 x 17.25 in. The inner dimensions are 7.75 x 17.00 x 10.60 in. Modules are guided into the rack by 12 sets of equally spaced (0.35-in) strips placed along the inner top and bottom surfaces of the rack. The initial two-dimensional position and orientation of the nozzle are unknown. Its height, however, is known. The threedimensional position and orientation of the receptacle holding the target are unknown. The only constraint in this experiment is that the FIS (nozzle, receptacle, and FIS target) be positioned in the field of view of the camera and be within reach of the end effector. The world coordinate system (Fig. 3) is defined with respect to the robotic workstation table and is divided into two areas: the mating element area and the target area. This means that in both experiments, the robotic system is expected to find the mating element anywhere in the mating area and locate the light target anywhere in the target area. All the dimensions mentioned above, along with the information corresponding to the relative positions of the objects being manipulated in this experiment, are stored in a database that is easily updatable by the operator and directly accessible by each sensor module during the running of the operation. The FIS and MIS experiments illustrating the cooperative use of multisensory information for autonomous operations are performed using (see Figs. 4 and 5 ) the following: A Cincinnati Milacron T3-726 six-degree-of-freedom industrial robot Several external sensors, including vision, range, proximity, touch, forceitorque, and sound (the range and sound sensors are not used in this experiment because they both use ambient air as a medium of transduction, hence, they are not appropriate for space applications). A PERCEPTICS-9200E image processor A VAX 11/785-VMS computer.

I"
Targets Area Mating Elements Area

Fig. 3 .

World coordinate system with respect to robotic workstation.

Fig. 4. Robotic system and its sensors used for FIS and MIS experiments. 1-vision; 2-range and sound; 3-proximity; 4-touch; and 5-force and torque.

1:
f
E
N

Data Fusion

Vision

E
Proximity
I

N
Control
I

M
c

Touch

Activation

- 1 4 1 Data Acquisition

g h-Tq
Sound

B . Description of the FIS and MIS Experiments

-.

robotic manipulation. Only the The FIS and MIS experiments can be summarized as fol- rig. 5 . Interface functions for autonomous sensors are used in this experivision, force/torque, proximity, and touch lows. A mating element appears anywhere in the mating area. ment. The range and sound sensors are not included because they cannot be supported in space. Its two-dimensional position and orientation on the table are unknown. The mating element is within the field of view of a camera rigidly mounted on the arm and within reach of the area is within the field of view of the camera and uses its virobot gripper. The light targets are within the field of view of sion sensor to determine (approximately) the two-dimensional the camera when the robot is in an arbitrary position within a position of the mating element. This measurement is later repredetermined area. The paradigm described in the previous fined by moving the robot arm closer to the mating element. section is followed here for the execution of the experiment. The robot then picks up the mating element after checking The robot is moved by the human operator such that the light for its presence by using its proximity and touch sensors. The target is visible; then, the robot is put in autonomous mode. robot then proceeds to insert the nozzle (FIS) or the exchange The robot determines the approximate position of the mount- module (MIS). During the entire experiment, the forcekorque ing rack using the vision sensor. This measurement is later sensor is active. refined once the robot knows the approximate position of the In Section 111, the vision aspects of this system are investarget. Afterward, the robot positions itself so that the mating tigated. Specifically, we cover in detail the two-dimensional

162

IEEE TRANSACTIONS ON ROBOTICS A N D AUTOMATION. VOL 6 . NO 2. APRIL 1990

mating element positions and the three-dimensional target positions. In Sections IV-VI, the state of the art and the properties of the sensors implemented in this work for forceltorque, proximity, and touch, respectively, are discussed. In each of these sections, we briefly discuss previously published work by others that relates to the topic of that section. Section VI1 is devoted to the experimental results.
FOR 111. VISION SENSING

lem may result from atmospheric occlusions, such as smoke, from other objects. as would be the case in picking a part from a bin, or from the robot hand itself if it is not possible to position the camera in an ideal location such as on the gripper itself. Although vision sensing is capable of performing many of the tasks required of an intelligent robot, its high complexity and cost limit its use [2], [7]-[9].

THE

INTERCHANGE SYSTEMS

B. Vision Subsystem
During the FIS and MIS experiments, the vision system performs two principal tasks: 1) identification of twodimensional position and orientation of the FIS and MIS mating elements and 2) identification of three-dimensional position and orientation of the mounting elements for the FIS and MIS. In the following, we highlight the two methods used in our vision system. At the same time, we mention some similar existing systems that were reported in the literature. 1) Two-Dimensional Position and Orientation of Mating Elements: Here, we describe a method for determining the arbitrary two-dimensional position and orientation of the mating elements of the FIS and MIS using a vision sensor. Many of the vision-based systems used for path planning for mobile robots, automated inspection, and mechanical assembly use similar techniques. A summary of these techniques may be found in [ 101 and [ 113. In this experiment, several preprocessing steps are performed on the image depicting the mating element. First, the grey-scale image I' is transformed to a binary image I using thresholding [l], [2]. The characteristic function for this image f ( x , y ) is equal to 0 for image points on the background and 1 for points on the object. We choose a point that represents the position of a twodimensional object. In this case, the center of the area of the object was selected. To calculate the center of area (X,J ) , we find the area A and the first moments M x and M y of the object using the following expressions [ 121:

A . Background
By vision sensing, one usually refers to the two-dimensional optical information provided by a vision sensor or camera. There are virtually no vision sensors built specifically for robotics applications. Most of the vision sensors used in robotics are borrowed from the television industry; hence, they are generally not sized properly, rugged enough, or sensitive enough. Two of the most common types of cameras mounted on a robot for vision sensing are vidicons and solidstate arrays, and both types are commonly available in resolutions varying from 32 x 32 to 512 x 512 sensing elements per image. In general, these sensors are rigidly fixed to the manipulator in order to simplify the coordinate transformation between world system and camera system. Because solid-state cameras are more rugged and of smdler size, they are a better choice for robotics applications. However, vidicons are generally cheaper and present fewer problems in interfacing to some image-processing systems; therefore, they may be preferred for fixed mounting in the work space (such as overhead). Of all the sensors, vision has the distinction of having the widest potential range of applications. Vision may be used as a substitute for, or in conjunction with, many of the other types of sensors for the purposes of object detection, recognition, location, and inspection. In addition, it provides a means for monitoring the environment for the presence of obstacles and for the occurrence of abnormal events [ 7 ] . Object detection, recognition, and location are tasks to which vision is ideally suited. Using vision, such tasks may be performed in a natural and efficient manner prior to contact, thus allowing the robot to plan its actions before any movement is made. This is contrasted with other sensors, which may require extensive robot motion to accomplish these tasks. Because processing a visual image involves a large amount of variable data, the algorithms required are generally of far greater complexity than those used for other sensors. There are a number of contributing factors, which tend to complicate the situation. One major problem results from the difficulty of deducing three-dimensional locations from a twodimensional image because there is a loss of information in the projection process. If the object is restricted to movement in a plane, such as on a table or conveyor belt, then deducing location may be fairly simple. However, if motion is unrestricted, then three-dimensional location may be much more difficult, and approaches such as structured lighting, stereo, and other multiple imaging techniques may be necessary. A great burden can also be placed on image-processing algorithms as a result of lighting conditions in the work space. Among these are inadequate or nonuniform light, shadows, reflectance variations, and occlusion of objects. This latter prob-

A Mx

=/lf(-Y,

Y)dXdY

(1) (2)

j6.f

0, dY Y)dX

My = / L Y f ( X . Y)dXdY.

(3)

The coordinates of the center of area X and J are computed as

X =MxIA
J =My/A.

(4)

For an N x N discrete image

x=
J

yX
N

x f k Y)/
N

CfC.?Y )
N

(6)

x = l y-I N

x=l y=l

C Y f ( X ,Y ) /
x=l y=l

y X f ( x ,Y ) .
x = l y=l

(7)

The usual practice to determine the two-dimensional orien-

ABIDI A N D GONZALEZ: MULTISENSOR DATA FOR ROBOTICM A rmc; A N D LEMA

r i i w I K SPACE
c y

163

tation of an object is to compute the axis of the least second moment or moment of inertia [12]. To accomplish this, we find the line for which the integral of the squared distance to points in the object is minimum, that is, minimize E with respect to 8 :

YMATING ELEMENT

where r ( x , y ) is the minimum distance from the point ( x ,y ) to the line being sought. To represent a line, we use the two parameters p and 0 (where p is the perpendicular distance from the origin to the line, and 0 is the angle between the x axis and the line measured counterclockwise) (Fig. 6). The minimization of E yields the following orientation angle:
r
1

Fig. 6.
IMAGE OF TARGEl

0-0 representation of a line.

TARGET

O2

where

Fig. 7.

Target points and their images in a pinhole camera model.

In an augmented coordinate system, the image coordinates

N N

( x ,y ) of an image point are related to the world coordinates ( X , y , 2) by

-chi

x=l y = l

ch2
(Y
-

N
My, =

H 2 f ( x ,Y )

(12)

ch3

- ch4 are the second moments of the image. The outcome of this operation is the two-dimensional position and orientation of the mating element ( X , j , e). 2) Three-Dimensional Position and Orientation of the where chi, ch2, ch3, and ch4 represent the elements of the Mounting Element: Here, we describe a method that deter- homogeneous coordinates [ 11. In this paper, we use the pinmines the three-dimensional position and orientation of four- hole camera model for our vision sensor (Fig. 7). The image light guiding targets, which are utilized to locate the FIS and coordinates ( x ,y ) and the world coordinates ( X , Y , Z ) are MIS mounting elements. With reference to Fig. 7, the objecrelated as follows [l], [2], [19]: tive is to recover the three-dimensional position (with respect to a coordinate system fixed to the robot) of each of the point targets P O , P I , Pz, and P3, knowing their respective images p ~ p , ~ p , , and p3 as well as the size of the target 2 [P, = ( X , , Y , , 2,)and p , = ( x , ,y,)]. Since the position and the orientation of the camera are known with respect to the robot, recovering the position of the target points in a camera Wc=AW (18) reference frame yields the position and the orientation of the where targets in a coordinate system fixed to the robot. The need for accurate, fast, and efficient positioning of w = ( X , Y , 1) (19) a robot arm with respect to a given target has been adwc = ( X C ,Y C ,zc,1) (20) dressed in many ways using painted lines, infrared beacons, and much more sophisticated three-dimensional vision techniques [13]-[17]. A summary of most of these techniques can be found in [18]. In this section, we show that the position of the target can be determined up to the desired accuracy using a four-point light target. The solution is direct and therefore superior to most of the other iterative schemes.

x=l y=l

z,

164

IEEE TRANSACTIONS ON ROBOTICS A N D AUTOMATION. VOL. 6 . NO 2. APRIL 1990

1'
to
RT

0
O

Ol
1J (28)

0
= R , . Rp

.Re.

The transformation RT denotes the total movement. Equations (25)-(27) introduce implicitly the idea that this transformation preserves distance. Hence, for RT

the following holds true:

and

a1a2 + a4a5

+ a7a8 = 0 ala3 + a4a6 + a7a9 = 0 a2a3 + a5a6 + a8a9 = 0.

(36) (37) (38)

Note that not all these equations are independent. In the case where the translation vector is not zero, the global movement can be described by a matrix similar to RT , which includes the coefficients a 10, a 1 I , and a 12 characterizing the translation. Obviously, (30)-( 38) still hold true:

-a1
A=
a4

a2 a5
08

a3
a6

010-

a11
(39)
a12

a7

a9

- cos0
-sin8

sin8 cos0

0 00 0
(25)

-0

1 -

Ro = -

0 0

0 0

1 0

1-

l o

11

allow some simplifying assumptions about the problem ge-

ABIDl AND GONZALEZ: MULTISENSOR DATA FOR ROBOTIC MATING AND DEMATING IN SPACE

165

Fig. 8. Transformation of world coordinate system into standard position T,transformation of camera coordinate system into ideal position S , and relationship between the two transformations. W ,- original world coordinates; Wp-standard position coordinates; Wy- ideal position coordinates; W:-original camera coordinates.

ometry and also indicate the adjustment to be applied to the solution to compensate for these steps. Throughout this paper, we use the vector w ; represent the augmented vector to ( X i , Y ; ,Z ; , l), where ( X i , Y ; ,Z ; ) are the ( X , Y , Z ) coordinates of point P ; . The three target points are denoted by Po, P I ,and P2. As before, any superscript on W indicates a corresponding superscript on X , Y , and Z and also refers to the coordinates of the point using a system other than the fixed world system. These transformations are shown diagrammatically in Fig. 8. They are briefly summarized below. Details of each transformation and its effect on the overall process in the recovery of the relative position of the light targets are given in Appendices 1-111. a) The first step transforms this general target positioning problem into an intermediate coordinate system in which the three target points are in what we shall define as standard position. The standard position is characterized by PO at the origin, P I on the positive X axis, and P2 in the first quadrant of the X Y plane (or on the positive Y axis). This transformation will be denoted by E its effect on the recovery of the targets image is detailed in Appendix I. b) The second preprocessing step must be performed on each image to be processed. It involves another transformation, which describes how the image would look to a camera that has its lens center in the same location as the real camera but is looking directly at the origin of a given a system and has its x axis in the same plane as the X axis of the CY system. We will refer to this fictitious camera as being in ideal position and will call the image seen by such a camera an ideal image. Such an image is characterized by the image of Po(p0) at the origin and the image of P ~ ( p lon the positive ) x axis. This transformation will be denoted by S ; its effect on the recovery of the target position is examined in Appendix 11. c) The transformation relating the coordinates of a given point from the standard position to the ideal position is described by matrix C. The computation of C and the combined effect of T and S are given in Appendix 111. (The transformation B is defined as an intermediary step in the computation of A . ) For implementation purposes, the three previous operations are summarized as follows: Step I : Compute the transformation matrix T using (58). Step 2: Compute the transformation matrix S-I using (77). Step 3: Compute the transformation matrix C using (102). Step 4: Compute the transformation matrix A using A = S-CT.

0 0

0 -

-cIlc2 1

M y
Xxq

0 0 0

x;xy x;xq xgxq

y;XT

x; x; x;

AY?
AY:

x;YT xgyp y;YF

c22

cI 3
c2 3

0
-

0 0

AY:

y;

AY? y;Xp

Y p q y;- -c340

0
0 0 - 0

MC

= Co.

(42)

Note that the first row of (41) is superfluous; it was added to make M a square matrix. Since we have six unknowns and five equations, we need to add one more equation. Using the fact that (C11, ( 2 2 1 , C31) is orthonormal and that C2, = 0, we may write

c:,+c:, 1. =

(43)

Because of the nature of (42), all the unknown terms can be written as a function of CII = f

d
~

1+r2

Without inverting M , we can compute r by combining the equations given in the preceding system:
= xP

MYB3 - x ~ B I -__ c1 3 XCiB 3 - B 2 ) - C I I I( 1

(45)

where

166
B2 = Y ~ Y P ( Y $ X ~ -L)$x~)(X;YY; -X;YY;)

IEEF. TRANSACTIONS ON ROBOTICS ANI1 AUTOMATION. VOL 6. NO 2 . APRIL 1990

deflecting laterally, angularly, or both whenever the end effector of a gripped workpiece experiences a force or torque B3 = Y;YY;y(y;xg yl;x;)(Yp - Y;). (46) 1221, WI. Active and passive forcehorque sensors are capable of enAlthough (44) suggests that CII can take two distinct values, hancing the manipulation capabilities of a robot in a variety of in reality this cannot happen at the same time because tasks, such as contact detection, contour following, tool insertion, and mechanical assembly. Force and torque sensors provide a safety device that can interrupt the robot motion when or an unexpected obstacle is encountered. Though often mounted at the end effector level, these sensors protect practically the entire arm because they are capable of sensing any relative motion between the end effector and the rest of the robot arm The case where C11 = 0 leads to an infinite value of r or as well. This is a major difference between these sensors and to P I , P2, and P3 being collinear, which contradicts our tactile sensors, which provide only local information. Equally first assumption about the relative position of these points. important is the use of force and torque sensors in manipuOnce C I I is computed, the remaining parameters are read- lation tasks. In dynamically constrained operations, excessive ily computed. Using (50),the matrix A is computed, and the levels of local force and torque are very common. These senP three-dimensional coordinates of P O , I ,P2, and P3 are each sors can be used as feedback devices that clip or eliminate excessive forces and/or torques. The peg-in-the-hole problem determined by solving (13) for each target. is a classical example where active and/or passive sensing can E SENSING THE INTERCHANGE FOR SYSTEMS IV. F ~ R C AND TORQUE be amlied. -. A . Background B . Experimental Setup Force and torque sensing generally refer to the measurement The force/torque sensor used in this work is the Lord FTof the global forces and torques applied on a workpiece by a robot manipulator. In force and torque measurement, we 30/100. The sensor is designed for wrist mounting. It uses eight piezoresistive strain gauges that provide real-time feeddistinguish between active and passive sensing. Active sensing is performed through either joint or wrist back of the location, magnitude, and orientation of forces and sensing. Early techniques for determining force and torque moments. The system is equipped with a processor that rewere based on measuring the individual forces acting on each solves the eight raw pieces of data into six Cartesian forces manipulator joint. These individual forces can be inferred and torques at a rate of 83 Hz. The converted force and torque from the fluid back pressure or current in the armature for measurements can be referenced with respect to any element hydraulic robots or electric robots, respectively. These tech- in the arm, including the sensor itself. This allows the canniques are highly sensitive to the weight and motion of the cellation of the effect of the load of the sensor, end effector robot itself. This problem is easily eliminated by using a wrist or tool, or workpiece, which would otherwise degrade the sensor since the force and torque components are measured accuracy of the measurements. This system has a maximum in a more direct manner. Wrist sensors are composed of a force and torque capacity of 30 lb and 100 in.lb, respectively. number of sensing elements arranged in some configuration. The system resolution is about 1 oz and 1 in.oz for force and Strain gauges are the most commonly used sensing elements torque, respectively. A one-chip Z8 microcomputer has been because of their reliability, durability, and low cost, although added to this system for communication between the sensor many other elements are suitable for this purpose. Most wrist and the main processor (VAX 11/785-VMS) for sensor biassensors produce eight raw readings that need to be converted ing, threshold setting, and interface to other sensors. During the entire experiment, including camera positioning into the six Cartesian components of force and torque along the major axes using a resolved force matrix. This is done and activation, data acquisition and processing, nozzle recogthrough computer software or the hardware processor that is nition and manipulation, and mating and demating of the FIS, usually provided by the manufacturer of off-the-shelf sensors. the forcekorque sensor is active. Its role is to continuously Wrist sensors are usually mounted at the base of the robot monitor each of the three force components F *, F , , and F Z hand and are capable of performing a single-point contact along the three axes X , Y, and Z , respectively, relative to the robot end effector. It also monitors the three components of measurement [ 11. Passive sensing refers to the added kinematic design of a torque T , , T y , and T z . At a rate of 83 Hz, the actual six forcekorque sensor that allows the annihilation of excessive components forces without the need for active servo feedback. The instrumented remote center compliance (IRCC) system typifies this concept. The IRCC is a multiaxis compliant interface between are read and compared against a preset set of values the robot arm and the end effector. It continuously monitors the location of the compliance center along the end-effector v x = ( 1 ~ ~ x 1 1 ~, ~ x lFy1,T ~ l , m 1 , l I T ?( ) / . axis as well as lateral and angular stiffness of the system taken The robot motion is immediately halted every time any comat the center of compliance. The key elements of the IRCC system are three elastometer shear pads, which respond by ponent of force or torque exceeds its preset threshold. This

lTyl,

ABIDI AND GONZALEZ. MULTISENSOR DATA FOR ROBOTIC MATING AND DEMATING IN SPACE

167

in characteristics. Some sensors are capable of detecting the presence of objects from great distances averaging several feet (ultrasonic and optical); others are capable of sensing only within a few millimeters (inductive and magnetic). CaIn this experiment, when the robot encounters excessive force pacitive, ultrasonic, and optical sensors are capable of detect(torque), it stops, waits for a preset time ( 5 s), then proceeds ing most materials but with significantly varying sensitivity, with the task where it left off. If these interrupts persist, the which is highly dependent on surface reflection properties and system attempts indefinitely to resume its task until it is reset orientation. On the other hand, inductive and magnetic senis manually. It is worth noting that the threshold vector Pax sors are sensitive only to metals or, more specifically, ferrous a dynamic variable that can be changed at any moment of the metals. Proximity sensing has a wide range of applications, execution time of a given task. For the FIS experiment, Pax such as collision avoidance, object location and acquisition, was set to and object tracking [ 7 ] , [22], [25].

means that at all times

Pa"(15, 15, 15, 50, 50, 50)' =

during motion (no collision is expected) and double this value during manipulation (collision is expected). The forces are in ounces; the torques are in ounce-inches. V. PROXIMITY SENSING THE INTERCHANGE OF SYSTEMS
A . Background

B. Experimental Setup
The proximity sensor used in this experiment is optical; it is made of two solid-state infrared light emitters coupled with two solid-state light receivers arranged in the configuration shown in Fig. 9 171. Both the emitter and the receiver are placed such that the light bouncing off an object in the sensing volume comes directly into the receiver. When the light intensity is greater than a threshold value, the sensor is activated. It is designed to detect the presence of an object between the parallel jaws of the end effector when the beam is interrupted. Conversely, it detects the presence of an object in front of the end effector when a return beam is sensed. In both the FIS and the MIS manipulation experiments, the proximity sensor performs checks on the position of the mating element. The manipulation experiment is halted whenever the actual reading is different from that which is expected.

Proximity sensing usually involves the detection of a disturbance brought to a monitored signal by the presence/absence of an object within a specific volume around the sensing element. As opposed to range sensors, which provide the actual distance between the targeted object and the sensor, proximity sensors provide only a binary output, indicating if this distance is below or above a predetermined threshold distance. Most proximity sensors are either inductive, magnetic, capacitive, ultrasonic, or optical. Inductive proximity sensors detect the variation in a current resulting from the deformation of the flux lines of a permanent magnet caused by putting the sensor close to a ferromagnetic material. Magnetic proximity sensors are based either on the Hall effect or on Reed switches. The Hall effect is manifested when a ferromagnetic material is placed close to a semiconductor magnet (the sensing element in the proximity sensor). This causes an increase in the strength of the magnetic field, hence a corresponding increase in magnitude of the Lorentz force applied on the magnet and the voltage across the semiconductor. Magnetic proximity sensors that are based on Reed switches have two Reed magnets in a glass tube. When the sensor is brought in the vicinity of a magnetic material, the resulting magnetic field causes the contact to close, which turns on the sensor. Capacitive proximity sensors detect the beginning or end of oscillations due to a change in capacitance of a resonant circuit caused by the change in the permittivity of the dielectric material in the sensing capacitor. This change is induced by the close proximity of the sensed object. Optical proximity sensors detect the presence of an object by sensing the echo of a light beam generated by the sensor. A typical setup includes an infrared light emitter such as a solid-state light-emitting diode (LED) and an infrared light receiver such as a photodiode. Ultrasonic proximity sensors are in principle identical to optical sensors. An electroacoustic transducer (generally piezoelectric) emits a narrow acoustic beam, which gets reflected back if the sensor is placed in close vicinity of an object [2], [24]. This variety in sensing modalities yields an equal variety

VI. TOUCH SENSING THE INTERCHANGE FOR SYSTEMS

A . Background

A touch sensor can provide a robot with valuable information that can be crucial in many applications requiring recognition, acquisition, and manipulation of complex workpieces. Touch sensors presently used in industry are often simple and crude devices with limited capability. As with many other sensing modalities, the state of the art in touch sensing is still very primitive. Up until the 1970's, microswitches, pneumatic jets, and ON/OFF pressure-sensitive pads were the only tactile sensors in existence. They were primarily used as limit switches for collision detection. Since the 1980's, however, considerable effort has been devoted to the development of more sophisticated touch sensors and to the integration of these sensors in robotic systems 181, 1261. The historical lack of interest in touch and many other contact sensors may be attributed to an early belief that vision sensors could capture texture and shape information with such a fidelity that little or no additional information would be gained from touch sensing. Obviously, for most generalpurpose robotic systems operating in semistructured or unstructured environments, the state of the art of vision sensing and scene understanding points toward a need for added sensory capability. With touch sensing, there are no counterparts to unfavorable optical conditions such as shadows, occlusions, and variations in illumination, reflectivity, and color, which are sometimes so severe that they simply preclude the use of vision. The amount of processed data and the complexity

168

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION. VOL 6. NO. 2. APRIL 1990

w: l~ ;? ,

I j

:w
~

L . N s; z & o , l -

marily for inspecting the presence or absence of a workpiece between the end effector jaws. The manipulation experiment is halted whenever the actual reading is different from that which is expected. (The robot is also equipped with 16 x 10 and 80 x 80 gray-scale-responsive array touch sensors, which are not used in the FIS and MIS experiments.)

RESULTS VII. EXPERIMENTAL

In this section, we describe the autonomous mating and demating of the FIS and the MIS using the concepts presented in the previous sections. A . The FIS: Setup and Experiment

Fig. 9. Proximity sensor configuration [7].

of the interpretation methods are much higher in vision than in touch. Intrinsic shape and texture information is readily present in raw touch data, whereas complex and approximate algorithms are required to extract these features from vision data. Touch sensors as presently used in robotics vary from a single-point ON/OFF microswitches to arrays of gray-scaleresponsive elements, which measure the magnitudes of forces applied at each point. Tactile information is often inferred from force measurements [26]. Binary touch sensors, the simplest type, indicate only the presence or absence of an object. This is sometimes referred to as simple contact sensing or simple touch. Despite its simplicity, binary sensing provides valuable information. The interface of such sensors to a processor and the interpretation of the information are both relatively easy. For instance, a simple-contact element on the inside of each finger of a robot end effector determines the presence or absence of a workpiece between the fingers. Mounted on the outside surfaces of the gripper, a simple-contact touch sensor can also be used to stop the motion of a robot arm upon collision with an unexpected workpiece to avoid any damage to the workpiece or the gripper itself. This is possible only when the relative speed between workpiece and end effector is limited. The second type of sensor is gray-scale responsive to the force applied to each point. Devices that provide continuous sensing of forces in array fashion form are often referred to as artificial skins. In these sensors, a number of sensitive elements arranged in an array are mounted on the fingertip of a sensing probe or inside the robots fingers. Current array sizes vary from 5 x 5 to 16 x 16 elements per finger tip. The spatial resolution is often on the order of 0.1 in. These sensors offer a complexity and resolution comparable to the human touch. These arrays are often used for the recognition of small workpieces by extracting simple shape and texture features. Often, the techniques used for touch recognition are directly inspired from computer vision work. However, processing of tactile data is generally less complicated because it is not affected by the many variables that affect vision, such as shadows, reflections, and variations in illumination [7], [27]. All the systems developed thus far suffer from a combination of the following problems: 1) low sensitivity, 2) low dynamic range, 3) limited bandwidth, 4) lack of dc response, 5) hysteresis and nonlinearity, 6) temperature and pressure stability, 7) fragility and lack of flexibility, and 8) susceptibility to the external world [22], [26].

The setup used in this experiment was shown in Section I1 (Fig. 4). The FIS experiment involves determining the twodimensional position and orientation of the nozzle as well as the three-dimensional position and orientation of the light target then performing mating and demating of the interchange system. It is important to note that the experiment is fully autonomous from the moment the light targets are placed within the field of view to the end of the mating/demating operation. First, the robot moves the camera to a predefined position high above the mating elements area (Fig. 10) then turns on its own lights to determine the two-dimensional position of the nozzle. An image is acquired, histogram equalized, and thresholded [2] to extract the object (nozzle) from the background (table) (Fig. 1l(a) and (b)). The image acquired here is 128 x 128 in size. Using (6) and (7), the center of the area of the nozzle is computed. In the experiment described here, the center coordinates were found to be ( i l , j l ) = (72, 55). The parameters iI and j l are the row and column pixel coordinates, respectively. Using the camera geometry, the corresponding coordinates with respect to the world coordinate system are ( X I ,Y 1 , Z I ) = (14.02, 24.14, 4.75). The Z I coordinate, which is the height of the nozzle, is known. The dimensions are measured in inches with respect to the world coordinate system. Usually, this measurement is inaccurate and thus cannot be relied upon to pick up the nozzle. Next, the robot moves the camera closer, directly above the nozzle, since its approximate position is known (Fig. 12), and acquires another image of the nozzle to determine its position and orientation more accurately. This image is similarly processed (Fig. 1l(c) and (d)), and the pixel coordinates of the center of the area are computed. In this case, they are ( i 2 , j 2 ) = (76, 63). Using the camera geometry, the corresponding coordinates with respect to the world coordinate system are ( X 2 , Y 2 , Z 2 ) = (13.23, 24.18, 4.75). The Z2 coordinate, which is the height of the nozzle, is known. Using (9), the orientation is calculated to be 47.1 O with respect to the X axis. Once these values are known, the robot checks for the presence of the nozzle using the proximity and touch sensors. In this experiment, the use of the proximity and touch sensors is limited to comparing the sensor readings to the expected values in the vicinity of the nozzle. After these checks, the B . Experimental Setup robot picks up the nozzle (Fig. 13) and proceeds to locate the In this work, two microswitches are mounted inside the receptacle. To locate the receptacle, the robot moves the camera to a end effectors textured compliant surfaces. They are used pri-

ABlDl AND CONZALEZ. MULTISENSOR DATA FOR ROBOTIC MATING AND DEMATING IN SPACE

169

Fig IO

Vision system acquires an image of the mating element area to determine the two-dimensional position of the nozzle

Fig 14

Robot acquires an image of the FIS target

(Cl (d) Fig. 11. Top view of mating element area: (a) Original image; (b) thresholded image: bottom-close-up of nozzle: (c) Original image; (d) thresholded image.

Fig. 15. Image of FIS target: (a) With lights on; (b) with lights off; (c) difference image.

Fig 12.

Vision system acquires an image of the nozzle to determine its two-dimensional position and orientation
..

predefined position so that the target area is within the field of view of the camera (Fig. 14). It turns on the four indicator lights on the rack containing the receptacle, acquires an image of the scene (Fig. 15(a)), then acquires another image (Fig. 15(b)) after it turns off the FIS target. The two images are subtracted (Fig. 15(c)), and the resulting patterns are thinned [l] to find the position of the four lights (target) in the image. Their respective pixel coordinates and sensor readings in inches are as follows:
'
~

Point P O p,

46 82

P?
P,

81 45

28 29 63 61

+ 0.067
- 0.036

+ 0.135

+ 0.032
+ 0.037

+ 0.070

- 0.033

+ 0.044

Using the algorithm presented in Section 111, we find the three-dimensional position and orientation of the receptacle. The resulting position vector of the lower left corner of the target in this experiment is
Fig. 13. Robot proceeds to pick up the nozzle after proximity and touch sensing checks are passed.

~ 7y :, z ,l , p I , W ) , 7e
= (23.49, -19.74, 9.76, 62.4, 94.3, 90.9).

170

IEEE TRANSACTIONS ON ROBOTICS A N D AUTOMATION. VOL. 6. NO. 2. APRIL 1990

Fig 16

Robot acquires a close-up image of the FIS target

Fig 18

Robot proceeds to insert the nozzle in the receptacle with the forceitorque sensor active during all operations

--

Fig 17

Close-up image of light target: (a) Wlth lights on, (b) with lights off, (c) difference image

The parameters X y , Y : , and Z ? are in inches; 8 1 , 61, and cy are in degrees. At this step, the robot has an approximate idea regarding the position of the mating rack. To refine its measurement, the robot moves the camera closer to and directly in front of the receptacle (Fig. 16) then repeats the measurement of the three-dimensional position and orientation (Fig. 17). The resulting pixel coordinates for the four light targets 2re as follows:
Point

Y o , and Zo. By contrast, the correction in the angles is more significant. This was typical of this experiment. Finally, the robot proceeds to insert the nozzle (Fig. 18). The flared rim of the receptacle guides the nozzle into the receptacle during the FIS matingldemating operations by creating forces on the nozzle. As the nozzle is inserted, the stainless-steel pins enter the V notches, which further enhance the alignment of the nozzle. Once the nozzle is inserted, the pins reach the bottom of the V notches. The robot then rotates the nozzle clockwise by 15". While the nozzle is rotating, the pins enter the grooves in the receptacle, and the nozzle is then released. Under the effect of the forces exerted on it by the spring-loaded pressure plate, the nozzle locks into place and rests in the notches, which are located at the end of the grooves. The pressure plate holds the pins locked into the notches and holds the nozzle in place. The mating of the nozzle into the receptacle is completed (Fig. 19). The demating operation of the FIS is performed using the same steps as those used to determine the position of the receptacle. The demating part is simply the reverse action. In the future, the FIS will include a fluid-tight connector, which will consist of a male connector that is inside the flared rim, a pressure-plate assembly, and a female connector (the inner surface of the nozzle). The male connector will include an O-ring that will achieve the fluid-tight connection of the mated FIS. B . The MIS: Setup and Experiment
The goal here is to autonomously identify, remove, and replace a faulty or spent module by a new module from a tool rack. A mock-up of the MIS has been designed and built. The system is vision driven with the forceltorque, proximity, and touch sensors used as safety devices. The MIS hardware consists of a rack (which would be part of the space vehicle to be serviced) and an exchange module, which includes connectors for electronics, as well as a mechanical interface. To perform the module exchange, we utilize a standard nuclear instrumentation module (NIM) system. The MIS experiment involves determining the two-dimensional position arid orientation of the exchange module as well as the three-dimensional position and orientation of the light target attached to the mounting rack then performing insertion and removal of the interchange module. The experimental setup was shown in Fig. 2.

Y
-0.072 +0.138 +0.135 -0.075

Po

29
29 110 110

103
27 28
104

p,

P Z
p3

+0.116 10.116 -0.115 -0.115

The corresponding position and orientation of the receptacle

are calculated as

w;,

y ; , z;,

6 2 , 62, cyz)

= (23.21, -19.46, 9.43, 65.9, 94.3, 88.3). Note the low disparity between the two measurements for X o,

ABlDl AND GONZALEZ: MULTISENSOR DATA FOR ROBOTIC MATING AND DFMATING IN SPACE

171

APPENDIX I EFFECT OF
THE

TRANSFORMATION T

Fig. 19

Fluid interchange system (FIS). Robot completes the insertion of the nozzle into the receptacle.

In Section 111, the recovery of the matrix A was decomposed into steps involving transformations T , S , and C . Appendix I deals with the effect of the first transformation T. This transforms a general problem to an intermediate coordinate system in which the three world points are in what we shall define as standard position. Standard position is characterized by Po at the origin, P I on the positive X axis, and Pz in the first quadrant of the X Y plane (or on the positive Y axis). Before finding the matrix T, which transforms our three points to standard position, we will examine the effect of this step on the solution. Using the superscript CY to refer to our intermediate system, we may write Wp = T W , to describe the transformation of points in world coordinates to intermediate coordinates and W , = T-l Wp to describe the inverse transformation. Because our goal is to solve for the transformation matrix A , which will transform a point W , in world coordinates to W-F in camera coordinates, we write

Wg
Making the substitution

=AW,.

(49)

W i = T-'WP
yields

w g
Fig. 20
Module interchange system (MIS) Robot completes the insertion of the module into the mating rack

=AT-'w~
(50)

W,C = BWY

Using procedures similar to those described in the FIS experiment, the MIS mating and demating is performed autonomously and successfully. In Fig. 20, the robot is shown completing the insertion of the module in the mating rack. A more complete description of this system may be found in t281, ~ 9 1 . VIII. CONCLUSIONS In this paper, we described a robotic system that autonomously performs realistic manipulation operations suitable for space applications. This system is capable of acquiring, integrating, and interpreting multisensory data to locate, mate, and demate a fluid interchange system and a module interchange system. In both experiments, autonomous location of a guiding light target, mating, and demating of the system are performed. We implemented vision-driven techniques that determine the arbitrary two-dimensional position and orientation of the mating elements as well as the arbitrary three-dimensional position and orientation of the light targets attached to the mounting elements. The system is also equipped with a forceltorque, proximity, and touch sensor. Both experiments were successfully accomplished on mockups built for this purpose, regardless of the initial position of the end effector, mating element, and light targets. This method is also robust with respect to variations in the ambient light. This is particularly important because of the 90-min day-night shift in space.

where B = A T - ' . Now, the problem is to solve (50) for the matrix B, which transforms points from intermediate coordinates to camera coordinates. This requires the use of the original image coordinates and the new a-world coordinates. Afterward, we solve for A = BT to get the solution to the original problem. For any three noncolinear points, a linear transformation T always exists, which will put the points in standard position. The three points form a triangle that lies in the new X Y plane, and this triangle must contain at least two interior angles, which are acute or right. One of the corresponding points is chosen arbitrarily as Po; the others are labeled PI and P z . Because of the use of augmented spaces, the matrix T may be determined by using a concatenation of intermediate matrix transformations. These steps will require the following: 1) Translate the coordinate system so that Po is at the origin. 2) Pan about the 2 axis so that the Y coordinate of P I is zero and the X coordinate is positive using Re. 3) Tilt about the Y axis so that the Z coordinate of P I is zero using Rg. 4) Rotate about the X axis so that the 2 coordinate of Pz is zero and the X and Y coordinates are nonnegative using R, . We therefore write T = R,RaReTo and subsequently will determine these matrices.

172

IEEE TRANSACTIONS ON ROBOTICS AND AUTOMATION. VOL. 6. NO. 2 . APRIL 1990

The first transformation translates Po to the origin using matrix To. We use Wp = ( X ; ,Y;, Z;, 1)' = T O W ;to refer to the points after this translation. The matrix that describes this translation is 1 0 0

(Xf, Y f , Z f , 1)'

= Rg W e ,where

Ro

-Xo
-Yo

0 To
=

1 0

l o
(51)

11
(53)

0 0

-zo
1

In ideal position, the point P 2 is required to be on the X Y plane; consequently, z(:= X : sin p Z : cos = 0 , or

0 0 0

where X O ,YO, and ZOare the coordinates of Po in the world system. After a pan through an angle 8 about the Zo axis, the points are located at ( X e , Y $ ,Ze), and we may write W$ = Re Wp, where Re is given by

p =tan-' (-Z~/X;).

Finally, a rotation aboGt the X B axis by an angle a will result in

w; = ( x p , Yp, z p , 1)'
-1

=R,WP

- cos0
-sin8

sin8 cos0

0 0 0

1 0
R,
=

0
cosa -sina

0
sina
COSCY

00

Re =
0 0 0 0
1 0

0 0 -0

0
1-

11

x axis; consequently, 7'1

In ideal position, we require the point P 2 to be on the positive = -x? sin e Y? cos e = 0 , or

e = tan-'(Yy/Xy).

(52)

We add x to this value of 8 if X y is less than zero so that Xy is positive. In addition, if X y = Yy = 0, we may arbitrarily let 0 be equal to zero. Next, a tilt of p about the T' axis will result in We =

(55)

(56) (57) Add x to 8 if X I < X O .If X I = X O and Y I = YO, then 8 = 0. Add x to a if the denominator is less thai cero. Combining these operations into a single matrix, T = R,RgRoTo, which is given as follows:
COS

p cos e

cos p sin 8

-sin

- X o cos p cos 8 - Yo cos p sin 0 +ZOsin -XO( -cos a sin 8

-Yo (cos a cos 0

-cos a sin 8 +sin a sin p cos 0

cos a cos 0 +sin a sin p sin e

sin a cos @

+ sin sin p cos 8) + sin a sin p sin 0 )

CY

-Zo (sin a cos 6)

sin a sin 8 +cos a sin 0cos 0 -sinacos8 +cos a sin p sin 8 cos8cosp -Xo(sinasinO + c o s a s i n p c o s O ) -Yo( -sin a cos 8

+ cos CY sin
1

sin e)

-zo(cos e cos p )
0 0 0

ABIDl A N D GONZALEZ MULTISFNSOR DATA FOR ROBOTIC MATING A N D DFMATING Ih SPACL

173

We now have the coordinates ( X p , Yp, Z y ) for each of the points Pi in the a intermediate system and have thereby determined the transformation T completely through the matrix T. Note that for a given problem, this step needs to be applied only once since the results are not dependent on camera position. APPENDIX I1 EFFECT OF
THE

Next, we pan the camera with an angle 4 about the new

Y axis using the transformation [ c o s 4 0 -sin4

TRANSFORMATION S

As mentioned in Appendix I, the recovery of the matrix A was decomposed into steps involving transformations T , S, and C. Appendix I1 deals with the effect of the second transformation S. This preprocessing step must be performed on each image taken of the target. It involves another transformation, which describes how the image would look to a camera that has its lens center at the same location as the real camera but is looking directly at the origin (in the 01 system) and has its x axis in the same plane as the X axis of the 01 system. We will refer to this fictitious camera as being in ideal position and will call the image seen by such a camera an ideal image. Such an image is characterized by po at the origin and pi on the positive x axis. As was true in the first preprocessing step, the transformation is composed of several intermediate matrices concatenated to form a single transformation. Each transformation consists of a rotation of the camera about the lens center. A rotation about this point will have a computable effect on the image. Assuming initially that the fictitious and the real cameras are at the same position, we will use the following sequence of steps to move the fictitious camera into ideal position: 1) Shift the camera axes to the lens center using T i . 2 ) Pan the camera about the lens center so that po is on the image y axis using R4. 3) Tilt the camera about the lens center so that po falls on the origin using R,. 4) Roll the camera about its Z axis so that P I falls on the positive x axis of the image using R,. The complete transformation may be written as S = R,R,RJx. The first transformation moves the origin to the lens center:

t o 0 0 11 The three points are then located at W: = ( X : , Y?, Z:, 1) = R,Wf, and their images are at x = -hX:/Z: dnd ! y! = -XY!/Z?. We select 4 so that xt = 0. Therefore, we require X : = x,X cos 4 - z,X sin 4 = 0, or

R4=lo

01

sin4

cos4 O

O I . 0

The camera is then tilted with an angle w about its new x axis bringing po to the image origin. We may write W ; = ( X y , Yy, Z ; , 1) = R , W: to compute the new location of P I after the tilt transformation, which is described by

10
0

0
cosw -sinw

O sinw
cosw

Ol
0
11

lo

After applying this transformation, our image points move to

Since po will now move to the origin, we know that y t and therefore Y t are equal to zero. We have Y = Y $ cos w +Z$ : sin w = 0, or

voo

TA=

0 1 0

O 0

0 0 1 - x

1.

(-Y,x/Z,X)cosC#l= y,xcos4/x cos 4 / A

(59)

= y;

lo o o

1 1

w = tan- (yo cos$/h),

0 5 / w / < n/2. (65)

We use W? = ( X ? , Y f , Z?, I) = T A W ? to refer to the position of the points after this translation. Note that only the Z coordinates are changed, and the projective equations become

After the pan and tilt, a roll about the camera z axis with an angle p is performed to move p i to the positive x axis. This is accomplished using the transformation

-sinp

cosp

0 0

0
where xf and yf are the image coordinates of P, , namely p I.

11

174

IFFF TRANSACTIONS ON ROBOTICS AND AUTOMATION VOL 6, NO 2. APRIL 1990

The new coordinates of P , following this transformation are given by Wy = ( X p , Y p , Zp, I)' = RpWY. The image points are then described by

yp

h(S21X" S22Ya - s23h) S31X" +s32ya -S33h *

(73)

xp

-mp/zy

In summary, the three angles defining the transformation S are

o 5 141< x / 2
yy cos w

(74) (75)

yp = -hYp/Zy. Because we require that P I now move to the positive x axis, we know y: = 0; therefore, Yy = 0. Substituting, we Yy = -XL; p + Y y cos p = O sin

+ xy sin 4 sin w
X ~ C O

h cos 4 sin w +Asin4 S ~

~

(76)

Y: tanp = - = Y ; cosw X';'

+ (x:s i n 4 + Z: cos4) sinw

X ~ C O S ~ -Z:sin$

~~~~

~~

We will add a to p in the event that the sign ofx';' is negative so that P I moves to the positive x axis. We can show that the sign of xy is the same as the sign of

(e x;)(xyx; + yyy;
-

+ h2).

For later processing, S - ' instead of S is needed for the final solution. We find S - ' by writing S - ' = Th'R,lR;lR;'. Substituting in the appropriate matrices and combining yields the following matrix for S-I :

(69)

cos 4 cos p +sin 4 sin w sin p

-cos 4 sin p +sin 4 sin w cos p cos w cos p

sin 4 cos w

S-1 =

cos w sin p -sin 4 cos p +cos 4 sin w sin p

(77) sin4sinp +cos 4 sin w cos p cos4cosw h

11

This sequence of steps gives a means of computing the image of three points as seen by a camera in ideal position. It also provides us with the matrix S = RpR,R+Th, which describes the transformation involved in moving the real camera into ideal position. A point in the real camera system would therefore transform to Wp = S Wc in the ideal camera system. We know that the coordinates of a point in the ideal image are related to its coordinates in standard position by

Before we performed this step, our goal was to solve for the matrix B where Wc = BW" and the image corresponded to the real camera system. Substituting in Wc = S - ' Wp yields S P 1WP = BW". Therefore WP = SBW" = CW" (78)

xp

m /zp p

(70) (71)

where C = S B , or B = S-IC. We may now solve (78) for C using the ideal image coordinates of each point, then solve for B using B = S-IC, and then solve for A using A = B T .

y; = -xrp/z;.

The new image coordinates are related to the original image coordinates by

I11 APPENDIX
SOLVING FOR
THE

MATRIX C

As mentioned in Appendices I and 11, the computation of the matrix A was decomposed into steps involving transformations

ABIDl AND GONZALEZ. MULTISENSOR DATA FOR ROBOTIC MATING AND D E M A T I N G IN SPACE

172

T , S , and C. Appendix 111 deals with the computation of the transformation C. After having applied these two preprocessing steps (transformations T and s), we are left with a situation in which the camera is looking at the standard origin with its x axis in the same plane as the standard X axis. This was termed the ideal position. In addition, the world points are located at the origin, on the X axis, and in the X Y plane. This was termed standard position. We may now solve for the elements of the matrix C by writing (70) and (71) in terms of X p , Y p , and ZF through the use of (78). Because the transformation T A moved the center of the coordinate system to the center of the camera lens, our projective equations no longer have the X term in the denominator. With this in mind, writing (23) and (24) for each point and using the fact that we now have X g , Yg , Z g , Y y , Z;Y , Zq , xg , yg, and y: all equal to zero yields the following set of equations: hCI4 hC24

we therefore make the simplifying assumption that the three points form a right angle at Po, which would result in a closcdform solution to this problem. This means that X y is equal to zero, and (86) and (87), respectively, become

Combining (88)-(90) and (92), may write each variable in we terms of C I I as follows:

=o
=0

(79)

where s1 and s2 each has a value of f 1. Substituting into (80) (91) and squaring both sides yields

xx;c11 +XY;C12

+x;x;c31
x

+ x;y;c32 + hc14 + X ; c 3 4
xx;c21 + xy;c22

where (83)

+ y,Px;c31
+ y;c34 = 0.
(84)

al a2 a3

= xX;x; = XTxyx,P = XY7x:x;

f y;y?c32 f hC24

From this system of equations, we may obtain simpler expressions. Substituting the second equation into the fourth equation yields
C2I

a4 = Y : X ~ ( X ; ) ~ a5
=

(x,P)~

=o.

(85)

a6 = ( Y ; t
Equation (96) may be further expanded by carrying out the squares, multiplying both sides by (a5-ahC:,), and collecting terms

Dividing (81) by xy and subtracting (83) divided by x ; , we obtain

bl

+ b2C:l + b3C:l = C I I ( ~ ~ C :b5)sIJl I

-

CY,

(97) where Finally, dividing (83) by x ; and subtracting (84) divided by yg yields

61

= a5a2- a 3
-

b2 = a5a: - a5az +a:

b 3

ai

aia6

= a6a:

a6a:

Equations (86) and (87) represent two linear equations with five unknowns. Using (30)-(35), we may write three nonlinear equations involving the C; s:

b4 = 2ala2a6 b5 = 2asala2

+ 2a3a4.
(98)

Squaring each side again and collecting terms, we obtain

C I +c~C:I +

c:,+c:, 1 = c:, + c:, + c:, = 1

CllC12 + 3 c 2= 0. c 13

(88)

(89) (90)

c ~ C+ c ~ C +csC~I O ~I ~I =

where
C I = bi
~2 = 2blb2 - b:

A closed-form solution to this system of equations in its genera1 form could not be found. In the following discussion,

176
~3
~4

IE!E TK4NSACTIONS O N ROBOTICS A N D AUTOMATION. VOL. 6. NO. 2. APRIL 1990

= 26163
= 2b2b3

+ bi + b: + 2b4b5
-

2b4b5

b:

cs = b:

+ b:.

versities of Florida, Michigan, Tennessee, Texas, and the Oak Ridge National Laboratory) under grant DOE-DE-FG0286NE37968. The FIS and MIS experiments were carried out under contract number NAG8-630-BO 1 199962 with NASA, Marshall Space Flight Center. The efforts of Dr. R. 0. Eason and B. Bernhard in developing the robotic workstation and documenting the sensor acquisition and interpretation modules are sincerely acknowledged. T. Chandra and J. Spears have programmed the FIS and MIS experiments; their contributions are also sincerely acknowledged. Finally, sincere thanks are due to Dr. W. L. Green, F. Nola, J . Turner, and T. Bryan for their support in this work. Mrs. Janet Smith has typed several versions of this manuscript; her efforts are appreciated. REFERENCES
111 K. S. Fu, R. C. Gonzalez, and C . S . G. Lee, Introduction to Robotics: Control, Sensing, Vision, and Intelligence. New York: McGraw-Hill. 1987. 121 R. C. Gonzalez and P. Wintz. Digital Image Processing. 2nd ed. Reading, MA: Addison-Wesley, 1987. 131 A. Cohen and J . D. Erickson, Future use of machine intelligence and robotics for the space station and implications for the U . S . economy, IEEE J . Robotics Automat., vol. RA-1, pp. 117-123, Sept. 1985. 141 M. A. Bronez, Vision concepts for space-based telerobotics, in Proc. Remote Syst. Robotics Hostile Envir. Conf. (Pasco. WA), Mar. 1987, pp. 150-157. 151 R. E. Olsen and A. Quinn, Telerobotic orbital servicing technology development, in Proc. Remote Syst. Robotics Hostile Envir. Conf. (Pasco, WA). Mar. 1987, pp. 158-161. A. K. Bejczy, Sensors, controls, and man-machine interface for advanced teleoperation, Science, vol. 208, pp. 1327-1335, 1980. R. 0. Eason and R. C. Gonzalez, Design and implementation of a multisensor system for an industrial robot, Tech. Rep. TR-ECE86-23, Univ. Tennessee, Final Rep. Martin Marietta Energy Syst.. Contract 37B-07685C-07, Feb. 1986. A. C. Kak and J . S . Albus, Sensors f o r Intelligent Robots ( S . Y. Nof. Ed.) New York: Wiley, 1985, chap. 13. pp. 214-230. G. Beni and S. Hackwood (Eds.). Recent Advances in Robotics. New York: Wiley, 1985. A. Pugh (Ed.), Robot Sensors: Vision. New York: Springer, 1986, vol. I . A. lkonomopoulos. N . Ghani, G. Doemens, E. Kutzer, and N. Roth. Image processing and analysis in multisensory systems. IEEE Trans. Circuits Syst., vol. CAS-34, pp. 1417-1431, Nov. 1987. B. K. P. Horn, Robot Vision. New York: McGraw-Hill, 1986. R. M. Haralick, Using perspective transformation in scene analysis, Comput. Graphics Image Process., vol. 13, pp. 191-221, 1980. I. Fukui, TV image processing to determine the position of a robot vehicle, Patt. Recogn., vol. 14, pp. 101-109, 1981. M. J. Magee and J . K . Aggarwal, Determining motion parameters using intensity guided range sensing, in Proc. 7th Int. Conf. Patt. Recogn. (Montreal, Canada), July 1984, pp. 538-541. E. S . McVey, K. C. Drake, and R. M. Inigo. Range measurement by a mobile robot using a navigation line, IEEE Trans. Paft. Ana/. Mach. Intell., vol. PAMI-8, pp. 105-109. Jan. 1986. I171 M. R. Kabuka and A. E. Arenas, Position verification of a mobile robot using standard pattern, lEEE J . Robotics Automat., vol. RA3 . pp. 505-516, Dec. 1987. R . Y. Tsai, A versatile camera calibration technique for highaccuracy 3D machine vision metrology using off-the-shelf TV cameras and lenses. IEEE J . Robotics Automat., vol. RA-3, pp. 323-344, Aug. 1987. E. L. Hall, Computer Image Processing and Recognition. New York, Academic, 1979. R. 0. Eason, M . A. Abidi, and R. C . Gonzalez, A method for camera calibration using three world points, in Proc. Int. Conf. Syst. Man Cyber. (Halifax, Canada), Oct. 1984, pp. 280-289. M. A. Abidi, R. 0. Eason, and R. C. Gonzalez, Camera calibration in robot vision, in Proc. 4th Scand. Conf. Image Anal. (Trondheim, Norway). June 1985. pp. 471-478.

CII = I
Cl2 = s I s 2 x z p d h J ( x ; ) ?
c l 3

>

+ (y;)
-

($)2t2

= s2y;(t2 - 1 ) J ( X ; )

+ (U;)

(y;l2f2

c , 2

=o
I s * y ; m J ( x y

c 2 2 =s

+ Oq
-

(ypt2

(102)

c 2 3 = s2x; J(x;)2

+( y y

(y;)2t2

with I;I = 1. This completes the computation of the C matrix s relating ideal position to standard position. ACKNOWLEDGMENT

Our Robotics facilities are supported in part by the DOE University Program in Robotics for Advanced Reactors (Uni-

ABlDl AND GONZALEZ MULTISFNSOR DATA FOR ROBOTIC MATING Ah[) D k M A T l h G I N SPACk

177

1221 A. Pugh (Ed.), Robot Sensors: Tactile and Non-Vision. New York: Springer. 1986, vol. 2. 1231 0. Khatib, A unified approach for motion and force control of robot manipulators: The operational space formulation. IEEE J . Robotics Automat., vol. RA-3. pp. 43-53, Feb. 1987. A. C. Staugaard, Robotics and AI: A n Introduction to Applied Machine Intelligence. Englewood Cliffs, NJ: Prentice-Hall. 1987. W. G . Holzbock, Robotic Technology: Principles and Practice. New York: Van Nostrand Reinhold, 1986. L. D. Harmon, Tactile Sensing for Robots (G. Beni and S . Hackwood. Eds.). New York: Wiley, 1986. ch. 10. pp. 389-424. P. Allen and R. Bajczy. Object recognition using vision and touch. in Proc. 91h J . Conf. Artificial Intell. (Los Angeles, CA), Aug. 1985, pp. 1131-1137. M. A. Abidi, T. Chandra, R. C. Gonzalez. W. L. Green. and J . Spears, Multisensor system for autonomous robotic space manipulations, Tech. Rep. TR-ECE-87-24, Univ. Tennessee, Final Rep. NASA, Contract NAG8-630-B01199962. Nov. 1987. M . A. Abidi, W. L. Green, T. Chandra, and J . Spears, Multisensor robotic system for autonomous space maintenance and repair, in Proc. SPIE Conf Space Automat. I V (Cambridge, MA), Nov. 1988. pp. 104-1 14.

Dr. Abidi is a member of several honorary and professional societies. including Tau Beta Pi. Phi Kappa Phi, and Eta Kappa Nu.

M. A. Abidi (S81-M87) attended the Ecole Nationale dlngenieur de Tunis, Tunisia. from 1975 to 1981 and received the Principal Engineer Diploma and the First Presidential Engineer Award in 1981. He received the M.S. degree in 1985 and the Ph.D. degree in 1987 from the University of Tennessee. Knoxville, both in electrical engineering. In 1987, he joined the Department of Electrical and Computer Engineering at the University of Tennessee. His research and teaching interests include digital signal processing, image processing. and robot sensing. He has published over 30 papers in these areas.

R. C. Gonzalez (S6S-M70~SM7S-F83) received the B.S.E.E. degree from the Universit). of Miami in 1965 and the M.E. and Ph.D. degrees in electrical engineering from the University of Florida. Gainesville. in 1967 and 1970, respectively. He was affiliated with the GT&E Corporation and the Center for Information Research at the University of Florida, NASA, and is presently President of Perceptics Corporation and Distinguished Service Professor of Electrical and Computer Engineering at the University of Tennessee, Knoxville. He is a frequent consultant to industry and government in the areas of pattern recognition. image processing, and machine learning. He received the 1978 UTK Chancellors Research Scholar Award, the 1980 Magnavox Engineering Professor Award, and the 1980 M.E. Brooks Distinguished Professor Award for his work in these fields. He was named Alumni Distinguished Service Professor at the University of Tennessee in 1984. He was awarded the University of Miamis Distinguished Alumnus Award in 1985, the 1987 IEEE Outstanding Engineer Award for Commercial Development in Tennessee, and the 1988 Albert Rose National Award for Excellence in Commercial Image Processing. He is also the recipient of the 1989 B. Otto Wheeley Award for Excellence in Technology Transfer and the 1989 Coopers and Lybrand Entrepreneur of the Year Award. He is coauthor of Pattern Recognition Principles. Digital Image Processing, and Syntactic Pattern Recognition: An Introduction ( Addison-Wesley). and of Robotics: Control, Sensing, Vision and Intelligence (McGraw-Hill). Dr. Gonzalez is an Associate Editor for the IEEE TRANSACTIONS O N SISTEMS, M.k\, AND CmmvETics and the International Journal of Computer and Information Sciences and is a member of several professional and honorary wcieties. including Tau Beta Pi, Phi Kappa Phi. Eta Kappa Nu. and Sigma X I .