(12) United States Patent
`Bachmann et al.
`
`(10) Patent No.:
`(45) Date of Patent:
`
`US 7,089,148 81
`Aug. 8, 2006
`
`(54)
`
`(75)
`
`I\U:1'UOO ANI) APPARATUS F"OR MOTION
`TRACKING OF AN A RTlCULAn: n RIGI!)
`800Y
`Inventors: Eric R. lJachmann. Oxford, 01-1 (US):
`Ilobcrt R. McGhee, Carmel, CA (US);
`XI~oping Vun, Salinas, CA (US);
`MlchlK'l J. Z yda, Cannel. CA (US):
`Douglas L. McKinney, Prunedale, CA
`(US)
`(73) Assignee: The United Shlks of America liS
`n'p res(' nt('d by tht' Sl~rehlry of Ihe
`Navy, Washington. OC (US)
`Subject to any disclaimer, the tern} of this
`patent is extended or adjusted under 35
`U.S ,C. I 54(b) by 33 d1ys.
`(21) App!. No.: 101963,007
`
`( *) Notice:
`
`(22) Filed:
`
`S ... p. 30, 2004
`
`Related U.S. Application I)at a
`
`(62) D ivision of application No. 10/020,719, filed on Oct.
`30,2001, now Pm . No. 6,820,025 .
`
`(60) Provisional application No . 601246,215, fikxl on CX:1.
`30, 2000.
`
`(51)
`
`Inl. CI.
`(2006.0 1)
`GOlC /7/00
`(2006.0 1)
`A61B 5/ /03
`___ _ ... ___ ._._. 7021151; 600/595
`(52) U.S. C I. _. ___ ... _.
`(58) Field o(Cla s~lfkado" Sea~h .. ___ ._._._._._. 7021151.
`7021150,141,142,94,53: 6001595; 345/473
`See application file for complete sca~h history.
`
`(56)
`
`l{efl'l'l'nces Citl'd
`
`U_S _ PATENT DOCUMENTS
`
`5.807.284 A • 911998 Fox1in _.
`5.819,206 A • 10/1998 Horton <'I aL
`6.636.826 BI' 1012003 Abed al.
`.
`
`6.691.074 "' . 212004 Moriya <'I aL
`
`6.8 20.025 BI' 1112004 Bachmann et al.
`
`.. 6001595
`7021150
`"- 7021 151
`7021190
`"- __ 702/94
`
`OTHER PUBLICATIONS
`
`Henault, G .. A Computer Simulalion Study and Component
`Evaluation for a Quaternion Filter for Sou~elcss Trucking
`of Human Limb Segment Motion, Mar. 1977, Naval
`PoslGroduate School."
`
`• cited by examiner
`
`Primary Examincr- John Barlow
`Assistant Examiner- Toan M. Le
`(74) Attorney, Agent. or Finn- Donald E. Lincoln
`
`(57)
`
`I\USTRt\CT
`
`One embodiment !he invention includes a method of delCr(cid:173)
`mining an oricntation of a sensor. 1ne method includes
`measuring a loca[ magnet ic field vector and a local gravily
`vector and using those measuremelUS to determine the
`orientation of the sensor. Embodiments can include measur(cid:173)
`ing the magnetic field veetor and the loca[ gravilY vector
`using quatemion coordinates. Another embodimelll includes
`measuring a local magnetic field veetor, a local gravily
`vector, and the angU[;lr velocity of !he sensor. These tluce
`vectors are processed 10 determine the orielUation of Ihe
`sensor_ In Olle eilloodilllent Ihe three vcctors C;lll all be
`measun.>d
`in quatemion coordinak>s. Another rnethexl
`embodiment includes delCnnining;l loc;ll gravity veclor by
`providing a accelerotion detector. moving the dete<:tor from
`;l st;lrt point to an end point over a time period. and summing
`;leeelerulion measurements over the time periexl. The local
`gravity vector is ca[culated using Ihe sunulll>d acceleration
`me;lsurements.
`
`5.645,077 A· 711997 Foxlin ." ...... " ............ . 600/587
`
`14 Claims. 6 I)rawing Sheels
`
`h,"';,'6i'-,1 f
`'--..... --'
`V
`
`ZTE Exhibit 1004
`Page 1 of 18
`
`

`

`U.S. Patent
`U.S. Patent
`
`Aug. 8, 2006
`Aug. 8. 2006
`
`Sheet 1 of 6
`Sheet 1 of 6
`
`US 7,089,148 81
`US 7,089,148 B1
`
`
`
`ZTE Exhibit 1004
`Page 2 of 18
`
`ZTE Exhibit 1004
`Page 2 of 18
`
`

`

`U.S. Patent
`
`Aug. 8. 2006
`
`Sheet 2 of 6
`
`US 7,089,148 81
`
`~ ~ /---- /
`J[,"" -}'(j)
`4t'eGU'<!',l/ElE.£5
`-<
`t:f, Az A,ff - )
`
`!14p4?"'d",dI
`
`I--
`(f~U'n1)T
`
`Z(I)~
`JJi
`
`-/ ~
`[y/] x T
`..3'8
`
`'#d>,vE$d/UE,!?.5
`("4"", "3)
`
`.-/
`
`.5'7
`
`..3'& /.7 -\( ----- jC
`
`A'#";C'/4E-
`.£A'7Z .%"#.$=
`(Pi?/)
`
`fi(cJ/,;?r)
`
`r.,<
`<1 .".""
`
`~"'1~'/?"
`,/.1',\
`-I'?
`-
`'" J ~ l'il ~
`'1 '-- '1
`
`•
`'I"
`
`'1
`
`+
`
`~i?J
`__ /4tltf' 4:J/
`(1)
`-- 2. _ __
`T1t:tL...)- - -.2._
`.2.
`-
`I ~ ---
`I
`
`/0./
`
`Ii #-1
`
`./tl.f'.
`
`\
`)
`
`ZTE Exhibit 1004
`Page 3 of 18
`
`

