Estimation of Multibody Kinematics Using Position Measurements

Transcription

1 Jechieh Lee Hery Flasher Departmet of Aerospace ad Mechaical Egieerig, Uiversity of Souther Califoria, Los Ageles, CA Jill L. McNitt-Gray Departmets of Kiesiology, Biomedical Egieerig, ad Biological Scieces, Uiversity of Souther Califoria, Los Ageles, CA Estimatio of Multibody Kiematics Usig Positio Measuremets A method for the estimatio of iematics of a system of rigid bodies coected by three degrees of freedom rotatioal joits usig positio measuremets is itroduced. I the proposed approach, system iematics are computed from experimetal measuremets while preservig importat physical ad iematic properties. These properties iclude system itegrity, i.e., preservig itercoectios betwee the bodies, ad the etire system dyamic properties, amely, ceter of mass iematics ad its agular mometum. The computatioal procedure cosists i solvig a sequece of optimizatios of appropriately formulated objective fuctios that icorporate the preservatio of physical ad iematic properties by employig the pealty fuctio approach. The cofiguratio of the segmet iematics of the system is computed via a quaterio parametrizatio of orietatio that leads to a efficiet computatio procedure. The sequece of optimizatio problems is solved usig a embedded iteratio process. Two studies are preseted to demostrate the performace of the proposed approach: estimatios of the iematics of a simulated three-li model ad of a experimetally measured 3D motio of huma body durig flight phase of a jump. The results of the two studies idicate fast covergece of the algorithm to a optimal solutio while accurately satisfyig the imposed the costraits. DOI: / Itroductio Estimatio of dyamic system iematics usig vector measuremets has importat applicatios i biomechaics 1, spacecraft attitude estimatio 2, ad robot visio 3. I aerospace applicatios, estimatio of the attitude of a sigle rigid body usig vector observatios has bee studied extesively 4 7. These algorithms are based o a problem proposed by Wahba 8 i which the optimal attitude determiatio problem is formulated i terms of a quadratic loss fuctio. Subsequetly, Daveport developed the q-method 2,9 that solves Wahba s problem usig quaterio parametrizatio of the attitude. Similar algorithms were developed for computer visio applicatios by Hor 10,11. A closed form solutio for the rotatio recostructio problem was developed i Ref. 12. I biomechaics, multibody models are employed to study the movemet of the huma body A essetial step i the multibody model developmet process is its validatio usig experimetal data 17. Kiematic data are typically collected by tracig the trajectories of marers at various locatios o the system 3,18,19. Durig the experimetal measuremet process, positio based measuremet errors are itroduced 20,21. Although the orietatio of each body i a body system ca be computed by solvig idepedet Wahba problems, such a approach may cause loss of system itegrity by creatig gaps at the locatio of the rotatioal joits. I additio, the iematic data with measuremet errors may ot satisfy the costraits imposed by dyamic laws goverig the etire system whole body motio. For example, durig free-flight, impulsemometum relatio implies that the locatio ad velocity of the system ceter of mass CM at every istat of time are determied by iitial coditios. I additio, i free-flight, the ability to cotrol whole body rotatio stems from the coservatio of total agular mometum of the system about its ceter of mass Cotributed by the Desig Egieerig Divisio of ASME for publicatio i the JOURNAL OF COMPUTATIONAL AND NONLINEAR DYNAMICS. Mauscript received February 20, 2010; fial mauscript received August 10, 2010; published olie December 15, Assoc. Editor: Olivier A. Bauchau. 22,23. Therefore, preservig coservatio of agular mometum by the iematic data is critical for studyig the cotrol logic used to cotrol the huma or robotic system. I this paper, the problem of estimatig the iematics of rigid bodies itercoected by joits with up to three rotatioal degrees of freedom is formulated. First, the problem of fidig the optimal orietatio of each li is formulated as coupled Wahba problems i which zero joit gap coditios are imposed. The, a adjustmet procedure is formulated for CM positios, CM liear, ad agular velocities i order to satisfy joit gap velocities ad liear ad agular system mometum costraits while miimizig the chage with respect to measuremet data. The highly oliear set of coupled Wahba problems is solved by usig quaterio parametrizatio of each li orietatio ad a specially devised iteratio process. The solutio of the optimizatio problems for the adjustmet procedure leads to a sequece of recursive equatios that are solved iteratively. Two examples are preseted to demostrate the performace of the proposed approach. I the first study, a plaar three-li model is used to evaluate the umerical efficiecy of the proposed algorithm. The secod example demostrates the applicatio of the approach to the study of huma motio. I this example, experimetally collected tracig marer data are used to estimate the iematics of a 14-li system with three degrees of freedom rotatioal joits that models a huma performig a jump. This paper is orgaized as follows. I Sec. 2, the problem of iematic estimatio is formulated. The solutio of the iematic estimatio problem is give i Sec. 3. I Sec. 4, a example of iematic estimatio for a simulated plaar three-li model is preseted. Sectio 5 discusses the estimatio of the iematics of a huma durig a jump usig experimetal data. Cocludig remars are give i Sec Formulatio of the Kiematic Estimatio Problem Cosider a model cosistig of rigid bodies segmets of mass m i,,...,, ad momet of iertia J i,,...,, as show i Fig. 1. It is assumed that the bodies are coected by rotatioal joits that ca have up to three rotatioal degrees of Joural of Computatioal ad Noliear Dyamics JULY 2011, Vol. 6 / Copyright 2011 by ASME Dowloaded From: o 12/08/2014 Terms of Use:

2 rotatio trasformatio from frame B i to frame I. The matrix i Ṙ o is give by Ṙ i i o = ir o 3 i is the agular velocity of body i relative to referece frame I expressed i I, Fig. 1 Cofiguratio of a chai of rigid bodies freedom. The orietatio of segmet i is determied by the orietatio of frame B i body frame attached to it with respect to a iertial frame I referece frame Fig. 1. It is assumed that at every time istat, the positios of a sufficiet umber of poits o each body i,,...,, are available i both body ad referece frames. The objective of the proposed computatio procedure is to use the positios of the poits to estimate the iematics of the segmets i the chai orietatios ad positios while maitaiig physical costraits. 2.1 Kiematic Costraits. The costraits iclude 1 system itegrity costraits that maitai the coectio ad zero relative velocity betwee bodies at joit locatios, 2 system liear mometum costraits that impose costraits o the trajectory of the system ceter of mass, ad 3 system agular mometum that imposes coservatio of the agular mometum of the system. I the followig, these costrais are formulated mathematically System Itegrity Costraits. The locatio of a joit coectig bodies i ad j i iertial frame ca be expressed usig iematics of either oe of the two bodies, deoted as r ij ad r ji, respectively. The, i the absece of measuremet errors, the iertial locatio ad velocity of the segmet ed poit expressed usig either body s iematics must be idetical, leadig to r ij = r ij r ji = r ci + R i o L ij r cj + R j o L ji =0 1 v ij = v ij v ji = ṙ ci + Ṙ i o L ij ṙ cj + Ṙ j o L ji =0 2 r ci is the positio of the CM of body i expressed i frame I, L ij deotes the relative locatio of the joit with respect of i body i CM expressed i frame B i Fig. 2, ad R o deotes the i = 0 iz iy iz 0 ix 4 iy ix 0 I the presece of measuremet errors, r ij 0 ad v ij 0imply joit gap positio ad velocity errors, respectively System Liear Mometum Costraits. Assume that the exteral forces F ext actig o the system are measured experimetally, e.g., usig a force plate at the iterface betwee the foot ad the eviromet. The, the locatio r c ad velocity v c of the ceter of mass of the system at every istat of time ca be determied from iitial coditios ad the total impulse applied to the system, t Mv c t v c 0 = m i v ci t v ci 0 F ext d 5 =0 leadig to the followig costraits: L =0 t F ext d m i v ci t v ci 0 =0 P = Mr c t m i r ci t =0 7 M is the total mass of the system. I particular, for freeflight i gravitatioal field, the exteral force is the weight of the system actig i the vertical directio F ext =0 Mg 0 T. Hece, mometum is coserved i the horizotal plae, ad the ceter of mass of the system describes a parabola i a vertical plae. Equatios 6 ad 7 imply that L is a fuctio of v c ad v ci, i =1,...,, ad P is a fuctio of r c ad r ci,,..., Agular Mometum Costraits. The agular mometum of a system with respect to the system CM is give by t H o t H o 0 = ci m i ci + J i i H o 0 M ext dt =0 8 yieldig the costrait H = H o t ci m i ci + J i i =0 6 9 Fig. 2 Itercoectio betwee two bodies ci =r ci r c, ci =ṙ ci ṙ c,,...,, ad H o t deotes the agular mometum of the system with respect to system CM at time t ad J i is the iertia matrix of body i. I the particular case of free-flight, exteral torque M ext =0 results i the coservatio of agular mometum. From Eq. 9, we have that H is a fuctio of r c, r ci, ṙ c, ṙ ci, ad i,,...,. 2.2 Formulatio of a Computatio Procedure. The iematics estimatio problem is formulated as a sequece of costraied optimizatio problems that is performed i two stages. I the first stage, the optimal orietatio of each li is determied while satisfyig zero joit gap coditios. I the secod stage, CM liear ad agular velocities are adjusted to satisfy joit gap velocities ad liear ad agular system mometum costraits while miimizig the chage with respect to the measuremet data. I the followig, it is assumed that the iitial estimates for / Vol. 6, JULY 2011 Trasactios of the ASME Dowloaded From: o 12/08/2014 Terms of Use:

