Method of Sensing the Position and Orientation of Elements in Space

The present invention relates to systems by which the location and orientation of an element in space (e.g. a robot arm or other platform) can be accurately determined by electrooptical means. The invention has applications either in open loop position locating systems or in closed loop position control systems such as may be used for the accurate control of robotic arms.

One of the more difficult current technological problems is to locate and orient a robotic arm with an accuracy sufficient to perform the tasks required. Such tasks might include the accurate drilling of holes, matching of surfaces or insertion of parts in predetermined positions. Many of these tasks require positioning accuracies greater than those presently achievable by the open-loop servo-mechanism controls presently employed. Most of the prior-art inaccuracies are traceable to the effects of the gravitational force vector on the robot arm, which acts to offset the arm position from that designated by the servo-mechanism controls.

It is therefore an object of the present invention to overcome the problems which are experienced by the prior art.

A more particular problem is to provide an improved method of sensing the position and orientation of an element (such as a robot arm) in space.

Still more particularly, it is an object of the invention to provide an improved method which permits sensing of the true position of an element in space, via an optical measuring system independent of gravitational effects, and to use the thus obtained information in (known per se) feedback control systems to effect positional changes that may be required to bring the element to the desired position.

The position of a point is uniquely determined by the intersection of a known line with a known plane which does not include the line. The orientation of any three-dimensional body in space is uniquely determined by the relative position of three known non-colinear points on that body. Thus, if the absolute position in space of three such non-colinear points affixed to a three-dimensional body (such as a robot arm) can be determined, then both the position and orientation of that body in space will also have been determined. According to the invention, the absolute point positions are determined by sensing that each lies simultaneously on a line and on a plane, the intersection of which is the point in question. Both the lines and planes are implemented by either projected light patterns or directions sensing capabilities of television cameras.

The invention will hereafter be described with reference to exemplary embodiments which are illustrated in the appended drawings. However, it is to be understood that this is by way of explanation only, and that no limitation is intended to the specifics of these embodiments.

FIG. 1 is a geometric representation showing how a single point can be located in space; FIG. 1A is a matrix illustration, showing the roles of different components employable in the system of FIG. 1;

FIG. 2 is a diagrammatic illustration of a first embodiment of the invention;

FIG. 3 is a view analogous to FIG. 2, but showing another embodiment;

FIG. 4 is another diagrammatic view, showing a further embodiment; and

FIG. 5 is a further diagrammatic view of yet another embodiment of the invention.

FIGS. 1 and 1A will serve to provide a simple geometric description for locating a single point in space (the term "space" should be understood to refer to terrestrial as well as extraterrestrial space).

The source of light required to make the system operative can be resident either in the plane and/or line, or in the intersecting point itself. The possible roles of these three components of the system are described by the matrix in FIG. 1A. For the particular implementation where the point C is the light source and both A and B are cameras, C is determined to lie on the intersection of two lines in space (rather than a line and a plane). This added dimension is redundant to the solution of the problem. Extending the configuration of FIG. 1 to three points in space requires only the means of handling multiple lines/planes emanating from the camera/projectors and means of properly corresponding particular lines and planes to particular points.

Descriptions of the implementations and operations of the various configurations is included with reference to the embodiments in FIGS. 2-5.

FIG. 2 shows an embodiment in which the light sources are located on the platform and cameras are located at positions off the platform. Three non-colinear sources 10 are shown mounted to the platform 11. A point light source is approximated for each source 10 by grinding a spherical surface onto the end of a respective one of three thin fibre optic bundles 12. The latter serves to carry the light from the light generating source 13 which can be located conveniently elsewhere on the platform. The light generating source 13 can be either a laser or a white light source. A means of providing a unique identity for each of three light sources 10 is implemented by the three light modulators 15 shown between the light generating source 13 and the fibre optic bundles 12. Each of these modulators 15 is driven by a unique code generated by the encoder or code generator 16 which imparts an on-off modulation to each light source 10.

Two TV-type cameras 17 and 18 are located off-platform and are so situated that the three light points from sources 10 on the platform 11 are visible to both cameras 17, 18. The camera 17 to platform 11 to camera 18 angle should be nominally 90.degree. and never less than 30.degree. or greater than 150.degree.. Angles outside this range will cause severe geometric distortion of precision. Each camera 17, 18 senses the images of three light sources 10. The image position on the TV field uniquely determines each source 10 to be along a specific line in space. The identity of the particular source 10 is determined by sensing the identifying the coded on-off light modulation associated with each source 10. The decoding process is done in the video processor 19, 20 associated with each TV camera 17, 18. The output of each video processor 19 or 20 is digital data describing the identity of each light source 10 and its angular direction with respect to the center of the TV field of view. These signals enter the computer 21 (known per se) which performs all position calculations and provides camera pointing control.

