Notes_5_3 o 8 Three-Dmesoal Epermetal Kematcs Dgte locatos o ladmarks { r } o bod or pots to at gve tme t All pots must be o same bod Use ladmark weghtg actor = pot k s avalable at tme t. Use = pot k ot avalable at gve tme t. r r r at gve tme t. Determe { } { } { } Mea values mea { r } = { r } / = = = = = / / / / k mea { r } { r } mea { r } { r } mea { r } { r } mea { r } { r } = mea [ X ] ({ r } { r } ){ r } { r } [ M] = ([ I3 ] trace( [ X] )) [ X] mea T ( ) / Veloct [ V] = mea T ( ) / { r } { r } { r } 32 { ω } = [ M] V V = ω ω = orm{ ω } V V 3 2 V V 23 3 2 ω ω
ω [ ω ] = ω ω { û} = { ω} ω / ω ω ω Notes_5_3 2 o 8 ot ISA s o the stataeous screw as or bod at the root o the perpedcular rom the cetrod o the ladmarks. Note that the ISA s ot attached to the bod. A pot o the bod cocdet wth the ISA has traslatoal veloct s alog the ISA. ISA ( ) or a pot attached to bod { r } = s { û} + [ ω ]{ r } { r} ISA mea mea 2 { r} = { r } + [ ω]{ r } / ω s = { û} T { r } mea Accelerato [ A] = mea T ( ) / { r } { r } { r } [ B] = [ A] [ ω ][ ω ][ X] 32 { ω } = [ M] B B = ω ω = orm{ ω } B B 3 2 B B 23 3 2 ω ω [ ω ] = ω ω [ β ] = [ ω ] ω ω ω ω ot IA s the stataeous accelerato pole or bod. Note that the IA s ot attached to the bod. ot o the bod cocdet wth IA has ero accelerato. IA ( ) { r } = [ β ]{ r } { r} or a pot attached to bod IA mea mea _ IA { r} = { r } [ β ] { r } or { r } _ at =
Notes_5_3 3 o 8 Jerk [ J] = mea T ( ) / { r } { r } { r } ( )[ X] [ H] = [ J] 2[ ω ] 32 { ω } = [ M] H H = ω ω = orm{ ω } H H 3 2 H H 23 3 2 ω ω [ ω ] = ω ω [ η ] = [ ω ] + 2[ ω ] ω ω ω ω ot IJ s the stataeous jerk pole or bod. Note that the IJ s ot attached to the bod. ot o the bod cocdet wth IJ has ero jerk. IJ ( ) { r } = [ η ]{ r } { r} or a pot attached to bod IJ mea mea _ IJ { r} = { r } [ η ] { r } or { r} _ at = Secod Order Screw As Ω Ω 2 { Ω } = Ω = [ ω]{ ω }/ ω Ω = orm{ Ω} { tˆ } = { Ω} / Ω d = T IA IHA T ({ tˆ } [ β]{ ( r} { r} )/ { tˆ } [ β]{ û} { c} = { r} IHA + d{ û} ( ) T ({ } [ ω IA 2 tˆ ][ β]{ ( c} { r} )/ ω ) s Ω ω S = /
Notes_5_3 4 o 8 t_lm2k3d.m - test 3D kematcs rom ladmark moto HJSIII, 4..4 clear eample data - CRS = [ ]; r = [. 9. 9... 9. 9.. ;.. -. -... -. -. ;.... 8. 8. 8. 8. ]; rd = [ -35.75-35.75-3.75-3.75-28.75-28.75-23.75-23.75 ; 27.5 22.5 22.5 27.5 27.5 22.5 22.5 27.5 ; 8... 8. 8... 8. ]; rdd = [ -45.25-8.25-9.25-56.25-58.25-2.25-32.25-69.25 ; -233. -222. -29.5-22.5-98. -87. -74.5-85.5 ; -3.375-7.375-7.375-3.375-5.875-92.875-92.875-5.875 ]; rddd = [ ; ; ]; vel_test = [ 5.773 ; -3.5-4.23 4.4257 ; 2.5 3.376 -.34 ]; accel_test = [ 8.75 2.644 ; 6.5 -.544 ; -5.5 3.799 ]; jerk_test = [.5. ; -53.825 ; 6.5625 9. ]; aode_test = [.622 5.773.385 ;.824.372 2.789 ;.6554.2734 3.8947 ]; eample data - RSUR = [ ]; r = [ -9.89-6.793-4.289-4.7924-7.83 -.396 ; -5.476-8.967-3.7733-6.829-4.284-8.539 ; -.9764 -.7795-7.258-4.469-5.6659-9.496 ]; rd = [.824 -.4639 7.5799 7.9 2.763 2.325 ;.79.3955.859.982.8737.4 ; -4.582-9.7-6.5-6.9559 -.33-4.864 ]; rdd = [ -4.638-5.2233-5.869-2.954 52.6698 43.384 ; 5.396 35.2375 76.664 98.628 78.2349 36.883 ; -48.5442-3.593-58.6743-58.854-3.879-48.724 ]; rddd = [ 596.567 523.6492 4.6583-44.9252-369.577 49.4732 ; 3.57-4.6333-59.9366-33.5496.48 29.444 ; 435.585-35.2544-34.466 236.644 76.957 86.642 ]; vel_test = [.383.423 -.297 ; -.3498 -.45.468 ;.895 -.945 -.694 ]; accel_test = [ 3.33-52.7472 ; -7.8355 8.887 ; 8.368-97.6836 ]; jerk_test = [ -2.364-22.3536 ; 6.642 32.7874 ;
Notes_5_3 5 o 8.585-2.8695 ]; aode_test = [ -5.543.6887 53.334 ; -.6-2.2.579 ; 2.2768 -.2983-2.964 ]; test ucto [ vel, accel, jerk, aode ] = lm2k3d(, r, rd, rdd, rddd ) bottom o t_lm2k3d
Notes_5_3 6 o 8 ucto [ vel, accel, jerk, aode ] = lm2k3d(, r, rd, rdd, rddd ) 3D kematcs o a rgd bod rom ladmark moto HJSIII, 4..4 USAGE [ vel, accel, jerk, aode ] = lm2k3d(, r, rd, rdd, rddd ) INUTS - vector o weghts - (j)= meas data vald, (j)= meas data ot avalable r - 3 matr o,, ladmark locato rd - 3 matr o,, ladmark veloct rdd - 3 matr o,, ladmark accelerato rddd - 3 matr o,, ladmark jerk OUTUTS vel = [ w_vec risa sisa ] w_vec = 3 agular veloct vector risa = 3 locato o ISA at root o perpedcular rom cetrod o ladmarks sisa = sldg veloct vector alog ISA accel = [ wd_vec ria rdd_at_ia ] wd_vec = 3 agular accelerato vector ria = 3 locato o accelerato pole rdd_at_ia = 3 accelerato o pot o bod at IA jerk = [ wdd_vec rij rddd_at_ij ] wdd_vec = 3 agular jerk vector rij = 3 locato o jerk pole rddd_at_ij = 3 jerk o pot o bod at IJ aode = [ OMEGA_vec c Sd ] OMEGA_vec = 3 rotato o secod order screw c = 3 cetral pot o geerator Sd = 3 sldg veloct vector alog secod order screw costats eps = e-4; umber o coordates ad ladmarks [ coord, ] = se( r ); mea values mat = dag(); s = trace( mat ); rm = sum( mat*r' )' /s; rdm = sum( mat*rd' )' /s; rddm = sum( mat*rdd' )' /s; rdddm = sum( mat*rddd' )' /s; cetered locato rc = r - rm*oes(,); X = rc * mat * rc' /s; M = trace(x) * ee(coord) - X; Mv = v( M ); veloct V = rd * mat * rc' /s; w_vec = Mv * [ V(3,2)-V(2,3) ; V(,3)-V(3,) ; V(2,)-V(,2) ]; w = orm( w_vec ); w_mat = skew_sm( w_vec ); geeral veloct soluto w > eps, u = w_vec / w; sd = u' * rdm; risa = rm + w_mat * rdm / w^2; sisa = sd * u; specal case - w=, pure traslato risa s at cetrod o ladmarks, sisa s traslato veloct sd = orm( rdm ); u = rdm / sd;
Notes_5_3 7 o 8 risa = rm; sisa = rdm; ed accelerato A = rdd * mat * rc' / s; B = A - w_mat*w_mat * X; wd_vec = Mv * [ B(3,2)-B(2,3) ; B(,3)-B(3,) ; B(2,)-B(,2) ]; wd = orm( wd_vec ); wd_mat = skew_sm( wd_vec ); beta_mat = wd_mat + w_mat*w_mat; geeral accelerato soluto abs(det(beta_mat)) > eps; ria = rm - v(beta_mat) * rddm; rdd_at_ia = eros(coord,); specal case - w=, wd=, pure traslato ria s at cetrod o ladmarks, rdd_at_ia s traslato accelerato w < eps, wd < eps, sdd = orm( rddm ); e = rddm / sdd; ria = rm; rdd_at_ia = rddm; specal case 2 - w=, wd>, pure agular accelerato smlar to geeral agular veloct soluto ria s at root o perpedcular to agular accelerato vector rom cetrod o ladmarks rdd_at_ia s traslato accelerato e = wd_vec / wd; sdd = e' * rddm; ria = rm + wd_mat * rddm / wd^2; rdd_at_ia = sdd * e; ed specal case 3 - w>, wd=, pure agular veloct smlar to ero agular veloct soluto wd < eps, e = u; sdd = e' * rddm; rdd_at_ia = sdd * e; ria = rm + (rddm-rdd_at_ia) / w*w; specal case 4 - w>, wd>, w_vec parallel to wd_vec e = wd_vec / wd; sdd = e' * rddm; rdd_at_ia = sd * e; ed ed ed w2a = w*w*ee(3) - wd_mat; ria = rm + v(w2a) * (rddm-rdd_at_ia); jerk J = rddd * mat * rc' / s; eta_mat_mwdd = 2*wd_mat*w_mat + w_mat*wd_mat + w_mat*w_mat*w_mat; H = J - eta_mat_mwdd * X; wdd_vec = Mv * [ H(3,2)-H(2,3) ; H(,3)-H(3,) ; H(2,)-H(,2) ]; wdd = orm( wdd_vec ); h = wdd_vec / wdd; wdd_mat = skew_sm( wdd_vec ); eta_mat = eta_mat_mwdd + wdd_mat; rij = rm - v(eta_mat) * rdddm; rddd_at_ij = eros(coord,); secod order screw
Notes_5_3 8 o 8 OMEGA_vec = w_mat * wd_vec /w/w; OMEGA = orm( OMEGA_vec ); t = OMEGA_vec / OMEGA; d = t' * beta_mat * (ria-risa) / (t' * beta_mat * u ); c = risa + d * u; Sd = ( t' * w_mat * beta_mat * (c-ria) /w/w ) - sd * OMEGA /w; Sd_vec = Sd * t; retur argumets vel = [ w_vec risa sisa ]; accel = [ wd_vec ria rdd_at_ia ]; jerk = [ wdd_vec rij rddd_at_ij ]; aode = [ OMEGA_vec c Sd_vec ]; bottom o lm2k3d