`
`
`
`
`
`
`
`I IIIII IIIIIIII Ill lllll lllll lllll lllll lllll lllll lllll lllll 111111111111111111
`
`US007089 l 48B 1
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`02)United States Patent
`
`Bachmann et al.
`
`US 7,089,148 Bl
`
`(10)Patent No.:
`(45) Date of Patent:
`Aug. 8, 2006
`
`5,807,284 A * 9/1998 Foxlin ........................ 600/595
`
`
`
`
`
`
`5,819,206 A * 10/1998 Ii01ton et al. .............. 702/150
`
`
`6,636,826 Bl* 10/2003 Abe et al .................... 702/151
`
`
`
`6,691,074 Bl* 2/2004 Moriya et al. .............. 702/190
`
`
`
`6,820,025 Bl * 11/2004 Bachmann et al. ........... 702/94
`
`(54)METHOD AND APPARATUS FOR MOTION
`
`TRACKING OF AN ARTICULATED RIGID
`BODY
`(75)Inventors: Eric R. Bachmann, Oxford, OH (US);
`
`
`
`
`Robert B. McGhee, Carmel, CA (US);
`OTHER PUBLICATIONS
`
`
`Xiaoping Yun, Salinas, CA (US);
`
`
`Michael J. Zyda, Cannel, CA (US);
`
`
`
`Henault, G., A Computer Simulation Study and Component
`
`Prunedale, CA
`
`Douglas L. McKlnney,
`
`
`
`
`Evaluation for a Quaternion Filter for Sourceless Tracking
`(US)
`
`of Human Limb Segment Motion, Mar. 1977, Naval
`
`PostGraduate School.*
`(73)Assignee: The United States of America as
`
`
`
`
`
`represented by the Secretary of the
`
`Navy, Washington, DC (US)
`Primary Examiner-John Barlow
`
`
`the tenn of this ( *) Notice: Subject to any disclaimer,
`
`-Toan M. Le
`
`Assistant Examiner
`
`
`
`patent is extended or adjusted under 35
`
`(74) Attorney, Agent, or Finn-Donald E. Lincoln
`
`U.S.C. 154(b) by 33 days.
`(21)Appl. No.: 10/963,007
`
`" cited by examiner
`
`(57)
`
`ABSTRACT
`
`(22)Filed:Sep. 30, 2004
`
`One embodiment the invention includes a method of deter­
`
`
`
`
`
`mining an orientation of a sensor. The method includes
`
`
`Related U.S. Application Data
`
`
`
`measuring a local magnetic field vector and a local gravity
`
`
`
`(62)Division of application No. 10/020,719, filed on Oct.
`
`vector and using those measurements to determine the
`30, 2001, now Pat. No. 6,820,025.
`
`
`
`
`orientation of the sensor. Embodiments can include measur­
`
`
`
`ing the magnetic field vector and the local gravity vector
`
`
`
`(60)Provisional application No. 60/246,215, filed on Oct.
`
`
`
`
`using quaternion coordinates. Another embodiment includes
`30, 2000.
`
`
`
`measuring a local magnetic field vector, a local gravity
`
`
`
`vector, and the angular velocity of the sensor. These three
`(51)Int. Cl.
`
`
`
`
`vectors are processed to detem1ine the orientation of the
`GOZC 17/00 (2006.01)
`
`
`sensor. In one embodiment the three vectors can aU be
`A61B 5/103 (2006.01)
`
`
`
`measured in quaternion coordinates. Another method
`702/151; (52)U.S. Cl. ....................................... 600/595
`
`
`
`
`
`
`
`embodiment includes determining a local gravity vector by
`
`Search ................ 702/151,
`(58)Field of Classification
`
`
`
`
`providing a acceleration detector, moving the detector from
`
`
`702/150, 141, 142, 94, 53; 600/595; 345/473
`
`
`a start point to an end point over a time period, and summing
`
`
`
`See application file for complete search history.
`
`
`acceleration measurements over the time period. The local
`
`
`
`
`gravity vector is calculated using the su11Ulled acceleration
`measurements.
`
`(56)
`
`
`
`References Cited
`
`U.S. PATENT DOCutvlENTS
`
`
`
`
`14 Claims, 6 Drawing Sbeets
`
`5,645,077 A * 7/1997 Foxlin . ...................... 600/587
`
`f,;¥.,VEJe?l�Uf,es
`(c,, .0,? .o�)
`
`001
`
` LG 10
`04
`
`