3 the positios, orietatios, ad liear ad agular velocities are provided, as well as the exteral forces actig o the system Estimatio of Segmets Orietatio. The objective is to fid the optimal orietatio of the system lis give by matrices R i i i o,,...,. Let a s ad b s deote the locatio of marer s o body i expressed i frames B i ad I, respectively. The, i the absece of measuremet errors, the followig equality is satisfied: s i R o i = b s i R o i a s i =0, i =1,...,, s =1,...,N i N i is umber of observatios o body i. Due to measuremet errors, we have that s i R o i 0. The optimizatio problem is the defied as a miimizatio of the sum of the squares of the errors s i R o i while eforcig the costrait o gap positio errors r ij =0. The costraits o the gap positio are icluded usig the pealty fuctio method as follows: N i mi J O = mi R 0 1 R0 R 0 1 R0 X i s i s R i o 2 s=1 + W i,j=1,ij X i s,,...,, ad s=1,...,n i deote weightig coefficiets. I this optimizatio for orietatio, it is assumed that r ci,,...,, are fixed; therefore, R i o,,...,, are the oly variables to be estimated. The variable W 1 1 is a sequece of weightig coefficiets parametrized by a set of parameters 1 = 1 1 ; i.e., W 1 1 is fixed for a give 1. It represets the relative importace of the joit locatio error with respect to the attitude estimatio error. Note that miimizatio of the first term of objective fuctio aloe ca be viewed as solvig idepedet Wahba problems, oe per each li of the system Adjustmet of Lis CM Positios, CM Velocities, ad Agular Velocities. Adjustmets i li CM positios, liear velocities, ad agular velocities are made by defiig objective fuctios that are desiged to miimize joit gap positios ad velocities, system liear ad agular system errors, ad the chage with respect to the measured data. Deoted by the superscript is the value of a variable obtaied from measuremet. The, the adjustmets are defied as r ci = r ci r ci adjustmet of li CM positios v ci = v ci v ci r ij R o i,r o j 2 adjustmet of li CM velocities i = i i adjustmet of li agular velocities These adjustmets are computed by solvig the followig optimizatio problems: i adjustmet of the li CM positios mi J CMP = mi r ci r ci r ci 2 + W 2 P 2,...,... ii + W 3 i,j=1,ij r ij 2 adjustmet of the lis agular velocities mi J = mi i i,...,... i 2 + V 1 H 2 + V 2 iii adjustmet of the li CM liear velocities i,j=1,ij 11 v ij 2 12 mi J CMv = mi ṙ ci ṙ ci v ci 2 + V 3 L 2 + V 4 H 2,...,... + V 5 13 i,j=1,ij W j j, j=1,...,3, ad V i i,,...,5, are sequeces of weightig coefficiets parametrized by j ad i, respectively, that are omitted here for clarity of otatio. v ij 2 3 Solutio of Kiematic Estimatio Problem 3.1 Computatio of Li Attitude. Followig the procedure itroduced by Daveport 2,24, the miimizatio problem i Eq. 10 is trasformed ito a maximizatio problem as follows: max J R =max J R 14 R 0 1 R0 R 0 1 R0 =1 J R is related to measuremets o body. Let set U cotai the idices of all the bodies coected to body. The, N J R = s=1 b s T R o a s + W 1 r cu r c T R o L u, + L T u, R u o T R o L,u 15 The rotatio matrix R o is the parametrized by the quaterio q 25. Usig the fact that the relative rotatio betwee bodies i ad j is give by q i j =q i qj, q i is the cojugate of q i, J R ca be expressed i terms of quaterios 24, g q = q T K o q + W 1 B = b 1 b 2 q T K u q + q 1 q T K u, q u q K o = KB,A b N, A = a 1 a 2 a N 16 K u = Kr cu r c,l u,, K u, = KL u,,l,u The operator KA,B, A ad B are matrices, is defied as K = S I 3 Z Z T 17 W = BA T, S = W T W Z = W 23 W 32,W 31 W 13,W 12 W 21 T, =trw Usig as Lagrage multipliers to eforce the coditio q 2 =1 ad otig that q =q 1, the problem i Eq. 14 ca be writte as g q q T q, =1,..., 18 max G =max q q yieldig the coditio for a maximum, G =0 K q U q = q, q =1,..., 19 Joural of Computatioal ad Noliear Dyamics JULY 2011, Vol. 6 / Dowloaded From: o 12/08/2014 Terms of Use:

