Determination of Relative Pose Using Artificial Magnetic Fields

Abstract

The pose or position and orientation of a wearable sensor assembly in a reference coordinate system are to be determined in real-time. One or more artificial magnetic field sources are located with known positions and orientations in the reference coordinates. Each source generates a magnetic field which varies in time according to a predetermined AC pulse code, so that each source is uniquely identifiable and distinguishable from ambient DC fields. The magnitude and direction in wearable coordinates of the AC magnetic field due to each source varies in a modelable way with position and orientation of the wearable, which comprises a nine-channel Inertial Measurement Unit (IMU) and a processor. The IMU senses the inertial motion of the wearable and the time-varying magnetic field to which it's exposed. These data are processed to estimate the position and orientation of the wearable in reference coordinates, along with various constant parameters.

Claims

1. A method for determining relative pose, comprising one or more magnetic field Emitters, a Wearable sensor assembly whose pose is to be determined, and a Pose Estimation Algorithm.

2. The Emitters of claim 1, each said Emitter further having known location and orientation in an inertially fixed or nearly inertially fixed Reference Coordinate System.

3. The Emitters of claim 2, each said Emitter further modulating its output magnetic field with a predetermined AC pulse code.

4. The Wearable of claim 1, further comprising an Inertial Measurement Unit (IMU) and a Processor.

5. The IMU of claim 4, further comprising a three-axis Gyroscope, a three-axis Accelerometer, and a three-axis Magnetometer.

6. The Processor of claim 4, further reading data periodically from said IMU and processing said data in said Pose Estimation Algorithm.

7. The Pose Estimation Algorithm of claim 1, further comprising the twelve major steps Read IMU, Gyro Processing, Attitude Propagation, Accelerometer Processing, Position Propagation, Covariance Propagation or any method of uncertainty propagation, Magnetometer Processing, Synchronization, Magnetic Field Prediction, Magnetic Field Error, Measurement Update by Kalman Filter innovations or any state estimation method or any means of calculating parameter values from measured data, and Emitter Tracking Update.

8. An alternate method for determining relative pose, comprising one or more magnetic field Emitters, a Wearable sensor assembly without inertial instruments, and an appropriately modified Pose Estimation Algorithm.

9. A method for determining pose of one moving body relative to another, comprising one or more magnetic field Emitters mounted on a Reference Body, inertial instruments mounted on said Reference Body, a Wearable sensor assembly whose pose relative to the Reference Body is to be determined, and an appropriately modified Pose Estimation Algorithm.

10. A method for determining unknown components of relative pose, given that one or more components are already known, comprising one or more magnetic field Emitters, a Wearable sensor assembly with fewer than six channels of inertial instruments, and an appropriately modified Pose Estimation Algorithm.

Description

BRIEF DESCRIPTION OF THE DRAWINGS

[0045] FIG. 1 An embodiment with two Emitters in a fixed Reference Coordinate System and a Wearable [0046] 100 Reference Coordinate System [0047] 110 Emitter 1 [0048] 112 Emitter 1 magnetic field lines [0049] 114 Emitter 1 magnetic field vector at Wearable location [0050] 120 Emitter 2 [0051] 122 Emitter 2 magnetic field lines [0052] 124 Emitter 2 magnetic field vector at Wearable location [0053] 130 Wearable [0054] 132 Wearable Coordinate System

[0055] FIG. 2 Emitter [0056] 200 Permeable core [0057] 202 Copper coil [0058] 204 Current drive [0059] 206 Code Generator

[0060] FIG. 3 Wearable Sensor Assembly [0061] 300 9-Channel Inertial Measurement Unit (IMU) [0062] 302 3-Axis Magnetometer [0063] 304 3-Axis Gyroscope [0064] 306 3-Axis Accelerometer [0065] 308 Processor