`

`
`U.S. Patent
`
`
`
`
`Aug.8, 2006
`
`
`
`
`
`Sheet 1 of6
`
`
`
`US 7,089,148 B1
`
`
`
`
`
`002
`
`

`

`
`U.S. Patent
`
`
`
`
`Aug.8, 2006
`
`
`
`
`Sheet 2 of6
`
`
`
`US 7,089,148 B1
`
`tf
`
`
`
`
`
`
`iS
`
`
`tl
`
`
`MCLLIYETEE
`Vb Ae by)
`
`
`
`
`
`KO
`
`Yr -HF)
`
`
`,
`GG
`Bt
`op ny
`
`
`
`
`
`
`
`[OVAbell) |0GF003)”
`
`
`MUEMETEMETELS |
`(2, £2 b3)
`
`ri yi
`wx] x
`
`
`
`IE
`
`SE
`
`
`
`
`
`F/
`P LS.
`
`
`
`: LHF
`
`
`
`
`“MVEIEMIGE
`
` MMIGLLYGE —
`
`003
`
`003
`
`

`

`
`U.S. Patent
`
`
`
`Aug.8, 2006
`
`
`
`
`Sheet 3 of6
`
`
`US 7,089,148 B1
`
`
`
`
`
`
`AOBCMIE4WL POLE’
`
`SEMIOE LLEVEMTATE
`
`Sif
`
`
`
`
`|MELEMEAMie |
`
`”|YLOUTYC22JESEM
`
`LIF
`
`
`
`LONPLITE GUATELLNAM
`
`ATHE
`
`
`
`
`5077
`
`ALM FBZ
`L1G2B(DD
`
`
`
`
`MWEASUEMEG4MOET |
`
`
`AWELD PELIOE
`
`LOOP
`
`
`
`
`MEADEME4GLBMG
`WETAC
`|
`,
`
`SLT
`
`
`
`
`!
`LEFELAYNING 4
`
`MAEPABOILSWE” (ELTAE”
`
`AL JMAGHETHLHL
`
`LECTZLHUTE!
`
`| ZL
`
`
`
`ip 0
`
`JEO#LLE, LESBD
`
`
`
`004
`
`004
`
`

`

`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 4 of 6
`
`
`
`US 7,089,148 B1
`
`
`JB Lit, FileSC2)
`
`
`Att"£16, fCELA)
`
`
`
`
`
`ALF
`
`
`
`
`
`
`LEMMEAFCOMPLE
`
`CHPLEENMEMTGEC
`
`COMEZWITEDPENILE
`
`
`
`
`EVENTATI
`
`
`
`CEMELLITIG KRALIFIE
`
`VEE THFLEEESF
`
`
`
`
`
`AACFIESHEWALMI
`
`
`
`
` WAMEMETHEGTELETOM
`
`
`
`
`WOTLEFOCltya
`
`
`QOEVNVTAITION) ELCLLIE™
`
`LSTMIME
`
`
`
`
` AACECEAE FEStd GF
`
`MEYGATED GRIESETOKLEE
`
`
`
`LDPIAIAIE BU
`
`PUPELLIS679FE
`
`
`LID
`
`LT
`
`
`
`LG
`
`ofa
`
`
` COBLMMPLIELMIE
`
`
`WSTECLAT/eeLESLL7
`
`
`
`
`
`
`
`
`005
`
`

`