4 K q U = K o + W 1 K u + q u T L K u, q u L ad for a quaterio a, a L a4 a3 a2 a1 a 3 a 4 a 1 a 2 a 2 a 1 a 4 a 3 a 1 a 2 a 3 a 4 20 = a L 21 Assumig that q u, are ow, Eq. 19 ca be viewed as a eigevalue problem that yields q. As i the q-method, the eigevector correspodig to the largest eigevalue of K is the quaterio that maximizes the fuctio G i Eq. 18. Give the segmet orietatios q, =1,...,, ad weightig coefficiet W 1, updated orietatios ca be computed solvig eigevalue problems give i Eq. 19. Note that icreasig the weightig W 1 icreases the pealty o the violatio of joit positio costraits, thus forcig the solutio closer to the feasible regio. A more detailed derivatio of the above formulatio that icludes systems with a umber of braches ca be foud i Ref Solutio of Adjustmet Problems. The solutio of the adjustmet problems will be demostrated by solvig the li CM adjustmet formulated i Eq. 11, 1 2 J CMp r c = r c r c + W 2 m Mr c m i r ci + W 3 r c r cu + R o L u R u o L u 22 U idicates the idices of all the bodies coectig to body. The coditio for extremum is r c + W 2 m m i r ci + W 3 r c r cu = = r c + W 2 m Mr c + W 3 R o L u R u o L u T Defie the 3 vectors pˆ =r c1 the r T c T ad ˆ p= 1T T T, ad pˆ c = p 1 ˆ p p = I 3 + W 2 M I T M I + W 3 C 3rL ˆ p = pˆ + W 2 M m + W 3 C rr 25 M I =m 1 I 3 m I 3 T, M m =Mm 1 r ct Mm r c T T, ad I 3 is a 33. The C 3rL 33 matrix defies the topology of itercoectio betwee the lis 24, ad the 31 vector C rr is give by C rr 3 2:3,1 = R o L,u + R o u L u, 26 Give the li CMs, r c, =1,...,, their orietatios q, =1,...,, ad weightig coefficiets W 2 ad W 3, updates for ceter of mass locatios r c, =1,...,, are obtaied usig Eq. 25 Solvig the optimizatio problems Eqs. 12 ad 13 of li agular velocities ad CM liear velocities yields recursive equatios i the form similar to Eq. 25, ˆ = 1 ˆ 27 1 vˆ c = v ˆ v 28 T ˆ = c1 T c T T, vˆ c=v c1 v T c T, ad the matrices = V 1,V 2, = ˆ,V 1,V 2, v = v V 3,V 4,V 5, ad v = v v c,v 3,V 4,V 5 are defied i the Appedix. Hece, give weightig coefficiets V i i,,...,5, ad estimates of agular ad liear velocities ˆ ad vˆ c, updated values of ˆ ad vˆ c ca be calculated. 3.3 Computatio Procedure. The computatio procedure cosists of two parts. I the first part deoted by Algo p, the set of positios P=q i,r ci,,...,, is computed. The secod part of the algorithm, deoted by Algo v, computes the set of velocities V= i,v ci,,...,. Algo p solves sequetially the optimizatio problems defied i Eqs. 10 ad 11 by applyig the iteratio process defied i Eqs. 19 ad 25. Two ested iteratio processes are coducted i each part: i Self iteratios: iteratio of the estimated positio P or estimated velocity V for give values of weightig coefficiets W i i,,...,3, or V j j, j =1,...,5. ii Weightig iteratios: icreasig weightig coefficiets W i ad V j by chagig the correspodig parameter. For example, oe ca use W i = i i, i, i are give costats ad is a sequece of predetermied ad icreasig itegers. The steps of the computatio algorithm are as follows. Algo p : 1. Set weightig iteratio couter =1. 2. Set q i =q i, r ci =r ci,,...,, ad coefficiets W m by settig = m, m=1,...,3. 3. Solve the problem i Eq. 10 for q i,,...,, usig Eq Set q i =q i,,...,, ad go to step Repeat steps 3 ad 4 util covergece. 6. Solve problem i Eq. 11 for r ci,,...,, usig Eq Set r ci =r ci,,...,, ad go to step Repeat steps 6 ad 7 util covergece. 9. Set =+1 ad perform weightig iteratios, i.e., repeat steps 3 8 util covergece. Algo v : 1. Set weightig iteratio couter =1. 2. Set i = i, v ci =v ci,,...,, ad V m by settig = m, m=1,...,3. 3. Solve the problem i Eq. 12 for i,,...,, usig Eq Set i = i,,...,, ad go to step Repeat steps 3 ad 4 util covergece. 6. Solve the problem i Eq. 13 for v ci,,...,, usig Eq Set v ci =v ci,,...,, ad go to step Repeat steps 6 ad 7 util covergece. 9. Set =+1 ad perform weightig iteratios; i.e., repeat steps 3 8 util covergece. The followig observatios are i order. i The iitial li CM positios r ci,,...,, are obtaied from measuremet data. ii The iitial quaterios q i,,...,, are obtaied by solvig idepedet Wahba problems for each li solvig Eq. 10 with W 1 =0. iii The iitial velocities i ad v ci are computed by iterpolatio of the set P computed by Algo p. 4 Estimatio of Kiematics for a Simulated Three- Li Plaar Model I this example, we evaluate the performace of the proposed algorithm usig a simulated experimet of a three-li plaar / Vol. 6, JULY 2011 Trasactios of the ASME Dowloaded From: o 12/08/2014 Terms of Use:

