Method and apparatus for determining location and orientation of objects

Abstract

An orthogonally related target point array is provided in fixed relationship to an object. A photo detector image plane obtains images of the target points projected through a lens located a fixed distance from the image plane. Simple algebraic manipulation of image plane position coordinates of the images corresponding to the target points provides for the establishment of a vector replica of the target point array. The vector replica, in turn, is sufficient data to provide spatial information about the object including position and orientation information with full six-degrees-of-freedom.

What is claimed is: 
     
       1. A method of determining spatial information about an object relative to a reference coordinate frame, the method comprising: providing an identifiable target point array in fixed relationship to said object, said target point array having, first and second target points defining a first target point pair lying along a projection of a first line,     third and fourth target points defining a second target point pair lying along a projection of a second line, and fifth and sixth target points defining a third target point pair lying along a projection of a third line, and   wherein the extension of said first, second, and third line projections define, at least implicitly, three right angles at a common intersection point of said first, second, and third line projections;     determining position coordinates of first, second, and third pairs of image points on said image plane corresponding to projections of said first, second, and third target point pairs on said image plane, respectively; and   determining the orientation of said object as a function of said position coordinates of said image points.   
     
     
       2. The method of claim 1 further comprising the step of determining position coordinates on said image plane of a reference image point corresponding to said common intersection point. 
     
     
       3. The method of claim 2 further comprising the steps of: determining a first coefficient as a function of said position coordinates of said image points;   determining a second coefficient as a function of said position coordinates of said image points;   determining a third coefficient as a function of said position coordinates of said image points;   determining a first direction vector in alignment with said first line projection as a function of (i) the product of said first coefficient and a vector describing a first image point, corresponding to said first target point, relative to an optical centerpoint through which said projections of target points pass, and (ii) a vector describing said first one of said image points relative to said reference image point;   determining a second direction vector in alignment with said second line projections as a function of (i) the product of said second coefficient and a vector describing a second image point, corresponding to said third target point, relative to said optical centerpoint, and (ii) a vector describing said second one of said image points relative to said reference image point;   determining a third direction vector in alignment with said third line projection as a function of (i) the product of said third coefficient and a vector describing a third image point, corresponding to said fifth target point, relative to said optical centerpoint, and (ii) a vector describing said third one of said image points relative to said reference image point;   determining said target plane orientation from said first, second, and third direction vectors.   
     
     
       4. The method of claim 3 further comprising the step of determining the position of said object relative to said reference coordinate frame as a function of said first, second, and third direction vectors and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       5. The method of claim 3 including the step of determining the position of said reference image point from said position coordinates of said image point pairs. 
     
     
       6. The method of claim 3 further comprising the step of correcting orthogonal deviations between said first, second, and third direction vectors. 
     
     
       7. The method of claim 1 wherein said second, fourth, and sixth target points are said common intersection point. 
     
     
       8. The method of claim 7 further comprising the steps of: determining a first coefficient as a function of squared position coordinate values of said image points; and   generating a rotation matrix having matrix values being a function of the product of said first coefficient and said position coordinates of said image points.   
     
     
       9. The method of claim 8 further comprising the step of determining the position of said object relative to said reference coordinate frame as a function of said first coefficient and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       10. The method of claim 1 further comprising the step of determining the position of said object relative to said reference coordinate frame from said position coordinates of said image points and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       11. The method of claim 1 further comprising the step of determining position coordinates of a reference image point corresponding to the common intersection point of fourth, fifth, and sixth line projections passing through said first, second, and third image point pairs. 
     
     
       12. An apparatus for obtaining spatial information about an object relative to a reference coordinate frame comprising: at least first, second, third and fourth target spots fixed in relation to said object, each of said target spots having a median point defining first, second, third and fourth target points, said fourth target point forming a common intersection point with first, second, and third line projections which pass separately through said first, second, and third target points, respectively, and said line projections forming, at least implicitly, three right angls at said fourth target point;   imaging means having a field of view including said target spots, said imaging means including an image plane for obtaining image spots corresponding to projections thereon of said target spots;   first means for determining position coordinates of a median image point of each image spot; and   second means for determining the orientation of said object as a function of said position coordinates of said image points.   
     
     
       13. The apparatus of claim 12 wherein said second means determines position coordinates of a reference image point corresponding to said common intersection point. 
     
     
       14. The apparatus of claim 13, wherein said second means determines (i) a first coefficient as a function of said position coordinates of said image points,   (ii) a second coefficient as a function of said position coordinates of said image points,   (iii) a third coefficient as a function of said position coordinates of said image points,   (iv) a first direction vector in alignment with said second line projection as a function of (i) the product of said second coefficient and a vector describing a second image point, corresponding to said first target point, relative to an optical centerpoint through which said projections of target points pass and, (ii) a vector describing said second one of said image points relative to a reference image point in said image plane corresponding to said common intersection point,   (v) a second direction vector in alignment with said first line projection as a function of (i) the product of said first coefficient and a vector describing a second image point, corresponding to said second target point, relative to said optical centerpoint and, (ii) a vector describing said second image point relative to said reference image point,   (vi) a third direction vector in alignment with said third line projection as a function of (i) the product of said third coefficient and a vector describing a third image point, corresponding to said third target point, relative to said optical centerpoint and, (ii) a vector describing said third image point relative to said reference image point, and   (vii) said object orientation from said first, second, and third direction vectors.   
     
     
       15. The apparatus of claim 14 wherein said second means corrects orthogonal deviations between said first, second, and third direction vectors. 
     
     
       16. The apparatus of claim 12 wherein said second means determines the position of said object relative to said reference coordinate frame from said position coordinates of said image points and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       17. An apparatus for obtaining spatial information about an object relative to a reference coordinate frame: a target spot array in fixed relationship to said object, said target spot array including, first and second target spots, each having a median point thereof defining a first target point pair lying along a projection of a first line,   third and fourth target spots each having a median point thereof defining a second target point pair lying along a projection of a second line, and   fifth and sixth target each having a median point thereof defining a third target point pair lying along a projection of a third line, and   wherein the extension of said first, second, and third line projections form, at least implicity, three right angles at a common intersection point of said first, second, and third line projections;     imaging means with a field of view including said target spots, said imaging means including an image plane for obtaining image spots corresponding to projections thereof of said target spots;   first means for determining position coordinates of a median image point of each image spot corresponding to said target points; and   second means for determining the orientation of said object as a function of said position coordinates of said image points.   
     
     
       18. The method of claim 17 wherein said second means determines the position coordinates of a reference image point corresponding to said common intersection point. 
     
     
       19. The apparatus of claim 18 wherein said second means determines (i) a first coefficient as a function of said position coordinates of said image points,   (ii) a second coefficient as a function of said position coordinates of said image points,   (iii) a third coefficient as a function of said position coordinates of said image points,   (iv) a first direction vector in alignment with said second line projection as a function of (i) the product of said second coefficient and a vector describing a first image point, corresponding to said first target point, relative to an optical centerpoint through which said projections of target point pass and, (ii) a vector describing said first image point relative to a reference image point in said image plane corresponding to said common intersection point,   (v) a second direction vector in alignment with said first line projection as a function of (i) the product of said first coefficient and a vector describing a second image point, corresponding to said third target point, relative to said optical centerpoint and, (ii) a vector describing said second image point relative to said reference image point,   (vi) a third direction vector in alignment with said third line projection as a function of (i) the product of said third coefficient and a vector describing said third image point relative to said optical centerpoint and, (ii) a vector describing said third one of said image points relative to said reference image point, and   (vii) said object orientation from said first, second, and third direction vectors.   
     
     
       20. The apparatus of claim 19 wherein said second means resolves orthogonal deviations between said first, second, and third direction vectors. 
     
     
       21. The apparatus of claim 18 wherein said second means determines the position of said reference image point from the position coordinates of said image points. 
     
     
       22. The apparatus of claim 17 wherein said second means, determines a first coefficient as a function of squared position coordinate values of said image points; and   generates a rotation matrix having matrix values being a function of the product of said first coefficient and said image point position coordinates.   
     
     
       23. The apparatus of claim 22 wherein said second means determines the position of said object relative to said reference coordinate frame as a function of said first coefficient and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       24. The apparatus of claim 17 wherein said second means determines the position of said object relative to said reference coordinate frame from said position coordinates of said image points and at least one known target point separation between said common intersection point and one of said target points. 
     
     
       25. The apparatus of claim 17 wherein said second, fourth, and sixth target points provide said common intersection point. 
     
     
       26. The apparatus of claim 17 wherein said second means determines the position coordinates of a reference image point corresponding to the common intersection point of fourth, fifth, and sixth line projections passing through first, second, and third pairs of image points, respectively, corresponding to said first, second, and third target point pairs.