 |
Description  |
|
|
BACKGROUND OF THE INVENTION
I. Field of the Invention
The present invention relates to a sensor and related method for measuring
the position and orientation (POSE) of an object.
II. Background Information
To measure the actual POSE of robotic and effectors and the like, various
sensors have been proposed. For example, one proposed sensor contemplates
the utilization of a target cube positioned on the end effector of a
robot. The target cube is used in connection with a hollowed-out tube
detector which contains in each wall thereof a trio of contact dial
gauges. The robot end effector target cube is moved into contact with the
gauges in the interior of the hollow cube detector. Upon contact with the
gauges, the actual position and orientation (POSE) of the target cube may
be calculated from the output of the gauges and compared against the
expected position of the target cube.
Such prior art sensors suffer from the obvious defect of requiring physical
contact between a target cube on the end effector and the detector cube.
Such contact may actually alter the position of the end effector and
thereby introduce error into the resultant POSE measurements.
Non-contact optical sensing systems have been developed to determine
position but not both position and orientation. Thus, such optical systems
can determine where the end effector of a robotic device is, but cannot
determine the orientation of that and effector and, hence, can provide
only limited information upon which robotic performance may be improved.
Non-contact sensing devices employing eddy current detection are also
known. However, like the known optical systems, such eddy current systems
can only measure the position of the face of a target cube and then, can
only measure that position if surfaces of the target cube are oriented
approximately parallel to the surfaces of the detector cube in which the
eddy current device is mounted. Accordingly, such systems have severe
limitations in that if the target cube was not moved accurately into the
detector cube in an approximately parallel orientation, substantial error
is incurred in the resultant position measurement.
It is, accordingly, a primary object of the subject invention to provide a
non-contact sensor which accurately measures both the position and
orientation of an object, such as the end effector of a robotic device.
A further object of the subject invention is to provide such a sensor which
does not require any electronic parts to be mounted on the object to be
measured and, therefore, causes no loading to the object and eliminates
the need to provide an electrical interface to the object.
A further object of the subject invention is to provide a sensor which
measures all six degrees of freedom of a standard commercial manipulator
and effector with substantial accuracy and range of measurement in a
relatively inexpensive manner.
Additional objects and advantages of the invention will be set forth in the
description which follows and will, in part, be obvious from the
description or may be learned by practice of the invention.
SUMMARY OF THE INVENTION
To achieve the foregoing objects, and in accordance with the purposes of
the invention as embodied and broadly described herein, a sensor for
measuring the position and orientation (POSE) of an object relative to the
POSE of a base is provided which comprises: a target fixed to either the
object or base and including three manually intersecting planar surfaces;
means, coupled to the other of the object and base, for directing a pair
of light beams onto each of the three planar surfaces; and means, coupled
to the other of the object and base, for detecting the reflection position
of the resultant reflections of each of the light beams, the reflection
positions thereby being available to determine the POSE of the object
relative to the POSE of the base.
Preferably the subject sensor further includes the actual means for
determining the POSE of the target as a function of the reflection
positions. It is also preferable that the planes of the target be mutually
perpendicular to each other and that the means for detecting include an
additional three mutually perpendicular planar surfaces and a
two-dimensional light sensitive position detector located on the inside
facing surfaces of each of the additional planar surfaces.
It is still further preferable that the means for detecting comprise three
pair of light sources, with one pair mounted on each of the inside facing
surfaces of each of the additional planar surfaces. The light sources of
each of the pair are preferably positioned on axes in the inside-facing
surfaces orthogonal to each other and which have their origin at the
center of the corresponding detector.
The subject invention further contemplates a corresponding method
comprising the steps of shining two light beams onto each of three
mutually intersecting reflective planes fixed to either the object or the
base; detecting the resultant reflection positions of the light beams; and
calculating the POSE of the object as a function of the reflection
positions. This method preferably includes the additional step of
utilizing the outputs of a plurality of light sensitive position detectors
during the step of detecting.
The method of the subject invention also preferably includes the additional
steps of calibrating the light sensitive position detectors, prior to the
step of utilizing, by shining beams of light onto the detectors at known
positions and thereby detecting error signals and by modifying the outputs
of the light sensitive position detectors during the step of utilizing
those detectors as a function of the error signals.
It is still further preferable that the method of the subject invention
include the additional steps of measuring the actual alignment of the
light beams, prior to the step of detecting, by measuring the actual POSE
of the intersecting reflective planes by utilizing a coordinate measuring
technique; measuring the actual reflection positions of the light beams
with the reflective planes in the measured POSE; determining the expected
reflection positions of the light beams with the reflective planes in the
measured POSE; calculating the difference between the actual position and
alignment of the light beams and the expected position and alignment of
the light beams as a function of the difference between the actual
reflection positions and the expected reflection positions and adjusting
the results of the step of calculating as a function of the difference
between the actual position and alignment of the light beams and the
expected position and alignment of the light beams.
BRIEF DESCRIPTION OF THE DRAWINGS
FIG. 1 discloses the relationship between a target cube employed in
accordance with the teachings of the subject invention and a base
coordinate system;
FIG. 2 illustrates a robotic hand fixture whose POSE is to be measured in
accordance with the teachings of the present invention;
FIG. 3 illustrates a detector fixture incorporating the teachings of the
subject invention;
FIG. 4 illustrates the relationship between the POSE of mutually
intersecting surfaces employed in the detector of FIG. 3 and the base
coordinate system illustrated in FIGS. 1 and 2;
FIG. 5a is a side view of the detector of FIG. 3 taken along line V.sub.a
--V.sub.a ;
FIG. 5b is a top view of the detector of FIG. 3 taken along line V.sub.b
--V.sub.b ;
FIG. 6 is a perspective view of one planar surface of the detector of FIG.
3;
FIG. 7 is a side view of a light source suitable for use in connection with
the detector of FIG. 3;
FIG. 8 illustrates the geometric relationship between the light beams of
the detector of FIG. 3 reflected from a target cube of FIGS. 1 and 2;
FIG. 9 is a perspective view of a coordinate measuring machine employed to
calibrate the sensor of the subject invention;
FIG. 10 illustrates the positioning of one plate of the detector fixture of
FIG. 3 in the work space of a conventional coordinate measuring machine
for calibration purposes using a six degree of freedom kinematic mount.
FIG. 11 is a position accuracy plot for a position sensitive device which
may be utilized in accordance with the teachings of the subject
convention;
FIG. 12 is a position accuracy plot for a position sensitive device which
is more accurate than the device corresponding to the plot of FIG. 11;
FIG. 13 illustrates a calibration set up for the sensor of the subject
invention;
FIGS. 14a-c illustrate light source alignment errors;
FIG. 15 is a block diagram of an illustrative circuit for the sensor of the
subject invention; and
FIG. 16 illustrates the output of either the sum and difference stages of
the circuit of FIG. 15.
DESCRIPTION OF THE PREFERRED EMBODIMENT
Reference will now be made in detail to the present preferred embodiment of
the invention as illustrated in the accompanying drawings.
In FIG. 1 there is illustrated a target in the form of a cube 10 which
includes three mutually intersecting planar surfaces 12, 14 and 16 which
are mutually perpendicular to one another. As illustrated in FIG. 1,
target cube 10 has a plurality of edges, each of which has a length H.
Target cube 10 also has a corner C which is the intersection of planar
surfaces 12, 14 and 16.
Cube 10 preferably is on the order of 4.times.4.times.4 centimeters, with
surfaces 12, 14 and 16 being highly reflective and smooth. Cube 10 may
either be mounted on a specifically dedicated end effector or can be part
of a robot's general end effector. Although it is preferable that the
target of the subject invention be arranged in the form of a cube, it
should be understood that all that is required in accordance with the
generic concepts of the subject invention is a target which consists of
three intersecting reflective planes. These planes need not be mutually
perpendicular. Also the surfaces can be arranged in a concave manner
rather then the convex arrangement in FIG. 1.
FIG. 1 also illustrates a first base coordinate system (X.sub.M, Y.sub.M,
Z.sub.M) which preferably is fixed in position and orientation with regard
to the detector base fixture. The frame (X.sub.M, Y.sub.M, Z.sub.M) can be
in an unknown position and orientation relative to the robot base. For
example, in FIG. 2 there is illustrated a robotic device 18 having a base
20 aligned with a second base coordinate system (X.sub.B, Y.sub.B,
Z.sub.B). There is also illustrated in FIG. 2 a detector base coordinate
system (X.sub.M, Y.sub.M, Z.sub.M) which is positioned and oriented in a
fixed or unknown relationship with regard to base coordinate system
(X.sub.B, Y.sub.B, Z.sub.B). As will be discussed in more detail below,
the detector base coordinate system (X.sub.M, Y.sub.M, Z.sub.M) is
preferably positioned and oriented within a detector fixture which
incorporates the teachings of the present invention.
There is also illustrated in FIG. 2 a hand fixture 22 of robotic device 18
which has fixed upon it target cube 10 from FIG. 1.
It is an object of the present invention to provide a sensor which
accurately and effectively, and without contact, permits measurement of
the position and orientation of target cube 10 and, hence, hand fixture
22, with regard to the detector base coordinate system (X.sub.M, Y.sub.M,
Z.sub.M).
Returning to FIG. 1, the position and orientation of cube 10 with regard to
coordinate system (X.sub.M, Y.sub.M, Z.sub.M) may be defined in many
different ways. By way of example and not limitation, the position of
target cube 10 will be defined hereinafter by the location of point C of
cube 10 in coordinate system (X.sub.M, Y.sub.M, Z.sub.M). As used
hereinafter, the particular position of corner C of target cube 10 will be
designated (.sub.C X.sub.M, .sub.C Y.sub.M, .sub.C Z.sub.M). Of course,
obtaining the values (.sub.C X.sub.M, .sub.C Y.sub.M, .sub.C Z.sub.M) with
regard to coordinate system (X.sub.M, Y.sub.M, Z.sub.M) merely locates the
position of corner C, but in no way defines the orientation of cube 10
about corner C. Such orientation of cube 10 about corner C may be defined
using a variety of different techniques. By way of illustration and not
limitation, the orientation of cube 10 will be defined hereinafter by
three vectors n, s, and a. Each of vectors n, s, and a are unit vectors
normal to planar surfaces 12, 14 and 16, respectively. Thus, the
orientation of vectors n, s and a and, hence, the orientation of cube 10
may be defined by the direction cosines b.sub.x1, b.sub.y1, b.sub.z1 for
surface 12; b.sub.x2, b.sub.y2, b.sub.z2 for surface 14; and b.sub.x3,
b.sub.y3 and b.sub.z3 for surface 16.
In accordance with the present invention, a target including three mutually
intersecting planar surfaces, such as cube 10 illustrated in FIG. 1, is
fixed to the object whose POSE is to be measured. Furthermore, in
accordance with the present invention, there is provided a base and means
coupled to that base for directing a pair of intersecting light beams onto
each of the planar surfaces of the target.
As illustrated in FIG. 3, there is shown a detector fixture 30 which may be
mounted at a fixed or unknown position and orientation relative to base 20
of FIG. 2 and which includes three additional mutually perpendicular
planar surfaces 32, 34 and 36, a two-dimensional light position sensitive
detector 38, 40 and 42 located on the inside-facing surface of each planar
surface 32, 34 and 36, respectively, and three pair of light sources 42a,
42b, 44a, 44b, and 46a, 46b, with one pair of light sources 42a, 42b
mounted on the inside-facing surface of planar surface 32, one pair of
light sources 44a, 44b mounted on the inside-facing surface of planar
surface 34, and one pair of light sources 46a, 46b mounted on the
inside-facing surface of planar surface 36. As will be discussed in more
detail below, each of light source pairs 42a, 42b, 44a, 44b, and 46a, 46b
are aligned so as to direct a pair of intersecting light beams onto a
respective planar surface 12, 14 and 16 of cube 10 (FIG. 1) as cube 10 is
moved into proximity with detector fixture 30. Furthermore, as will be
described in more detail below, position detectors 38, 40 and 42 detect
the reflection positions of each of the light beams from sources 42a, 42b,
44a, 44b, and 46a, 46b and these reflection positions are available to
determine the POSE of cube 10 relative to the POSE of detector 30 and,
hence, relative to the POSE of the detector base coordinate system
(X.sub.M, Y.sub.M, Z.sub.M).
In order to explain how the detection of the reflection points by position
detectors 38, 40 and 42 may be utilized to determine the POSE of cube 10,
the basic relationships between planar surfaces 32, 34 and 36 and
coordinate system (X.sub.M, Y.sub.M, Z.sub.M) must first be defined. In
this regard, attention is directed toward FIG. 4 wherein the base
coordinate system (X.sub.M, Y.sub.M, Z.sub.M) of FIGS. 1 and 2 is
illustrated as being positioned within the interior of the hollow detector
cube defined by the inside surfaces of planar surfaces 32, 34 and 36 of
detector fixture 30. As shown in FIG. 4, each surface 32, 34 and 36 has
(x.sub.i, y.sub.i, z.sub.i) coordinate system. Each of coordinate systems
(x.sub.i, y.sub.i, z.sub.i) are orthogonal to base coordinate system
(X.sub.M, Y.sub.M, Z.sub.M) in the sense that each axis of coordinate
systems (x.sub.i, y.sub.i, z.sub.i) is orthogonal to two axes of base
coordinate system (X.sub.M, Y.sub.M, Z.sub.M). In addition, each of
coordinate system (x.sub.i, y.sub.i, z.sub.i) is illustrated in FIG. 4 as
having a Z-axis which is aligned with the X.sub.M, Y.sub.M and Z.sub.M
axes of the base coordinate system, respectively. Moreover, each of
coordinate systems (x.sub.i, y.sub.i, z.sub.i) has an origin displaced a
distance d.sub.0 from the origin of base coordinate system (X.sub.M,
Y.sub.M, Z.sub.M).
Using coordinate systems (x.sub.i, y.sub.i, z.sub.i) of planar surfaces 32,
34 and 36, the position and orientation of detectors 38, 40 and 42 and
light sources 42a, 42b, 44a, 44b, and 46a, 46b may be explained with
reference to FIGS. 5a, 5b, and 6. Specifically, as shown in these figures,
each position detector 38, 40 and 42 is aligned to lie in the XY plane of
a respective coordinate system (x.sub.i, y.sub.i, z.sub.i) and is
positioned to have its center located approximately at the origin of that
coordinate system. In addition, each pair of light sources 42a, 42b, 44a,
44b and 46a, 46b is positioned along the X and Y axes a distance X.sub.e
and Y.sub.e from the origin of the respective (x.sub.i, y.sub.i, z.sub.i)
coordinate systems. For example, as shown in FIG. 5a and FIG. 6, light
source 44a is located a distance X.sub. e from the origin or coordinate
system (x.sub.2, y.sub.2, z.sub.2) and in FIGS. 5b and 6 a light source
44b is shown positioned along the Y axis a distance Y.sub.e from the
origin of coordinate system (x.sub.2, y.sub.2, z.sub.2). As may also be
seen in FIGS. 5a, 5b and 6, each of the light sources is aligned an angle
.theta..sub.B from normal to the corresponding position sensor surface to
intersect the Z axis of corresponding (x.sub.i, y.sub.i, z.sub.i)
coordinate system at (0, 0, z.sub.m). Light sources 42a, 42b, 44a, 44b and
46a, 46b preferably are attached to a light emitting diode (LED)-coupler
arrangement as shown in FIG. 7. The LED-coupler is used to couple light
into a fiber cable which carries the light to a collimator lens comprising
light sources 42a, 42b, 44a, 44b, 46a, 46b. Specifically, the LED-coupler
coupled to light source 44a is illustrated in FIG. 7 as including an LED
50, lenses 52 and 54, and an optical fiber 56. Collimator lenses
comprising light sources 42a, 42b, 44a, 44b, 46a, 46b are preferably
commercially available SELFOC lenses which have a beam collimation error
of less than one degree.
It is also preferable that angle .theta..sub.B of FIGS. 5a, 5b and 6 be
approximately 37 degrees.
As is illustrated in FIG. 6, a Light Beam No. 1 from light source 44a is
directed toward a surface 14 of target cube 10 and is reflected from that
surface back to position sensitive detector 40, striking detector 40 at a
location (x.sub.r1, y.sub.r1). Likewise, light source 44b provides a Light
Beam No. 2 which strikes surface 14 of cube 10 and is thereby reflected
back to position sensor detector 40, striking position sensitive detector
40 at a location (x.sub.r2, y.sub.r2). It should be understood that
similar reflections occur with regard to light sources 42a and 42b off of
surface 12 and from light sources 46a, 46b off reflective surface 16 of
target cube 10 to provide corresponding reflection positions (x.sub.r1,
y.sub.r1) and (x.sub.r2, y.sub.r2) for each of position sensitive
detectors 38, 40, 42.
In accordance with the present invention, a mechanism and corresponding
method is provided for determining the POSE of a target as a function of
the reflection positions determined by the position sensitive detectors of
the subject invention. In order to fully appreciate the mechanism and
method involved, the mathematics involved will be set forth below.
Specifically, the following mathematical discussion begins with a
determination of the direction cosines of a face of the target cube,
continues with a determination of the distance from such a face to the
hollow cube detector fixture and concludes with an analysis of the
relative POSE of the target cube in relationship to the POSE of the base
as defined by the detector base coordinate system (X.sub.M, Y.sub.M,
Z.sub.M). Although the following mathematical analysis is provided in
order to provide a more complete understanding of the subject invention,
it should be understood that this analysis is being provided by way of
example and not limitation, it being understood that the basic principles
disclosed by the following analysis may be presented and utilized through
variations which come within the spirit and scope of applicant's
invention.
DETERMINATION OF THE DIRECTION COSINES OF A FACE OF THE TARGET CUBE
A unit normal vector to a face of target cube 10 is written in face
coordinates as:
##EQU1##
Coefficients b.sub.x, b.sub.y and b.sub.z are the direction cosines of the
plane. The direction cosines are easily determined by observing in FIG. 8
that the plane 60 passing through the points (x.sub.e, 0, 0), (0, 0,
z.sub.m) and (x.sub.r1, y.sub.r1, 0) and the plane 62 passing through the
points (0, -y.sub.e, 0) (0, 0, z.sub.m) and (x.sub.r2, y.sub.r2, 0)
iintersect in a line 64 which is normal to the corresponding surface of
target cube 10.
The points (x.sub.e, 0, 0), (0, 0, z.sub.m) and (x.sub.r1, y.sub.r1, 0)
determine the plane:
##EQU2##
The points (0, -y.sub.e, 0), (0, 0, z.sub.m) and (x.sub.r2, y.sub.r2, 0)
determine the plane:
##EQU3##
The intersection of the planes given by Equations (2) and (3) is the line:
##EQU4##
The direction cosines of the intersection line 64 (which are also the
direction cosines of the normal to a face of target cube 10) are
calculated using only the detected reflection position coordinates
(x.sub.r1, y.sub.r1) and (x.sub.r2, y.sub.r2), and the geometric constants
x.sub.e, y.sub.e and z.sub.m.
DETERMINATION OF THE DISTANCE TO A FACE OF THE TARGET CUBE
The distance of d.sub.F between a face of the target cube and a
corresponding face of detector fixture 30, FIG. 3, along the z axis of a
face coordinate system can be determined from the reflection position
coordinates if the direction cosines of the normal to the target cube face
are known. The position where a reflected beam falls on the position
sensitive detector is written as a function of the orientation of the
target cube face's normal and the distance, d.sub.F to the face. In the
resulting expresson, d.sub.F is the only unknown. Only one measured
position is required to solve for d.sub.F, so there are three more
equations than are required. However, for computational reasons, as will
be seen below, the equation resulting from one of the two reflected beam
positions x.sub.r1 or y.sub.r2 is the best choice to solve for d.sub.F. To
determine where the reflected beam falls on a position sensitive detector,
it is first necessary to determine the path of the light beam. The
following analysis is presented for Light Beam No. 1 and the results for
Light Beam No. 2 are also included. The equation of the emitted Light Beam
No. 1 of FIG. 6 is:
(x-x.sub.E)=-tan z .theta..sub.B (6)
The equation of the target cube face plane in the face coordinate frame in
terms of the knowns b.sub.z,b.sub.y,b.sub.z and the unknown, d.sub.f, is
written as:
b.sub.x x+b.sub.y y+b.sub.z (z-d.sub.F)=0 (7)
Coefficients b.sub.x, b.sub.y, b.sub.z are the direction cosines of the
normal to the target cube face in the face coordinate system and x, y, z
are the coordinates of the face coordinate system.
The intersection of Light Beam No. 1 of FIG. 6 with the face of the target
cube is found by solving Equations (6) and (7) simultaneously and by using
the face that the intersection occurs in the y=0 plane. The intersection
point (x.sub.F1, y.sub.F1, z.sub.F1) is given by:
##EQU5##
x.sub.F1 =-z.sub.F1 tan .theta..sub.B +x.sub.E (8b)
y.sub.F1 =0 (8c)
Using the fact that the angle of incidence equals the angle of reflection,
the direction cosines of the reflected beam (v.sub.x1, v.sub.y1, v.sub.z1)
can be found using a known method:
v.sub.x1 =(2 sin .theta..sub.B)b.sub.x.sup.2 -(2 cos .theta..sub.B)b.sub.x
b.sub.z -sin .theta..sub.B (9a)
v.sub.y1 =(2 sin .theta..sub.B)b.sub.x b.sub.y -(2 cos
.theta..sub.B)b.sub.y b.sub.z (9b)
v.sub.z1 =-(2 cos .theta..sub.B)b.sub.z.sup.2 +(2 sin .theta..sub.B)b.sub.x
b.sub.z +cos .theta..sub.B (9c)
Using Equations (8a-c) and (9a-c), the equation of the reflected Light Beam
No. 1 is found as:
##EQU6##
The intersection of the reflected beam with the z=0 plane (a position
sensitive detector) is found by substituting z=0, x=x.sub.r1 and
y=y.sub.r1 into Equation (10) resulting in:
##EQU7##
The reflection position coordinates (x.sub.r1, y.sub.r1) are measured by a
position sensitive detector. Expressions similar to Equations (11a) and
(11b) result for x.sub.r2 and y.sub.r2 when Light Beam No. 2 is used:
##EQU8##
where:
v.sub.x2 =-(2 sin .theta..sub.B)b.sub.x b.sub.y -(2 cos
.theta..sub.B)b.sub.x b.sub.z (13a)
v.sub.y2 =-(2 sin .theta..sub.B)b.sub.y.sup.2 -(2 cos .theta..sub.B)b.sub.y
b.sub.z +sin .theta..sub.B (13b)
v.sub.z2 =-(2 cos .theta..sub.B)b.sub.z.sup.2 -(2 sin .theta..sub.B)b.sub.y
b.sub.z +cos .theta..sub.B (13c)
##EQU9##
y.sub.F2 =z.sub.F2 tan .theta..sub.B -y.sub.E (14b)
x.sub.F2 =0 (14c)
When Equations (8a-c) are substituted into Equation (11a) and the resulting
equation is solved for d.sub.F, the following expression results:
##EQU10##
When Equations (8a-c) are substituted into Equation (11b) and the resulting
equation is solved for d.sub.F, the following expression results:
##EQU11##
When Equations (14a-c) are substituted into Equation (12a) and the
resulting equation is solved for d.sub.F, the following expression
results:
##EQU12##
When Equations (14a-c) are substituted into Equation (12b) and the
resulting equation is solved for d.sub.F, the following expression
results:
##EQU13##
The preferred operating mode of the sensor of the subject invention is with
the faces of the detector fixture 32 and the faces of the target cube 10
approximately parallel. Therefore the direction of the normal to the
target cube faces are approximately parallel to a face coordinate system Z
axis and the following conditions are valid:
.vertline.b.sub.z .vertline.>>0 (19a)
.vertline.v.sub.z2 .vertline.>>0 (19b)
.vertline.v.sub.z2 .vertline.>>0 (19c)
But v.sub.x1, v.sub.y1, v.sub.x2 and v.sub.y2 are normally very small and
could be zero. Thus, Equations (16) and (17) should not be used to solve
for d.sub.F because division by zero could result.
Equation (15) can be used to solve for d.sub.F except when:
v.sub.x1 /v.sub.z1 =-tan .theta..sub.B (20)
Equation (20) is satisfied only when the reflected beam lies in the plane
formed by the line x=x.sub.E and the point (0,0,z.sub.m). A beam reflected
in this manner is not incident on the position sensitive detector. Thus,
Equation (15) is valid within the entire sensor range.
Equation (18) can be used to solve for d.sub.F except when:
v.sub.y2 /v.sub.z2 =-tan .theta..sub.B (21)
Equation (21) is satisfied only when the reflected beam lies in the plane
formed by the line Y=-Y.sub.E and the point (0,0,z.sub.m). A beam
reflected in this manner is not incident on the position sensitive
detector. Thus Equation (18) is valid within the entire sensor range.
Either Equation (15) or (18) can be used to solve for d.sub.F. This
redundancy is exploited by averaging the two solutions for d.sub.F to
improve the accuracy of the sensor.
RELATIVE POSE OF THE TARGET CUBE
The direction cosines and the distances between the face of the target cube
and the face of the detector fixture are used to write equations for the
three faces of the target cube in the associated face coordinate frame.
The three equations are transformed to the detector base frame and are
used to determine the pose of the target cube relative to the detector
base frame (X.sub.M, Y.sub.M, Z.sub.M).
The relationship between each of the three target cube face coordinate
systems (x.sub.i, y.sub.i, z.sub.i) and the base coordinates (x.sub.M,
y.sub.M, z.sub.M) are known (see FIG. 4). These relationships are given in
standard transformation matrix form by: Face #1:
##EQU14##
The direction cosines of the vector n in FIG. 1 (n.sub.x, n.sub.y,
n.sub.z) in base frame (X.sub.M, Y.sub.M, Z.sub.M) are found by
transforming the direction cosines of target face 12 (b.sub.x1, b.sub.y1,
b.sub.z1) to the base frame using Equation (22a):
n.sub.x =-b.sub.z1 (22b)
n.sub.y =-b.sub.x1 (22c)
n.sub.z =b.sub.y1 (22d)
Face #2:
##EQU15##
The direction cosines of the vector s in FIG. 1 (s.sub.x, s.sub.y,
s.sub.z) in base frame (X.sub.M, Y.sub.M, Z.sub.M) are found by
transforming the direction cosines (b.sub.x2, b.sub.y2, b.sub.z2) of
target face 14 to the base frame using Equation (23a):
s.sub.x =b.sub.y2 (23b)
x.sub.y =-b.sub.z2 (23c)
s.sub.z =-b.sub.x2 (23d)
Face #3:
##EQU16##
The direction cosines of the vector a in FIG. 1 (a.sub.x, a.sub.y,
a.sub.z) in base frame (X.sub.M, Y.sub.M, Z.sub.M) are found by
transforming the direction cosines of target face 16 (b.sub.x3, b.sub.y3,
b.sub.z3) to the base frame using Equation (24a):
a.sub.x =-b.sub.x3 (24b)
a.sub.y =b.sub.y3 (24c)
a.sub.z =-b.sub.z3 (24d)
In the above equations d.sub.oi is the distance from each face of the
detection fixture to the detection mixture frame origin. A practical
choice is to make do.sub.i all equal to a fixed value d.sub.o.
By using the relationships expressed in Equations (22a-d), (23a-d) and
(24a-d), the equation of each of the target cube fixture faces expressed
by Equation (7) are written in the detector fixture frame as:
Face 12:
n.sub.x (x.sub.M -d.sub.o +d.sub.F1)+n.sub.y y.sub.M +n.sub.z z.sub.M =0
(25a)
Face 14:
s.sub.x x.sub.M +s.sub.y (y.sub.M -d.sub.o +d.sub.F2)+s.sub.z z.sub.M =0
(25b)
Face 16:
a.sub.x x.sub.M +a.sub.y y.sub.M +a.sub.z (z.sub.M -d.sub.o +d.sub.F3)=0
(25c)
The intersection of these three planes is found by solving Equations
(25a-c) simultaneously. The intersection point (.sub.c x.sub.M, .sub.c
y.sub.M, .sub.c z.sub.M), and therefore the position vector of the corner
C of the targer cube, is given in the base detector frame by:
##EQU17##
Where:
##EQU18##
DET=n.sub.x (s.sub.y a.sub.z -a.sub.y s.sub.z)+n.sub.y (a.sub.x s.sub.z
-s.sub.x a.sub.z)+n.sub.z (s.sub.x a.sub.y -a.sub.x s.sub.y) (26e)
L=n.sub.x (d.sub.o -d.sub.F1) (26f)
M=s.sub.y (d.sub.o -d.sub.F2) (26g)
N=a.sub.z (d.sub.o -d.sub.F3) (26h)
The position of the target cube frame origin (see FIG. 1) is given by:
##EQU19##
where:
##EQU20##
Substituting Equations (28) and (26a) into Equation (27) gives the
expressions for .sub.p x.sub.M, .sub.p y.sub.M and .sub.p z.sub.M in the
detector fixture frame:
##EQU21##
The POSE of the target cube frame coordinate system
(x.sub.h,y.sub.h,z.sub.h) oriented with the n, s, a vectors and positioned
at the center of the target cube in FIG. 1, relative to the detection
fixture frame is described by the following homogeneous transformation:
##EQU22##
The 4.times.4 homogeneous transformation matrix in Equation (30) contains
all the information specifying the POSE of the target cube. The 3.times.3
upper left submatrix specifies the target cube's orientation and the 3X1
upper right submatrix specifies the position of the target cube. Equation
(30) is used to express a vector written in the target cube coordinates
(X.sub.h,Y.sub.h,Z.sub.h) in the detector base frame
(X.sub.M,Y.sub.M,Z.sub.M).
In Equation (30) n.sub.x, n.sub.y, and n.sub.z are given by equations
(22b-d), s.sub.x, s.sub.y, and s.sub.z are given by Equations (23b-d),
a.sub.x, a.sub.y, and a.sub.z are given by Equations (24b-d) and .sub.p
x.sub.M, .sub.p y.sub.M, and .sub.p z.sub.M are given by Equations
(29a-c).
In the description of the sensor presented so far, it has been assumed that
the light beams are mounted at exactly (x.sub.e,0,0) and (0,y.sub.e,0) and
aimed exactly at angle .theta..sub.B. However, when a sensor in accordance
with the present invention is machined at a reasonable cost, large
tolerances are inherently employed and, as a consequence, the position of
the beams and their alignments are not exactly the design parameters.
Accordingly, a calibration procedure is required to determine the exact
beam positions and alignments. Also, the position sensitive detectors may
not necessarily be mounted precisely on the detector fixture face with the
exact orientation as the face and may not be mounted exactly at the
desired position. Accordingly, a calibration procedure may be required to
determine these orientation and position errors as well.
A description of this calibration procedure follows.
Each surface 32, 34, 36 of detector fixture 30 (FIG. 2) is preferably
finely machined by a numerical control machine so that the three surfaces
of detector fixture 30 can be used to establish the orientation and
position of each surface of detector fixture 30 using a conventional
coordinate measuring machine. To obtain the orientation and position of
each such surface, detector fixture 30 is mounted on a six degree of
freedom kinematic fixture 70 of a conventional coordinate measuring
machine 72 as shown in FIG. 9. The equations of each of the three surfaces
of the plate making up each of planar surfaces 32, 34 and 36 of detector
fixture 30 are found by physically contacting each of those surfaces with
the probe of a conventional coordinate measuring machine. Thus, the face
coordinate system (x.sub.i, y.sub.i, z.sub.i) of each surface of detection
fixture 30 may be obtained (see FIG. 10). As should be apparent to one
skilled in the art, this procedure establishes the reference coordinate
system on each surface of detector fixture 30. This procedure actually
establishes a reference coordinate system at the corner of each surface 32
of detector fixture 30. However, this coordinate system may be readily
translated as shown in FIG. 10 to the center of each position detector,
such as is illustrated for position detector 38 in FIG. 10.
A coordinate system is associated with each position sensitive detector as
illustrated in FIG. 10 by PSD coordinates which are disoriented from the
detector fixture face coordinates as is also illustrated in FIG. 10. The
PSD coordinate system origin may be considered located at the origin of
the face coordinate system. A conventional coordinate measuring machine
may be used to determine the orientation of the PSD coordinate system with
respect to the face coordinate system and thereby determine the requisite
transformation required between the face coordinates and the PSD
coordinates. During operation of a position sensor, beam position is
determined in the PSD coordinates and the transformation between PSD
coordinates and the face coordinates may be used to derive the beam
position in face coordinates.
The position sensors of the subject invention are preferably designed for
position accuracy of at least 10.sup.-4 inches and an orientation accuracy
of at least 10.sup.-4 radians. Conventional position sensitive detectors
can achieve a position resolution well below 10.sup.-4 inches, but
conventional position sensitive detectors are not accurate to 10.sup.-4
inches due to non-linearity. To overcome this negative effect of
non-linearity, the plot of position accuracy is recommended.
For example, a position accuracy plot of a Hamamatsu S1200 is illustrated
in FIG. 11. A position accuracy plot for a more sophisticated and, hence,
more expensive position sensitive detector, such as the Hamamatsu S1880,
is illustrated in FIG. 12. In either case, a map of output versus actual
beam position is preferably determined and stored in a memory in order to
meet the intended accuracy requirement of 10.sup.-4 inches and 10.sup.-4
radians. It should be understood that position resolution of position
sensitive detectors utilized in accordance with the subject invention is
more important than position accuracy because the accuracy can be improved
through calibration while the resolution limits the maximum theoretical
accuracy obtainable.
A map of output versus actual beam position may be determined by the
calibration set up illustrated in FIG. 13. In FIG. 13 a collimated light
source 90 is illustrated as being mounted on a kinematic fixture 92 of a
conventional coordinate measuring machine. A position sensitive detector
94 employed in accordance with the teachings of the subject invention is
shown mounted on a planar surface of a detector fixture of the subject
invention which, in turn, is mounted on a kinematic fixture 96 of a
conventional coordinate measuring machine. Light source 90 has a cube 98
mounted to an external surface thereof. As should be understood by those
skilled in the art, the exact position and alignment of a collimated beam
produced by light source 90 may readily be determined using conventional
methodology with regard to the position and orientation of cube 98.
During calibration, the set up of FIG. 13 is employed to direct the output
beam from light source 90 at a known position and orientation. Position
sensor 94 is then moved through a sequence of known positions so that a
grid of output versus actual beam position in terms of the face coordinate
system of sensor 94 is measured. It should be understood that it is not
critical if the center of the PSD of sensor 94 is exactly placed at the
center of the face coordinate system of the corresponding detector fixture
because misalignment errors may be eliminated through calibration in
accordance with the set up of FIG. 13.
It is presently anticipated that the position and alignment of light
sources used in accordance with the subject invention may have tolerances
on the order of 10.sup.-2 inches and 0.05 degrees, since these tolerances
can presently easily be met using inexpensive machining methods. However,
with these relaxed tolerances, the resultant beams will not be positioned
and aligned exactly as designed. Instead, position and alignment errors
may be introduced. For example, as illustrated in FIGS. 14a-c each light
source positioned along an X axis of a face coordinate system may actually
be located at a position (x.sub.E +.DELTA.X.sub.E, .DELTA.Y.sub.e1,0)
instead of (x.sub.E,0,0). Likewise, each light source located along a Y
axis may actually be positioned at (.DELTA.x.sub.E2,-y.sub.E
+.DELTA.y.sub.E2,0) instead of (0,-y.sub.E,0). Moreover, as is illustrated
in FIGS. 14b and c, each light source for Beam #1 may be oriented at an
angle .theta..sub.B +.DELTA..theta..sub.B 1, in the XZ plane instead of
.theta..sub.B and each light source for Beam #2 may be oriented at an
angle .theta..sub. B +.DELTA..theta.b.sub.2 in the YZ plane. Additionally,
each beam may be oriented out of the XZ plane, or YZ plane at an angle
.DELTA..phi..sub.1 or .DELTA..phi..sub.2 instead of being positioned
precisely in the XZ plane or YZ plane.
The position errors .DELTA.x.sub.E1, .DELTA.y.sub.E1, .DELTA.x.sub.E2, and
.DELTA.y.sub.E2, and the alignment errors .DELTA..theta..sub.B1,
.DELTA..phi..sub.1, .DELTA..theta..sub.B2, and .DELTA..phi..sub.2, may be
determined in accordance with the following procedure which is set forth
with regard to Light Beam No. 1, it being understood that the procedure
for Light Beam No. 2 is identical.
First, the actual equation is written for each beam. This equation includes
the effects of beam positioning and alignment errors. All equations which
follow are written in P | | |
