Method and apparatus for determining location and orientation of objects

• US 4,672,564 A
• Filed: 11/15/1984
• Issued: 06/09/1987
• Est. Priority Date: 11/15/1984
• Status: Expired due to Term

• Current Assignee: Honeywell Incorporated (Honeywell International Inc.)
• Original Assignee: Honeywell Incorporated (Honeywell International Inc.)
• Inventors: Miller, John W., Setterholm, Jeffrey M., Egli, Werner H.
• Primary Examiner(s): Chin, Gary
• Assistant Examiner(s): Teska, Kevin J.
• Application Number:
  US06/671,728
• Time in Patent Office: 936 Days
• Field of Search: 364/516, 364/525, 364/550, 364/559, 358/107, 358/903, 356/138, 356/152, 356/153, 356/363, 250/561
• US Class Current: 702/153
• CPC Class Codes: G01S 5/163 Determination of attitude using inertial means G01C9/00; control of attitude G05D1/49

Abstract

A target plane in fixed relationship to an object provides a first triad of three collinear target points which lie on a first line projection and a second triad of target points which lie on a second line projection which is in nonalignment with the first triad. A photo detector image plane obtains images of the target points projected on the image plane 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 vector replicas of the position of the target points. The vector replicas, in turn, are sufficient data to provide spatial information about the object including position and oriention information with full six-degrees-of-freedom.

Claims

1. A method of determining spatial information about an object relative to a reference coordinate frame comprising:

providing at least three identifiable collinear target points which lie on a single line projection, and in which said target points are in fixed relationship to said object;

obtaining image points on an image plane corresponding to said three collinear target points projected thereon;

determining position coordinates of said image points on said image plane; and

determining a direction vector in alignment with said collinear target points from said position coordinates of said image points.

2. The method of claim 1 including the step of determining first and second scaled vector replicas corresponding to the position of first and second ones, respectively, of said three collinear target points from said position coordinates of said image points, said scaled vector replicas being in a reference coordinate system defined by said image plane and an optical centerpoint through which said projections of said target points pass, each of said scaled vector replicas having direction and magnitude replicating a vector from said optical centerpoint to said corresponding target 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 said first scaled vector replica as a function of the product of said first coefficient and a vector describing a first one of said image points relative to said optical centerpoint;

determining said second scaled vector replica as a function of the product of said second coefficient and a vector describing a second one of said image points relative to said optical centerpoint; and

determining said direction vector as a function of said first and second scaled vector replicas.

4. The method of claim 3 wherein said first and second target points, P1 and P2, having corresponding image points, a and b, are the extremities of said three collinear target points, and said first and second coefficients are of the form:

##EQU22## where X and Y denote coordinates of said image points on said image plane relative to the origin thereof defined by the intersection of said image plane and a line perpendicular to the image plane and passing through said optical centerpoint, and image point m corresponds to said third target point.

5. The method of claim 3 further comprising the step of determining an object position vector relative to said reference coordinate frame as a function of a known target separation between at least two of said target points and the vector magnitude of the difference between said first and second scaled vector replicas.

6. The method of claim 1 further comprising the step of determining the position of said target points relative to said reference frame from said position coordinates of said image points and a known target separation between at least two of said target points.

7. The method of claim 1 further comprising the step of modifying said image point coordinate values to determine three modified image points which correspond to said target points.

8. A method of determining spatial information about an object relative to a reference coordinate frame comprising:

providing a first triad of collinear target points lying on a single line projection including at least first and second identifiable target points thereof, and a second triad of collinear target points including at least first and second identifiable target points thereof, said first and second triads each being in fixed relationship to said object and each having different direction vectors so as to define a target plane;

determining position coordinates of image points on an image plane, said image points corresponding to projections of said first and second triad of target points onto said image plane; and

determining the orientation of said target plane relative to said reference coordinate frame from said position coordinates of said image points.

9. The method of claim 8 including the step of determining first and second scaled vector replicas corresponding to the position of first and second ones, respectively, of said first triad of target points, and third and fourth scaled vector replicas corresponding to first and second ones, respectively, of said second triad of target points, from said position coordinates of said image points, said scaled vector replicas being in a reference coordinate system defined by said image plane and an optical centerpoint through which said projections of said target points pass, each of said scaled vector replicas having direction and magnitude replicating a vector from said optical centerpoint to said corresponding target point.

10. The method of claim 9 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 said first scaled vector replica as a function of the product of said first coefficient and a vector describing a first one of said image points relative to said optical centerpoint and corresponding to said first one of said first triad of said target points;