`
`U.S. Patent
`
`
`
`
`Aug.8, 2006
`
`
`
`
`Sheet 5 of6
`
`
`
`US 7,089,148 B1
`
`
`SIAL7
`
`
`
`
`
`
`ALEOEIOWMG4GETING
`
`LETTEP=FEUSOL
`
`
`GEMMGUMTECMIOM
`
`
`
`
`_| AIEEE AMIFGUETIC
`
`
`YYELE PECTLE
`
`
`MIEABUCIMGO GEQUUTY
`
`LECVIE
`
`
`
`
`LESEMG Fo
`
`
`AELLLNMMEN/TF LECIALE |
`
`
`
`Sty JHEAMERETIO
`
`
`
`LLL LEOFAEBML Til
`
`
`
`
`CELTY LEOTAE
`
`
`
`
`
`
`
`GOL
`
`
`GF
`
`ZOF
`
`
`LO
`
`
`GOP
`
`
`
`
`
`
`
`
`
`
`LLLATEAACOLMIEYTED
`
`
`
`ApEASLLELNEMYEOTOE
`
`
`
`
`
`
`AktABS
`
`
`AVE BL)
`
`
`
`JP ALLL
`
`
`LUG L{B)
`
`ABE
`
`
`
`006
`
`006
`
`

