Novint Falcon 1,, 3, Novint Falcon Novint Falcon Novint Falcon Novint Falcon Novint Falcons 13 Daniel J. Block et al., licensee Versita Sp. z o. o. This work is licensed under the Creative Commons Attribution-NonCommercial-NoDerivs license, which means that the text may be used for non-commercial purposes, provided credit is given to the author. Novint Falcon E-mail: d-block@illinois.edu E-mail: mark.michelotti@gmail.com E-mail: rsree@illinois.edu 18
i (x, y, z) O P p A i r E i c a, b, d e A i,b i C i Θ 1i, Θ i Θ 3i (u i,v i,w i ) A i P (u i,v i,w i ) cos φ i sin φ i p i = sin φ i cos φ i p + 1 r φ i i (x, y, z) p i =(p ui p vi p wi ) T p ui,p vi p wi p ui p vi p wi = a cos θ 1i c +(d + e + b sin θ 3i ) cos θ i = b cos θ 3i = a sin θ 1i +(d + e + b sin θ 3i ) sin θ i θ 3i (x, y, z) i i i θ 3i = ± cos 1 p vi b. θ 3i θ i θ i p ui p wi θ i (p ui + c) + p wi + a a(p ui + c) cos θ 1i αp wi sin θ 1i = (d + e) + (d + e)b sin θ 3i + b sin θ 3i. (1) 183
t 1i = tan θ 1i sin θ 1i = t 1i 1+t 1i cos θ 1i = 1 t 1i, 1+t1i l i l 1i l i l i t 1i + l 1i t 1i + l i =, = p wi + p ui +cp ui ap ui b sin θ 3i be sin θ 3i bdθ 3i de ac + a + c d e = 4αp wi = p wi + p ui +cp ui ap ui b sin θ 3i be sin θ 3i bdθ 3i de ac + a + c d e t 1i θ 1i θ 3i θ i (x, y, z) C θ 3i θ 1i (θ 11,θ 1,θ 13 ) (x, y, z) Dimension a b c d e r Value 6 mm 13 mm 16.3 mm 1 mm 1 mm 37 mm ϕ 1 14.44 ϕ 15.56 ϕ 3 5.56 function [theta1] = inverse_kinematics(x, y, z) r = 37; a = 6; b = 13; c = 16.3; d = 1; e = 1; phi =.5 %radians pu = x*cos(phi) + y*sin(phi) r; pv = x*sin(phi) + y*cos(phi); pw = z; theta3 = acos(pv/b); l = pw^+pu^+*c*pu *a*pu+a^+c^ d^ e^ b^*sin(theta3)^ *b*e*sin(theta3) *b*d*sin(theta3) *d*e *a*c; l1 = 4*a*pw; l = pw^+pu^+*c*pu+*a*pu+a^+c^ d^ e^ b^*sin(theta3)^ *b*e*sin(theta3) *b*d*sin(theta3) *d*e+*a*c; t1 = ( l1 sqrt(l1^ 4*l*l))/(*l); theta1 = atan(t1)*; x k =(x k y k z k ) T k f (x, y, z) (θ 11 θ 1 θ 13 ) T B k k f f 184
x = initial guess B = initial Jacobian approximation for k =, 1,,... end Solve Bksk = f(xk) for sk xk+1 = xk + sk yk = f(xk+1) f(xk) Bk+1 = Bk + ((yk Bksk)sk T )/(sk T sk) (x, y, z) x, y, z k p k i k d k p k d k i (x, y, z) k p =,k d =. k i = 1 (x, y, z) =(, 14, 135) (x, y, z) =( 4, 3, 11) D(z) = z 1 T sz D(z) = Ts z 1 T s = 1 3 185
Component Description 1 TMS3 Digital Controller Encoder LED emitters and photosensors 3 Supplementary Sensors 4 Motor Leads 5 Controller Buttons 186
A B A B SS SCK MISO MOSI ± OUT A OUT B LOAD SUPPLY BRAKE 187
Actual Distance (mm) 3 5 15 1 5 y =.8886531x +.6539367x R² =.9531446853 1 3 4 5 6 7 8 9 Pixel Distance τ = Iα d x = rα dt I = mr 3 d x = 3g sin dt θ θ d x = 3g dt θ x = 188
Θ x Θ y x y ( π ) p ix = p ix d i (1 cos γ) cos + tan 1 m ( π ) p iy = p iy d i (1 cos γ) sin + tan 1 m z p 1,p p 3 x y b x b y ( p iz = d i mb ) x + b y sin γ m +1 d dt x 1 = x d dt x = 3g θ. (x, y, z) OAC ODE m y = mx γ ABC DEF AB BC = DE EF sin θ x sin θ y = sin α cos θ x sin π/ α cos θ y tan θ x sin α = tan θ y cos α m = tan α = tan θ x. tan θ y γ ABC tan γ = AB BC = sin θ x sin α cos θ x = tan θ x sin α θ x = θ y = p 1,p p 3 y = mx 3g/ λ 1, = J ± 4J 1 +J J J = ( 3g 1 θ x 1 3g θ x ) = ( ) 1. J 1 J θ < θ < x 1 x θ = k p x 1 k d x R θ = k p (R x 1 ) k d ( d dt R x ) H(s) = (3g/)k ds + (3g/)k p s + (3g/)k d s + (3g/)k p. d l = mp ix + p iy m +1. k p k d k p > k d > 189
m/s m/s k p k d ζ =.6 g =9.81m/s t r ω n = ( ( )) 1 1 ζ π tan 1 =.77 1 ζ ζ ω n =.77rad/s ωn = 14.751k p k p =.5 () ζω n = 14.715k d k d =.3 (3) d x = 3g sin dt θ θ = sin 1 (u/3g) d x = dt u u θ ωn = k p =7.67 ζω n = k d =3.3 attenuating differentiator G(s) = ωs s+ω ω =5rad/s G(s) G(z) = 5z 5 z.8187 k p =.5 k d =.3 19
.1 Position (m).5 4 6 8 1 1.5 Time (sec) k p =.5 k d =.3 k p =1 k d =1 k p = 7.67 k d = 3.3 k p = 1 k d = 1 Position (m).15.5 4 6 8 1 1.5.15 Time (sec) k p = 3 k d = 1 Position (m).1.5 5 1 15.5 Time (s) 191
Position (m).15.1.5.5 5 1 15 Time (sec).6 http://dl.dropboxusercontent.com/u/986481/iweb/site / Control Education & Applications files/thesis.m4v y position (m).4...4.6.6.1.4 x position (m) References http://www.novint.com/index.php/products/novintfalcon Proc. Australasian Conference on Robotics and Automation 19
193