determining said second scaled vector replica as a function of the product of said second coefficient and a vector describing a second one of said image points relative to said optical centerpoint and corresponding to said second one of said first triad of said target points;

determining a first direction vector in alignment with said first triad of target points as a function of said first and second scaled vector replicas;

determining a third coefficient as a function of said position coordinates of said image points;

determining a fourth coefficient as a function of said position coordinates of said image points;

determining said third scaled vector replica as a function of the product of said third coefficient and a vector describing a third one of said image points relative to said optical centerpoint and corresponding to said first one of said second triad of said target points;

determining said fourth scaled vector replica as a function of the product of said fourth coefficient and a vector describing a fourth one of said image points relative to said optical centerpoint and corresponding to said second one of said second triad of said target points; and

determining a second direction vector in alignment with said second triad of target points as a function of said third and fourth scaled vector replicas.

11. The method of claim 10 further comprising the step of synthesizing image points m and n from position coordinate values of image points a, b, c, and d, said image points m and n being a common point corresponding to the intersection of a projection of a first line passing through points a and b with a projection of a second line passing through image points c and d.

12. The method of claim 10 wherein said first and second target points, P1 and P2, having corresponding image points a and b, are the extremities of said first triad of target points and said first and second coefficients are of the form:

##EQU23## and wherein said first and second target points, Q1, Q2, projecting image points c and d, are the extremities of said second triad of target points and said third and fourth coefficients are of the form;

##EQU24## where X and Y denote coordinates of said image points on said image plane relative to the origin thereof defined by the intersection of said image plane and a line perpendicular to the image plane and passing through the optical centerpoint, and m and n are image points projected from third target points P3 and Q3 of said first and second triads of target points.

13. The method of claim 10 wherein said direction vectors of said target point triads are orthogonal and said method further comprises the step of resolving orthogonal deviations between said first and second calculated direction vectors.

14. The method of claim 9 further comprising the step of determining an object position vector relative to said reference coordinate frame as a function of a known target separation between said first and target points of one of said triads and the vector magnitude of said first and second scaled vector replicas.

15. The method of claim 8 wherein said first and second triad of target points share a common third target point.

16. The method of claim 15 wherein a pair of each of said target points of each of said triads forms a diagonal of a parallelogram and said third target point is the intersection of said diagonals.

17. The method of claim 16 wherein said parallelogram is a square.

18. The method of claim 15 including the step of synthesizing the position coordinates of an image point corresponding to said third target point from said image point position coordinate values corresponding to said first and second target points of each of said triads.

19. The method of claim 8 further including the steps of:

providing an out-of-plane target point having a known third target separation between said out-of-plane target point and one of said target points in said target plane;

determining position coordinates of an image point corresponding to the projection of said out-of-plane target point on said image plane;

determining from said out-of-plane image point position coordinates and said third target separation a third direction vector in alignment with a vector from one of said target points in said target plane to said out-of-plane target point; and

correcting said first and second direction vectors as a function of said third vector replica.

20. The apparatus of claim 19 further comprising means for operating on said image point coordinate values and generating modified image point position coordinates which lie on a single line projection.

21. The method of claim 8 further comprising the step of determining the position of said object relative to said reference frame from said position coordinates of said image points and a known target separation between said first and second target points of one of said triads.

22. An apparatus for obtaining spatial information about an object relative to a reference coordinate frame comprising:

at least first, second, and third collinear target spots, each having a median target point defining substantially collinear first, second, and third target points, respectively, and in which said first, second, and third target points lie on a single line projection, and in which said target points are, fixed in relation to said object;

imaging means having a field of view including said target spots, said imaging means including an image plane for obtaining first, second, and third image spots corresponding to projections thereon of said target spots;

first means for determining position coordinates of median image points of each image spot; and

second means for determining a direction vector in alignment with said collinear target points from signals representative of said position coordinates of said image points.

23. The apparatus of claim 22 wherein said second means determines first and second scaled vector replicas corresponding to the position of first and second ones, respectively, of said three collinear target points from said position coordinates of said image points, said scaled vector replicas being in a coordinate system defined by said image plane and an optical centerpoint through which said projection of said target points pass, each of said scaled vector replicas having direction and magnitude replicating a vector from said optical centerpoint to said corresponding target point.

24. The apparatus of claim 23 wherein said second means,

determines a first coefficient as a function of said position coordinates of said image points;

determines a second coefficient as a function of said position coordinates of said image points;

