Analytical and Numerical Methods Used in Studying the Spatial kinematics of Multi-body Systems. Bernard Roth Stanford University

Size: px

Start display at page:

Download "Analytical and Numerical Methods Used in Studying the Spatial kinematics of Multi-body Systems. Bernard Roth Stanford University"

Gary Stephens
5 years ago
Views:

1 Analytical and Numerical Methods Used in Studying the Spatial kinematics of Multi-body Systems Bernard Roth Stanford University

3 Closed Loop Bound on number of solutions=2 6 =64

7 Open chain constraint

8 C-C dyad and equivalent screw

9 Overconstrained 6R linkage chains

10 Bennett four-bar

11 DLR Arm Puma Barrett WAM

12 θ7

16 z 1 (x lg2 -x 1G3 ) 2 + (y lg2 -y 1G3 ) 2 + (z 1G2 - z 1G3 ) 2 = (G 2 G 3 ) 2 x lg2 =a 1 cosφ; x 1G3 =-a 4 +a 3 cosφ; y lg2 =a 1 sinφ; y 1G3 = -s 4 sinα 4 +a 3 sinψ cosα 4 z 1G2 =s 1 ; z 1G3 =-s 4 cosα 4 a 3 sinψ sinα 4 ; (G 2 G 3 )=a 2 By application of trigonometric identities and ordering of terms this becomes A sinψ + B cosψ = C A = - cosα 4 sinφ + s 1 sinα 4 /a 1 B = -a 4 /a 1 cosφ C =-(a 12 -a 22 + a 32 + a 42 + s 12 + s s 1 s 4 cosα 4 )/2a 1 a 3 -a 4 cosφ/a 3 -s 4 sinα 4 sinφ/a 3

18 Manipulator Kinematics Link Description Denavit-Hartenberg Notation Forward Kinematics Inverse Kinematics

19 Link Description Axis i Axis (I-1) Link I-1 a i-1 == α i-1 ==

20 Kinematic Chain jointi 1 joint 1 Linki 2 Link i z i joint i Link 1 zi 1 Linki 1 R i y i z, z 0 1 Link 0 y 1 Ri 1 yi 1 xi 1 x i Link n R n z n joint n θ R0 1 y0 x R 0 1 x 1 Linkn 1 x n y n

21 Forward Kinematics Axis (i-1) Axis i Link i Link i-1 z i x i a i } d i z i-1 x i-1 a i-1 θ i α i-1 i - 1 T = 1 cθ i -sθ i 0 a i-1 sθ i cα i-1 cθ i cα i-1 -sα i-1 -sα i-1 d i sθ i sα i-1 cθ i sα i-1 cα i-1 cα i-1 d i

22 Forward Kinematics Axis (i-1) Axis i Link i Link i-1 z i x i a i } d i z i-1 x i-1 a i-1 θ i α i-1 Forward Kinematics: 0 N 0 T = T T... T N-1 N

23 Stanford Scheinman Arm

24 Example - RPRR L 5 Z 1 Z 3 O 3 O 4 L 2 O 0,O 1 O 2 Z 2 Z 4 L 4 O 5 z 5 x 5 i α i-1 a i-1 d i θ i θ d L 2 0 θ L θ L 4 0 0

25 T= 0 6 L NM XXcccs sss+ css cds sd XXsccs+ css+ ssc sds+ cd XX scs 245+ cc 5 2 dc O QP d 2 Z 0 Y 0 X 6 X 0 d 3 Z 6 Y 6 x x P r1 = r 2 = r3 CSd SSd C [ C ( C C C S S ) S S C ] S ( S C C + C S ) S [ C ( C C C S S ) S S C ] + C ( S C C + C S ) S ( C C C S S ) C S C C[ C( CCS + SC) + SSS] S( SCS + CC) S [ C ( C C S + S C ) + S S S ] + C ( SCS + CC) S ( C C S + S C ) + C S S C ( C C S + S C ) S S S S ( C C S + S C ) + C S S C d Sd Cd SCS + CC

