Method and apparatus for determining remote object orientation and position

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.

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.sup.T S and normalizing to determine the position unit vector r;
• forming PH.sup.-1 P.sup.T ;
• post multiplying the signal matrix S by said PH.sup.-1 P.sup.T and building a 4.times.4 matrix Q;
• finding v=Qq.sub.t-1 where q is a quaternion representing orientation and q.sub.t-1 is the last computed quaternion;
• normalizing said v to determine the orientation vector;
• and
• determining range r from ##EQU39##

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.sup.T S and normalizing to determine said position vector r,
• means for forming PH.sup.-1 P.sup.T,
• means for post multiplying the signal matrix S by said PH.sup.-1 P.sup.T and building a 4.times.4 matrix Q,
• means for determining v=Qq.sub.t-1 were q is a quaternion representing orientation and q.sub.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##

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.sup.T S and normalizing to determine the position unit vector r,
• forming PH.sup.-1 P.sup.T,
• post multiplying the signal matrix S by said PH.sup.-1 P.sup.T to form a matrix X=SPH.sup.-1 P.sup.T,
• forming .DELTA.A=A.sub.t-1.sup.T X,
• where A is a matrix representing orientation and A.sub.t-1 is the last computed orientation matrix,
• extracting .DELTA.q from .DELTA.A where q is a quaternion,
• determining the current quaternion representation of orientation q.sub.t =.DELTA.qq.sub.t-1 where q.sub.t-1 is the last computed quaternion, and
• normalizing q.sub.t to determine orientation.

The method of claim 16 in which said step of extracting .DELTA.q from .DELTA.A further includes:

• calculating the trace of .DELTA.A;
• determining whether said trace is positive;
• if said trace is positive then finding .DELTA.q from
• .DELTA.q.sub.0 =trace+.epsilon. where .epsilon. is 1/3 Euclidean norm of .DELTA.A
• .DELTA.q.sub.1 =-.DELTA.A.sub.23 +.DELTA.A.sub.32
• .DELTA.q.sub.2 =-.DELTA.A.sub.31 +.DELTA.A.sub.13
• .DELTA.q.sub.3 =-.DELTA.A.sub.12 +.DELTA.A.sub.21;
• if said trace is not positive and .DELTA.A.sub.11 is larger than .DELTA.A.sub.22 and .DELTA.A.sub.33, then finding .DELTA.q from
• .DELTA.q.sub.0 =-.DELTA.A.sub.23 +.DELTA.A.sub.32
• .DELTA.q.sub.1 =.epsilon.+.DELTA.A.sub.11 -.DELTA.A.sub.22 -.DELTA.A.sub.33
• .DELTA.q.sub.2 =.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.3 =.DELTA.A.sub.13 +.DELTA.A.sub.31;
• if said trace is not positive and .DELTA.A.sub.22 is larger than .DELTA.A.sub.11 and .DELTA.A.sub.33, then finding .DELTA.q from
• .DELTA.q.sub.0 =-.DELTA.A.sub.31 +.DELTA.A.sub.13
• .DELTA.q.sub.1 =.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.2 =.epsilon.-.DELTA.A.sub.11 +.DELTA.A.sub.22 -.DELTA.A.sub.33
• .DELTA.q.sub.3 =.DELTA.A.sub.23 +.DELTA.A.sub.32 ;
• and
• if said trace is not positive and A.sub.33 is larger than A.sub.11 and A.sub.22, then finding .DELTA.q from
• .DELTA.q.sub.0 =-.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.1 =.DELTA.A.sub.13 +.DELTA.A.sub.31
• .DELTA.q.sub.2 =.DELTA.A.sub.23 +.DELTA.A.sub.32
• .DELTA.q.sub.3 =.epsilon.-.DELTA.A.sub.11 -.DELTA.A.sub.22 +.DELTA.A.sub.33.

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.sup.T S and normalizing to determine said position vector r,
• means for forming PH.sup.-1 P.sup.T,
• means for post multiplying the signal matrix S by said PH.sup.-1 P.sup.T and forming a matrix X=SPH.sup.-1 P.sup.T ;
• means for forming .DELTA.A=A.sub.t-1.sup.T X where A is a matrix representing orientation and A.sub.t-1 is the last computed orientation matrix,
• extracting means for extracting .DELTA.q from .DELTA.A where q is a quaternion,
• means for determining the current quaternion representation of orientation q.sub.t=.DELTA.qq.sub.t-1 where q.sub.t-1 is the last computed quaternion, and
• means for normalizing q.sub.t to determine orientation.

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

• means for calculating the trace of .DELTA.A;
• determining means for determining if said trace is positive;
• calculating means responsive to said determining means for calculating .DELTA.q;
• said calculating means programmed to calculate .DELTA.q as
• .DELTA.q.sub.0 =trace+.epsilon. where .epsilon. is 1/3 Euclidean norm of .DELTA.A,
• .DELTA.q.sub.1 =-.DELTA.A.sub.23 +.DELTA.A.sub.32,
• .DELTA.q.sub.2 =-.DELTA.A.sub.31 +.DELTA.A.sub.13
• .DELTA.q.sub.3 =-.DELTA.A.sub.12 +.DELTA.A.sub.21 if said trace is positive;
• said calculating means further programmed to calculate .DELTA.q as
• .DELTA.q.sub.0 =-.DELTA.A.sub.23 +.DELTA.A.sub.32
• .DELTA.q.sub.1 =.epsilon.+.DELTA.A.sub.11 -.DELTA.A.sub.22 -.DELTA.A.sub.33
• .DELTA.q.sub.2 =.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.3 =.DELTA.A.sub.13 +.DELTA.A.sub.31
• if said trace is not positive and .DELTA.A.sub.11 is larger than .DELTA.A.sub.22 and .DELTA.A.sub.33 ;
• said calculating means further programmed to calculate .DELTA.q as
• .DELTA.q.sub.0 =-.DELTA.A.sub.31 +.DELTA.A.sub.13
• .DELTA.q.sub.1 =.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.2 =.epsilon.-.DELTA.A.sub.11 +.DELTA.A.sub.22 -.DELTA.A.sub.33
• .DELTA.q.sub.3 =.DELTA.A.sub.23 +.DELTA.A.sub.32 ;
• if said trace is not positive and .DELTA.A.sub.22 is larger than .DELTA.A.sub.11 and .DELTA.A.sub.33 ;
• and
• said calculating means further programmed to calculate .DELTA.q as
• .DELTA.q.sub.0 =-.DELTA.A.sub.12 +.DELTA.A.sub.21
• .DELTA.q.sub.1 =.DELTA.A.sub.13 +.DELTA.A.sub.31
• .DELTA.q.sub.2 =.DELTA.A.sub.23 +.DELTA.A.sub.32
• .DELTA.q.sub.3 =.epsilon.-.DELTA.A.sub.11 -.DELTA.A.sub.22 +.DELTA.A.sub.33
• if said trace is not positive and .DELTA.A.sub.33 is larger than .DELTA..sub.11 and .DELTA..sub.22.