The pointing angle of each camera 17, 18 is servocontrolled to center the TV field of view about the platform 11. Thus the TV optical magnification can be maximized, consistent with keeping all three light sources 10 within the field of view. The high optical magnification optimizes the angle determining precision of the system. The respective 2-axis TV pointing servo 22, 23 is controlled from the computer 21 which has calculated platform position within the field of view. The servos 22, 23 are thus controlled to keep the three sources 10 centered within the field of view.

Data resident in the computer 21 on a real time basis includes the camera pointing angles (field of view centers) and the angular positions of each light source 10 with respect to these field of view centers. Thus each light source 10 is determined to be on a unique line in space emanating from each camera 17, 18. The computer 21 calculates the interaction of these lines which determines the position of each light source 10 in space. Knowing the fixed mechanical position of the light sources 10 with respect to the platform 11 is then sufficient to enable the computer 21 to calculate and output the platform position and orientation in space.

FIG. 3 shows an embodiment in which light sensors are located on the platform and projectors at positions off the platform. The cameras of FIG. 2 are here replaced by light projectors 30, 31 and the light sources by light sensors 32. These may be small "chips" which convert the projected light from projectors 30, 31 impinging on them to electrical signals. These are amplified and thresholded to provide digital signals to the computer 21, indicative of the presence or absence of projected light on the sensors 32. Thus, the computer 21 knows the instance in time when each sensor 32 is illuminated.

The projectors 30, 31 generate light beams in space whose angular position is appropriately modulated with time by commands from the computer 21. By correlating the detected time of sensor illumination with the projected beam position at that time, the computer 21 can thus determine the angular position of each sensor 32 with respect to each projector 30, 31. The angular position of the optical axis of each projector 30, 31 is servo-controlled by commands from computer 21 to keep it centered on the platform. This optimizes the resolution and accuracy of the projected light patterns by minimizing the angular coverage required to illuminate all three light sensors 32. From FIG. 1 it is clear that one of the projectors 30, 31 need only project a plane in space while the other must project a line. Both the plane and line must be scanned in time so that each sensor 32 is illuminated somewhere during the scan.

Several alternative methods exist for implementing these projections. The plane projector can be implemented by optically generating a light plane, using cylindrical lenses 40 to collimate the rays from a light source. The thickness of a plane would be commensurate with the required resolution and the width sufficient to illuminate all sensors in one scan. As shown in FIG. 4, a rotating mirror system 41 will provide scanning orthogonal to the plane with the scan angle sufficient to illuminate all sensors. For that projector from which a projected line is required, 2 orthogonal light planes scanned as above can effect the same result. In this implementation, first one plane would be turned on and scanned across the field once. Then a second orthogonal plane would be turned on and scanned once across the field orthogonal to the first. Alternating the scans between the two planes produces two illumination pulses on each sensor 32. These two time marks uniquely locate the sensor along a single line emanating from the projector.

The scanning mechanism for all three light planes (two orthogonal planes from one projector and a single plane from the other) can be run continuously with the light planes sequentially turned on and off as orchestrated by the computer 21. Thus the illumination pulses from each sensor 32 can be attributed to a particular scanning plane. These nine pulse arrival times when correlated with the scan position time all three scanning planes and the servo-pointing data enable the computation of position and orientation of the platform 11 in space.

Alternative methods of implementing the plane/line projection pattern include a raster scan with a pencil light beam to implement a line projection directly; (any of the methods common to projection TV systems and the like are appropriate for this implementation); scanning of a plane by using a sequence of coded multi-plane projections as known from the prior art; obtaining fractional planes via the ratio of variable intensity projected inverse light patterns or ratio of light patterns as known from the prior art; and orthogonal coded patterns of the general type mentioned in the preceding sentence effecting a line projection in a manner similar to scanned orthogonal planes. In the use of coded patterns as described in the two preceding sentences, the computer processing must include the associated decoding logarithms to appropriately determine the specific planes on which the sensor lies.

A final embodiment involves the use of reflectors on the platform 11 and of one projector and one camera off the platform. Three non-colinear reflectors 50 are shown mounted to the platform 11. These are physically small diffuse reflectors colored so as to reflect one of the three prime colors (i.e. one red, one green, and one blue). These reflectors are illuminated by an off-platform plane projector 51, similar to the scanning plane or a coded multi-plane projector implementation described with respect to FIG. 3. In this case, the spectral characteristics of the projected light must be nearly white to achieve proper reflection from the three reflectors 50. Also mounted off platform is a color TV camera 52 to individually sense the reflected images of the three reflectors 50 and identify each reflector 50 by color.

Thus, each reflector 50 is identified and its position in space determined as the intersection of a line (defined by the camera image position) and a plane (defined by the plane projector). Both the projector 51 and camera 52 pointing positions 21 are servo-controlled (servos 22, 23) via the computer 21 to track the platform 11 as in FIGS. 1 and 2.

Without further analysis, the foregoing will so fully reveal the gist of the present invention that others can, by applying current knowledge, readily adapt it for various applications without omitting features that, from the standpoint of prior art, fairly constitute essential characteristics of the generic or specific aspects of this invention, and therefore, such adaptations should and are intended to be comprehended within the meaning and range of equivalence of the following claims.