`

`
`U.S. Patent
`
`
`
`
`Aug. 8, 2006
`
`
`
`
`Sheet 6 of 6
`
`
`
`US 7,089,148 B1
`
`
`band ABO
`A“Ola)
`
`
`
`BLL
`
`
`
`T2#LZOF
`
`
`SIG LAG
`
`
`
`
`
`
`
`CELAING AWBx
`
`LELWIE GECICIE(AT
`
`LEPLEBLLTELVON
`
`FLNETIOY
`
`
`
`
`
`
`
`
`
`
`
`
`
`QMEWGTHECOLITELVON
`
`LMTAMCELiflle
`
`
`
`
`AAD GOMTELNMW
`
`
`
`
`LEOE ESMATE
`
`
`MITTEETINE VE
`
`LMLIGTED (LMATEVOUUIEU
`
`
`LEIS, ESTIME
`
`GLF
`
`
`GLP
`
`
`Ai7
`
`Gif
`
`
`
`
`
`
`
`nesteJAE
`
`
`
`
`LLIFLT
`
`
`
`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 claimspriority from the
`
`
`
`
`
`
`
`USS. 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 estimationfiltering and displaying the posture of the
`
`body.
`
`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
`
`
`
`
`
`
`environmentin a natural manner. Asa 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 systemsa 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 orderto
`
`
`
`
`
`
`
`
`use a larger working area, user movement must be modified
`
`
`
`
`
`
`
`
`or scaled in some manner. As a result, the magnitude and
`
`
`
`
`
`
`
`frequencyof position andorientation 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,
`
`
`
`
`
`
`
`
`poweredelectrical 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 approachis 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 becomepart of the synthetic environment. Previ-
`
`
`
`
`
`
`
`
`
`ous motion tracking systems of the type known in the art
`
`
`
`
`
`
`
`have a numberoflimitations that substantially limit their
`usefulness.
`
`
`
`
`
`
`
`
`Currently available motion tracking technologiesare 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 sometypeof 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
`
`
`
`
`
`
`
`whichthe link may be maintainedis 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
`
`
`
`
`
`
`
`
`In accordance with the principles of the present invention,
`
`
`
`
`
`
`
`
`systems, method, and apparatus for body tracking is dis-
`
`
`
`
`
`
`closed. A method embodimentfor 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 sensororientation.
`
`
`
`
`
`
`
`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 sensororientation 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 measurementvector. 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 measurementvectoris
`
`
`
`
`
`
`
`
`
`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 methodoftracking 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 quaternion math-
`ematics.
`
`
`
`
`
`
`
`
`The measurement vector is compared with the computed
`
`
`
`
`
`
`
`measurement vector to generate an 6x1 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 quaternion error estimate. The
`
`
`
`
`
`
`quaternion error estimate is integrated and normalized to
`
`
`
`
`
`
`
`produce a new estimated sensororientation 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
`
`
`
`
`
`
`
`25
`
`
`
`30
`
`
`
`35
`
`
`
`40
`
`
`
`45
`
`
`
`50
`
`
`
`55
`
`
`
`60
`
`
`
`
`
`
`4
`
`
`
`
`
`
`
`
`point over a time period; taking measurements ofthe 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 summedtotal 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 embodimentfur-
`
`
`
`
`
`
`
`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(6) depict reference and body coordinate
`
`
`systems respectively
`
`
`
`
`
`FIG.3 is a simplified block diagram ofa filtering method
`
`
`
`
`
`embodimentin accordance with the principles of the present
`invention
`
`
`
`
`
`
`FIG.4 is a block diagram of a system embodimentof 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 drawingslike 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
`
`
`
`
`
`
`
`
`makespossible 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 embodimentof 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 ofan articulated rigid body can be usedto 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
`
`
`
`
`
`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
`
`
`
`
`
`
`
`
`
`
`
`aboutthe north axis, second rotation about the east axis, and
`third rotation about the down axis. These would be analo-
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`gous to “roll”, “elevation”, and “azimuth”. By convention,
`
`
`
`
`
`
`
`
`
`roll angle is designated by “o”, elevation by “0”, and
`
`
`
`
`
`
`
`
`
`azimuth “w’’. 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.
`
`
`
`
`
`
`
`
`
`
`
`
`Theposition 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 »
`
`
`
`
`
`
`
`
`
`(e.g., about the XE-axis described with respect to FIG. 2(a)),
`
`
`
`
`
`
`the following rotation transform can be used.
`
`
`
`
`
`x yg
`
`
`
`
`
`
`= [rot(x,¢)]
`
`x yg
`
`
`
`
`
`
`
`
`
`,
`x
`
`
`Z
`
`y =|ni
`
`
`1...
`
`
`
`
`0...0
`
`
`
`
`
`0... sing ... cosy
`
`
`
`
`
`
`
`
`
`
`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 0 and @ 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 aboutall three axes. One example
`of such a rotation matrix is shown below.
`
`
`
`
`
`
`
`
`
`
`
`
`
`
`Fy = [rot(z, W)][rot(y, @)][rot(x, g)|?v =
`
`
`
`
`
`
`(cosycos@) ... (coswsin@sing— sinwcosy) ... (coswsindcosy + sinwsing)
`
`
`
`
`
`
`(sinycos@) ... (cosycosy + singsindsing) ... (cosysing + sinysinécosy)
`
`
`
`
`
`(-sin) ... (cos@sing) ... (cos@cosy)
`
`
`|v = R8v
`
`
`
`
`
`
`
`
`
`
`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 usedrefer-
`
`
`
`
`
`
`
`
`
`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(6) 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 (Xr, YE, Ze) and the subscript
`
`
`
`
`
`
`
`
`“B” designates body reference coordinates (Xp, Ys, Zp).
`
`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 muchtimethat they can noteffectively
`
`
`
`
`
`
`
`
`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(orthe rigid body
`
`
`
`
`
`
`
`
`
`to which it is attached)is tilted to a 90° (degree) angle. For
`
`
`
`
`
`
`
`
`example, if a rigid bodyis tilted such that it points straight
`
`
`
`
`
`
`
`
`
`
`up. At such a point, the roll and azimuth axesare 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 significantdifficulty to conventional body tracking
`
`systems.
`
`
`
`
`
`
`Theprinciples of the present invention use magnetometer
`
`
`
`
`
`
`
`
`and accelerometer input subject to filtering to track body
`
`
`
`
`
`
`
`
`posture.
`In one implementation, Euler angles and their
`related coordinate transform matrices are used to calculate
`
`
`
`
`
`
`
`
`
`
`
`
`body orientation. Although computationally intensive,
`
`
`
`
`
`
`
`
`embodiments of the present invention can use Euler angle
`
`
`
`
`
`
`calc