26 Inverse Kinematics Finding joint positions given end-effector position and orientation Solvability, Existence, Multiplicity Closed Form/Numerical Solutions

27 Analytic, Synthetic and Numerical Geometry in Mechanism Science A Long family history Analytic geometry classic approach Synthetic geometry pre-classic approach Numerical geometry neo-classic approach

28 Methods for solving equations Elimination (resultants) Continuation (homotopy) Gröbner bases (standard) Symbolic Number of Solutions Bezout count M-homogenous count BKK bound Linear product decomposition

29 Methods to get polynomials Tangent half-angle substitution cosθ 1 x 1 x 2 = + 2 sin 2 2x θ θ = x = tan 1 + x 2 Additional equations cosα sinβ 3= 0 xy 3= cosβsinα 5= 0 xy 5= 0 x x y1 = y2 = 1

30 Advanced methods to avoid sines and cosines Isotropic coordinates in the plane Mappings Quaternions in 3-D Soma coordinates Rodriques parameters Euler parameters Matrix exponents (Lie groups)

31 Elimination Methods Methods require an eliminant (or a resultant), which, at best, is a polynomial in a single unknown. Advantages: Gives all solutions to a set of nonlinear equations. Gives analytical means to determine the influence of the system parameters on the number and character of the solutions.

32 f 1 : Example 2 equations, 2 unknowns; nonlinear f :3x x + 2x + 9= 0 f :6x x + x + 8= equations, 3 unknowns; linear if each power product is considered an unknown (3 x ) x + (2) x + (9)1 = (6x + 1) x + (8)1 = equations, 3 unknowns; linear and linearly independent equations (3 x ) x + (2) x + (9)1 = (6x + 1) x + (8)1 = (6x + 1) x + (8) x =

33 2 2 3x2 2 9 x x x = 0 6x Determinant is Sylvester type resultant det 516x 12x + 7 = 0 x = 0.129; x = x x 1 = 0 6x 1 x x = 35.1; x =

34 Objective: Find minimal number of new power products.

35 ELIMINATION PROCEDURE 1. Rewrite equations with one variable suppressed. 2. Define the remaining power products as new linear unknowns. 3. Use the original equations to manufacture new linearly independent equations so as to have as many linearly independent homogeneous equations as linear unknowns. 4. Set the determinant of the coefficient matrix to zero, and obtain a polynomial in the suppressed variable. (If interested in only numerical solutions, this step can be omitted if we calculate eigenvalues in Step 5.) 5. Determine the roots of the characteristic polynomial of the matrix, or the eigenvalues of a derived matrix. (This yields all possible values for the suppressed variable.) 6. Substitute (one of the roots or eigenvalues) for the suppressed variable and solve the linear system for the remaining unknowns. Repeat this for each value of the suppressed variable. Note: Step 3 is the big issue!

36 Minumum n Minimum n not easy to determine. When n is known, it is not easy to determine the equations. Size of nxn system is not directly related to the degree of suppressed variable ( ax + c) y + ( dx ) y + ( bx + ex + f ) = 0 Example: suppress x power products balance occurs at n Suppress y ( hx) y + ( kx) y + ( gx + ix + jx + lx + m) = 0 4 = 6 ( b) x + ( dy + e) x + ( ay ) x + ( cy + f ) = 0 gx+ ix+ jx+ hy+ ky + lx+ m= 5 power products balance occurs at n= ( ) ( ) ( ) ( ) 0