5 Fig. 3 Cofiguratio of the three-li plaar model model show i Fig. 3. The assumed marer locatios o each segmet are show as solid blac dots i Fig. 3. The system motio was simulated for a free-flight tas. The time history of marer locatios durig the flight phase was used as experimetal measuremets iput to the proposed computatioal algorithm. The masses of the segmet are m i =10 g,,2,3, the torques at J 1 ad J 2 are T 1 t=t 2 t=5 N m for all t0, ad the simulatio period was t f =1 s. The iitial coditios are x0 =0 m, y0 =0 m, 1 0 = 45 deg 2 0 = 60 deg, 3 0 = 75 deg ẋ0 =2 m/s, ẏ0 =4 m/s, 10 = 20 deg/s 20 = 50 deg/s, 30 = 75 deg/s 4.1 Sesitivity ad Covergece Properties. Perturbatio i marer locatios itroduce traslatioal ad attitude estimatio errors i segmet iematics leadig to violatio of itercoectio costraits at joit locatios. We demostrate the sesitivity of the algorithm to differet types of errors ad demostrate its covergece properties by studyig the performace of the proposed algorithm i a sigle frame at t=0.2 s. I this frame, we itroduced two types of errors: traslatio error of the ceter of mass of each li i the horizotal directio x=0.1 m ad a rotatio of each li by =10 0. The umber of self-iteratio steps iteratio of the idepedet variable was j =15. Positio weightig sequeces were W i = 1 i, =0.01, =1,2,...,,,2,3, ad i is differet for differet weightig sequeces. The maximum weightig iteratio step idex is =10 for each of the weightig coefficiets. Similarly, velocity weightig sequeces are V j = 1 j+3, j=1,...,5. Figure 4 shows the system cofiguratios computed for differet weightig coefficiet sequeces ad compares them to the omial cofiguratio, i.e., cofiguratio with o measuremet errors. The otatio 5,2,2deotes the values of the parameter i i the sequeces W i,,2,3, respectively. The covergece of joit gaps r ij 0.2 show i Fig. 5 ad the Fig. 5 Three-li model: covergece of joit gaps Fig. 4 Three-li model: system cofiguratio ad joit locatios for differet values of weightig coefficiet at t=0.2 s Fig. 6 Three-li model: covergece of joit gap velocity ad system s agular mometum for t=0.2 s Joural of Computatioal ad Noliear Dyamics JULY 2011, Vol. 6 / Dowloaded From: o 12/08/2014 Terms of Use:

6 Fig. 9 Huma body motio: model cofiguratio ad marer locatios Fig. 7 Three-li model: joit gap positios ad velocities covergece of the velocity of the first joit v ad the system agular mometum H0.2 are show i Fig. 6. As ca be see, i the presece of measuremet errors, the algorithm coverges rapidly to a feasible solutio that is close to the omial cofiguratio. 4.2 Kiematic Estimatio. Kiematic aalysis was performed by itroducig marer locatio errors that obey a ormal distributio with a mea of 1 cm ad a stadard deviatio =1 cm. Weightig sequeces were as defied before with 1,..., 8 =5,2,2,2,2,5,5,5 ad =0.01. Figure 7 shows the time histories for the joit gap positios r ij t ad gap velocities v ij t before ad after correctios were made by the proposed algorithm. As ca be observed, the gaps for both positio ad velocity are drastically reduced. It was foud that after correctio, maxr ij t10 13 m ad maxv ij t0.02 m/s. Figure 8 shows the error i CM positio Pt, liear mometum Fig. 8 Three-li model: errors i system CM positio P t, liear mometum L t, ad agular mometum L t Fig. 10 Huma body motio: system cofiguratio at t =0.03 s / Vol. 6, JULY 2011 Trasactios of the ASME Dowloaded From: o 12/08/2014 Terms of Use:

7 Lt, ad agular mometum Ht as a fuctio of time. It was foud that Pt10 15 m, maxlt10 11 g m/s, ad maxht510 2 g m 2 /s. 5 Kiematic Estimatio A 3D Model of Huma Jumpig Motio A three-dimesioal experimet i which a subject jumped from a 0.63 m high platform ad laded o the groud was performed. The trajectories of 75 marers located o the body were recorded by seve cameras with a samplig rate of 100 Hz. A model cosistig of 14 rigid segmets ad 13 three-dimesioal rotatioal joits is show i Fig. 9. Hollow ad blac circles i the figure idicate frot ad bac marers, respectively. The physical characteristics of the segmets were computed usig the method itroduced by de Leva 16. I this approach, dimesios of the various segmets of the model, as well as their mass properties, are computed usig the height ad the weight of the idividual see also Refs. 13,14. The midpoit of two lateral marers was used for ale, ee, wrist, ad elbow joit estimatio. It was assumed that the shoulder ad hip are o the coectig lie betwee lateral marers placed o shoulders ad hips, respectively. I order to eforce liear ad agular mometum costraits, the parabolic trajectory of whole body CM ad the iitial agular mometum were computed, yieldig r c t = t t t t T H 0 = T Weightig sequeces of the form W i = i ad V i = i+3 used with the followig umerical values i this example, 1,..., 8 = 2,10,10,2,2,10,10,10, = 0.01 were = 0,1,...,10 Results for iematic estimatio for the time iterval 0t 0.1 are preseted. Figure 10 shows the computed cofiguratio of the body for t=0.03 s. Solid lies represet the computed body segmet cofiguratio ad dotted lies are the experimetal estimatio results computed from raw data estimatio for each rigid body. Figure 11 shows the joit positio ad velocity gaps for differet joits before ad after the proposed computatioal algorithm was applied. As ca be observed, the gaps for both positio ad velocity are sigificatly reduced. It was foud that after correctio, maxr ij t 10 4 m ad maxv ij t0.02 m/s. Figure 12 shows whole body errors i CM trajectory ad liear ad agular mometa before ad after correctios were computed. As ca be observed, the errors i CM positio ad liear mometum were essetially reduced to zero, while the agular mometum error is below 1 g m 2 /s. 6 Coclusios A ew method for the estimatio of the iematics of a system modeled as a chai of rigid bodies itercoected by three degrees of freedom rotatioal joits was developed. Li iematics are computed while preservig importat system properties that are crucial i the aalysis of multibody systems. The iematic esti- Fig. 11 Huma body motio: computatio of joit gap positios ad velocities Fig. 12 Huma body motio: errors i system CM positio P t, liear mometum L t, ad agular mometum H t Joural of Computatioal ad Noliear Dyamics JULY 2011, Vol. 6 / Dowloaded From: o 12/08/2014 Terms of Use:

