- 113 Views
- Uploaded on
- Presentation posted in: General

Robotic Kinematics – the Inverse Kinematic Solution

Download Policy: Content on the Website is provided to you AS IS for your information and personal use and may not be sold / licensed / shared on other websites without getting consent from its author.While downloading, if for some reason you are not able to download a presentation, the publisher may have deleted the file from their server.

- - - - - - - - - - - - - - - - - - - - - - - - - - E N D - - - - - - - - - - - - - - - - - - - - - - - - - -

Robotic Kinematics – the Inverse Kinematic Solution

ME 3230

Kinematics and Mechatronics

Dr. R. Lindeke

ME 3230

Cartesian Space

Joint Space

Joint Space

- In FKS we built a tool for finding end frame geometry from Given Joint data:
- In IKS we need Joint models from given End Point Geometry:

Cartesian Space

ME 3230

- It a more difficult problem because:
- The Equation set is “Over-Specified”:
- 12 equations in 6 unknowns

- Space can be “Under-Specified”:
- Planer devices with more joints than 2

- The Solution set can contain Redundancies:
- Multiple solutions

- The Solution Sets may be un-defined:
- Unreachable in 1 or many joints

- The Equation set is “Over-Specified”:

ME 3230

- Building Workspace Maps
- Allow “Off-Line Programming” solutions
- The IKS allows the engineer to equate Workspace capabilities with Programming realities to assure that execution is feasible(as done in ROBCAD & IGRIP)
- The IKS Aids in Workplace Design and Operational Simulations

ME 3230

R Frame Skeleton (as DH Suggest!)

Same Origin Point!

ME 3230

ME 3230

ME 3230

In the Inverse Problem, The RHS MATRIX is completely known (perhaps from a robot mapped solution)! And we use these values to find a solution to the joint equations that populate the LHS MATRIX

ME 3230

If 2 Matrices are Equal then EACH and EVERY term is also Uniquely equal as well!

- Examining these two matrices
- n, o, a and d are “givens” in the inverse sense!!!
- (But typically we want to build general models leaving these terms unspecified)

- Term (1, 4) & (2,4) on both sides allow us to find an equation for :
- (1,4): C1*(d2+cl2) = dx
- (2,4): S1*(d2+cl2) = dy

- S1/C1 = dy/ dx
- Tan = dy/dx
- = Atan2(dx, dy)

ME 3230

- After is found, back substitute and solve for d2:
- (1,4): C1*(d2+cl2) = dx
- Isolating d2: d2 = [dx/Cos1] - cl2

ME 3230

- Form A1-1 then pre-multiply both side by this ‘inverse’
- Leads to: A2 = A1-1*T0ngiven

ME 3230

- Selecting and Equating (1,4)
- 0 = -S1*dx + C1*dy
- Solving: S1*dx = C1*dy
- Tan() = (S1/C1) = (dy/dx)
- = Atan2(dx, dy) – the same as before

- d2 + cl2 = C1*dx + S1*dy
- d2 = C1*dx + S1*dy - cl2

ME 3230

- First let’s consider the concept of the Spherical Wrist Simplification:
- All Wrist joint Z’s intersect at a point
- The ‘n Frame’ is offset from this Z’s intersection point at a distance dn (the hand span) along the a vector of the desired solution (3rd column of desired orientation sub-matrix!) which is the z direction of the 3rd wrist joint
- This follows the DH Algorithm design tools as we have learned them!

ME 3230

- We can now separate the POSE effects:
- ARM joints
- Joints 1 to 3 in a full function manipulator (without redundant joints)
- They function to maneuver the spherical wrist to a target POSITIONrelated to the desired target POSE

- WRIST Joints
- Joints 4 to 6 in a full functioning spherical wrist
- Wrist Joints function as a primary tool to ORIENT the end frame as required by the desired target POSE

- ARM joints

ME 3230

- We will define a point (called the WRIST CENTER) where all 3 z’s intersect as:
- Pc = [Px, Py, Pz]
- We find that this position is exactly: Pc = dtarget - dn*a
- Px = dtarget,x - dn*ax
- Py = dtarget,y - dn*ay
- Pz = dtarget,z - dn*az

Note: dn is the so called ‘Handspan’ (a CONSTANT)

ME 3230

- Cartesian (2 types)
- Cantilevered
- Gantry

ME 3230

J2

J3

X0

P-P-P Configuration

Y0

Z0

J1

ME 3230

P-P-P Configuration

X0

Z0

Y0

J3

J2

J1

ME 3230

- Doing IKS – given a value for:
- X, Y and Z of End
- Compute , Z and R

Z0

R

Y0

X0

ME 3230

- Developing a IKS (model):
- Given Xe, Ye, & Ze
- Compute , & R

R

Z0

Y0

X0

ME 3230

Z0

3

L2

L1

X0

Y0

2

1

ME 3230

- Prismatic:
- q1 = d1= Pz (its along Z0!) – cl1
- q2 = d2 = Px or Py - cl2
- q3 = d3= Py or Px - cl3

- 1 = Atan2(Px, Py)
- d2 = Pz – cl2
- d3 = Px/C1 – cl3 {or +(Px2 + Py2).5 – cl3}

ME 3230

- Spherical:
- 1 = Atan2(Px, Py)
- 2 = Atan2( (Px2 + Py2).5 , Pz)
- D3 = (Px2 + Py2 + Pz2).5 – cl3

ME 3230

- Articulating:
- 1 = Atan2(Px, Py)
- 3 = Atan2(D, (1 – D2).5)
- Where D =

- 2 = -
- is: Atan2((Px2 + Py2).5, Pz)
- is:

ME 3230

- 2 = Where D =