determines said first scaled vector replica as a function of the product of said first coefficient and a vector describing a first one of said image points relative to said optical centerpoint corresponding to said first one of said target points;

determines said second scaled vector replica as a function of the product of said second coefficient and a vector describing a second one of said image points relative to said optical centerpoint corresponding to said second one of said target points; and

means for determines said direction vector as a function of said first and second scaled vector replicas.

25. The apparatus of claim 24 wherein said first and second target points, P1 and P2, having corresponding image points, a and b, are the extremities of said three collinear target points, and said first and second coefficients are of the form:

##EQU25## where X and Y denote coordinates of said image points on said image plane relative to the origin thereof defined by the intersection of said image plane and a line perpendicular to the image plane and passing through said optical centerpoint, and image point m corresponds to said third target point.

26. The apparatus of claim 24 wherein said second means determines an object position vector as a function of a known target separation between at least two of said target points and the vector magnitude of the difference between said first and second scaled vector replicas.

27. The apparatus of claim 22 wherein said second means determines the position of said points relative to said reference coordinate frame from signals representative of said position coordinates of said image points and a known value of target separation between at least two of said target points.

28. An apparatus for obtaining spatial information about an object relative to a reference coordinate frame comprising:

at least first, second, third, and fourth coplanar target spots, each having a median target point defining first, second, third and fourth target points, respectively, and in which said target points are fixed in relation to said object, said first and second target points lying along a projection of a first line passing therethrough, and said third and fourth target points lying along a projection of a second line passing therethrough and intersecting said first line;

imaging means having a field of view including said target spots, said imaging means including an image plane for obtaining first, second, third, and fourth image spots thereon corresponding to projections thereon of said target spots;

first means for determining position coordinates of first, second, third, and fourth median image points corresponding to said first, second, third, and fourth target points, respectively;

second means for,

determining a first direction vector in alignment with said first line and a second direction vector in alignment with said second line from signals representative of said position coordinates of said image points, and

determining the orientation of said object relative to said reference coordinate frame from said first and second direction vectors.

29. The apparatus of claim 28 wherein said seond means determines first and second scaled vector replicas corresponding to the position of said first and second target points, and third and fourth scaled vector replicas corresponding to said third and fourth target points, from said position coordinates of said image points, said scaled vector replicas being in a reference coordinate system defined by said image plane and an optical centerpoint through which said projections of said target points pass, each of said scaled vector replicas having direction and magnitude replicating a vector from said optical centerpoint to said corresponding target point.

30. The apparatus of claim 29 wherein said second means,

determines a first coefficient as a function of said position coordinates of said image points;

determines a second coefficient as a function of said position coordinates of said image points;

determines said first scaled vector replica as a function of the product of said first coefficient and a vector describing said first image point relative to said optical centerpoint;

determines said second scaled vector replica as a function of the product of said second coefficient and a vector describing said second image point relative to said optical centerpoint;

determines said first direction vector as a function of said first and second scaled vector replicas;

determines a third coefficient as a function of said position coordinates of said image points;

determines a fourth coefficient as a function of said position coordinates of said image points;

determines said third scaled vector replica as a function of the product of said third coefficient and a vector describing said third image point relative to said optical centerpoint;

determines said fourth scaled vector replica as a function of the product of said fourth coefficient and a vector describing said fourth image point relative to said optical centerpoint; and

determines said second direction vector as a function of said third and fourth scaled vector replicas.

31. The apparatus of claim 30 wherein said first, second, third, and fourth coefficients, α

, β

, α

₂, and β

₂, respectively, are of the form;

##EQU26## wherein said first and second target points, P1 and P2, are the extremities of said first triad of target points and where said first and second target points, Q1 and Q2, are the extremities of said second triad of target points and project image points c and d, and where X and Y denote coordinates of said image points on said image plane relative to the origin thereof defined by the intersection of said image plane and a line perpendicular to the image plane and passing through the optical centerpoint, and m and n are image points projected from third target points, P3 and Q3 of said first and second triads of target points.

32. The apparatus of claim 30 wherein the projections of said first and said second lines are orthgonal and said apparatus further comprises means for correcting orthogonal deviations between said first and second direction vectors.

33. The apparatus of claim 29 further including:

an out-of-plane target spot non-coplanar with said first, second, third, and fourth target spots, said out-of-plane target spot having a median target point defining an out-of-plane target point, and said out-of-plane target point having a known third target separation between said out-of-plane target point and a reference target point in the plane of said coplanar target points; and

wherein said first means determines position coordinates of a fifth image point corresponding to the projection of said out-of-plane target point on said image plane; and

