Method and apparatus for determining remote object orientation and position

• US 4,737,794 A
• Filed: 12/09/1985
• Issued: 04/12/1988
• Est. Priority Date: 12/09/1985
• Status: Expired due to Term

• Current Assignee: Chittenden Bank (People's United Financial Incorporated)
• Original Assignee: Mcdonnell Douglas Corporation (The Boeing Co.)
• Inventors: Jones, Herbert R.
• Primary Examiner(s): Blum, Theodore M.
• Assistant Examiner(s): CAIN, DAVID C
• Application Number:
  US06/806,668
• Time in Patent Office: 855 Days
• Field of Search: 342/386, 342/422, 342/423, 342/445, 342/448
• US Class Current: 342/448
• CPC Class Codes: F41G 3/225 Helmet sighting systems
• CPC Class Codes: G01B 7/004 for measuring coordinates of points
• CPC Class Codes: G01S 3/14 Systems for determining direction or deviation from predetermined direction aerial arrangements for changing or varying the orientation or the shape of the directional pattern H01Q3/00; combinations of different interacting aerial units for giving a desired directional characteristic H01Q21/29; aerials or aerial systems providing at least two radiation patterns H01Q25/00
• CPC Class Codes: G01S 5/0247 Determining attitude
• CPC Class Codes: G05B 2219/37045 Probe detects electromagnetic fields from grid, antenna like digitizing tablet
• CPC Class Codes: G05B 2219/37068 Setting reference coordinate frame

Abstract

A method and apparatus is provided for determining remote object orientation and position with an electromagnetic coupling. A plurality of radiating antennas are provided for radiating electromagnetic energy. Each of the radiating antennas have independent components for defining a source reference coordinate frame. A transmitter is provided for applying electrical signals to the radiating antennas for generating a plurality of electromagnetic fields. The signals are multiplexed so that the fields are distinguishable from one another. A plurality of receiving antennas are disposed on a remote object for receiving the transmitted electromagnetic fields. The receiving antennas have a plurality of independent components for detecting the transmitted electromagnetic fields and defining a sensor reference coordinate frame. An analyzer is provided for receiving the output of the receiving antennas and converting the components of the transmitted electromagnetic fields into remote object position and orientation relative to the source reference coordinate frame. The analyzing means includes means for processing the components of the electromagnetic fields independently into remote object position in cartesian coordinates and remote object orientation using a quaternion processing strategy. The processing technique provides an uncoupled, noniterative, closed form solution for both position and orientation which is fast and continuous. Further, in the case where dipole antennas are used and the size of the antennas is significant relative to the separation distance between the radiating and receiving antennas, an aperture compensation technique is provided for compensating for distortion in the dipole fields. Still further, the processing technique provides an arrangement for smoothing or filtering the solution for position and orientation by blending previous solutions into the current solution.

Claims

1. An apparatus for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

a transmitter means for applying to said plurality of radiating means electrical signals which generate a plurality of radio frequency electromagnetic fields, said electromagnetic fields being distinguishable from one another;

a plurality of receiving means for receiving said transmitted electromagnetic fields, said receiving means being disposed on a remote object and said receiving means having independent components for detecting each of said transmitted electromagnetic fields and defining a sensor reference coordinate frame; and

analyzing means for receiving the output of said receiving means and converting the components of said transmitted electromagnetic fields received by said receiving means into remote object position and orientation relative to said source reference coordinate frame, said analyzing means including processing means for processing the components of said electromagnetic fields into remote object orientation using quaternions and blending means for blending a prior solution for said orientation into a new solution for said orientation.

2. The apparatus of claim 1 wherein said radiating means each comprise a dipole antenna and said analyzing means further comprises radiator aperture compensation means for compensating for distortion in the dipole fields caused by the size of the radiating antennas relative to the separation distance between said radiating means and said receiving means.

3. The apparatus of claim 2 wherein said receiving means each comprise a dipole antenna and said analyzing means further comprises receiver aperture compensation means for compensating for distortion in the received components of said electromagnetic fields caused by the size of the receiving antennas relative to the separation distance between said radiating means and said receiving means.

4. The apparatus of claim 1 wherein said analyzing means further comprises means for processing the components of said electromagnetic fields into remote object position and means for blending an old solution for position into a new solution for position.

19. The apparatus in claim 1 in which said blending means includes means for selectively increasing or decreasing the proportion of said prior solution for orientation blended into said new solution to regulate the amount of lag in the apparatus.

5. A method for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

providing a source of electromagnetic radiation including a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

applying multiplexed electrical signals to said radiating means and thus transmitting a set of distinguishable electromagnetic fields;

providing a sensor including a plurality of receiving means disposed on a remote object for defining a sensor reference coordinate frame, receiving said electromagnetic fields and generating a set of signals that is representative of the received components of said electromagnetic fields;

processing the components of said electromagnetic fields into remote object orientation using quaternions; and

blending a prior solution for orientation into a new solution for said orientation.

6. The method of claim 5 further including the method steps of providing a plurality of dipole radiating antennas and compensating for distortion in the dipole field created when the size of the radiating antennas is significant relative to the separation distance between the source and the sensor.

7. The method of claim 6 further including the method steps of providing a plurality of dipole receiving antennas and compensating for distortion caused when the size of the receiving antenna is significant relative to the separation distance between the source and the sensor.

8. The method of claim 5 further including the steps of processing the components of said electromagnetic fields into remote object position and blending an old solution for position into a new solution for position.

9. A method for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

providing a source of electromagnetic radiation including a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

applying multiplexed electrical signals to said radiating means and thus transmitting a set of distinguishable electromagnetic fields;

providing a sensor including a plurality of receiving means disposed on a remote object for defining a sensor reference coordinate frame, receiving said electromagnetic fields and generating a set of signals that is representative of the received components of said electromagnetic fields; and

processing the components of said electromagnetic fields into remote object position and orientation by;

establishing a signal matrix S representative of the received components of said electromagnetic fields;

finding S^T S and normalizing to determine the position unit vector r;

forming PH^-1 P^T ;

post multiplying the signal matrix S by said PH^-1 P^T and building a 4×

4 matrix Q;

finding v=Qq_t-1 where q is a quaternion representing orientation and q_t-1 is the last computed quaternion;

normalizing said v to determine the orientation vector; and

determining range r from ##EQU39##

10. The method of claim 9 further including the step of introducing a smoothing factor for r after said step of find S^T S.

11. The method of claim 9 further including the step of compensating for aperture effects by subtracting the aperture effect terms from the signal matrix S just after said step of establishing the signal matrix.

12. An apparatus for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

a transmitter means for applying to said plurality of radiating means electrical signals which generate a plurality of electromagnetic fields, said electromagnetic fields being distinguishable from one another;

a plurality of receiving means for receiving said transmitted electromagnetic fields, said receiving means being disposed on a remote object and said receiving means having independent components for detecting each of said transmitted electromagnetic fields and defining a sensor reference coordinate frame; and

analyzing means for receiving the output of said receiving means and converting the components of said transmitted electromagnetic fields received by said receiving means into remote object position vector r, orientation vector v and range value r relative to said source reference coordinate frame, said analyzing means including means for establishing a signal matrix S representative of the received components of said electromagnetic fields,

means for determining S^T S and normalizing to determine said position vector r,

means for forming PH^-1 P^T,

means for post multiplying the signal matrix S by said PH^-1 P^T and building a 4×

4 matrix Q,

means for determining v=Qq_t-1 were q is a quaternion representing orientation and q_t-1 is the last computed quaternion,

means for normalizing v to determine said orientation vector v and

means for determining said range value r according to ##EQU40##

13. The apparatus of claim 13 further comprising means for introducing a smoothing factor for said position vector r.

14. The apparatus of claim 13 further comprising means for compensating for aperture effects, said compensating means including means for subtracting the aperture effect terms from said signal matrix S.

15. A method for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

providing a source of electromagnetic radiation including a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

applying multiplexed electrical signals to said radiating means and thus transmitting a set of distinguishable electromagnetic fields;

providing a sensor including a plurality of receiving means disposed on a remote object for defining a sensor reference coordinate frame, receiving said electromagnetic fields and generating a set of signals that is representative of the received components of said electromagnetic fields; and

processing the components of said electromagnetic fields into remote object position and orientation by;

establishing a signal matrix S representative of the received components of said electromagnetic fields,

finding S^T S and normalizing to determine the position unit vector r,

forming PH^-1 P^T,

post multiplying the signal matrix S by said PH^-1 P^T to form a matrix X=SPH^-1 P^T,

forming Δ

A=A_t-1^T X,

where A is a matrix representing orientation and A_t-1 is the last computed orientation matrix,

extracting Δ

q from Δ

A where q is a quaternion,

determining the current quaternion representation of orientation q_t =Δ

qq_t-1 where q_t-1 is the last computed quaternion, and

normalizing q_t to determine orientation.

16. The method of claim 16 in which said step of extracting Δ

q from Δ

A further includes;

calculating the trace of Δ

A;

determining whether said trace is positive;

if said trace is positive then finding Δ

q from

space="preserve" listing-type="equation">Δ

q.sub.0 =trace+ε

where ε

is 1/3 Euclidean norm of Δ

A

space="preserve" listing-type="equation">Δ

q.sub.1 =-Δ

A.sub.23 +Δ

A.sub.32

space="preserve" listing-type="equation">Δ

q.sub.2 =-Δ

A.sub.31 +Δ

A.sub.13

space="preserve" listing-type="equation">Δ

q.sub.3 =-Δ

A.sub.12 +Δ

A.sub.21;

if said trace is not positive and Δ

A₁₁ is larger than Δ

A₂₂ and Δ

A₃₃, then finding Δ

q from

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.23 +Δ

A.sub.32

space="preserve" listing-type="equation">Δ

q.sub.1 =ε

+Δ

A.sub.11 -Δ

A.sub.22 -Δ

A.sub.33

space="preserve" listing-type="equation">Δ

q.sub.2 =Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.3 =Δ

A.sub.13 +Δ

A.sub.31;

if said trace is not positive and Δ

A₂₂ is larger than Δ

A₁₁ and Δ

A₃₃, then finding Δ

q from

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.31 +Δ

A.sub.13

space="preserve" listing-type="equation">Δ

q.sub.1 =Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.2 =ε

-Δ

A.sub.11 +Δ

A.sub.22 -Δ

A.sub.33

space="preserve" listing-type="equation">Δ

q.sub.3 =Δ

A.sub.23 +Δ

A.sub.32 ; and

if said trace is not positive and A₃₃ is larger than A₁₁ and A₂₂, then finding Δ

q from

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.1 =Δ

A.sub.13 +Δ

A.sub.31

space="preserve" listing-type="equation">Δ

q.sub.2 =Δ

A.sub.23 +Δ

A.sub.32

space="preserve" listing-type="equation">Δ

q.sub.3 =ε

-Δ

A.sub.11 -Δ

A.sub.22 +Δ

A.sub.33.

17. An apparatus for determining the position and orientation of a remote object relative to a reference coordinate frame comprising:

a plurality of radiating means for radiating electromagnetic energy, said radiating means having independent components defining a source reference coordinate frame;

a transmitter means for applying to said plurality of radiating means electrical signals which generate a plurality of radio frequency electromagnetic fields, said electromagnetic fields being distinguishable from one another;

a plurality of receiving means for receiving said transmitted electromagnetic fields, said receiving means being disposed on a remote object and said receiving means having independent components for detecting each of said transmitted electromagnetic fields and defining a sensor reference coordinate frame; and

analyzing means for receiving the output of said receiving means and converting the components of said transmitted electromagnetic fields received by said receiving means into remote object position, and orientation relative to said source reference coordinate frame, said analyzing means including means for establishing a signal matrix S representative of the received components of said electromagnetic fields,

means for determining S^T S and normalizing to determine said position vector r,

means for forming PH^-1 P^T,

means for post multiplying the signal matrix S by said PH^-1 P^T and forming a matrix X=SPH^-1 P^T ;

means for forming Δ

A=A_t-1^T X where A is a matrix representing orientation and A_t-1 is the last computed orientation matrix,

extracting means for extracting Δ

q from Δ

A where q is a quaternion,

means for determining the current quaternion representation of orientation q_t=Δ

qq_t-1 where q_t-1 is the last computed quaternion, and

means for normalizing q_t to determine orientation.

18. The apparatus in claim 18 in which said extracting means comprises:

means for calculating the trace of Δ

A;

determining means for determining if said trace is positive;

calculating means responsive to said determining means for calculating Δ

q;

said calculating means programmed to calculate Δ

q as

space="preserve" listing-type="equation">Δ

q.sub.0 =trace+ε

where ε

is 1/3 Euclidean norm of Δ

A,

space="preserve" listing-type="equation">Δ

q.sub.1 =-Δ

A.sub.23 +Δ

A.sub.32,

space="preserve" listing-type="equation">Δ

q.sub.2 =-Δ

A.sub.31 +Δ

A.sub.13

space="preserve" listing-type="equation">Δ

q.sub.3 =-Δ

A.sub.12 +Δ

A.sub.21 if said trace is positive;

said calculating means further programmed to calculate Δ

q as

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.23 +Δ

A.sub.32

space="preserve" listing-type="equation">Δ

q.sub.1 =ε

+Δ

A.sub.11 -Δ

A.sub.22 -Δ

A.sub.33

space="preserve" listing-type="equation">Δ

q.sub.2 =Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.3 =Δ

A.sub.13 +Δ

A.sub.31

if said trace is not positive and Δ

A₁₁ is larger than Δ

A₂₂ and Δ

A₃₃ ;

said calculating means further programmed to calculate Δ

q as

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.31 +Δ

A.sub.13

space="preserve" listing-type="equation">Δ

q.sub.1 =Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.2 =ε

-Δ

A.sub.11 +Δ

A.sub.22 -Δ

A.sub.33

space="preserve" listing-type="equation">Δ

q.sub.3 =Δ

A.sub.23 +Δ

A.sub.32 ;

if said trace is not positive and Δ

A₂₂ is larger than Δ

A₁₁ and Δ

A₃₃ ; and

said calculating means further programmed to calculate Δ

q as

space="preserve" listing-type="equation">Δ

q.sub.0 =-Δ

A.sub.12 +Δ

A.sub.21

space="preserve" listing-type="equation">Δ

q.sub.1 =Δ

A.sub.13 +Δ

A.sub.31

space="preserve" listing-type="equation">Δ

q.sub.2 =Δ

A.sub.23 +Δ

A.sub.32

space="preserve" listing-type="equation">Δ

q.sub.3 =ε

-Δ

A.sub.11 -Δ

A.sub.22 +Δ

A.sub.33

if said trace is not positive and Δ

A₃₃ is larger than Δ

₁₁ and Δ

₂₂.