[0066] FIGS. 4A & 4B An embodiment of the Pose Estimation Algorithm using a 9-Channel IMU [0067] 400 Algorithm Step 1, Read IMU [0068] 402 Algorithm Step 2, Gyro Processing [0069] 404 Algorithm Step 3, Attitude Propagation [0070] 406 Algorithm Step 4, Accelerometer Processing [0071] 408 Algorithm Step 5, Position Propagation [0072] 410 Algorithm Step 6, Covariance Propagation [0073] 412 Algorithm Step 7, Magnetometer Processing [0074] 414 Algorithm Step 8, Synchronization [0075] 416 Algorithm Step 9, Magnetic Field Prediction [0076] 418 Algorithm Step 10, Magnetic Field Error [0077] 420 Algorithm Step 11, Measurement Update [0078] 422 Algorithm Step 12, Emitter Tracking Update

[0079] FIG. 5 An embodiment with two Emitters for co-orbiting spacecraft relative motion [0080] 500 Host Vehicle [0081] 502 Host Vehicle Coordinate System [0082] 504 Host Vehicle IMU [0083] 506 Host Vehicle RF antenna [0084] 514 Emitter 1 magnetic field vector at Co-orbiting Vehicle location [0085] 524 Emitter 2 magnetic field vector at Co-orbiting Vehicle location [0086] 530 Co-orbiting Vehicle [0087] 532 Co-orbiting Vehicle Coordinate System

DETAILED DESCRIPTION OF THE INVENTION

First Embodiment (FIGS. 1 Through 4)

[0088] One embodiment of the invention is shown in FIG. 1. A Reference Coordinate System (100) is defined. It may be inertially fixed or approximated as inertially fixed; for instance, it may be fixed in a room which rotates with the Earth. One or more Artificial Magnetic Field Sources or Emitters (110, 120) are placed in the Reference Coordinate System such that the locations and orientations of the Emitters in Reference coordinates are known. Each Emitter creates a magnetic field (112, 122) whose magnitude and direction vary in a modelable way with position. Each Emitter modulates its output with a predetermined pulse code that makes each Emitter's magnetic field uniquely identifiable and distinguishable from other field sources.

[0089] A Wearable device (130) is free to translate and rotate with respect to the Reference Coordinate System. Attached to the Wearable is its own Wearable Coordinate System (132). The Wearable senses the time-varying values of 1) its inertial angular velocity, 2) its nongravimetric inertial acceleration, and 3) the magnetic field to which the Wearable is exposed. The Wearable uses its sensed data to estimate in real-time the time-varying values of the position and orientation of the Wearable Coordinate System with respect to the Reference Coordinate System.

[0090] FIG. 2 shows the construction of each Emitter. A conducting Coil (202) is wrapped around a solid Core (200) made of any material with high magnetic permeability. A Current Drive electronic circuit (204) pushes current around the Coil, inducing a magnetic field. The Current Drive modulates the current according to a pulse code output by a Code Generator (206).

[0091] FIG. 3 shows the hardware components of the Wearable. An Inertial Measurement Unit or IMU (300) comprises three types of sensor: a three-axis Magnetometer (302), a three-axis Gyroscope or Gyro (304), and a three-axis Accelerometer (306). The IMU is typically a monolithic integrated circuit using solid state devices based on Hall Effect and MEMS technology. But it could also be a set of individual components, and the components could use alternate sensing technologies to achieve higher performance. Alternate sensing technologies include fluxgates or other types of magnetometers; vibrating mass gyros, fiber optic gyros, ring laser gyros, spinning rotor gyros, or other types of gyroscopes; and seismic mass, vibrating mass, or other types of accelerometers. Sensor data from the IMU are read at a uniform sample rate by a Processor (308).

[0092] The Processor processes the sensor data with a strapdown navigation or Pose Estimation Algorithm as shown in FIGS. 4A and 4B. The Algorithm estimates some eighteen quantities or states: three attitude or orientation angles, three components of Gyro bias, three components of position, three components of velocity, three components of Accelerometer bias, and three components of non-Emitter magnetic field. The Algorithm also calculates the estimate covariance or uncertainty. There are twelve major steps executed once per sample frame, as follows.