wherein said second means, generates a third scaled vector replica corresponding to said out of plane target point, and

corrects said first and second determined direction vectors as a function of said third scaled vector replica.

34. The apparatus of claim 33 wherein said out-of-plane target point is such that a line projection passing therethrough and said point of intersection of said first and second lines is substantially perpendicular to a plane defined by said coplanar target points.

35. The apparatus of claim 29 wherein said second means determines an object position vector relative to said reference coordinate frame as a function of a known target separation between at least two of said target points and the vector magnitude of the difference between said first and second scaled vector replicas.

36. The apparatus of claim 28 wherein first and second pairs of said target points form diagonals of a parallelogram having a common intersection defining a fifth target point.

37. The apparatus of claim 36 wherein said parallelogram is a square.

38. The apparatus of claim 36 wherein said second means determines from said image point position coordinates the position coordinates of an image point corresponding to said fifth target point.

39. The apparatus of claim 28 further including:

an out-of-plane target spot non-coplanar with said first, second, third, and fourth target spots, said out-of-plane target spot having a median target point defining an out-of-plane target point, and said out-of-plane target point having a known third target separation between said out-of-plane target point and a reference target point in the plane of said coplanar target points;

means for obtaining a fifth image point on said image plane corresponding to the projection thereon of said out-of-plane target point;

means for determining position coordinates of said image point corresponding to said out-of-plane target point; and

means for correcting said first and second calculated direction vectors as a function of said position coordinates of said fifth image point.

40. The apparatus of claim 39 wherein said out-of-plane target point is substantially perpendicular to a plane defined by said coplanar target points and to the point of intersection of said first and second lines.

41. The apparatus of claim 28 wherein said second means determines the position of said target plane relative to said reference coordinate frame from said position coordinates of said image points and a known target separation between at least two of said target points.

42. The apparatus of claim 28 wherein said second means determines position coordinates of a fifth image point from said position coordinates of said first, second, third, and fourth image points, said fifth image point being on a projection of a first line passing through said first and second image points and being on a projection of a second line passing through said third and fourth image points.

43. A method of determining spatial information about an object relative to a reference coordinate frame comprising:

providing first, second, third, and fourth coplanar target points, said target points each being fixed in relationship to said object, said first and second target points defining a first line projection having associated therewith a direction vector, and said third and fourth target points defining a second line projection having a different direction vector, and wherein said first and second line projections intersect at a common point;

determining position coordinates of image points on an image plane, said image points corresponding to projections of said image points onto said image plane; and

determining the orientation of said target plane relative to said reference coordinate frame from said position coordinates of said image points.

44. The method of claim 43 including the step of synthesizing the position coordinates of an image point corresponding to the point of intersection of said first and second line projections from said image point position coordinate values.

45. Method of claim 44 further comprising the step of determining the position of said object relative to said reference frame from said position coordinates of said image points and a known target separation between said first and second target points.

46. The method of claim 43 including the step of determining first, second, third, and fourth scaled vector replicas corresponding to the position of said first, second, third, and fourth target points, respectively, from said position coordinates of said image points, said scaled vector replicas being in a reference coordinate system defined by said image plane and an optical centerpoint through which said projections of said target points pass, each of said scaled vector replicas having direction and magnitude replicating a vector from said optical centerpoint to said corresponding target point.

47. The method of claim 46 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 said first scaled vector replica as a function of the product of said first coefficient and a vector describing a first one of said image points relative to said optical centerpoint and corresponding to said first one of said first triad of said target points;

determining said second scaled vector replica as a function of the product of said second coefficient and a vector describing a second one of said image points relative to said optical centerpoint and corresponding to said second one of said first triad of said target points;

determining a first direction vector in alignment with said first triad of target points as a function of said first and second scaled vector replicas;

determining a third coefficient as a function of said position coordinates of said image points;

determining a fourth coefficient as a function of said position coordinates of said image points;

determining said third scaled vector replica as a function of the product of said third coefficient and a vector describing a third one of said image points relative to said optical centerpoint and corresponding to said first one of said second triad of said target points;

determining said fourth scaled vector replica as a function of the project of said fourth coefficient and a vector describing a fourth one of said image points relative to said optical centerpoint and corresponding to said second one of said second triad of said target points; and

determining a second direction vector in alignment with said second triad of target points as a function of said third and fourth scaled vector replicas.

48. The method of claim 43 wherein said first and second points define a first diagonal and said third and fourth target points define a second diagonal of a parallelogram defined by said first, second, third, and fourth target points.