This application is a Continuation of application Ser. No. 11/343,486, filed Jan. 31, 2006, and which application(s) are incorporated herein by reference.
The disclosed technique relates to motion tracking in general, and to methods and systems for hybrid electromagnetic-inertial tracking, in particular.
Tracking systems and methods are known in the art. In general, a tracking system is used to calculate and determine the position and orientation of an object relative to a given coordinate system in a platform. The tracked object can be, for example, a medical probe relative to a human body, the head of a pilot relative to the cockpit of a fighter jet, a person relative to a room of a building, or a ship relative to a body of water. The platform can be, for example, the human body, the cockpit of a fighter jet, a room of a building, or an entire planet. In general, tracking systems can be classified into two main categories namely, inertial trackers and non-inertial trackers.
Inertial trackers were first developed in the early twentieth century as an aid in marine navigation and later in aerial navigation. Whereas inertial trackers have grown in complexity since they were first invented, and are now used for a variety of different applications (i.e., navigational and other), the general principles behind inertial tracking remain the same. Inertial trackers essentially include a device, referred to in the art as an inertial sensor, capable of continuously measuring the linear acceleration {right arrow over (a)}, and the angular velocity {right arrow over (ω)}, of an object relative to an inertial reference frame. The linear acceleration {right arrow over (a)} may be measured using an accelerometer, and the angular velocity {right arrow over (ω)} may be measured using a gyroscope. It is noted that through mathematical integration techniques known in the art, the position and orientation of the object can be determined from {right arrow over (a)} and {right arrow over (ω)} by integrating the equations of motion. Inertial tracking requires that the initial position and orientation of an object be recorded and stored. {right arrow over (a)} and {right arrow over (ω)} of the object are typically measured at rapid intervals. Since the initial position and orientation of the object is known, using the equations of motion, the dynamic position and orientation of an object can be continuously predicted, based on the mathematical integration of {right arrow over (a)} and {right arrow over (ω)} measurements. Therefore, given a previous estimate of the position and orientation of an object, the current position and orientation of the object can be determined by integrating the equations of motion based on newly measured values of {right arrow over (a)} and {right arrow over (ω)}.
Inertial trackers incorporate two additional requirements. The first requirement is an accurate model of the inertial sensor, so that raw data measured from the inertial sensor can be corrected for bias, scale factor and non-orthogonality. The second requirement is an estimation of the position and orientation of the object at sufficiently close time intervals. Accordingly, when the second requirement is met, one can assume that the current position and orientation of the object are determined by {right arrow over (a)} and {right arrow over (ω)} alone, and not in addition to any other higher-order derivates of the quantities of motion, such as jerk, {right arrow over ({dot over (a)} (the rate of change of acceleration) and rotational acceleration, {right arrow over (α)}, where {right arrow over (α)} is the derivative of {right arrow over (ω)}. If the initial position and orientation of the object are not known, then an inertial tracker cannot determine the dynamic position and orientation of the object. If either of the two aforementioned requirements is not met, then the position and orientation determined by an inertial tracker may be inaccurate.
Furthermore, inertial trackers suffer from drift. Due to the noise or jitter in the measurements of the inertial tracker, the integration of these measurements over a period of time results in a drift error, which gradually increases the inaccuracy of the final results. Drift is usually corrected by using very expensive gyroscopes or accelerometers to implement the inertial tracking measurements.
Electromagnetic (EM) tracking operates on a different principle than inertial tracking. In general, electromagnetic tracking involves determining the position and orientation of an electromagnetic receiver relative to an electromagnetic source. An electromagnetic source generates an electromagnetic field within a platform (e.g., the cockpit of an airplane). The electromagnetic source includes at least one electromagnetic coil that transmits an electromagnetic field. For example, the electromagnetic source may include three mutually perpendicular transmitting coils, each defining a respective axis. An electromagnetic receiver unit on the tracked object (e.g., the helmet of the pilot) measures the generated electromagnetic field. The electromagnetic receiver unit includes at least one sensor (e.g., three) that measures values associated with the electromagnetic field. Each sensor includes at least one electromagnetic coil that receives an electromagnetic field. For example, each sensor may include three mutually perpendicular receiving coils, each defining a respective axis. The electromagnetic source generates a unique electromagnetic field, in the sense that each point in the motion platform has a unique measurable quantity associated with the electromagnetic field (e.g., the electromagnetic field strength as a vector quantity). The position and orientation of the sensor is obtained by analyzing and comparing the electromagnetic field generated by the transmitting coils and the electromagnetic field measured by the receiving coils.
One approach for EM tracking is based on using a mathematical model of the generated electromagnetic field. The mathematical model is characterized by a set of model parameters, with one set for each transmitting coil. The model predicts the electromagnetic field value (i.e., the measurable quantity associated with the electromagnetic field) at each point within the platform. The EM tracker solves for the position and orientation at which the electromagnetic field values predicted by the mathematical model of the electromagnetic field best match the field values measured by the receiving coils.
A cost function defines the difference between the measured EM field and the model. By minimizing the cost function using a best-fit algorithm, the EM tracker finds the closest match between the model of the EM field and the measured EM field. Noise and other factors influence the final result.
The calculation of position and orientation in EM tracking in this manner necessitates two requirements. The first requirement is that the EM tracker possesses an accurate model of the EM field transmitted by the electromagnetic source, for every point within the platform. It is noted that there are a number of methods known in the art for obtaining all the necessary model parameters, such as analytical modeling (as described in F. Raab, E. Blood, T. Steiner and H. Jones, “Magnetic Position and Orientation Tracking System”, IEEE Transactions on Aerospace and Electronic Systems, 1979, Vol. AES-15, No. 5, pp. 709-717; and U.S. Pat. No. 3,868,565 to Kuipers, entitled “Object Tracking and Orientation Determination means, System and Process”), and adaptive modeling (as described in US Patent Application No. 2004/0034515 A1 to Bar Tal et al, entitled “Estimating Position and Orientation in Electromagnetic Systems”). The second requirement is that the EM tracker possesses an accurate model of the receiving coils (i.e., size, shape, and other physical parameters of the coils). This model is a rigid structure characterized by the vector position of each sensor in the electromagnetic receiver unit, and the calibration parameters of each sensor.
Several EM trackers suffer from latency. Typically, in order to reduce the noise in the EM field measurements, a large number of previous measurements are averaged. This procedure results in a substantial delay, or latency, between the position and orientation determined by the EM tracker, and the actual current position and orientation of the tracked object.
It is noted that EM tracking is a special case of the broader category of non-inertial tracking. Other forms of non-inertial tracking, such as optical, acoustic, or radio frequency (RF), involve generating and receiving some form of energy or signal (i.e., light waves, sound waves, or radio frequencies, respectively).
Non-inertial trackers may be influenced by external factors in the platform environment. For example, EM trackers may be influenced by electrical or magnetic objects in the environment, which can interfere with the electromagnetic field generated by the electromagnetic source, or with the electromagnetic field received by the electromagnetic receiver unit. Another example involves optical trackers, where objects near the tracked object may block a direct line-of-sight from a sensor to a transmitter, and prevent the sensor from receiving the transmitted optical signal. These types of interference may result in limitations for the environment in which non-inertial tracking is implemented.
U.S. Pat. No. 5,645,077 to Foxlin entitled “Inertial Orientation Tracker Apparatus Having Automatic Drift Compensation for Tracking Human Head and Other Similarly Sized Body”, is directed to an inertial angular orientation tracking apparatus which utilizes additional sensors for drift correction. A tracking device includes a set of drift sensitive sensors, a set of drift compensating sensors, an integration module, and a drift compensation module. The drift sensitive sensors are typically angular rate sensors, such as rate gyroscopes, and are subject to drift over time. The drift compensating sensors are angular position sensors, typically inclinometers and compasses, and are relatively impervious to drift over time. The angular rate sensors and drift compensating sensors are mounted upon a human body member, such as a head. The angular rate sensors and drift compensating sensors are impervious to interference from electromagnetic, acoustic, optical and mechanical sources.
The set of angular rate sensors generate signals corresponding to the rate of change of the angular orientation of the individual sensors. The integration module integrates the signals, to provide a set of orientation angles. The set of drift compensating sensors generate an angular position signal related to the angular orientation of the body member to which the sensors are attached. The angular position signal is provided to the drift compensation module. The drift compensation module uses the angular position signal to generate a signal representing the angular orientation corrected for errors in the angular orientation provided by the integration module. These errors may arise due to drift. Drift results from integrating over time a signal which is noisy or has a bias. Drift would make the virtual world appear to gradually rotate about the head of the user even when the user is not moving. The corrected angular orientation signal is provided as output to external equipment, such as a virtual reality system, a computer, or a teleoperator.
The drift compensating module may include a verifier that periodically measures the orientation of the body member, by a means different from the angular rate sensors. The verifier generates an orientation drift compensation signal based on the verification measurement, to reduce the effect of drift. The verifier may take into account characteristic features of human motion, such as stillness periods. The drift compensating module may be implemented using a Kalman filter, which may utilize statistical data about human head motion.
U.S. Pat. No. 6,409,687 to Foxlin entitled “Motion Tracking System”, is directed to an inertial motion tracking apparatus which utilizes acoustic measurements for drift correction. A tracking device includes an Inertial Measurement Unit (IMU), an array of three Ultrasonic Range Measurement (URM) Units, and a processor. The processor is coupled the IMU and the URMs. The tracking device may be fixed on the head of an operator, such as to a Head-Up Display (HUD).
The IMU includes three angular rate sensors and three linear acceleration sensors. The sensors are arranged along three orthogonal axes that are fixed in the reference frame of the tracking device. Each acceleration sensor provides a signal proportional to the acceleration along the corresponding axis. Each angular rate sensor provides a signal proportional to the rotation rate about the corresponding axis. The processor determines the position and orientation of the tracking device using the inertial measurements received from the IMU.
An ultrasonic beacon array includes a plurality of ultrasonic beacons at fixed locations on the environment, such as arranged on the ceiling of the room in a regular pattern. The URM receives acoustic signals sent by the ultrasonic beacons. The processor estimates a range measurement relating to the distance between the beacons and the tracking device, using the received signals along with the known three-dimensional location of the beacons. The processor updates the position and orientation of the tracking device using the range measurements.
Each ultrasound beacon sends an acoustic pulse to the URMs. Each URM broadcasts infrared (IR) signals to all the ultrasonic beacons. The IR signals include address information, so that only a selected number of ultrasound beacons respond. In response to an IR signal, the intended ultrasonic beacons immediately send acoustic pulses, which are received by one or more URMs. The processor determines the time of flight, by measuring the delay from issuing the IR command to detecting the ultrasonic pulse. The range measurement may be based on the time of flight.
The tracking device combines measurements from both measurement modes, and adjusts its estimates of position and orientation to reflect measurements from both modes as they are available. The processor may implement an extended Kalman Filter to combine the measurements and maintain ongoing estimates of the position and orientation of the tracking device, along with the degree of uncertainty of these estimates.
The tracking device continues using inertial measurements when acoustic measurements are unavailable, such as due to occlusion of the beams. Drift correction is then made once the acoustic measurements become available.
U.S. Pat. No. 6,474,159 to Foxlin et al. entitled “Motion-Tracking”, is directed to an inertial tracker for tracking motion relative to a moving platform. An inertial head-tracking system on board a moving platform includes a tracking Inertial Measurement Unit (IMU), a reference IMU, and a processor unit. The tracking IMU is mounted on the tracked object. The reference IMU is mounted on the moving platform, such as on an external X-bar fixed to the environment (e.g., the canopy of the simulator cab or the cockpit).
Each of the reference IMU and the tracking IMU may include three angular inertial sensors having angular rate sensors. The processor unit receives signals from the tracking IMU and the reference IMU, and determines the orientation of the object relative to the moving platform. In particular, the processor unit computes the orientation of the object with respect to a fixed inertial reference frame using the signals from the tracking IMU. The processor unit further computes the orientation of the moving platform with respect to the same fixed inertial reference frame using the signals from the reference IMU. Based on these two orientations, the processor unit then computes the orientation of the object relative to the moving platform. The orientation relative to a given reference frame is determined by integrating a relative angular rate signal measured by the IMU.
A non-inertial tracking subsystem may make independent measurements of the orientation of the object relative to the moving reference frame. The inertial head-tracking system may use these measurements to correct for any drift that may occur in the inertial orientation integration. The non-inertial tracking subsystem may use optical, acoustic, magnetic, radio frequency, or electromagnetic technologies.
Each of the reference IMU and the tracking IMU may further include three linear accelerometers. The processor unit may further determine the position of the object relative to the moving platform, using signals from the tracking IMU and the reference IMU. In particular, the processor unit computes the position of the object with respect to a fixed inertial reference frame using the signals from the tracking IMU. The processor unit further computes the position of the moving platform with respect to the same fixed inertial reference frame using the signals from the reference IMU. Based on these two positions, the processor unit then computes the position of the object relative to the moving platform. The position relative to a given reference frame is determined by double-integrating a relative linear acceleration signal measured by the IMU.
The calculation of the linear acceleration signal may include compensation for tangential, Coriolis, and centripetal acceleration effects caused by the angular velocity and angular acceleration of the moving platform. The non-inertial tracking subsystem may further make independent measurements of the position of the object relative to the moving reference frame. The inertial head-tracking system may then use these measurements to correct for any drift that may occur in the inertial position integration.
US Patent Application No. 2004/0073360 A1 to Foxlin entitled “Tracking, auto-calibration, and map-building system”, is directed to a method and system for navigating a vehicle in a certain environment. The navigation system builds a map of the environment, calibrates the sensors, and determines the location of the vehicle in the environment frame of reference. A navigation system includes sensors and a data processing unit. The sensors provide measurement data. The data processing unit processes the data provided by the sensors.
The system includes a sensor subsystem and an estimation subsystem coupled thereto. The sensor subsystem includes inertial sensors and sensing devices or targets. The sensing devices or targets, referred to as Pose Sensing Elements (PSEs), may be in the vehicle (referred to as “mobile PSE”), or fixed in the environment (referred to as “fixed PSE”). The inertial sensors provide inertial measurements to the navigation system. The PSEs provide sensor measurements, which are measurements between elements in the vehicle and elements external to the vehicle. Sensor measurements relate to measurements between a mobile PSE and a fixed PSE, and depend on aspects of the relative pose of the two PSEs.
The estimation subsystem accepts configuration data and measurement information from the sensor subsystem. The measurement information relates to the relative position and orientation of sensing elements. The configuration data enables selective performance of different functions for the estimation subsystem. The estimation subsystem repeatedly updates a state estimate, which characterizes an estimate of the location of the vehicle, according to the accepted measurement data and configuration data. The state estimate may be an estimate of the position or orientation of the vehicle relative to the environment, the position and orientation, relative to the vehicle, of a sensing device fixed to the vehicle, or the position and orientation, relative to the environment, of a sensing device fixed in the environment. Updating the state estimate may be performed using a Kalman filter.
In accordance with the disclosed technique, there is thus provided an apparatus for determining the position of a selected object relative to a moving reference frame. The apparatus includes at least one reference frame transceiver assembly firmly attached to the moving reference frame, at least one object transceiver assembly firmly attached to the selected object, an inertial measurement unit (IMU) firmly attached to the selected object, an inertial navigation system (INS) firmly attached to the moving reference frame, and a tracking processor coupled with the object transceiver assembly, with the inertial measurement unit and with the inertial navigation system. The object transceiver assembly communicates with the reference frame transceiver assembly using magnetic fields. The IMU produces IMU inertial measurements of motion of the selected object with respect to an inertially fixed reference frame. The INS produces INS inertial measurements of motion of the moving reference frame with respect to the inertially fixed reference frame. The tracking processor receives electromagnetic measurements resulting from the magnetic communication between the reference frame transceiver assembly and the object transceiver assembly. The tracking processor determines the position of the selected object relative to the moving reference frame by using the IMU inertial measurements and the INS inertial measurements to optimize the electromagnetic measurements.
The apparatus may further include a transmitter interface coupled with the reference frame transceiver assembly, and a sensor interface coupled with the object transceiver assembly. The transmitter interface generates a digital signal representing the characteristics of a magnetic field to be transmitted. The sensor interface receives the electromagnetic measurements from the object transceiver assembly and provides the electromagnetic measurements to the tracking processor.
In accordance with the disclosed technique, there is further provided a method for determining the position of a selected object relative to a moving reference frame. The method includes the procedure of communicating using magnetic fields. The method further includes the procedures of producing IMU inertial measurements of motion of the selected object with respect to an inertially fixed reference frame, and producing INS inertial measurements of motion of the moving reference frame with respect to the inertially fixed reference frame. The method further includes the procedures of receiving electromagnetic measurements resulting from the magnetic communication, and determining the position of the selected object relative to the moving reference frame by using the IMU inertial measurements and the INS inertial measurements to optimize the electromagnetic measurements.
The disclosed technique will be understood and appreciated more fully from the following detailed description taken in conjunction with the drawings in which:
The disclosed technique overcomes the disadvantages of the prior art by providing a hybrid electromagnetic-inertial tracker to determine the position of a selected object relative to a moving reference frame. The hybrid tracker obtains both EM measurements and inertial measurements. The hybrid tracker utilizes the raw inertial measurements to optimize the position and orientation obtained from the EM measurements, thereby improving the accuracy and stability of the tracking system.
Trackers are characterized by the number of degrees of freedom (DOF) tracked. The general case deals with six DOF, consisting of the position and orientation of the object in three-dimensional space. The location of the tracked object is characterized by the three-dimensional coordinates of a reference point fixed in the object, with respect to a coordinate system fixed in the platform under consideration. The orientation of the tracked object is specified as a set of angles, typically the modern Euler angles (i.e., azimuth, elevation, and roll; alternately known as pitch, yaw, and roll) which characterize the rotation of the tracked object with respect to the aforementioned coordinate system. Location is denoted as {right arrow over (r)}, and orientation is denoted as {tilde over (R)}, herein below. The term “position” herein below, refers either to the location, to the orientation, or both the location and the orientation, of an object in a three-dimensional coordinate system.
It is noted that the disclosed technique is applicable to the tracking of any type of object relative to any particular platform (i.e., a moving reference frame). For example, the tracked object may be a medical probe relative to a human body, the helmet of a pilot relative to the cockpit of an aircraft, the helmet of a tank crew member relative to the inside of a tank, a selected body part of a person relative to a room, and the like. The following includes a description of the disclosed technique used in conjunction with tracking the helmet of a pilot relative to the cockpit of an aircraft.
Reference is now made to
Transmitter interface 110 is coupled with amplifier 114. Amplifier 114 is further coupled with transmitter assembly 116. Sensor assemblies 1201, 1202, 1203 and 120N are coupled with sensor interface 124. Tracking processor 130 is coupled with sensor interface 124, with IMU 126, and with INS 128.
Transmitter assembly 116 includes at least one transmitting coil (not shown), for transmitting a magnetic field. For example, transmitter assembly 116 may include three mutually perpendicular transmitting coils, each defining a respective spatial axis. It is noted that hybrid tracker 100 may also include several transmitter assemblies. More generally, transmitter assembly 116 may be a reference frame transceiver assembly, which can either transmit a magnetic field or receive a magnetic field.
Each of sensor assemblies 1201, 1202, 1203 and 120N includes at least one receiving coil (not shown), for receiving a magnetic field. For example, each of sensor assemblies 1201, 1202, 1203 and 120N may include three mutually perpendicular receiving coils, each defining a respective spatial axis. More generally, sensor assemblies 1201, 1202, 1203 and 120N may be an object transceiver assembly, which can either transmit a magnetic field or receive a magnetic field. Each of sensor assemblies 1201, 1202, 1203 and 120N are mounted on a selected object to be tracked (e.g., the helmet of a pilot), while transmitter assembly 116 is fixed at a general location within the platform (i.e., the cockpit). Alternatively, transmitter assembly 116 is mounted on the tracked object, and sensor assemblies 1201, 1202, 1203 and 120N are fixed at a general location within the platform. The following description of the disclosed technique uses the example that hybrid tracker 100 includes a single sensor assembly (herein denoted sensor assembly 1201), which is firmly attached to the tracked object, and a transmitter assembly 116 which is firmly attached to the platform.
Transmitter interface 110 generates a digital signal representing the characteristics of a magnetic field to be transmitted. For example, the digital signal may be a mathematical function comprised of several different individual frequencies, each at different amplitudes. Alternatively, the digital signal may be any generalized waveform that indicates the characteristics of the magnetic field to be transmitted. The digital signal may be predetermined, or calculated in accordance with real-time properties of hybrid tracker 100. Transmitter interface 110 provides the digital signal to amplifier 114 via a digital to analog converter (not shown), which converts the digital signal to a set of analog signals. Each analog signal may be respective of a different spatial axis. Amplifier 114 amplifies the respective analog signals and provides the amplified signals to transmitter assembly 116.
Transmitter assembly 116 receives the amplified analog signals and generates a set of magnetic fields. Due to the different spatial orientation of each transmitting coil, transmitter assembly 116 transmits a number of different magnetic fields. In addition, each transmitting coil in transmitter assembly 116 generates a magnetic field oscillating at a different frequency. For example, transmitter assembly 116 transmits at least two magnetic fields (i.e., each magnetic field having three components), in order to achieve sufficient mathematical observability of the parameters of the magnetic field model and the receiving coil calibration parameters (described further below), thereby allowing the object to be accurately tracked. Magnetic field 118 is the superposition of the individual magnetic fields generated by each transmitting coil of transmitter assembly 116, where each generated magnetic field is at a different frequency. Transmitter assembly 116 thereby generates a multiple-component, multiple-frequency magnetic field 118 throughout the platform. Magnetic field 118 is unique in the sense that every point within the platform has a unique measurable quantity. For example, the unique measurable quantity may be the magnetic field vector at each point. Typically, magnetic field 118 includes frequencies on the order of tens of kilohertz.
Magnetic field 118 results in a voltage difference in the receiving coils of sensor assembly 1201, as a result of EM induction. In each receiving coil, the induced voltage is proportional to the magnitude of the magnetic field perpendicular to that receiving coil. Sensor assembly 1201 uses the induced voltage in the receiving coils to measure the local magnetic field vector of magnetic field 118. The measurement of magnetic field 118 is herein below referred to as an “EM measurement”. Sensor assembly 1201 produces a respective analog signal corresponding to the EM measurement in that sensor assembly, and provides the analog signal to sensor interface 124 via an analog to digital converter (not shown), which converts the analog signal to a digital signal. Sensor interface 124 provides the received digital signal of the EM measurement, denoted as {right arrow over (F)}i,j,kmeasured, to tracking processor 130.
IMU 126 is mounted on the object to be tracked (e.g., the helmet of a pilot). IMU 126 measures all three components of the angular velocity {right arrow over (ω)} of the object, and all three components of the linear acceleration {right arrow over (a)} of the object. The measurements of {right arrow over (a)} and {right arrow over (ω)} are taken relative to an inertially fixed reference frame. IMU 126 may be implemented with gyroscopes for measuring the angular velocity {right arrow over (ω)}, and accelerometers for measuring the linear acceleration {right arrow over (a)}. For example, IMU 126 includes three gyroscopes (i.e., one for each spatial axis) and three accelerometers (i.e., one for each spatial axis). If for example the motion of the tracked object is restricted to one degree of freedom, then IMU 126 may measure angular velocity and acceleration with respect to a single spatial axis, using one gyroscope and one accelerometer. IMU 126 provides the measurements of {right arrow over (a)} and {right arrow over (ω)} to tracking processor 130. The measurements of {right arrow over (a)} and {right arrow over (ω)} are herein referred to as “IMU inertial measurements”.
INS 128 is mounted on a moving platform (e.g., the aircraft cockpit), in which the tracked object is present. INS 128 measures all three components of the angular velocity {right arrow over (ω)}platform of the moving platform relative to the inertially fixed reference frame, and all three components of the linear acceleration {right arrow over (a)}platform of the moving platform relative to the inertially fixed reference frame. INS 128 may be implemented with gyroscopes for measuring the angular velocity {right arrow over (ω)}platform, and accelerometers for measuring the linear acceleration {right arrow over (a)}platform. For example, INS 128 includes three gyroscopes (i.e., one for each spatial axis) and three accelerometers (i.e., one for each spatial axis). If for example the motion of the tracked object is restricted to one degree of freedom, then INS 128 may measure angular velocity and acceleration with respect to a single spatial axis, using one gyroscope and one accelerometer. INS 128 provides the measurements of {right arrow over (ω)}platform and {right arrow over (a)}platform to tracking processor 130. The measurements of {right arrow over (a)}platform and {right arrow over (ω)}platform are herein referred to as “INS inertial measurements”. The IMU inertial measurements and INS inertial measurements are collectively referred to as “inertial measurements” herein below. It is noted that IMU 126 and INS 128 are identical elements, in that both measure angular velocity and linear acceleration. However in practice, IMU 126 and INS 128 may have different accuracy, weight, cost, hardware, or other factors associated therewith.
Tracking processor 130 receives as input data {right arrow over (F)}i,j,kmeasured from sensor interface 124, as well as {right arrow over (a)} and {right arrow over (ω)} from IMU 126 and {right arrow over (ω)}platform and {right arrow over (a)}platform from INS 128. Tracking processor 130 determines the position and orientation of the pilot helmet by using the inertial measurements to optimize the electromagnetic measurements, as described further with reference to
Hybrid tracker 100 has several requirements. The first requirement is an accurate model of the magnetic field generated by transmitter assembly 116, denoted {right arrow over (F)}emModel, for every point within the cockpit. The model {right arrow over (F)}emModel is characterized by a set of model parameters hiem, one for each transmitting coil of transmitter assembly 116. The model {right arrow over (F)}emModel predicts the field at each point {right arrow over (r)} in the cockpit. Another requirement for hybrid tracker 100 is an accurate model of the receiving coils of sensor assembly 1201. This model is characterized by the vector position {right arrow over (d)}k of sensor assembly 1201, and the calibration parameters ck,jem of sensor assembly 1201. A further requirement for hybrid tracker 100 is having calibration parameters cinertial of IMU 126, to correct for bias, scale factor, and non-orthogonality. It is noted that calibration parameters are also required for INS 128, although in practice, INS 128 is an independent self-contained system and provides data that is already calibrated. Yet another requirement for hybrid tracker 100 involves estimates of noise in the EM measurements and inertial measurements. The noise in the kth sensor assembly is denoted σk2, the noise in the inertial estimate of position is denoted σinertial-pos2, and the noise in the inertial estimate of orientation is denoted σinertial-ang2.
It is noted that several of the required parameters, such as calibration parameters ck,jem and cinertial, may change over time (e.g., due to temperature, aging, stress, and the like), and hybrid tracker 100 may not possess an accurate enough model of these changes. Furthermore, the actual magnetic field in the platform may drift from its modeled value {right arrow over (F)}emModel. However, as long as these changes vary at a relatively slow rate, or change abruptly but remain fixed afterwards, the changes can be corrected using techniques known in the art, such as Kalman filters. For example, assuming that calibration parameters ck,jem and cinertial are constant but different from their current values, hybrid tracker 100 may find the values of δck,jem and δcinertial that best describe a “batch” of samples. Another example involves calculating optimal values of the model parameters hiem by a process known as “adaptation”, which is described herein below in
Reference is now made to
Tracking processor 150 includes a position estimator 152, a relative angular velocity estimator 154, and a Kalman filter module 156. Kalman filter module 156 is coupled with position estimator 152 and with relative angular velocity estimator 154.
In the embodiment of
Position estimator 152 estimates the location and orientation ({right arrow over (p)}em and {right arrow over (o)}em) of sensor assembly 1201 based on the received EM measurements using EM tracking, as is known in the art. Relative angular velocity estimator 154 estimates the relative angular velocity {right arrow over (ω)}relative between IMU 126 and INS 128, by subtracting the angular velocity measurements {right arrow over (ω)} and {right arrow over (ω)}platform. The relative angular velocity {right arrow over (ω)}relative represents the angular velocity of the object with respect to the moving platform. Kalman filter module 156 integrates relative angular velocity {right arrow over (ω)}relative, to obtain an inertial based orientation estimate, by the following equation:
{right arrow over (o)}inertial={right arrow over (o)}final-previous+{right arrow over (ω)}relative·Δt (1)
where {right arrow over (o)}final-previous is the orientation value determined by Kalman Filter module 156 in the previous time-step. Equation (1) involves integrating over a single time-step, rather than continuously integrating over multiple time-steps.
Kalman filter module 156 receives the EM based orientation estimate {right arrow over (o)}em, from position estimator 152. Kalman filter module 156 determines a final orientation value {right arrow over (o)}final of the tracked object, using the following equation:
{right arrow over (o)}final={right arrow over (o)}final-previous+K({right arrow over (o)}em−{right arrow over (o)}inertial) (2)
where K is a weighting factor that quantifies the level of confidence of the two types of estimates with respect to one another. By changing the value of K, the weight given to each of the two types of estimates can be varied as a function of the quality of the particular estimate. For example, if the accuracy of the EM components of hybrid tracker 100 becomes degraded for a certain period of time, then the inertial based orientation estimate {right arrow over (o)}inertial may be given a higher weight than the EM based orientation estimate {right arrow over (o)}em during this period. As a result, the accuracy of final orientation value {right arrow over (o)}final is less influenced by the degradation of the EM components of hybrid tracker 100.
Kalman filter module 156 feeds back the final orientation value {right arrow over (o)}final to relative angular velocity estimator 154. Relative angular velocity estimator 154 uses the final orientation value from the previous time-step {right arrow over (o)}final-previous in order to properly subtract the angular velocity measurements {right arrow over (ω)} and {right arrow over (ω)}platform.
It is noted that the location estimate may also be filtered, if desired, by double integrating a relative acceleration value, obtained from the linear acceleration measurements a from IMU 126, and {right arrow over (a)}platform from INS 128. Hybrid tracker 100 is primarily concerned with the measurement of orientation, and therefore for the sake of simplicity, only the orientation measurement is discussed herein below. However, it is appreciated that analogous techniques may be applied with regard to location (e.g., by substituting linear acceleration measurements from the inertial components for angular velocity measurements and performing a double integration instead of a single integration).
It is further noted that hybrid tracker 100 utilizes inertial measurements to reduce noise in the EM measurements, which does not result in any additional latency between the location and orientation determined by tracking processor 150, and the actual current position and orientation. This feature is also applicable to the embodiments of
Reference is now made to
Tracking processor 170 includes a position estimator 172, an EM angular velocity estimator 174, a relative angular velocity estimator 176, an angular velocity optimizer 178, and a Kalman filter module 180. Kalman filter module 180 is coupled with position estimator 172, with relative angular velocity estimator 176, and with angular velocity optimizer 178. Relative angular velocity estimator 176 is coupled with angular velocity optimizer 178. Angular velocity optimizer 178 is further coupled with EM angular velocity estimator 174. Position estimator 172 is optionally coupled with EM angular velocity estimator 174.
Tracking processor 170 receives EM measurements from sensor interface 124, and inertial measurements from IMU 126, and from INS 128.
Position estimator 172 estimates the location and orientation ({right arrow over (p)}em and {right arrow over (o)}em) of sensor assembly 1201 based on the received EM measurements using EM tracking, as is known in the art. EM angular velocity estimator 174 estimates an EM based angular velocity {right arrow over (ω)}em based on the received EM measurements. EM angular velocity estimator 174 may use numerical differentiation of the orientation {right arrow over (o)}em provided by position estimator 172 (i.e., by subtraction of the orientation at successive points). Alternatively, EM angular velocity estimator 174 estimates the angular velocity directly from the received EM measurements. With reference to
Relative angular velocity estimator 176 estimates the relative angular velocity {right arrow over (ω)}relative between IMU 126 and INS 128, by subtracting the angular velocity measurements {right arrow over (ω)} and {right arrow over (ω)}platform. The relative angular velocity {right arrow over (ω)}relative represents the angular velocity of the object with respect to the moving platform. Angular velocity optimizer 178 optimally combines the EM based angular velocity {right arrow over (ω)}em and the inertial based angular velocity {right arrow over (ω)}relative to obtain a final relative angular velocity value {right arrow over (ω)}opt, using the following equation:
{right arrow over (ω)}opt=α·{right arrow over (ω)}em+β·{right arrow over (ω)}relative (3)
Kalman filter module 180 receives the optimal angular velocity value {right arrow over (ω)}opt from angular velocity optimizer 178, and determines a final orientation value {right arrow over (o)}final of the tracked object using the following equation:
{right arrow over (o)}final={right arrow over (o)}final-previous+{right arrow over (ω)}opt·Δt (4)
where {right arrow over (o)}final-previous is the orientation value determined by Kalman Filter module 180 in the previous time-step. Kalman filter module 180 outputs the final orientation value {right arrow over (o)}final. Kalman filter module 180 also feeds back the final orientation value {right arrow over (o)}final to relative angular velocity estimator 176. Relative angular velocity estimator 176 uses the final orientation value from the previous time-step {right arrow over (o)}final-previous in order to properly subtract the angular velocity measurements {right arrow over (ω)} and {right arrow over (ω)}platform.
Angular velocity optimizer may be a Kalman Filter, operating at a different rate than Kalman Filter module 180. Alternatively, Kalman Filter module 180 performs the calculation of optimal angular velocity value {right arrow over (ω)}opt, instead of angular velocity optimizer 178.
Reference is now made to
In the embodiment of
Position optimizer unit 192 receives all the raw input data, including the EM measurements and the inertial measurements. Position optimizer unit 192 determines a final orientation value {right arrow over (o)}final of the tracked object by minimizing the following general cost function: χ2, defined as:
where the function Angles(U) extracts the three Euler orientation angles from an input rotation matrix:
Angles({tilde over (R)}(Ψazimuth,Θelevation,Φroll))≡(Ψazimuth,Θelevation,Φroll).
Equation (5) is minimized using a non-linear minimization technique, as known in the art. Position optimizer unit 192 outputs the final orientation value {right arrow over (o)}final.
Reference is now made to
It is recalled that hybrid tracker 100 uses a model {right arrow over (F)}emModel of the magnetic field, which predicts the field value for every point in the cockpit. The model {right arrow over (F)}emModel may need to be updated if certain conditions occur, such as if the electromagnetic environment changes (e.g., the pilot adjusts his metallic seat or introduces a new metallic object into the cockpit), if one of the EM components of hybrid tracker 100 changes (e.g., if a component heats up, ages, or is replaced), or if the location and orientation of transmitter assembly 116 changes (e.g., if transmitter assembly 116 or any part thereof is removed and replaced). In addition to the position calculation, hybrid tracker 100 also uses model {right arrow over (F)}emModel for performing a self-consistency check of accuracy. During the calculation of position, hybrid tracker 100 also calculates a figure-of-merit (FOM). The FOM represents the degree of accuracy of the position calculation. In particular, the FOM is the difference between the measured magnetic field {right arrow over (F)}i,j,kmeasured and the magnetic field model {right arrow over (F)}emModel at the best estimate of position. If the FOM is low, then the calculated position is expected to be accurate, and thus the current model {right arrow over (F)}emModel still accurately describes the magnetic field for the current environmental conditions. If, however, the FOM is high, then the accuracy of the calculated position is degraded, and thus the current model {right arrow over (F)}emModel no longer accurately describes the magnetic field for the current environmental conditions.
When a high FOM value is obtained at a sufficient number of points in the cockpit, hybrid tracker 100 develops a new model {right arrow over (F)}emModel of the magnetic field, in a process known as “adaptation”. Following adaptation, hybrid tracker 100 possesses an accurate model {right arrow over (F)}emModel of the magnetic field, but lacks a fixed transformation, which is essential if the new model is to be used. This transformation is the orientation of the magnetic field model coordinate system with respect to the coordinate system of the platform (e.g., the aircraft cockpit). The process of finding this transformation is known as “harmonization”.
In the embodiment of
EM adaptive unit 196 receives all the input data and performs harmonization using a minimization cost function. For each EM measurement, the new magnetic field model {right arrow over (F)}emModel provides an estimate of the orientation of the helmet in the magnetic field model coordinate system, denoted collectively as {Oi}. EM adaptive unit 196 seeks the single transformation {tilde over (H)}harmonization which minimizes the cost function:
EM adaptive unit 196 outputs the new magnetic field model {right arrow over (F)}emModel, along with the necessary transformations, which are stored in tracking processor 194.
It is noted that the calculation performed by EM adaptive unit 196 requires that the acceleration of the moving platform changes in direction from measurement to measurement. The calculation also assumes that the acceleration of the head relative to the platform averages to zero over the entire set of measurements. This can be ensured if measurements are taken over a sufficiently long period of time, or if measurements are only taken when the head is not accelerating relative to the platform.
Reference is now made to
In procedure 304, the local magnetic field vector of the generated magnetic field is measured. With reference to
In procedure 306, the position of the sensor assembly is estimated, using EM tracking. With reference to
In procedure 308, the angular velocity of the tracked object and the angular velocity of the moving platform are measured. With reference to
In procedure 310, the relative angular velocity, of the object with respect to the moving platform, is estimated. With reference to
In procedure 312, the relative angular velocity is integrated over a single time-step, to obtain an inertial based estimate of the orientation of the tracked object. With reference to
In procedure 314, the EM based orientation estimate and the inertial based orientation estimate are provided, as input to a Kalman filter. With reference to
In procedure 316, a final orientation value of the tracked object is determined. With reference to
Reference is now made to
In procedure 326, the position of the sensor assembly is estimated using EM tracking. With reference to
In procedure 328, the angular velocity of the tracked object is measured from EM measurements. With reference to
In procedure 330, the angular velocity of the tracked object and the angular velocity of the moving platform are measured from inertial components. Procedure 330 is analogous to procedure 308 discussed with reference to
In procedure 332, the relative angular velocity of the object with respect to the moving platform is estimated. With reference to
In procedure 334, both angular velocity estimates are optimally combined. With reference to
In procedure 338, the EM based orientation estimate and the optimal angular velocity are provided as input to a Kalman filter. With reference to
Reference is now made to
In procedure 356, the EM measurements are provided to an optimizing unit. With reference to
In procedure 358, the angular velocity of the tracked object and the angular velocity of the moving platform are measured. Procedure 358 is analogous to procedure 308 discussed with reference to
In procedure 362, all inputted raw data is optimized to obtain a final orientation value of the tracked object. With reference to
Reference is now made to
In procedure 386, the EM measurements and the current magnetic field model are provided to an adaptation unit. With reference to
In procedure 388, the linear acceleration of the tracked object and the linear acceleration of the moving platform are measured. With reference to
In procedure 390, the linear acceleration measurements are provided to an adaptation unit. With reference to
In procedure 392, the current magnetic field model is updated and harmonization is performed in an adaptation unit. With reference to
It will be appreciated by persons skilled in the art that the disclosed technique is not limited to what has been particularly shown and described hereinabove. Rather the scope of the disclosed technique is defined only by the claims, which follow.
Number | Date | Country | Kind |
---|---|---|---|
167648 | Mar 2005 | IL | national |
Number | Name | Date | Kind |
---|---|---|---|
3868565 | Kuipers | Feb 1975 | A |
5645077 | Foxlin | Jul 1997 | A |
5812257 | Teitel et al. | Sep 1998 | A |
5847976 | Lescourret | Dec 1998 | A |
6176837 | Foxlin | Jan 2001 | B1 |
6263264 | Herman et al. | Jul 2001 | B1 |
6281970 | Williams et al. | Aug 2001 | B1 |
6292750 | Lin | Sep 2001 | B1 |
6311129 | Lin | Oct 2001 | B1 |
6369564 | Khalfin et al. | Apr 2002 | B1 |
6408245 | An et al. | Jun 2002 | B1 |
6409687 | Foxlin | Jun 2002 | B1 |
6474159 | Foxlin et al. | Nov 2002 | B1 |
6480152 | Lin et al. | Nov 2002 | B2 |
6577976 | Hoff et al. | Jun 2003 | B1 |
6658354 | Lin | Dec 2003 | B2 |
6681629 | Foxlin et al. | Jan 2004 | B2 |
6691033 | Li et al. | Feb 2004 | B1 |
6725173 | An et al. | Apr 2004 | B2 |
6738714 | McCall et al. | May 2004 | B2 |
6820025 | Bachmann et al. | Nov 2004 | B2 |
7000469 | Foxlin et al. | Feb 2006 | B2 |
7089148 | Bachmann et al. | Aug 2006 | B1 |
7162367 | Lin et al. | Jan 2007 | B2 |
7212921 | Jeerage et | May 2007 | B2 |
7277834 | Bar Tal et al. | Oct 2007 | B2 |
7355561 | Clarke et al. | Apr 2008 | B1 |
7409290 | Lin | Aug 2008 | B2 |
20010020216 | Lin | Sep 2001 | A1 |
20020021245 | Lin et al. | Feb 2002 | A1 |
20020116125 | Lin | Aug 2002 | A1 |
20020116126 | Lin | Aug 2002 | A1 |
20020120400 | Lin | Aug 2002 | A1 |
20020194914 | Foxlin et al. | Dec 2002 | A1 |
20030023192 | Foxlin | Jan 2003 | A1 |
20030149528 | Lin | Aug 2003 | A1 |
20030176970 | Lin | Sep 2003 | A1 |
20040034515 | Bar Tal et al. | Feb 2004 | A1 |
20040073360 | Foxlin | Apr 2004 | A1 |
20040143176 | Foxlin | Jul 2004 | A1 |
20040149036 | Foxlin et al. | Aug 2004 | A1 |
20040236509 | Jeerage et al. | Nov 2004 | A1 |
20050203701 | Scherzinger | Sep 2005 | A1 |
20050234644 | Lin | Oct 2005 | A1 |
20050240347 | Yang | Oct 2005 | A1 |
20050285591 | Higgins et al. | Dec 2005 | A1 |
Number | Date | Country | |
---|---|---|---|
Parent | 11343486 | Jan 2006 | US |
Child | 11413936 | US |