[0093] First, the current IMU outputs are read (400). Then the Gyro outputs are preprocessed (402) and used to propagate (404) the orientation or attitude estimate from its value at the previous sample time to its predicted value at the present sample time. Then the Accelerometer outputs are preprocessed (406) and used to propagate (408) the velocity and position estimates. Then the Gyro and Accelerometer data are used to propagate (410) the estimate covariance or uncertainty estimate.

[0094] Next, the Magnetometer outputs are preprocessed (412). Then, the recent history of the Magnetometer data is used to synchronize (414) the Wearable clock with the time base used by the Emitters.

[0095] After synchronization, if the pulse code has not incremented since the last sample, the algorithm terminates and the predicted position, velocity, and attitude are taken as the estimated position, velocity, and attitude. Otherwise, the algorithm continues as follows.

[0096] The propagated position and attitude are used to predict (416) the Magnetometer measurements. The predictions are differenced with those measurements to create error signals (418). Then a measurement update (420) is used to improve the estimates of all eighteen states, along with the covariance. The estimation algorithm is typically an Extended Kalman Filter, but it could also be an alternate estimation algorithm such as the Square Root Covariance Filter, Information or Square Root Information Filter, Particle Filter, Unscented Kalman Filter, Quadrature Kalman Filter, Cubature Kalman Filter, or other Bayesian or Least Squares estimation algorithm. It could also be an AI algorithm such as a convolutional neural network or any other data fusion algorithm.

[0097] Finally, a dynamic list of Emitters in range is updated (422). This list is used to improve the computational efficiency of the Synchronization step (414).

Advantages

[0098] The choice of states to estimate is based on our analysis of state observability, coupled with our experiments on performance of low cost IMUs and interfering DC magnetic fields (Fis1). This research provides guidelines for sizing and locating Emitters. It shows that two properly located Emitters are sufficient to produce pose estimates which are long term stable and accurate to centimeters; that Emitters placed periodically around a space of interest can provide unlimited coverage range; and that performance is rather insensitive to Emitter placement.

[0099] Our invention is ideal for use indoors, where GPS signals typically are not available. Because the Emitter fields are encoded AC, it is not sensitive to the Earth's magnetic field or spurious magnetic field sources. Unlike systems based on optical or RF signals, it does not require clear lines of sight from signal source to Wearable. It's also very low cost, as the Wearable can use IMU hardware found in existing mobile devices.

Second Embodiment

[0100] In another embodiment, the Wearable does not incorporate the Gyroscope and Accelerometer. While the system's time response may be slower, our research (Fis2) shows that two or more properly placed Emitters are sufficient to make the 6D pose observable from the Emitter magnetic fields alone. Alternately, data from any other type of pose sensor may be fused with the Magnetometer data to improve accuracy or response time.

Third Embodiment

[0101] In another embodiment, the Reference Coordinate System is not inertially fixed. For example, the Reference Coordinate System could be attached to a spacecraft in orbit, the Emitters could be mounted in that spacecraft, and the objective could be determination of the relative pose of a co-orbiting spacecraft (FIG. 5). An additional set of inertial sensors is attached to the Reference Coordinate System, and the data from those sensors is transmitted to the Processor. The Gyro and Accelerometer outputs from the IMU are differenced with the corresponding outputs from the Reference to produce relative angular velocity and acceleration components. The relative angular velocity and acceleration are used in the Pose Estimation Algorithm, which otherwise is unchanged.

Fourth Embodiment

[0102] In another embodiment, the Wearable is mechanically constrained to have fewer than six degrees of freedom. The Pose Estimation Algorithm is modified to take the constrained coordinates as given and estimate only the unconstrained coordinates. Sensors or individual sensor channels which become redundant may be eliminated.