37 x cosθ1 sinθ1 0 0 cosθ2 sinθ2 0 a1 a2 y sinθ1 cosθ1 0 0 sinθ2cosα cosθ2cosα sinα sinαs 2 0 = z sinθ2sinα cosθ2sinα cosα cosαs x= a cosθ cosθ a cosαsinθ sinθ + s sinαsinθ + a cosθ y = a sinθ cosθ + a cosαcosθ sinθ s sinαcosθ + a sinθ z = a sinαsinθ + s cosα x = a cos θ cos θ + a cos αsin θ sin θ + s sin αsin θ + a cos θ 2a cosθ cosθ sinθ sinθ + 2a s sinαcosθ cosθ sinθ + 2aa cos θ cosθ as cosαsinαsin θ sinθ 2aa cosαcosθ sinθ sinθ + 2sa sinαsinθ cosθ x + y = a cos θ + a cos αsin θ + ( s sin α + a ) + 2aa cosθ 2a s cosαsinαsinθ x + y + z = ( a + a + s ) + 2 a ( a s )sinαcosαsinθ + 2aa cosθ

38 x= a cosθ cosθ a cosαsinθ sinθ + s sinαsinθ + a cosθ y = a sinθ cosθ + a cosαcosθ sinθ s sinαcosθ + a sinθ z = a sinαsinθ + s cosα ( a cos θ ) cosθ ( a cosαsin θ ) sinθ + ( s sinαsinθ + a cos θ x) 1 = ( a sin θ ) cosθ + ( a cosαcos θ ) sinθ ( s sinαcosθ + a sin θ y) 1 = ( a sin α) sinθ + ( s cos α z) 1 = ( a cos θ ) ( a cosαsin θ ) ( s sinαsinθ + a cos θ x) ( a sinθ ) ( a cosαcos θ ) ( s sinαcosθ + a sin θ y) ( a cosα cos θ ) ( s cosα z) = 0 xa sinα sinθ + ya sinαcos θ + a ( s zcos α) = Asinθ + Bcosθ + C = 0 1 1

39 General 6-degree-of-freedom 0 = hand hand T T T T T T T 2 TTT 3 4 = 1 T 1 0 T 1 0 T 5 T hand hand

41 Original 6 equations: L P Manufactured 8 new equations L P, L P, P P, ( P P) L 2( L P) P Total 14 scalar eqations with the same power products

42 Direct kinematics of platform manipulator

43 For In-Parallel Mechanisms The general 6/6 platform mechanism has, at most, 40 different poses associated with a set of leg lengths. For different special geometries derived from the 6/6 platform there may be, at most, 4, 8, 12, 16, 24 poses for a given set of leg lengths.

45 Planar and similar platforms 16 solutions but polynomial degree=2

46 Multi-step Elimination f ( x, y, z) = 0 h( z) = * g1( x, y) = 0 h1 ( y) = 0 * 2 2 * g2( x, y) = 0 h2( y) = 0 3 = 3 = f ( x, y, z) = 0 h ( z) = 0 g ( x) = 0 f ( x, y, z) 0 h ( z) 0 solve x suppress x eliminate y suppress x& y eliminate z

47 Multivariate Problems -resultants Dixon (1908)-resultant is a determinant, gotten by introducing dummy parameters Maccaulay (1916) resultant is ratio of two determinants Sturmfels and Zelevinski (1994) r groups of variables; multiply each equation by all variables of only one group; yields Sylvester type resultant. Emris (1993, 1994) Use of Newton polytopes of equations to construct sparse mixed resultant.

48 Multihomogeneous Sturmfels & Zelevinsky π( j) < π( k) mk = ( dk 1) lk + d k Multiply each equation by all terms of degree m k in the variable groups x k. Example: 3xxx xx x2+ 6= 0 6xx + 6x xx 5= xxx 8x + 2x 3= 0 l k j = l j d k = degree # of variables (3 x ) x x + (5 x ) xw + (2) wx + (6) ww = (6) xx + (6) xw + ( x) wx + ( 5) ww = (7 x ) x x + ( 8) xw + (2) wx + ( 3) ww =

49 x m m { } { } = x, w, x = x, w = (1 1)1+ 1(0) = 0 = (1 1)1 + 1(1) = x3 2 5x3 6 xx x wx x xxw = 3x3 2 5x wxw x xw x ww 0 1 2

50 Homotopy