`

`U.S. Patent
`
`Aug. 8. 2006
`
`Sheet 3 of 6
`
`US 7,089,148 81
`
`5.vtL?T
`
`.455Y-t/E A' #l/<!/E ~£7P
`5/R.5c?P £7/'/E//7:217/£7//
`
`.d27/
`
`4//"qs-.4:!'}'R5 .4R .4Rery/,,?£
`/"R.t?i?q ,:?/" $E ..5T#.:W.e
`
`/
`i?LJ'wP~ t;l<!/.4TERRd:?.U'
`R ,,?/c
`
`#5,,:'1
`[,b)
`
`L
`4//"?.5,(/~A/t9.4 ~:77~
`P'E4!7 .PE,c'7VR
`
`.:f2
`
`L
`4/fi.52".B',UtF ? E/,.4F/T~
`VEG7,:?£
`
`L
`~fo?/AQ/'u5 4'
`M'E"?"%/.erM'C'A-r 'yEC7Z'E
`.,c-~ M',,?5#E:??:' P'ELP
`YECTae O"gy'#Tq'
`UFeW£
`
`ZTE Exhibit 1004
`Page 4 of 18
`
`

`

`U.S. Patent
`
`Aug. 8. 2006
`
`Sheet 4 of 6
`
`US 7,089,148 81
`
`'?/h'&~)
`
`.d7.:i'
`
`> OWamw/~E.P v-
`.c7/.7 V
`~JVhWJAG" $.F $?7POA/ V
`,1W7,y-,(f'/?TE 7:¥'E 5.tY# c?r' V
`,
`
`~E4'5C'REA441T ;:;rt?7ZJ£'
`q?,# E.577/t#/EP 3FA/5c7£"
`£7.e/E~.477£7,u
`
`~/#fR/?77'</6" H''</ E£'£"c:?£'
`;YEC'?C?£" &/?T ..?"E/"fo'f5
`.&' C"/?"/7E.EJf:l.</ .,c,C'A/C"7/V'#
`
`I-0'PC'7/o.</7Z? armr H'P
`
`~ftf,k747/?,A/ E£"~
`E .5T/MR'TE
`
`$ 7
`
`9P
`
`iYE/,9~?, c?b'E~77CPUft1,e
`Es.77M.47.!F .#.</£1
`IcWrffc,(f',w?,# .,E'4 TE
`
`Jc./
`
`R?,/#,4L/,?7P6" WE
`ifC/TE5E'#T/c?d/ £ / 5 CZT
`Tc:? r"'£"e?£1&/C'E
`~ A/£ k/ E.57?M'/?TE c?.,c
`5E.</5C£" .a6"EA//A';7?,,,,u'
`
`C27Tr"'.tYT
`
`ZTE Exhibit 1004
`Page 5 of 18
`
`

`

`U.S. Patent
`
`Aug. 8. 2006
`
`Sheet 5 of 6
`
`US 7,089,148 81
`
`( .s;;;;t£T )
`
`,P,£'C?Jo/£}/f/u 4' 5Y?.E77A/5
`B/7,11',4;;' Or-..5EA/Sc?e
`aEYE.</P?.7/c?-i/ q:'':/r?7TR -i//c?//
`
`/"
`.,HE.4.5.Y'R,wG r4' ,#4dit/.h7C
`86(£? iYECW,P
`
`/ '
`4/E#5</E/,</O' .4 c?~i77?f
`PECT£?,£'
`
`/ '
`
`!P'.e;;-R'#a-'G #
`~.4.,w,EEdiEN'T YECWR
`[r"a:>,4?' /WE ~E7?C
`WE,,£? YEC'/cPR #A/£? 777E
`c;'pA'Y/?,/ YECWR
`
`/ '
`C'4:'c?at-4.7/,</c; .,4 c?c?.H?'V'72""R
`#~.7V'~#MT PEC'Tc7E
`C.ff /.</G 4".v'.A'T FE -<//c'</
`.4/A'$.FUA'T/c?5
`
`6.
`
`M
`
`~a
`
`.6<
`
`#«6./;7
`
`ZTE Exhibit 1004
`Page 6 of 18
`
`

`

`U.S. Patent
`
`Aug. 8. 2006
`
`Sheet 6 of 6
`
`US 7,089,148 81
`
`/?c.##dcY'
`/96'" diP)
`
`d././
`
`c:6rc7)
`
`&"/.-</E,84/7.-</& //',(/ ,by./'
`E.E'.E'c?£ ;l{FC'W-E &//'/
`/'dl'lYE5 -4 C.E'/$$CA/
`r<?YC'T/C.-</
`
`/I4I;(//,I////Y5" $E C'//T,R7/CA/
`r<?YC'7/c7./t1' /?'#/' CV7 .-?P7P#5"
`A' ~x/ cPU'A'TE.E'./tI'/CA/
`E-E.E'C.e ./577#47'r
`
`~E<!FE477.uC; .?W'E
`a>'Effh7E.c? <?U'..4'TEE.-0'C./tI'
`ERC'CE ES7//v47E
`
`.k'4E#..4'L/?/~6" T/'/E
`wffE?A'7EP cPU'..4'TE/A//CA/
`E?/C,R E577#/?';'iF /27
`/",,R7c7U'C'E .4 '-</E&:/
`E:f?77UA'T.!P 5.E#5C.e
`C'$£<-7,4T/<?"v 4'lWTE.EMaV
`
`6'./..:7
`
`aG'./.-:?
`
`6/7
`
`/:
`
`C?.,Y7;P~
`
`ZTE Exhibit 1004
`Page 7 of 18
`
`

`

`US 7,089,148 8 1
`
`)n:TJlOO AN'I) APPARATUS FOR MOTION
`TRAC K1N'G OF Ai'" ARTICULATED RIGID
`BOIlY
`
`REL<\TED API'UCATION
`
`This application is rclntcd to. and claims priority from tbe
`U.S. I'rovisional .. \pplic1l1Km Ser. No. 60/246.215, en titled
`"Appllrnlus for Real 'lime Tracking and Display of The
`Position and Post uTC of a Body lIsing Hybrid Sourceless
`Sensors. RF Positioning. a nd a Quatcrnion Based Comple(cid:173)
`mentary Filtering Algorithm", filed on Oct. 30. 2000. TItis
`provision.11 application is hereby incorporated by reference
`in its enlirt.'1Y.
`
`TECHNICAL FIELD
`
`'111C invcnlion described herein relates to methods and
`appnmlllS for ttacking the oricn14l1ion (a lso referred to herein
`as POSIIIfI:) of rul object. More pal1icuJarly, the iuvemioll
`((' JaR'S 10 methods and app;.lnltus for trucking the posture of
`an aniculawd rigid body. Still more particularly. Ihe inven(cid:173)
`tion relates 10 llK11nds and apparatus for lracking the
`poslure of :Irtieulaloo rigid bodies using qualernion based
`:ll1ilude csl imation filteri ng and displaying the poslUre of the
`body.
`
`BACKGROUND
`
`Embodiments o f III!;' pre>ctu
`invention are directed 30
`towards methods and ocviccs fo r tracking the oricot.1lion
`(poslUre) of human bodics. hI panK:ular soch o ricotation
`inform..1tion e..1n be inserted into a synthetic (computer
`generated) environment where Ihe motion of the tracked
`booy eM become pru1 of !he synthetK: environment. Previ- l~
`ous motio n trocki l!); syst('ms of the type known in Ihe art
`have a number of limitations that substantially limit their
`uscfulness.
`Cu rrently available Illation tro.:king t~'ChJ}()logi.:s are lim(cid:173)
`ited by their reliance on a genl.Tated signal and/o r the n<:cd
`to have the tr:K:ked body remain in sight of fix.:d stalions
`positionl-d around a workins volume. In either case there is
`a I\.'quircment 10 nwintain oome type of link over a distance.
`Rcyardlcss of Ihe Iype of signal us<--d, it can be generally
`referl\.-d to :IS a ··oource." Usually, tbe clfcctive ronge over 45
`which lhe link n-":Iy be maintained is limited. Moreover. data
`updmc niles may be limited by Ihe physK:al characlerislics of
`the source USl.-d. Addition;llIy. inlcrferencewilh. ordislortion
`of. Ihe source C<ln resuh in erroncous orientation measure(cid:173)
`m.:nts. If Ihe link is broken. a complete loss of track will
`result.
`Due ty(l'C of Itrlcking syst.:m known in the art is the
`so~llcd Illl..'ChimicaltrJCkiilg ~1's lelil. Such sysl.:nlS ust: an
`nJ1 ifieinl exo-skeleio n. which is worn by til<: us<:r of a
`synthetic envirolllll.:nt (Iypicnlly, a com pUl':T-created simu- 55
`latoo enVir()llnlCllt). &nsor.; (e.g .. goniolllCters) within t1lC
`s keletal linkages o f the .::w:o·sk.:I':10 n have- a. gcncral corre(cid:173)
`spondence 10 tllC 3ClUaljoinlS ofthc llsl.'f.loint angl<: data is
`fed iillO killetuntic algorithms that are used 10 octernline
`body postu re and limb position. How.:ver. sin.:.: the <:xO- 60
`s k.:l<:lon is \110m by the use-r. o the-r syst.:ms must be used 10
`ascerta in th.: posi tion of the lNT within the silllulatoo
`e-nvironme-nt. Sudl syslems nre fraughl with numerous
`drnwbacks. For on.:. aligning th.: goniometers with the joints
`of a human body is dillicult. .:spccially with multiple ocgrcc 65
`of fl\.wom (DOF) joints. Addi tiona lly, the joints of th.:
`exo-skeleton cannot perfectly replicatc the range of motion
`
`2
`of tbe joillls of a human body. Thll" , suc h teclmologies can
`provide o nly a rough approximalion o f ..1ctual body mov<:(cid:173)
`mcm . Another limitaliun siems from the facl thai human
`bodies are of dilT.:rcnt si7.cs and dimensiollS. As a result, lhe
`5 e:w:o-skeleton must be n.'Ca librnted for .:ach user. Yet aoothcr
`limitation is imJlOS(.-d by lhe ellcwnbrance o f the exo(cid:173)
`skeleton ilself. lllC wdglu and awkward configumtionofthe
`exo-skeleton prevC\l1 a human uS<:!" from interacting with his
`environment in a nnlural numn.:r. As a resul t. it is unlikely
`10 th.1t the user will ix'Come imm.:rscd in th.: synthetic envi(cid:173)
`ronment in the d..-sin.'<i nUllincr.
`Another widely used sySlem is a magneli.: trad:ing sys(cid:173)
`tem. In such syslems a hlrge !II..1gll.:tic field is generok-d :lIld
`15 calibrnted. '111e IIscr has Illany s!IIall sensors mount.:d at
`various points 011 his body. l11c scnsors are S<'nsitiv.: to the
`g.:nemh .. 'd Ill~gnelic field . ThllS, eh:mgcs in position and
`orkntation of the Ilscrs body with rcspm to th.: g.:n.:rat<'(1
`magneti.: field cun bll dct<-'Ctcd by th.: magncti.: sensors.
`20 Som.: ofdf!lwhack~ of such syslems include very short range
`and difliculty in clllibrUling the gen.:ratoo magnetic fi.:ld .
`lbc .I-.ort mngc stems from lhe facl Ih~t m~gnctic fields
`dl'Cre:Jsc in ]lOwer inversely wilh the square of th.: distance
`from the S"uenning sOurce. This restricts Ihe usc of such
`H systems 10 areas about Ihe si".e o f a small room. In order to
`me a larg<:r work ing area. u ~r movemenl must be mooiliOO
`or 5(;31,-od in some m:UlJI<-T. As ,I result, Ihe mllgni ludc and
`frequ ency of posi tion and orientation errors increase rapidly.
`Addi tion .... lIy. Ihe p1\.'SC1lCC of ferromagn.:tic ma terial (like
`!he mewl in belt buckk'S or w.:apons) dislo rts the generated
`magne-l ie fields .. ,,-ddilionally. Ihe magn':lie sensors pick up
`noise from other maS-lll..1ic fi.:lds gen':l"ated in o r near Ihe
`envirollllenl. Unfo nunalely. these d istorting maglletic fields
`are co mmo nplace. bIlil1g (.-asily gel1era t(.-d by a pletoora o f
`dt.-viccs. includil1g comput.:r monitors. fluorescent lighting.
`powen.-d ek'Ctri('-dl \\- iring in Ille wa ll~. as well as many oth.:r
`sources. Additio nnlly. other sources of magnetic field .:rror
`.:xist. Only wi th the .... id of eXlrcmely dctailOO loo k-up tabl('S
`can even moderately lIe-cumte lIIea~urclllCIlIS be obwinl-d.
`-III lbus, mnS-llCti.: tracking based on a gen('rotl'<i mugn<:tic tkld
`is subj(.'Ct 10 positional .... nd o rientation inacCllmci('S which
`are highly v,triable and uupn.:dicUible.
`Another ~y~teJl1 for dct<-'Ct ing position ,Ind orielliation of
`a body IISCS oo-e:llkod opti.:al sensing. Optical sensing, in
`g.:neml. covers a brge and varying colll'Ction of tl'Chnolo(cid:173)
`gies. All of these t~'Ch)\ologies de]lCnd on the sensing of
`SOllie type of light to provide position and orientation
`infoTlllaiion. Conscquenlly. all of Ihese Icc;hnologics are
`subj<-'CI 10 inaccun...:ies whenever a required light path is
`so blocked. Additionally. these lechno logies suffer from inler(cid:173)
`ference from otbcr lighl Sourc':S. All o flbese optical S<:IIsing
`systems r<--quire sJlI-'Cially prep.m .od enviromllents h.avillg the
`necessary emillers and sensors. This prevcots widespread
`usage and prescnls a signifie:llll and ,:xpCllSive limitat ion.
`Yet another a ppflXlch is a trncking sys wm using acoustic
`trackers. Like the previously ~ribcd mapK'l ic trackers,
`such systems are limitoo itl rang.: due to Ih.: inherent
`limitations of sound propagation. Addil ionally. the physics
`o f sound limit acc uracy. information update rate. and Ihe
`overall mngll of an acoUSl ie tracking syst.:m. Morcover. d ue
`to th.: rclmiv.:ly d irect io nal na ture of sound. ci.:ar lines o f
`sight mU$1 be m ointn irK.-d in OrUcr to obtain aCCllrate r.:ad(cid:173)
`ings. Addiliotwlly. dll': to the rcl3tivcly slow speed o f sound.
`there are latency problems with such acoustic scnsor sys(cid:173)
`tems. As is evident from the foregoing discussioll. collven(cid:173)
`lional appro.1ehes for the tfllCking of body orientation h.we
`some SCriOIlS limitations.
`
`ZTE Exhibit 1004
`Page 8 of 18
`
`

`

`3
`SUMfvlARY OF THE INVENTION
`
`US 7,089, 148 BI
`
`In accordance with the principles orlhe present invention,
`systems, method, and apparatus for body tracking is dis(cid:173)
`closed. A method embodiment for tracking the orientation of
`a sensor comprises measuring an angular velocity of the
`sensor 10 generate angul:lT role values which are integrated
`and nomtalized to produce an estimate of sensor orientation.
`The method cominucs by measuring a local magnetic field
`vector and measuring local gruvilY vector and com.-'Cting the 10
`estimate of sensor orientation using the local magnetic field
`vector and local gravity ve<:tor.
`A.nolhcr method embodiment comprises measuring all
`angular velocity of the sensor to generate an angular ratc
`quutcrnion, integrating and nomKllizing the angular rale 15
`quatcrnion to produce an estimated sensor orientation
`quaternion. nlC local magnetic field vector and local grovity
`vector arc measured. A measurement vector is determined
`from the local magnetic held and local gravity vectors. A
`compll1ed measurement vector is calculated from the esti(cid:173)
`mated sensor oricntation quatcrnion and comparing with the
`measurement vector to generate an error vector that dehnes
`<t criterion function . The criterion function is minimiZl-d ;Hld
`an error estimate qllaternion is output. 'llle erro r estirrmte
`quaternion is integrated and normalized to produce a now
`estinmtL-d seusororient<ltion 411~ternion which can be output
`as a sensor orientation signal. 111e entire process is repeated
`except that the new estimated sensor orientation quaternion
`is USL-d for calculating a computL-d me<lsurement IICl:tor. The
`process continues utlliltracking is no longer desired.
`Another method embooiment of tracking the orientation
`of <l SL"IlSOr comprises providing a starting LostimMe ofscllsor
`orientation. measuring the local magnetic field vector and
`measuring the local gravity vector. A measurement vector is
`determillL'"d from the local 111llg.netic fidd vector and thc local 35
`grovit)' wctor . .1\ computed mellsurement vector is clllcu(cid:173)
`lated from the estimate of sensor orientation and compared
`10 the measurement vector 10 geneolle lIn error VL'"Ctor tlwt
`defines a criterion function. A GlIuss-Newton itel1l1ion is
`perlormed, resulting in a minimized criterion fnnct ion gen(cid:173)
`emting an error eSlimate thai is iIllL'YIIIL'd lInd nIJfllllllized to
`produce a ncw estimate of sensor orientation. As with the
`forgoing embodinlent, the entire process is repeated except
`Ihat lite new estimate of senSlJr IJrientatiIJn is used for
`calculating a compllled meaSliremelli vector. 'Jbe process
`COlltinues lIllIil tracking is no longer desired.
`Yct another method oftrdcking the Orielll<Hiun of <t scnsor
`comprises providing a starting estimate of sensor orientation
`quaternion measuring a local magnetic field vector and a
`loc<tl grdVity VL'Ctor and determining a measurement VL"Ctur so
`from the local magnetic field vector and the local grovity
`vector. A computed measureme111 vector is calculated from
`the estimate of SL"lISOr orientation, using quatemion math(cid:173)
`ematics.
`The mcasurement vector is compared with the compmed
`mellsurement vector to generate an 6xl error vector tlwt
`defines a criterion function alKI a mathematical operation is
`performed tlwt results in the minimi:z.1lion of the criterion
`function and outputs a 4xl quatemion error estimate. nte
`quaternion error estimate is intcgf3ted and oonnalizLxl to 60
`produce a new estimated sensor orientation quaternion. The
`elllire process is repeatLxl except that the new estimated
`sensor orientation quaternion is used lor calculating a com(cid:173)
`puted measuremelll veclor. The process continues until
`trocking is no longer desired.
`A method lor delemlining a local gravity vector values
`comprises, moving the sensor from a start point to lIn end
`
`4
`point over a time perioo; taking measurements of the total
`acceleration vector during the time period. time weighled
`slunming Ihc measurements of the accelcrotion vector ovcr
`the time period, and caleulating gravity vector values using
`the summed total acceleration measuremellls.
`Embodiments of the invcntion also include sensor appa(cid:173)
`roms comprising a magnetic field detector configured to
`measure a magnetic field vector and OlllpUi a local magnetic
`field vector signalllnd an acceleration detector configored to
`detect a local gravitational field vector and output a local
`gravitational field vector signal. A relatLxl embodiment fur-
`ther includes an lIngulnr velocity detector COnfigllJ'L--rl to
`detect an angu lar velocity vector of the sensor and output
`lIngular velocity signaL
`Embodiments oflhe invention includc a system for track-
`ing the posture and orientation 01" body, the system com(cid:173)
`prising a body having mounted thereon at least one sensor,
`each sensor including 11 nwgnetometer for measuring a
`magnetic held vector and a acceleration detector lor ma1-
`20 suring a body acceleration vector, and 111 least one processor
`for receiving input from the magnetometer and aceelcration
`detector and using said input to calculate a local magnetic
`field wctor <tnd <t local gravity VL'Ctor <md to delL'TIlline the
`oricntation of the booy. 'Jbe processor of the system can be
`25 filrther configured to correct lor olrset between body coor(cid:173)
`dinates and sensor I:oordinates. The system call further
`inelude a display for displaying the position and orientation
`of the body with respect to a synthetic environmelll. Further
`embodiments usc SL'"lISOrs that includc angular velocity
`30 detectors.
`Other aspects and advantages of the invention will
`bL'COllle appaTL'Ilt from the following dL1:1ilL-d dL""SCription
`lInd accompanying drowings which illust11l1e, 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
`40 with the accompanying drawings. in which:
`FIG. 1 is 11 figurotive depiction oftllC earth's surfacc and
`its relationship with a example pair of a magnetic field
`vector and grovity vector.
`FIGS. 2(a) and 2(b) dq.ict refcrencc and body coordinate
`45 systems respectively
`FIG. J is a simplified block diagram of a filtering method
`cmbodimcnt in accordance with thc principles ofthc present
`invention
`FIG . 4 is a block diagram of a system embodiment of the
`prescnt invention.
`FIGS. 5 and 6 arc flow diagrams illustrating method
`embodiments in accordance with the principles of the
`present invention.
`It is to be understood tlwt in the drawings like reference
`55 numerals designate like stntClltraJ elemellls.
`
`DETAILED DESCRIPTION OF THE DRAWINGS
`
`The em1xxiiments of the prescnt invention providc a
`method and apparallls for tracking the posture of a booy
`withom the need for a generated field (or source) or a
`plurality of fixed stalions. Advances in the field of miniature
`sensors over the last decade make possible inenialJmagnctic
`trllcking of the orientation of trocked body in three dimen-
`6S sions. In pllrticular such sensors can be used to trock human
`body limb segments in three dimensions. In accordance with
`the principles of the present invention such tracking incor-
`
`ZTE Exhibit 1004
`Page 9 of 18
`
`

`

`5
`pomles the passive mcaSlIJ'l'Jll{'nt of physical quantities that
`are din.'Ctly related 10 the rate of rotation and orientation of
`a rigid body. lnc "sourceless" nature of this technique
`makes possible full body posture tracking of multiple uwrs
`over an area that is only limited by the range of a wireless
`LAN. Since orientation estimates arc based only on passive
`measurements, nearly all latency in such a system is due 10
`the complIIalional demands of the data processing algo(cid:173)
`rithms involved and not physical characteristics of the
`gcncroled source.
`An embodiment oflhe pn-osent invention makes use ortlle
`fact thm a local mag.netic field vector and local gravity
`IIttlOr can be defined FIG. 1 is a simplified figurative
`illustration of the surfhce of the earth 10 sho~'ing a local
`gravity vectors 11 and a local magnetic field ve<:tor 13 . In 15
`gc-nerol. for any object position('d on the ('arth 10. the grovity
`ve<:tors always points to the earth's center of mass. ~'hich
`may be defined as "down". This phenomenon is simply
`illustrated in FIG. I , Additionally. the magnetic field ve<:tor
`13 always points to magnetic north.
`Sensor embodiments of the present invention, by tracking
`changes in the orientation of the sensor with respe<:t to the
`local magnetic field vector and the local gravity vector. can
`det<.-ct changes in orientation of the sensor. An appropriately
`designed sensor can track the orientation of a body. Signifi- 25
`cantly. a system having a plurality of sensors, each moullled
`to a limb of an articulated rigid body can be used to track the
`oric-ntation of each limb. [n such systems, 1xxIy posture can
`be tracked and intrcx:luced into a synthetic environment,
`thereby allowing a user to interface with the synthetic 30
`environmc-nL
`The Nature of the Mathematical Problem
`A harrier to an etlective implemelllation of effective body
`tracking systems stems from certain mathematical difficul(cid:173)
`ties inherent in mapping body orientation from one coordi(cid:173)
`nate system to another. These difficulties have presented
`
`US 7,089, 148 BI
`
`6
`Euler angles represCllt the orientation of a rigid body using
`three rotations about spt.'Cified axes in a speCified order of
`rotation. For example, rclc rring to FIG. 2(a ), first rotation
`about the nonh axis, se<:ond rotation about the cast axis, and
`th ird rotation about the down axis, These would be analo(cid:173)
`gous to "roll" , "elevation", and "azimuth", By convention,
`roll angle is designated by "'r', elevation by " If', and
`azimuth ''1\,,', It is tu be noted tlklt if the temporal urder of
`rotations is reversed. body axis rotations yic-ld exxtly the
`10 same orientation as reference axis rotations ,
`The position of a poilll in space can be describ,--'d using a
`three-dimensional point vector. A rigid body can be
`describt.--d in teons of a plurality of point vectors , An
`example v,-><:tur can be R>pTCSellled by a v,-><:tor V =[x y z].ln
`order to describe rotational behavior. matrix tronsfomls 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 tTansfonn can be used.
`
`Thus, the original x, y, z coordinates can betranslated into
`the rotated coordinates >::', y. z' through the application of a
`3;.;3 rotation matrix. [n the depicted example, the 3;.;3 matri x
`is designed to accomplish a rotation about the X-axis
`(represented by [rot(x. <)I)]. Similar 3;.;3 rotation matrices
`exist for 0 and ¢> rotations about the Y- and Z-axis, respec-
`tively. Such rota1ion tl'rulsfonns are known to tbose having
`ordinal)' skill in the art and will not be described here in
`great detail. Additionally. a single 3x3 rotation matrix can be
`35 U$l.--d to describe rotation about:l11 three axes One example
`of such a rotat ion matrix is shown below.
`
`(co..,co.o) ... (cow.in9.illjO _ .illl/Jeoo<p) ... (co..,. inlko"!' ,," . illl,!-<in'fJ r
`
`.. (C""".in'f " .irll,"ino.x...;)
`
`\' • !I' ,
`
`(.i,yco<O) .. . (C~"co,+, "" <i"f'ittl.i n'f)
`(- "n) .. . (co.osi"",J .. . (c"""",,,,,!,l
`
`[
`
`problems which convemional approaches bave not been able
`to resolve for over 100 years.
`TIle nature of the problem is briefly outlined in the
`following p..1ragrnphs, A conventional way of describing the so
`oriemation of a rigid body uses " Euler angles" to describe
`the orientation of a rigid body in tlm."C dimensions. Eulcr
`angles describe the orientation of a rigid 1xxIy using three
`rotations ab011l specified axes. One cOllllllonly used refer(cid:173)
`ence coordinate system is the local "flat Eanh" system. Such 55
`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-diR'Ction corresponds to local cast. and the
`Z-dilwtion corresponds to down as depicted in FIG. 2(a). 60
`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 point ing "out of the nose" in a positive dil'l.'Ction, Y
`out the right side. and Z down. TIle subscript " E" designates 65
`e<lr1h referencc coordinates (Xr, Yr. Zr ) and the subscript
`"8" designates body refercnce coordinates (XB. VB. ZB).
`
`As can be seen above. one of the difficulties inherent in
`using Euler angles and the afofCmentioned transfomlmatri(cid:173)
`ces is the sheer number of computations require to calculate
`each motion ora body. The problem becomes nmgnilied as
`more and more bodies arc simultancously tracked. In the
`end, the complcx calculations required using thcse existing
`algoritluns require so much time that thcy can not ellbctively
`be used to track body position and orientation in real-time.
`lbis is <t signifil'illlt dnlwb;.(ck,
`In addition. Euler angle systems present certain compu(cid:173)
`tat ional ditliculties. In particular is the problcm of singu(cid:173)
`larities. Singnlarities 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 bOOy is tilted such that it points straight
`up. At such a poill1, the roll <lnd azimuth axes are co-linear.
`This I'I."Sults in a situ,1tion where neither the roll nor azimuth
`angles are uniquely defined. only their dill'erence or sum can
`be spe<:ified uniquely. This problem becomes magnified
`when the time rote of change of Euler angles are used to
`quantify rates of angular rotation. This singularity problem
`
`ZTE Exhibit 1004
`Page 10 of 18
`
`

`

`US 7,089,148 8 1
`
`8
`eters can be USt'CI to measure the direction of the local yavity
`vector. In onc cx.mllp[e. accelerometers .11 can be used to
`determine the local gravity vector by measuring the combi(cid:173)
`nation of foru-d linear accelt'l<ltion and the rl'uclioll force
`5 duc to grovity . .'\ s such 3CCl'lerometer data can be used to
`determine a local gt"Jvity VectOf. 1Mt is because the aceel-
`
`a IOta[ acceleration vector a _""""",,
`
`-
`
`erometer nK:~Isurt'S
`derm<.-d by
`
`D_--r a + /1
`
`7
`is known \0 those ~wing ordinary skill in the an and
`presents signiflCllm dillicully \0 convClltional body tracking
`systems.
`The prindplcs oftnc present in"cnlion usc magJll.'tollK'ter
`and accelerometer inpUl subject to filtering to track body
`posture. In one implementation. Euler angles and their
`related coordinmc transform mmrices are used \0 ca lculate
`body oricnl(uion. Allhough computationally
`intensive.
`embodiments o f the pn;'Scnt invention can usc Euler angle
`calculations to trnck body oricnl:llion. Additioll3l1y, angul;-.{ ]0
`velocity information can be used \0 correct for lime lag
`errors. Ho .... .::vcr, other embodiments of the present inven(cid:173)
`tion pn.'$cnl a pnrticuL'Irly advant:rgoous approach for
`achieving body tracking using quatcrnion mathematics.
`Such embodiments climi!)ml,' the Euler angle singularity 15
`problem, ond red1lcc th e computational complexity of the
`invention.
`[n nccordance with the principles of the present invention,
`sensor sigmlls nrc input into Hn al1itude estimation filter.
`Such a sensor can function using ordinary Euler angle
`nmthemll1ies. But. in preferred emlxxliment, the filter is a
`qlllUcrnion hased complementalY anilUde estimation fi1ICr.
`USing qua\('mion mathematics rc~ults in an approx.imate
`lOO-fold inerc~se in processing efficiency. Although the
`pn..'SCI}t implement nlion is novel , the field of quaternion
`nUll hemm ics is koown to those having ordinary skill in the
`an and is cllplnin(:d in detail in numerous mathematical
`telllS. One example. is Kuipers.. J, "Quatemionsand Rotation
`si.."Iucl)(;cs". I'rinet..1on University Press, Inc., Princeton,
`N.J .. 1998 (hereinaftl'r "Kuip<'rs"). which is hereby incor- .JO
`porated hy reterence.
`Such a fill ... ,. is USl.-d in (:onjullctioll with data suppliL-d by
`Sl't"LSOrs to producca Sl.'nsororielllation l'stimatl' expressed in
`quatcrnion limn. In one embodiment. the sensors include a
`t[\fl:c·axis IllilgJK:tometer and a thIl .. '{'-allis accelerometer. In l~
`anot he r sensor embodim<.'Ilt. the magnetometers and ucre[(cid:173)
`erometers arc supplcmcu\<.'d with angular rate detectors
`eonfil!,ut<.-d to dct ... 'Ct the nn}l,ular velocity of the sensor
`(comprising so-cal1t..'(] Magrl{:tic. A.ngular Rate. Gravity
`( MAKG) sensors). Each MAK(i scnsor contains angular rate ¥I
`uCH.'Ctors. 1It..'Cckroll)dcrs. and magnetometers. Estimation
`error is minimized using Ga uss-Newton iteration. Unlike.
`othcr sensors kn()wn in thc Hrt. sensor embodiments of the
`invcntion call correct for drin contin uously without lilly
`R'quirement for still periods.
`[n an articulated rigid hody. posture is dctennilled by
`estim;,t inl!, indivkluallimb scg.tuL-nt oril.-"Iltations through the
`al1achme1l1 of sensors. The orientation estimates arc used to
`animate a simple human model or avatar in real-time. The
`model din.~1Iy at.."Ct:pts limb Sl.""Slll<,·nt orientation infolll}atioll so
`in qnatemion form relative to a f}Xed reference frame (for
`example. an ~'al1h fiXL'd reference frame). Simple calibration
`pn.x:cdul\."S l,re uSt..'CI to ildjust SCilliOf scale factors and null
`points, as wd[ as aCColUlt for offsets hclween the sensor
`coordinate frames and the fr .... mes associated with the limb 55
`seg.ments to which they are altao:.:hoo.
`Body Tracking Usi ng S(.'[lSOfS with MagrletolllctCl"S and
`j\ CC"l.'ieromctl·rs
`Referring to FIG . .1. q dcfi U("l> tile varia ble for the orien(cid:173)
`tation quatemion and q dl'finl'S values for the estimated 60
`orientation quatt'rnion. Accelerometers measure and retu rn
`an approximation to the l\Xa[ gravity vector (the local
`vertical). the unit WClor h 3 I. The rnagnetoml.'ll'rs measnre
`and return the dirt.'Ction of the local magnl.'lic field vC(:tor
`(the utlit vcctor b) 32.
`[n short, m:ISnctometers cm} be uscd to measure the
`direction of the [OC~I [ lU(lgnetic field vC(:tor and (lcre[erom-
`
`'0
`
`-
`
`In one embodiment. a Ihr~'4,l-axis ao:.:celerometcr can be
`USl."-d 10 mcasure totnlnccclcralion (forCl..'(] [incar acceleration
`and grJvitatiollnl reaction force) a ",,,,,,,,,,.1 ovcr a fixed time
`period. By conducting II time wcighkxl summing (or inte(cid:173)
`Sr.ltinS) of acceleration valu<"s over some relatively short
`t