Atan2((Px2 + Py2).5, Pz) -

ME 3230

- This is called the d2 offset problem
- A d2 offset is a problem that states that the nth frame has a non-zero offset along the Y0 axis as observed in the solution of the T0n with all joints at home
- (like the 5 dof articulating robots in our lab!)

ME 3230

Here: ‘The ARM’ might contain a prismatic joint (as in the Stanford Arm – discussed in text) or it might be the a2 & a3 links in an Articulating Arm as it rotates out of plane

A d2 offset means that there are two places where 1 can be placed to touch a given point (and note, when 1is at Home, the wrist center is not on the X0 axis!)

ME 3230

F1

11

ME 3230

- We will have a Choice of two poses for 1:

ME 3230

- We have two 1’s
- These lead to two 2’s (Spherical)
- One for Shoulder Right & one for Shoulder Left

- Shoulder Right Elbow Up & Down
- Shoulder Left Elbow Up & Down

ME 3230

- Orientation IKS again relies on separation of joint effects
- We (now) know the first 3 joints control positions
- they would have been solved by using the appropriate set of equations developed above

ME 3230

- This ‘model’ separates Arm Joint and Wrist Joint Contribution to the desired Target Orientation
- Note: target orientation is a ‘given’ for the IKS model!

ME 3230

- Lets begin by considering ‘Euler Angles’ (they are a model that is almost identical to a full functioning Spherical Wrist as defined using the D-H algorithm!):
- Step 1, Form a Product:
- Rz1*Ry2*Rz3
- This product becomesR36 in the model on the previous slide

ME 3230

this matrix, which contains the joint control angles, is then set equal to a ‘U matrix’ prepared by multiplying the inverse of the ARM joint orientation sub-matrices and the Desired (given) target orientation sub-matrix:

NOTE: R03 is Manipulator dependent! (Inverse required a Transpose)

ME 3230

Rij are terms from the product of the first 3 Ai’s (rotational sub-matrix)

ni, oi & ai are from given target orientation

– we develop our models in a general way to allow computation of specific angles for specific cases

ME 3230

U as defined on the previous slide! (a function of arm-joint POSE and Desired End-Orientation)

ME 3230

- Selecting (3,3) C= U33
- With C we “know” S = (1 - C2).5
- Hence: = Atan2(U33, (1-U332).5)
- NOTE: leads to 2 solutions for !

ME 3230

- To solve for : Select terms: (1,3) & (2,3)
- CS = U13
- SS = U23
- Dividing the 2nd by the 1st: S /C = U23/U13
- Tan() = U23/U13
- = Atan2(U13, U23)

ME 3230

- To solve for : Select terms: (3,1) & (3,2)
- -SC = U31
- SS = U32 (and dividing this by previous)
- Tan() = U32/-U31 (note sign migrates with term!)
- = Atan2(-U31, U32)

ME 3230

- = Atan2(U33, (1-U332).5)
- = Atan2(U13, U23)
- = Atan2(-U31, U32)

Uij’s as defined on the earlier slide!

– and –

U is manipulator and desired orientation dependent

ME 3230

Same Origin Point

Here drawn in ‘Good Kinematic Home’ – for attachment to an Articulating Arm

ME 3230

ME 3230

Uij’s as defined on the earlier slide!

– and –

U is manipulator and desired orientation dependent

ME 3230

Note: R4’s Inverse

ME 3230

ME 3230

- Let’s select Term(3,3) on both sides:
- 0 = C4U23 – S4U13
- S4U13 = C4U23
- Tan(4) = S4/C4 = U23/U13
- 4 = Atan2(U13, U23)

ME 3230

- For 5: Select (1,3) & (2,3) terms
- S5 = C4U13 + S4U23
- C5 = U33
- Tan(5) = S5/C5 = (C4U13 + S4U23)/U33
- 5 = Atan2(U33, C4U13 + S4U23)

- S6 = C4U21 – S4U11
- C6 = C4U22 – S4U12
- Tan(6) = S6/C6 = ([C4U21 – S4U11]/[C4U22 – S4U12])
- 6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])

ME 3230

- We removed ambiguity from the solution
- We were able to solve for joints “In Order”
- Without noting – we see the obvious relationship between Spherical wrist and Euler Orientation!

ME 3230

- 4 = Atan2(U13, U23)
- 5 = Atan2(U33, C4U13 + S4U23)
- 6 = Atan2 ([C4U22 – S4U12], [C4U21 – S4U11])

ME 3230

- Cylindrical Robot w/ Spherical Wrist
- Given would be a Target matrix from Robot Mapping! (it’s an IKS after all!)
- The d3 “constant” is 400mm; the d6 offset (the ‘Hand Span’) is 150 mm.
- 1 = Atan2((dx – ax*150),(dy-ay*150))
- d2 = (dz – az*150)
- d3 = ((dx – ax*150)2+p(dy-ay*150)2).5 - 400

ME 3230

Note “Dummy” Frame to account for Orientation problem with Spherical Wrist – might not be needed if we had set wrist kinematic home for Cylindrical Machine!

ME 3230

NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm (as drawn) – this becomes a “part” of the arm space – not the wrist space!

ME 3230

ME 3230

- 4 = Atan2(U13, U23)= Atan2((C1ax + S1az), ay)
- 5 = Atan2(U33, C4U13 + S4U23)= Atan2{ (S1ax-C1az) , [C4(C1ax+S1az) + S4*ay]}
- 6 = Atan2 ([C4U21 - S4U11], [C4U22 - S4U12]) = Atan2{[C4*ny - S4(C1nx+S1nz)], [C4*oy - S4(C1ox+S1oz)]}

ME 3230