51 Polynomial continuation To solve F 1 (X) =0 Solve instead: F m (X)=(1-t)F 0 (X)+tF 1 (X) =0 Start with F 0 (X)=0, with known solutions. t is varied in steps from 0 to 1. Use solution at each step as an initial guess for the equations of the next step Disadvantages: Tracks one solution at a time, and no advantage to having other solutions. Tracks unwanted solutions at infinity.

52 Polynomial Continuation Example f : 3x x + 2x + 9= 0 f : 6x x + x + 8= 0 g : x 1 = 0 x = 1, 1, i, i g : x 1= 0 x = 1, H ( x, t) = (1 t) e ( x 1) + t(3xx+ 2x+ 9) = 0 iθ H ( x, t) = (1 t) e ( x 1) + t(6xx+ x+ 8) = 0 iθ at t = 0, we know 8 solutions: (1,1),(1, 1),( 1,1), ( 1, 1), ( i,1), ( i, 1), ( i,1), ( i, 1)

54 Important results with continuation First proof that the 6R manipulators has 16 solutions for the inverse kinematics First proof that the generalized Stewart Platform has 40 solutions for the direct kinematics problem First synthesis for 9 points on coupler curve of planar 4-bar

55 Continuous functions In an n-dimensional space Algebraic set of dimension k and deg. d Intersects a linear subspace of dimension n-k In d points Examples: Curve (k=1) of deg. d, in the 3 space plane (n=2) (n=3) Intersects a line (n-k=1) in d points plane (n-k=2)

56 The procedure in genereral Take (n-k) to determine the dimension of the generic linear subset that does the cutting. (This is the co-dimension of the original space.) The generic (n-k)-dimensional linear subset is gotten by using k hyperplanes

57 Dummy variable added to n x n system f 1 (x 1, x 2, x 3,.., x n,x (n+1) )=0 f 2 (x 1, x 2, x 3,.., x n,x (n+1) )=0 f 3 (x 1, x 2, x 3,.., x n,x (n+1) )=0... f n (x 1, x 2, x 3,.., x n,x (n+1) )=0 a 1 x 1 +a 2 x 2 +a 3 x 3 + +a n x n +a (n+1) x (n+1) =0

58 Abreviated procedure For ordinary continuation, dimension=zero, we get isolated points. To find a curve, dimension=1, we use a hyperplane, and vary it. To find a surface, dimension=2, we need two hyperplanes (hyperline). To find a volume, dimension=3, we need three hyperplanes. Etc.

59 Witness points Find the d points of intersection using polynomial continuation. These d points are named witness points. Repeatedly vary the planes in n-space to get a large set of witness points. Fit these witness points to a polynomial of degree d ( or use a projection to a lower dimension, or use Monodromy algorithm.) (See Sommese, Vershelde, Wampler)

60 Current Situation Continuation methods for isolated solutions Highly developed in 1980 s, 1990 s Numerical algebraic geometry started 1995 Builds on the methods for isolated roots Treats positive-dimensional sets Witness sets are the key construct Bertini-beta is available Isolated solutions: multi-homogeneous homotopy Positive-dimensional: irreducible decomposition Multiple precision for high-degree systems

61 Gröbner Bases Uses the ideas similar to Gaussian elimination to produce a triangular set of equations. Ideally resulting in one equation is one unknown. Determines a simple set of equations with same ideal as original set.

62 Symbolic Mathematica Maple Etc Is there a general theory for symbolic solution generation? How to deal with trade-offs of degree and number? How to deal with getting simplifications?

63 Thank you!

ROBOTICS 01PEEQW. Basilio Bona DAUIN Politecnico di Torino

ROBOTICS 01PEEQW. Basilio Bona DAUIN Politecnico di Torino ROBOTICS 01PEEQW Basilio Bona DAUIN Politecnico di Torino Kinematic Functions Kinematic functions Kinematics deals with the study of four functions(called kinematic functions or KFs) that mathematically