8 matio problem was cast as a series of optimizatio problems by formulatig appropriate objective fuctios with system properties icluded via the pealty fuctio approach. The coditios for optimum were formulated, ad a sequetial computatio procedure was proposed to solve the optimizatio problem. Quaterio parametrizatio of attitude used i the formulatio led to a efficiet computatioal procedure that cosists of solvig a sequece of liear algebraic problems. Two examples of a simulated system ad a experimetal study demostrated fast covergece to a solutio that is close to the experimetally measured data. Appedix: Adjustmet of Kiematic Variables 1 Adjustmet of Agular Velocities Let J be the objective fuctio i Eq. 12. The, 1 J = 2 + V 1 J T H o ci m i ci + J i i + V 2 R o L,u T v cu v c R u o L u,u + R o L,u A1 is calculated either from marer velocities or from iterpolatio of the orietatio quaterio. Settig J / =0 ad collectig terms i Eq. A1 leads to + V 1 J T J i i + V 2 R o L,u T R u o L u,u + R o L,u = A2 = + V 1 J TH o v c The, we get ci m i ci V 2 R o L,u T v cu A3 1 ˆ = ˆ A4 ˆ = 1T T c T ad ˆ = T 1 T T. Let J i o ad J i i deote the momet of iertia of body i represeted i frames I ad B i, respectively. The, J i = J i o = R i o J i i R i o T A5 Defie J=J 1 J, H =H o ci m i ci, ad the = I 3 + V 1 J T J + V 2 C L ˆ = ˆ + V 1 H T J T + V 2 C R c The 33 matrix C L is related to joit velocity costraits ad serves as a mappig betwee bodies. The expressio C L i, j deotes the submatrix o the ith row bloc ad the jth colum bloc, C L i,i = R i o L iu T R i o L iu C L i, j = C L j,i = R i o L ij T R j o L ji if bodies i ad j are coected C L i, j = C L j,i = 0 33 if bodies i ad j are ot coected The 33 matrix C R is defied as follows: C R i,i = R i o L iut C R i, j = C R j,i = R i o L ijt if bodies i ad j are C R i, j = C R j,i = 0 33 ot coected if bodies i ad j are ot coected 2 Adjustmet of Velocities of the Ceter of Mass Let J CMv be the objective fuctio to be miimized i Eq. 13, we have 1 2 J CMv v c = v c v c V 3 m Mv c V 4 m ch o m i v ci + J i i + ci m i v ci v c + V 5 v c v cu + Ṙ o L,u Ṙ o u L u, v c is the iitial guess calculated either by employig the velocities of the marers or by iterpolatio of the estimated r ci ad subsequet differetiatio. Collectig terms, we have v c + V 3 m m i v ci + V 4 m c ci m i v ci + V 5 v c v cu = v A6 v = v c + V 3 m Mv c + V 4 m ch o T Let vˆ c=v c1 J i i + ci m i v c V 5 Ṙ o L,u Ṙ o u L u, A7 v T c T ad v= T v1 T v T vˆ c = v 1 ˆ v v = I 3 + V 3 M I T M I + V 4 M T M + V 5 C vl * v = vˆ c + V3 M T I Mv c + V 4 M T H v + V 5 C vr A8 M = m 1 c1 m c, H v = H o J i i + ci m i v c C vl isa33 matrix that depeds o the topology of the iematic chai 24, ad the 31 colum vector C vr i is give by C vr i= Ṙ i o L i,u +Ṙ u o L u,i. Refereces 1 Hatze, H., 1980, Neuromusculoseletal Cotrol Systems Modelig A Critical Survey of Recet Developmets, IEEE Tras. Automat. Cotrol, 25, pp Wertz, J., 1978, Spacecraft Attitude Determiatio ad Cotrol, Reidel, Bosto, MA. 3 Hor, B., 1986, Robot Visio, McGraw-Hill, New Yor. 4 Shuster, M., ad Oh, S., 1981, Three-Axis Attitude Determiatio From Vector Observatios, J. Guid. Cotrol, 41, pp Cohe, C., 1996, Global Positioig System: Theory ad Applicatios, America Istitute of Aeroautics ad Astroautics, Washigto, D.C. 6 Shuster, M. D., 2000, A Suboptimal Algorithm for Attitude Determiatio Form Multiple Star Cameras, Spaceflight Mechaics 2000: Proceedigs of the AAS/AIAA Space Flight Mechaics Meetig, pp / Vol. 6, JULY 2011 Trasactios of the ASME Dowloaded From: o 12/08/2014 Terms of Use:

