`
`(12) United States Patent
`Baehmann et a].
`
`(10) Patent No.:
`
`(45) Date of Patent:
`
`US 7,089,148 B1
`Aug. 8, 2006
`
`(54)
`
`(75)
`
`(73)
`
`METHOD AND APPARATUS FOR MOTION
`TRACKING OF AN ARTICULATED RIGID
`BODY
`
`Inventors: Eric R. Baehmann. Oxford. OH (US):
`Robert B. McGhee. Carmel. CA (US):
`Xiaoping Yun. Salinas. CA (US):
`Michael J. Zyda. Carmel. CA (US);
`Douglas L. McKinney. Prunedalc. CA
`(US)
`The United States of America as
`
`Assignee:
`
`(*)
`
`Notice:
`
`represented by the Secretary of the
`Navy. Washington. DC (US)
`
`Subject to any disclaimer. the tenn oi'this
`patent is extended or adjusted under 35
`U.S.C. 154(b) by 33 days.
`
`(21)
`
`Appl. No.: 10/963,007
`
`(22)
`
`Filed:
`
`Sep. 30, 2004
`
`Related U.S. Application Data
`
`(62)
`
`(60)
`
`Division ofapplication No. 10/020.719. filed on Oct.
`30. 2001. now Pat. No. 6.820.025.
`
`Provisional application No. 60/246215. filed on Oct.
`30. 2000.
`
`(51)
`
`Int. Cl.
`
`(2006.01)
`6016‘ 17/00
`(2006.01)
`A6113 5/103
`U.S. Cl.
`. ...................................... 702/151: 600/595
`Field of Classification Search ................ 702/151.
`702/150. 141. 142. 94. 53: 600/595: 345/473
`See application file for complete search history.
`
`References ( .‘ited
`
`U.S. PATENT DOCUMENTS
`
`(52)
`(58)
`
`(56)
`
`5.807.284 A “
`5.819.206 A ‘
`6.636.826 BI “
`6.691.074 Bl ‘
`6.820.025 BI “
`
`90998 Foxlin ........................ 600.1595
`1051998 Iiorton et al.
`.
`...... 702.1150
`1032003 Abe et a1. ............... 702.1151
`
`.............. 702.1190
`2’2004 Moriya et a].
`1132004 Bachmann et al.
`702.94
`
`OTHER PUBLICATIONS
`
`Henault. G.. A Computer Simulation Study and Component
`Evaluation for a Quaternion Filter for Sourceless Tracking
`of Human Limb Segment Motion. Mar. 1977. Naval
`PostGraduate School.*
`
`* cited by examiner
`
`Primary Examiner—John Barlow
`Assistant EmmineriToan M. Le
`
`(74) Attorney. Agent. or FirmiDonald E. Lincoln
`
`(57)
`
`ABSTRACT
`
`One embodiment the invention includes a method of deter-
`mining an orientation of a sensor. The method includes
`measuring a local magnetic field vector and a local gravity
`vector and using those measurements to determine the
`orientation of the sensor. Embodiments can include measur—
`
`ing the magnetic field vector and the local gravity vector
`using quatemion coordinates. Another embodiment includes
`measuring a local magnetic field vector.
`at
`local gravity
`vector. and the angular velocity of the sensor. These three
`vectors are processed to detemtine the orientation of the
`sensor. In one embodiment
`the three vectors can all be
`
`measured in quatemion coordinates. Another method
`embodiment includes detennining a local gravity vector by
`providing a acceleration detector. moving the detector from
`a start point to an end point over a time period. and summing
`acceleration measurements over the time period. The local
`gravity vector is calculated using the sununed acceleration
`measurements.
`
`5.645.077 A "
`
`7‘1997 Foxlin ........................ 6008587
`
`14 (Ilaims, 6 Drawing Sheets
`
`AWM/WWJ
`
`[flaw/£47 4
`re A. a -2
`
`
`
`Google 1004
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`
`Sheet 1 0f 6
`
`
`
`US 7,089,148 B1
`
`
`
`
`
`002
`
`002
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 2 0f 6
`
`
`
`US 7,089,148 B1
`
`
`
`
`“71/
`/
`,
`
`flfgdtng/Z‘fi
`a, xéz 4f , )
`
`
`
`34/
`
`
`
`/
`
`
`WMMMJ
`
`Zia
`
`
`
`
`5;
`
`
`//
`2—74 47;?)
`
`
`J 7'
`A—/ A A‘l’ A
`/0
`
`
`
`
`
`
`(g ”W ,9 )7
`or.
`
`
`fl/Jflifi/l/éfépj -
`
`f
`
`—/ 7
`
`
`
`47/
`
`
`
`
`
`.37
`
`
`
`J;
`
`44/
`
`
`" 4/ 4?;
`
`
`JfliJ/fl? -—
`
`
`
`
`’Efé’flfl "
`
`003
`
`003
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 3 of 6
`
`
`
`US 7,089,148 B1
`
`
`
`
`45/11/54 M/J; Fa?
`
`fiflfdé’W/flflf/flfl
`
`“22/
`
`
`
`
`Mf/ZfWi/xl/flfli/Z/K 3
`
`’ WiZifl/Wfl/ijflfli
`‘
`
`Kat/PW yyflrz‘flwfl
`
`8475
`
`
`
`ffflfl #227
`/—7ifl)
`
`
`
`
`
`7
`
`
`
`Wffliflf/flE/Wél/é'f/Z '
`
`
`fl/Zfl fizzy/5
`
`
`Mf/f/MJ/ii/W/Z i
`
`
`
`{/7
`
`
`
`
`/J/
`
`fl?
`
`
`
`
`
`
`Jim/W5 /
`
`May/{WW /éZ/7Zi
`
`
`AW WéZ/fl/f/ZAZfl
`
`
`
`
`a; m
`
`Z5273? gay/7y
`
`‘ VEZ’WE
`
`7% £275, @571)
`
`004
`
`004
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 4 of 6
`
`
`
`US 7,089,148 B1
`
`
`7a 7:944 94%)
`
`
`W"$7.4 flé .574)
`
`
`
`
`
`097.77
`
`
`£29"
`
`£7
`
`
`
`
`
`
`
`
`Kfl/flfllflf/flifld’fl/f/Pfl/Z’fl
`
`fliflflWfl/W/fl’fi’
`
`
`
`ffl/V/W/Z’fljf/Vfflf
`
`fli/fWWfl/V
`
`
`
`
`
`WWW/V5“ flflffflfli
`
`753/3? Wfflfiffl/fé‘
`
`
`/Wig—”WMWif/fl/i/
`
`
`
`
`
`
`
`WMZ/Vé/Z/[dflf/fldl/
`
`
`flflflflfl/Z Jim/4M
`
`
`WE/Wflflfl/fi/ gag/z
`
`fifW/f!
`
`
`
`
`
`Wiiflf! ”if/M p;
`
`”£75472?WE/V/Zflflfl/Zflé
`
`
`
`{5/7/14472‘ //Vfl
`
`Iflflff/Mfl/Vflfz’
`
`
`
`
`
`
`
`flfl/flflZ/Z/fli W
`
`flfif/flf/flfl XII/[7
`
`
`7W flfflfl/fff
`
`
`’ Mfiflifl/MWZ’&/
`
`6055/? fl/Z/i/VZZV/fl/V
`
`
`
`
`
`
`
`005
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 5 of 6
`
`
`
`US 7,089,148 B1
`
`
`5W5?”
`
`
`
`
`
`
`fliay/pW5/Wz/7/Vé
`
`gyf/M/ga/jg/wag
`
`afl/{z/MWflw/zzfl/V/afl
`
`
`
`
`
`
`MfflfW/X/é flMJ/fo/C
`
`
`wfl KKK/3,67
`
`
`fizzy/”Fwy / iflW/Z/
`
`757746
`
`
`
`
`flWMt/é ,4
`
`
`flffl/éZ/é‘A/f air/ax? ’
`
`
`
`
`aw WJ/Mééx‘flifld
`
`
`
`442.5 .f/Mfdgfl/f/fl 7Z4;
`
`
`
`flay/7,1 VKZMK
`
`
`
`
`
`
`
`
`54/
`
`
`54/
`
`
`M7
`
`
`Kay
`
`
`
`
`
`fl/Z/flzflf/Mé/fifl/l/fl/Zf’fl
`
`
`MflfflfiMé/‘V/‘Vcrffdi
`
`
`
`
`flf/x/é a/fl/‘ffl/V/y/t/
`
`flf/z/E/Mflf/[ffi'
`
`
`
`
`
`
`
`Wyfié17
`
`
`x47; é/é)
`
`
`72 #7527
`
`x475 é/A)
`
`”W?
`
`4K7}
`
`
`
`
`
`006
`
`006
`
`
`
`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 6 0f 6
`
`
`
`US 7,089,148 B1
`
`
`7&fo
`
`Wié/fi?
`
`
`[fa/1% 62¢?
`
`
`”‘75 5/47)
`
`54/
`
`
`
`
`
`
`
`
`
`
`Wig/flag fl/éy/
`
`(566%? fl‘Z’WflZ‘V/f
`
`flat/”@3-14fli/Eflfl/V
`
`
`
`F/flflWfl/V
` é/f
`
`
`
`
`
`
`
`
`
`
`
`MMMW/VJWfl/flfi/fl/V
`
`iMflflfl/flflfl/f/fl/‘V/X/é
`
`
`
`1 464/ @fl/w/V/pfl
`
`
`
`55/2? [fly/7;
`
`
`5/5
`
`
`W/t’flflflé‘ 24,;
`
`”gay/2W @fl/ZZZZWW
`
`
`[flffl/P [EM/#7"!
`
`'
`
`
`é //
`
`
`
`gray/z! flfliz/
`
`
`flW/Z/Z/flé‘ 7/!
`
`WWfl/W‘ifl yJ/fii/V/a/l/
`
`442330,? [SW/W19” W
`
`
`
`{577%,475? jZ/Vj‘flf
`
`Jazz/may flawzzx/m/u
`
`007
`
`007
`
`
`
`
`
`US 7,089,148 B1
`
`1
`
`METHOD AND APPARATUS FOR MOTION
`
`
`
`
`TRACKING OF AN ARTICULATED RIGID
`
`
`BODY
`
`
`
`
`
`
`RELATED APPLICATION
`
`
`
`
`
`
`
`
`
`
`
`
`
`This application is related to, and claims priority from the
`
`
`
`
`
`
`
`US. Provisional Application Ser. No. 60/246,215, entitled
`
`
`
`
`
`
`
`
`
`“Apparatus for Real Time Tracking and Display of The
`
`
`
`
`
`
`
`Position and Posture of a Body using Hybrid Sourceless
`
`
`
`
`
`
`
`Sensors, RF Positioning, and a Quaternion Based Comple-
`
`
`
`
`
`
`
`
`mentary Filtering Algorithm”, filed on Oct. 30, 2000. This
`
`
`
`
`
`provisional application is hereby incorporated by reference
`
`
`
`in its entirety.
`
`TECHNICAL FIELD
`
`
`
`
`The invention described herein relates to methods and
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`apparatus for tracking the orientation (also referred to herein
`
`
`
`
`
`
`
`
`as posture) of an object. More particularly, the invention
`
`
`
`
`
`
`
`
`relates to methods and apparatus for tracking the posture of
`
`
`
`
`
`
`
`
`
`an articulated rigid body. Still more particularly, the inven-
`
`
`
`
`
`
`
`
`
`tion relates to methods and apparatus for tracking the
`
`
`
`
`
`
`
`posture of articulated rigid bodies using quaternion based
`
`
`
`
`
`
`
`attitude estimation filtering and displaying the posture of the
`
`body.
`
`10
`
`
`
`15
`
`
`
`20
`
`25
`
`
`
`BACKGROUND
`
`
`
`2
`
`
`
`
`
`
`
`
`
`
`of the joints of a human body. Thus, such technologies can
`
`
`
`
`
`
`
`provide only a rough approximation of actual body move-
`ment. Another limitation stems from the fact that human
`
`
`
`
`
`
`
`
`
`bodies are of different sizes and dimensions. As a result, the
`
`
`
`
`
`
`
`
`exo-skeleton must be recalibrated for each user. Yet another
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`limitation is imposed by the encumbrance of the exo-
`
`
`
`
`
`
`
`skeleton itself. The weight and awkward configuration of the
`
`
`
`
`
`
`
`
`exo-skeleton prevent a human user from interacting with his
`
`
`
`
`
`
`environment in a natural manner. As a result, it is unlikely
`
`
`
`
`
`
`
`
`
`that the user will become immersed in the synthetic envi-
`ronment in the desired manner.
`
`
`
`
`
`
`
`
`
`
`
`Another widely used system is a magnetic tracking sys-
`
`
`
`
`
`
`
`
`tem. In such systems a large magnetic field is generated and
`calibrated. The user has many small sensors mounted at
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`various points on his body. The sensors are sensitive to the
`
`
`
`
`
`
`
`generated magnetic field. Thus, changes in position and
`
`
`
`
`
`
`
`orientation of the users body with respect to the generated
`
`
`
`
`
`
`
`
`magnetic field can be detected by the magnetic sensors.
`
`
`
`
`
`
`
`
`Some of drawbacks of such systems include very short range
`
`
`
`
`
`
`
`
`and difficulty in calibrating the generated magnetic field.
`
`
`
`
`
`
`
`
`
`
`The short range stems from the fact that magnetic fields
`
`
`
`
`
`
`
`
`decrease in power inversely with the square of the distance
`
`
`
`
`
`
`
`
`
`from the generating source. This restricts the use of such
`
`
`
`
`
`
`
`
`systems to areas about the size of a small room. In order to
`
`
`
`
`
`
`
`
`use a larger working area, user movement must be modified
`
`
`
`
`
`
`
`
`or scaled in some manner. As a result, the magnitude and
`
`
`
`
`
`
`
`frequency of position and orientation errors increase rapidly.
`
`
`
`
`
`
`Additionally, the presence of ferromagnetic material (like
`
`
`
`
`
`
`
`
`the metal in belt buckles or weapons) distorts the generated
`
`
`
`
`
`
`
`magnetic fields. Additionally, the magnetic sensors pick up
`
`
`
`
`
`
`
`
`
`noise from other magnetic fields generated in or near the
`
`
`
`
`
`
`environment. Unfortunately, these distorting magnetic fields
`
`
`
`
`
`
`are commonplace, being easily generated by a plethora of
`
`
`
`
`
`
`devices, including computer monitors, fluorescent lighting,
`
`
`
`
`
`
`
`
`powered electrical wiring in the walls, as well as many other
`
`
`
`
`
`
`
`sources. Additionally, other sources of magnetic field error
`
`
`
`
`
`
`
`
`
`exist. Only with the aid of extremely detailed look-up tables
`can even moderately accurate measurements be obtained.
`
`
`
`
`
`
`
`
`
`
`
`
`
`Thus, magnetic tracking based on a generated magnetic field
`
`
`
`
`
`
`
`is subject to positional and orientation inaccuracies which
`
`
`
`
`
`are highly variable and unpredictable.
`
`
`
`
`
`
`
`Another system for detecting position and orientation of
`
`
`
`
`
`
`
`
`
`a body uses so-called optical sensing. Optical sensing, in
`
`
`
`
`
`
`
`general, covers a large and varying collection of technolo-
`
`
`
`
`
`
`
`
`
`gies. All of these technologies depend on the sensing of
`
`
`
`
`
`
`
`
`
`some type of light
`to provide position and orientation
`
`
`
`
`
`
`
`information. Consequently, all of these technologies are
`
`
`
`
`
`
`subject to inaccuracies whenever a required light path is
`
`
`
`
`
`
`
`blocked. Additionally, these technologies suffer from inter-
`
`
`
`
`
`
`
`
`
`ference from other light sources. All of these optical sensing
`
`
`
`
`
`
`
`systems require specially prepared environments having the
`
`
`
`
`
`
`
`necessary emitters and sensors. This prevents widespread
`
`
`
`
`
`
`
`usage and presents a significant and expensive limitation.
`
`
`
`
`
`
`
`Yet another approach is a tracking system using acoustic
`
`
`
`
`
`
`
`trackers. Like the previously described magnetic trackers,
`
`
`
`
`
`
`
`
`
`such systems are limited in range due to the inherent
`
`
`
`
`
`
`limitations of sound propagation. Additionally, the physics
`
`
`
`
`
`
`
`
`
`of sound limit accuracy, information update rate, and the
`
`
`
`
`
`
`
`overall range of an acoustic tracking system. Moreover, due
`to the relatively directional nature of sound, clear lines of
`
`
`
`
`
`
`
`
`
`sight must be maintained in order to obtain accurate read-
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`ings. Additionally, due to the relatively slow speed of sound,
`
`
`
`
`
`
`
`
`
`there are latency problems with such acoustic sensor sys-
`tems. As is evident from the foregoing discussion, conven-
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`tional approaches for the tracking of body orientation have
`some serious limitations.
`
`
`
`
`
`
`
`
`
`
`
`invention are directed
`Embodiments of the present
`
`
`
`
`
`
`
`
`towards methods and devices for tracking the orientation
`
`
`
`
`
`
`
`
`(posture) of human bodies. In particular such orientation
`
`
`
`
`
`
`
`information can be inserted into a synthetic (computer
`generated) environment where the motion of the tracked
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`body can become part of the synthetic environment. Previ-
`
`
`
`
`
`
`
`
`
`ous motion tracking systems of the type known in the art
`have a number of limitations that substantially limit their
`
`
`
`
`
`
`
`usefulness.
`
`
`
`
`
`
`
`
`Currently available motion tracking technologies are lim-
`
`
`
`
`
`
`
`
`ited by their reliance on a generated signal and/or the need
`to have the tracked body remain in sight of fixed stations
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`positioned around a working volume. In either case there is
`
`
`
`
`
`
`
`
`a requirement to maintain some type of link over a distance.
`
`
`
`
`
`
`
`Regardless of the type of signal used, it can be generally
`
`
`
`
`
`
`
`referred to as a “source.” Usually, the effective range over
`which the link may be maintained is limited. Moreover, data
`
`
`
`
`
`
`
`
`
`
`
`
`
`update rates may be limited by the physical characteristics of
`the source used. Additionally, interference with, or distortion
`
`
`
`
`
`
`
`of, the source can result in erroneous orientation measure-
`
`
`
`
`
`
`
`
`
`ments. If the link is broken, a complete loss of track will
`
`
`
`
`
`
`
`
`result.
`
`
`
`
`
`
`
`
`
`
`
`is the
`One type of tracking system known in the art
`
`
`
`
`
`
`
`so-called mechanical tracking system. Such systems use an
`artificial exo-skeleton, which is worn by the user of a
`
`
`
`
`
`
`
`
`
`
`
`
`
`synthetic environment (typically, a computer-created simu-
`
`
`
`
`
`
`
`lated environment). Sensors (e.g., goniometers) within the
`
`
`
`
`
`
`
`skeletal linkages of the exo-skeleton have a general corre-
`
`
`
`
`
`
`
`
`spondence to the actual joints of the user. Joint angle data is
`fed into kinematic algorithms that are used to determine
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`body posture and limb position. However, since the exo-
`skeleton is worn by the user, other systems must be used to
`
`
`
`
`
`
`
`
`ascertain the position of the user within the simulated
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`environment. Such systems are fraught with numerous
`
`
`
`
`
`
`
`
`
`drawbacks. For one, aligning the goniometers with the joints
`
`
`
`
`
`
`
`
`of a human body is difficult, especially with multiple degree
`
`
`
`
`
`
`
`
`
`of freedom (DOF) joints. Additionally,
`the joints of the
`
`
`
`
`
`
`
`exo-skeleton cannot perfectly replicate the range of motion
`008
`
`30
`
`
`
`35
`
`
`
`40
`
`
`
`45
`
`
`
`50
`
`
`
`55
`
`
`
`60
`
`
`
`65
`
`
`
`008
`
`
`
`
`3
`SUMMARY OF THE INVENTION
`
`
`
`
`
`
`
`US 7,089,148 B1
`
`4
`
`
`
`
`
`
`
`
`In accordance with the principles of the present invention,
`
`
`
`
`
`
`
`
`systems, method, and apparatus for body tracking is dis-
`
`
`
`
`
`
`closed. A method embodiment for tracking the orientation of
`
`
`
`
`
`
`
`
`a sensor comprises measuring an angular velocity of the
`
`
`
`
`
`
`
`
`sensor to generate angular rate values which are integrated
`
`
`
`
`
`
`and normalized to produce an estimate of sensor orientation.
`
`
`
`
`
`
`
`The method continues by measuring a local magnetic field
`
`
`
`
`
`
`
`
`
`vector and measuring local gravity vector and correcting the
`
`
`
`
`
`
`
`
`estimate of sensor orientation using the local magnetic field
`
`
`
`
`
`vector and local gravity vector.
`
`
`
`
`
`
`Another method embodiment comprises measuring an
`
`
`
`
`
`
`
`
`angular velocity of the sensor to generate an angular rate
`
`
`
`
`
`
`
`quaternion,
`integrating and normalizing the angular rate
`
`
`
`
`
`
`
`quaternion to produce an estimated sensor orientation
`
`
`
`
`
`
`
`
`
`quaternion. The local magnetic field vector and local gravity
`vector are measured. A measurement vector is determined
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`from the local magnetic field and local gravity vectors. A
`20
`
`
`
`
`
`
`
`computed measurement vector is calculated from the esti-
`
`
`
`
`
`
`
`
`mated sensor orientation quaternion and comparing with the
`
`
`
`
`
`
`
`measurement vector to generate an error vector that defines
`a criterion function. The criterion function is minimized and
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`an error estimate quaternion is output. The error estimate
`
`
`
`
`
`
`quaternion is integrated and normalized to produce a new
`
`
`
`
`
`
`
`estimated sensor orientation quaternion which can be output
`
`
`
`
`
`
`
`
`as a sensor orientation signal. The entire process is repeated
`
`
`
`
`
`
`
`
`except that the new estimated sensor orientation quaternion
`
`
`
`
`
`
`
`
`is used for calculating a computed measurement vector. The
`
`
`
`
`
`
`
`process continues until tracking is no longer desired.
`
`
`
`
`
`
`Another method embodiment of tracking the orientation
`
`
`
`
`
`
`
`of a sensor comprises providing a starting estimate of sensor
`
`
`
`
`
`
`
`
`orientation, measuring the local magnetic field vector and
`
`
`
`
`
`
`
`measuring the local gravity vector. A measurement vector is
`determined from the local magnetic field vector and the local
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`gravity vector. A computed measurement vector is calcu-
`lated from the estimate of sensor orientation and compared
`
`
`
`
`
`
`
`
`to the measurement vector to generate an error vector that
`
`
`
`
`
`
`
`
`defines a criterion function. A Gauss-Newton iteration is
`
`
`
`
`
`
`
`
`
`
`
`
`performed, resulting in a minimized criterion function gen-
`
`
`
`
`
`
`
`erating an error estimate that is integrated and normalized to
`produce a new estimate of sensor orientation. As with the
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`forgoing embodiment, the entire process is repeated except
`the new estimate of sensor orientation is used for
`that
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`calculating a computed measurement vector. The process
`
`
`
`
`
`
`continues until tracking is no longer desired.
`Yet another method of tracking the orientation of a sensor
`
`
`
`
`
`
`
`
`
`
`
`
`
`comprises providing a starting estimate of sensor orientation
`
`
`
`
`
`
`
`quaternion measuring a local magnetic field vector and a
`
`
`
`
`
`
`
`local gravity vector and determining a measurement vector
`
`
`
`
`
`
`
`
`
`
`from the local magnetic field vector and the local gravity
`vector. A computed measurement vector is calculated from
`
`
`
`
`
`
`
`
`
`
`
`
`
`the estimate of sensor orientation, using quatemion math-
`ematics.
`
`
`
`
`
`
`
`
`The measurement vector is compared with the computed
`measurement vector to generate an 6><l error vector that
`
`
`
`
`
`
`
`defines a criterion function and a mathematical operation is
`
`
`
`
`
`
`performed that results in the minimization of the criterion
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`function and outputs a 4x1 quatemion error estimate. The
`
`
`
`
`
`
`quaternion error estimate is integrated and normalized to
`
`
`
`
`
`
`
`produce a new estimated sensor orientation quaternion. The
`
`
`
`
`
`
`
`
`entire process is repeated except that the new estimated
`
`
`
`
`
`
`
`sensor orientation quaternion is used for calculating a com-
`
`
`
`
`
`
`
`puted measurement vector. The process continues until
`65
`
`
`
`
`tracking is no longer desired.
`
`
`
`
`
`
`
`
`A method for determining a local gravity vector values
`
`
`
`
`
`
`
`
`comprises, moving the sensor from a start point to an end
`009
`
`5
`
`
`
`10
`
`
`
`15
`
`
`
`25
`
`
`
`30
`
`
`
`35
`
`
`
`40
`
`
`
`45
`
`
`
`50
`
`
`
`55
`
`
`
`60
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`point over a time period; taking measurements of the total
`
`
`
`
`
`
`
`
`acceleration vector during the time period, time weighted
`
`
`
`
`
`
`summing the measurements of the acceleration vector over
`
`
`
`
`
`
`
`
`
`the time period, and calculating gravity vector values using
`the summed total acceleration measurements.
`
`
`
`
`
`Embodiments of the invention also include sensor appa-
`
`
`
`
`
`
`
`
`
`
`
`
`
`ratus comprising a magnetic field detector configured to
`
`
`
`
`
`
`
`
`measure a magnetic field vector and output a local magnetic
`
`
`
`
`
`
`
`field vector signal and an acceleration detector configured to
`
`
`
`
`
`
`
`
`detect a local gravitational field vector and output a local
`
`
`
`
`
`
`
`gravitational field vector signal. A related embodiment fur-
`
`
`
`
`
`
`
`ther includes an angular velocity detector configured to
`
`
`
`
`
`
`
`
`detect an angular velocity vector of the sensor and output
`
`
`
`angular velocity signal.
`
`
`
`
`
`
`Embodiments of the invention include a system for track-
`
`
`
`
`
`
`
`
`
`
`ing the posture and orientation of body, the system com-
`
`
`
`
`
`
`
`
`prising a body having mounted thereon at least one sensor,
`
`
`
`
`
`
`each sensor including a magnetometer for measuring a
`
`
`
`
`
`
`
`
`magnetic field vector and a acceleration detector for mea-
`
`
`
`
`
`
`
`
`suring a body acceleration vector, and at least one processor
`
`
`
`
`
`
`
`
`for receiving input from the magnetometer and acceleration
`
`
`
`
`
`
`
`
`detector and using said input to calculate a local magnetic
`
`
`
`
`
`
`
`
`
`field vector and a local gravity vector and to determine the
`
`
`
`
`
`
`
`orientation of the body. The processor of the system can be
`
`
`
`
`
`
`
`
`further configured to correct for offset between body coor-
`
`
`
`
`
`
`
`
`dinates and sensor coordinates. The system can further
`
`
`
`
`
`
`
`
`include a display for displaying the position and orientation
`
`
`
`
`
`
`
`of the body with respect to a synthetic environment. Further
`
`
`
`
`
`
`
`embodiments use sensors that
`include angular velocity
`detectors.
`
`
`
`
`
`
`
`
`
`Other aspects and advantages of the invention will
`
`
`
`
`
`
`
`become apparent from the following detailed description
`
`
`
`
`
`
`
`and accompanying drawings which illustrate, by way of
`
`
`
`
`
`example, the principles of the invention.
`BRIEF DESCRIPTION OF THE DRAWINGS
`
`
`
`
`
`
`
`
`
`
`
`The following detailed description of the embodiments of
`
`
`
`
`
`
`
`the invention will be more readily understood in conjunction
`
`
`
`
`
`with the accompanying drawings, in which:
`
`
`
`
`
`FIG. 1 is a figurative depiction of the earth’s surface and
`
`
`
`
`
`
`
`its relationship with a example pair of a magnetic field
`
`
`
`
`vector and gravity vector.
`FIGS. 2(a) and 2(b) depict reference and body coordinate
`
`
`
`
`
`
`
`
`
`
`systems respectively
`
`
`
`
`
`FIG. 3 is a simplified block diagram of a filtering method
`
`
`
`
`
`embodiment in accordance with the principles of the present
`invention
`
`
`
`
`
`
`FIG. 4 is a block diagram of a system embodiment of the
`
`
`present invention.
`
`
`
`
`
`
`FIGS. 5 and 6 are flow diagrams illustrating method
`embodiments in accordance with the principles of the
`
`
`
`
`
`
`
`
`present invention.
`It is to be understood that in the drawings like reference
`
`
`
`
`
`
`
`
`numerals designate like structural elements.
`
`
`
`
`
`DETAILED DESCRIPTION OF THE DRAWINGS
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`invention provide a
`The embodiments of the present
`
`
`
`
`
`
`
`
`
`method and apparatus for tracking the posture of a body
`without the need for a generated field (or source) or a
`
`
`
`
`
`
`
`
`
`plurality of fixed stations. Advances in the field of miniature
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`sensors over the last decade make possible inertial/magnetic
`
`
`
`
`
`
`tracking of the orientation of tracked body in three dimen-
`sions. In particular such sensors can be used to track human
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`body limb segments in three dimensions. In accordance with
`
`
`
`
`
`
`
`
`the principles of the present invention such tracking incor-
`
`009
`
`
`
`
`
`US 7,089,148 B1
`
`5
`
`
`
`
`
`
`
`
`porates the passive measurement of physical quantities that
`
`
`
`
`
`
`
`
`are directly related to the rate of rotation and orientation of
`
`
`
`
`
`
`
`
`
`a rigid body. The “sourceless” nature of this technique
`
`
`
`
`
`
`
`
`makes possible full body posture tracking of multiple users
`
`
`
`
`
`
`
`
`over an area that is only limited by the range of a wireless
`
`
`
`
`
`
`
`
`LAN. Since orientation estimates are based only on passive
`
`
`
`
`
`
`
`measurements, nearly all latency in such a system is due to
`
`
`
`
`
`
`
`the computational demands of the data processing algo-
`
`
`
`
`
`
`
`
`rithms involved and not physical characteristics of the
`
`
`generated source.
`
`
`
`
`
`
`An embodiment of the present invention makes use of the
`
`
`
`
`
`
`
`
`
`fact that a local magnetic field vector and local gravity
`
`
`
`
`
`
`
`
`vector can be defined. FIG. 1 is a simplified figurative
`
`
`
`
`
`
`
`illustration of the surface of the earth 10 showing a local
`
`
`
`
`
`
`
`
`
`gravity vectors 11 and a local magnetic field vector 13. In
`
`
`
`
`
`
`
`
`
`
`general, for any object positioned on the earth 10, the gravity
`
`
`
`
`
`
`
`
`vectors always points to the earth’s center of mass, which
`
`
`
`
`
`
`
`may be defined as “down”. This phenomenon is simply
`
`
`
`
`
`
`
`illustrated in FIG. 1. Additionally, the magnetic field vector
`
`
`
`
`
`13 always points to magnetic north.
`
`
`
`
`
`Sensor embodiments of the present invention, by tracking
`
`
`
`
`
`
`
`
`changes in the orientation of the sensor with respect to the
`
`
`
`
`
`
`
`
`
`
`local magnetic field vector and the local gravity vector, can
`
`
`
`
`
`detect changes in orientation of the sensor. An appropriately
`
`
`
`
`
`
`
`
`designed sensor can track the orientation of a body. Signifi-
`
`
`
`
`
`
`
`cantly, a system having a plurality of sensors, each mounted
`
`
`
`
`
`
`
`
`
`to a limb of an articulated rigid body can be used to track the
`
`
`
`
`
`
`
`
`orientation of each limb. In such systems, body posture can
`
`
`
`
`
`
`
`be tracked and introduced into a synthetic environment,
`
`
`
`
`
`
`
`
`thereby allowing a user to interface with the synthetic
`environment.
`
`The Nature of the Mathematical Problem
`
`
`
`
`
`
`
`
`
`
`A barrier to an effective implementation of effective body
`
`
`
`
`
`
`
`tracking systems stems from certain mathematical difficul-
`
`
`
`
`
`
`
`
`ties inherent in mapping body orientation from one coordi-
`
`
`
`
`
`
`
`nate system to another. These difficulties have presented
`
`10
`
`
`
`15
`
`
`
`20
`
`25
`
`
`
`30
`
`
`
`35
`
`
`
`
`6
`
`
`
`
`
`
`
`
`Euler angles represent the orientation of a rigid body using
`
`
`
`
`
`
`
`three rotations about specified axes in a specified order of
`
`
`
`
`
`
`
`
`rotation. For example, referring to FIG. 2(a), first rotation
`about the north axis, second rotation about the east axis, and
`
`
`
`
`
`
`
`
`
`
`
`third rotation about the down axis. These would be analo-
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`gous to “roll”, “elevation”, and “azimut ”. By convention,
`
`
`
`
`
`
`
`
`
`roll angle is designated by “(I)”, elevation by “6”, and
`
`
`
`
`
`
`
`
`
`azimuth “11)”. It is to be noted that if the temporal order of
`
`
`
`
`
`
`
`
`rotations is reversed, body axis rotations yield exactly the
`same orientation as reference axis rotations.
`
`
`
`
`
`
`
`
`
`
`
`
`The position of a point in space can be described using a
`
`
`
`
`
`
`
`three-dimensional point vector. A rigid body can be
`
`
`
`
`
`
`
`
`
`described in terms of a plurality of point vectors. An
`
`
`
`
`
`
`example vector can be represented by a vector V:[x y Z]. In
`order to describe rotational behavior, matrix transforms are
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`used. For example, if a vector is rotated about an angle (1)
`
`
`
`
`
`
`
`
`
`(e.g., about the XE-axis described with respect to FIG. 2(a)),
`
`
`
`
`
`
`the following rotation transform can be used.
`
`X
`
`= [rot(x, 90)] y
`
`
`
`
`
`
`
`
`
`
`X VZ
`
`
`
`
`
`
`
`
`
`/
`
`x
`
`
`z’
`
`l...
`
`
`
`
`O...O
`
`
`0... singo
`
`
`
`
`cosgo
`
`
`
`
`
`
`y’ =|O...cosgo...—singo
`
`
`
`
`
`
`
`
`Thus, the original x, y, Z coordinates can be translated into
`
`
`
`
`
`
`the rotated coordinates x', y', z' through the application of a
`
`
`
`
`
`
`
`
`
`3x3 rotation matrix. In the depicted example, the 3x3 matrix
`
`
`
`
`
`
`
`is designed to accomplish a rotation about
`the X-axis
`
`
`
`
`
`
`
`
`(represented by [rot(x, ¢)]. Similar 3x3 rotation matrices
`
`
`
`
`
`
`
`
`
`exist for 6 and q) rotations about the Y- and Z-axis, respec-
`
`
`
`
`
`
`
`
`tively. Such rotation transforms are known to those having
`ordinary skill in the art and will not be described here in
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`great detail. Additionally, a single 3x3 rotation matrix can be
`used to describe rotation about all three axes. One example
`
`
`
`
`
`
`
`
`of such a rotation matrix is shown below.
`
`
`
`
`
`
`
`Ev = [rot(z, ammo, 6)][ror<x, @ng =
`
`
`
`
`
`
`
`(costflcosG)
`(coswsinOSingo—sinwcosgo)
`(cos¢sin0cosgo+sin¢singo)
`
`
`
`
`
`
`(coswcosgo + singosinOSingo)
`(costflsingo+ sinwsin0cosgo)
`(sianosO)
`
`
`
`
`
`
`(cosOsingo)
`(—sin)
`(cochosgo)
`
`
`
`
`
`
`8v = REV
`
`
`
`
`
`
`
`
`
`
`
`problems which conventional approaches have not been able
`to resolve for over 100 years.
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`The nature of the problem is briefly outlined in the
`
`
`
`
`
`
`following paragraphs. A conventional way of describing the
`
`
`
`
`
`
`
`orientation of a rigid body uses “Euler angles” to describe
`the orientation of a rigid body in three dimensions. Euler
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`angles describe the orientation of a rigid body using three
`
`
`
`
`
`
`
`
`rotations about specified axes. One commonly used refer-
`
`
`
`
`
`
`
`
`
`ence coordinate system is the local “flat Earth” system. Such
`
`
`
`
`
`
`
`
`reference systems use an arbitrarily selected origin on the
`surface of the earth with respect to coordinate axes X, Y, and
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Z. By convention,
`the X-direction corresponds to local
`north,
`the Y-direction corresponds to local east, and the
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Z-direction corresponds to down as depicted in FIG. 2(a).
`
`
`
`
`
`
`Additionally,
`it is important to specify a body coordinate
`
`
`
`
`
`
`
`
`system which is attached to the body being tracked. FIG.
`
`
`
`
`
`
`
`
`2(b) depicts such a system. This is also an X-Y—Z system
`
`
`
`
`
`
`with X pointing “out of the nose” in a positive direction, Y
`65
`
`
`
`
`
`
`
`
`
`
`out the right side, and Z down. The subscript “E” designates
`earth reference coordinates OCE, YE, ZE) and the subscript
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`“B” designates body reference coordinates C(B, YB, ZB).
`
`50
`
`55
`
`60
`
`As can be seen above, one of the difficulties inherent in
`
`
`
`
`
`
`
`
`using Euler angles and the aforementioned transform matri-
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`ces is the sheer number of computations require to calculate
`
`
`
`
`
`
`
`each motion of a body. The problem becomes magnified as
`
`
`
`
`
`
`
`
`
`more and more bodies are simultaneously tracked. In the
`
`
`
`
`
`
`
`
`end, the complex calculations required using these existing
`
`
`
`
`
`
`
`
`
`algorithms require so much time that they can not effectively
`
`
`
`
`
`
`
`
`be used to track body position and orientation in real-time.
`
`
`
`This is a significant drawback.
`
`
`
`
`
`
`
`
`In addition, Euler angle systems present certain compu-
`
`
`
`
`
`
`
`tational difficulties. In particular is the problem of singu-
`
`
`
`
`
`
`
`
`
`larities. Singularities result when a sensor (or the rigid body
`
`
`
`
`
`
`
`
`
`to which it is attached) is tilted to a 90° (degree) angle. For
`
`
`
`
`
`
`
`
`example, if a rigid body is tilted such that it points straight
`up. At such a point, the roll and azimuth axes are co-linear.
`
`
`
`
`
`
`
`
`
`
`This results in a situation where neither the roll nor azimuth
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`angles are uniquely defined, only their difference or sum can
`
`
`
`
`
`
`
`be specified uniquely. This problem becomes magnified
`
`
`
`
`
`
`
`
`
`
`when the time rate of change of Euler angles are used to
`
`
`
`
`
`
`
`quantify rates of angular rotation. This singularity problem
`
`
`
`
`
`
`
`
`
`010
`
`010
`
`
`
`
`
`US 7,089,148 B1
`
`7
`
`
`
`
`
`
`
`
`
`
`
`in the art and
`is known to those having ordinary skill
`
`
`
`
`
`
`presents significant difficulty to conventional body tracking
`systems.
`
`
`
`
`
`
`
`The principles of the present invention use magnetometer
`
`
`
`
`
`
`
`
`and accelerometer input subject to filtering to track body
`
`
`
`
`
`
`
`
`posture.
`In one implementation, Eul