9 7 Bar-Itzhac, I., ad Oshma, Y., 1985, Attitude Determiatio Form Vector Observatios: Quaterio Estimatio, IEEE Tras. Aerosp. Electro. Syst., AES-211, pp Wahba, G., 1965, A Least Squares Estimate of Satellite Attitude, SIAM Rev., 73, pp Keat, J., 1977, Aalysis of Least Squares Attitude Determiatio Routie DOAOP, Report No. CSC/TM-77/ Hor, B. K. P., 1987, Closed-Form Solutio of Absolute Orietatio Usig Uit Quaterios, J. Opt. Soc. Am. A, 4, pp Hor, B. K. P., Hilde, H. M., ad Negahdaripour, S., 1988, Closed-Form Solutio of Absolute Orietatio Usig Orthoormal Matrices, J. Opt. Soc. Am. A, 57, pp Xiao, J., Chai, J., ad Kaade, T., 2006, A Closed-Form Solutio to No- Rigid Shape ad Motio Recovery, It. J. Comput. Vis., 672, pp Zatsiorsy, V. M., ad Seluyaov, V. N., 1983, The Mass ad Iertia Characteristics of the Mai Segmets of the Huma Body, J. Biomech., 8B, pp Zatsiorsy, V., Seluyaov, V., ad Chuguova, L., 1990, Methods of Determiig Mass-Iertial Characteristics of Huma Body Segmets, Cotemporary Problems of Biomechaics, Mir, Moscow, pp Zatsiorsy, V., Seluyaov, V., ad Chuguova, L., 1990, I Vivo Body Segmet Iertial Parameters Determiatio Usig a Gamma-Scaer Method, Biomechaics of Huma Movemet: Applicatio i Rehabilitatio, Sports ad Ergoomics, Bertec Corp., Worthigto, OH, pp de Leva, P., 1996, Adjustmets to Zatsiorsy-Seluyaov s Segmet Iertia Parameters, J. Biomech., 299, pp Allard P., ad Stoes, I. A. F., 1995, Three-Dimesioal Aalysis of Huma Movemet, Huma Kietics, Champaig, IL. 18 Spoor, C. W., ad Veldpaus, F. E., 1980, Rigid Body Motio Calculated From Spatial Coordiates of Marers, J. Biomech., 134, pp Stoddart, A. J., Mraze, P., Ewis, D., ad Hyd, D., 1999, Marer Based Motio Capture i Biomedical Applicatios, IEE Colloquium o Motio Aalysis ad Tracig, Vol. 103, pp. 4/1 4/5. 20 Liu, H., Holt, C., ad Evas, S., 2007, Accuracy ad Repeatability of a Optical Motio Aalysis System for Measurig Small Deformatios of Biological Tissues, J. Biomech., 40, pp Reischmidt, V., de Bogert, A. J. V., Nigg, B., Ludberg, A., ad Murphy, N., 1997, Effect of Si Movemet o the Aalysis of Seletal Kee Joit Motio Durig Ruig, J. Biomech., 307, pp Requejo, P., McNitt-Gray, J. L., ad Flasher, H., 2004, Modificatio of Ladig Coditios at Cotact via Flight Phase Cotrol, Biol. Cyber., 90, pp Requejo, P., McNitt-Gray, J. L., ad Flasher, H., 2002, A Approach for Developig a Experimetally Based Model for Simulatig Flight Phase Dyamics, Biol. Cyber., 87, pp Lee, J. C., 2009, A Approach to Experimetally Based Modelig ad Simulatio of Huma Motio, Ph.D. thesis, Uiversity of Souther Califoria, Los Ageles, CA. 25 Kuipers, J. B., 2002, Quaterios ad Rotatio Sequeces: A Primer With Applicatios to Orbits, Aerospace ad Virtual Reality, Priceto Uiversity, Priceto, NJ. Joural of Computatioal ad Noliear Dyamics JULY 2011, Vol. 6 / Dowloaded From: o 12/08/2014 Terms of Use: