Download presentation
Presentation is loading. Please wait.
1
Inverse Kinematics –IKS Solutions ME 4135 – Robotics and Controls R.R. Lindeke, Ph.D., Fall 2011
2
FKS vs. IKS 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 POSE Geometry: Joint Space Cartesian Space Joint Space Cartesian Space
3
So this IKS is ‘Nasty’ – it is a so-call Hard Mathematical Problem It a more difficult problem because: – The Equation set is “Over-Specified” 12 equations in 6 unknowns – Space may be “Under-Specified” for example ‘Planer devices’ with more joints than 2 – The Solution set can contain Redundancies Thus they exhibit Multiple solutions – The Solution Sets may be un-defined Unreachable in 1 or many joints
4
But the IKS in Critical in Robotics Builds Workspace limits and controller maps Allows “Off-Line Programming” solutions Thus, compares Workspace capabilities with Programming desirability to assure that execution is feasible Aids in Workplace design and operational simulations
5
Doing a Pure IKS solution: the R Manipulator R Frame Skeleton
6
LP Table and A i ’s FramesLinkVar dl SS CC S C 0 → 11R + 90009010C1-S1 1 → 22P0d2 + cl2001010
7
FKS is A1*A2:
8
Forming The IKS:
9
Examining the two sides of this FKS equation – n, s, a and d are given in an inverse sense – since we desire to put the manipulator on a particular target – But we will build the general solution only Term (1, 4) & (2,4) both side allow us to find an equation for : Select (1,4): C1*(d 2 +cl 2 ) = d x Select (2,4): S1*(d 2 +cl 2 ) = d y Form a ratio of Term 2,4 by Term 1,4 to build an equation for Tan( ) and then : S1/C1 = d y / d x Tan = d y /d x = Atan2(d x, d y )
10
Forming The IKS: After is found, back substitute and solve for d 2 : Choose term(1,4): C1*(d 2 +cl 2 ) = d x Isolating d 2 : d 2 = [d x /C1] - cl 2
11
Alternative Method – doing a pure inverse approach Form [A 1 ] -1 then pre-multiply both side by this ‘inverse’ Leads to: A2 = A 1 -1 *T 0 n given
12
After Simplifying the RHS:
13
Solving: Selecting and Equating terms (1,4) 0 = -S1*d x + C1*d y Solving: S1*d x = C1*d y Tan( ) = (S1/C1) = (d y /d x ) = Atan2(d x, d y ) Selecting and Equating terms (3,4) -- after back substituting solution – and note, after the above step, is known as an angle d 2 + cl 2 = C1*d x + S1*d y d 2 = C1*d x + S1*d y - cl 2 d 2 = Cos[Atan2(d x, d y )]*d x + Sin[Atan2(d x, d y )]*d y –cl 2
14
Performing IKS For Industrial Robots: First lets consider the previously defined Spherical Wrist simplification – All Wrist joint Z’s intersect at a point – The n Frame is offset from this intersection as a distance d n along the a vector of the desired solution (3 rd column of desired orientation sub- matrix) – This is as expected by the DH Algorithm
15
Performing IKS We can now separate the effects of the ARM joints – Joints 1 to 3 in a full function manipulator (without redundant joints) – They function to position the spherical wrist at a target POSITION related to the desired target POSE Arm Joints are separated from the 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
16
Performing IKS: Focus on Positioning We will define a point (the WRIST CENTER) as: – P c = [P x, P y, P z ] – Here we define P c = d target - d n *a P x = d target,x - d n *a x P y = d target,y - d n *a y P z = d target,z - d n *a z
17
Focusing on the ARM Manipulators in terms of P c : Prismatic: q 1 = P z (its along Z 0 !) – cl1 q 2 = P x or P y - cl2 q 3 = P y or P x - cl3 Cylindrical: 1 = Atan2(P x, P y ) d 2 = P z – cl2 d 3 = P x *C1 – cl3 or +(P x 2 + P y 2 ).5 – cl3
18
Focusing on the ARM Manipulators in terms of P c : Spherical: 1 = Atan2(Px, Py) 2 = Atan2( (P x 2 + P y 2 ).5, P z ) D3 = (P x 2 + P y 2 + P z 2 ).5 – cl3
19
Focusing on the ARM Manipulators in terms of P c : Articulating: 1 = Atan2(Px, Py) 3 = Atan2(D, (1 – D 2 ).5 ) – Where D = 2 = - – is: Atan2((Px 2 + Py 2 ).5, P z ) – is:
20
One Further Complication: This is called the d 2 offset problem A d 2 offset is a problem that states that the n frame has a non-zero offset along the Y 0 axis as observed with all joints at home, in the solution of the 0 T n This leads to two solutions for 1 the So-Called Shoulder Left and Shoulder Right solutions
21
Defining the d 2 Offset issue Here: ‘The ARM’ might be a prismatic joint as in the Stanford Arm or it might be l 2 & l 3 links in an Articulating Arm and 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 1 is at Home, the wrist center is not on the X0 axis!)
22
Lets look at this Device “From the Top” L2’L2’ L3’L3’ 11 X1X1
23
Solving For 1 We will have a Choice (of two) poses for :
24
In this so-called “Hard Arm” We have two 1 ’s These lead to two 2 ’s (Spherical) Or to four 2 ’s and 3 ’s in the Articulating Arm Shoulder Right Elbow Up & Down Shoulder Left Elbow Up & Down
25
The Orientation Model Evolves from: Separates Arm Joint and Wrist Joint Contribution to Target (given) orientation
26
Focusing on Orientation Issues Lets begin by considering Euler Angles (they are a model that is almost identical to a full functioning Spherical Wrist): Form Product: – R z1 *R y2 *R z3 – This becomes R 3 6
27
Euler Wrist Simplified: And this matrix is equal to a U matrix prepared by multiplying the inverse of the ARM joint orientation matrices inverse and the Desired (given) target orientation NOTE: R 0 3 is Manipulator dependent!
28
Simplifying the RHS: (our so-called U Matrix)
29
Continuing:
30
Finally:
31
Solving for Individual Orientation Angles (1 st ): Selecting (3,3) → C = U 33 With C we “know” S = (1-C 2 ).5 Hence: = Atan2(U 33, (1-U 33 2 ).5 NOTE: 2 solution for !
32
Re-examining the Matrices: To solve for : Select terms: (1,3) & (2,3) C S = U 13 S S = U 23 Dividing the 2 nd by the 1 st : S /C = U 23 /U 13 so Tan( ) = U 23 /U 13 leading to: = Atan2(U 13, U 23 )
33
Continuing our Solution: To solve for : Select terms: (3,1) & (3,2) -S C = U 31 S S = U 32 so Tan( ) = U 32 /-U 31 (note the ‘minus’ sign track the appropriate term!) And Thus = Atan2(-U 31, U 32 )
34
Summarizing: = Atan2(U 33, (1-U 33 2 ).5 = Atan2(U 13, U 23 ) = Atan2(-U 31, U 32 )
35
Let’s examine a Spherical Wrist:
36
IKSing the Spherical Wrist FramesLinkVar dl 3 → 44R 44 00-90 4 → 55R 55 00+90 5 → 66R 66 d6d6 00
37
Writing The Solution:
38
Lets See (By Pure Inverse) Technique:
39
Simplifying
40
Solving: After Examination here let’s select (3,3) both sides: 0 = C4U 23 – S4U 13 S4U 13 = C4U 23 Tan( 4 ) = S4/C4 = U 23 /U 13 4 = Atan2(U 13, U 23 ) With the given and back-substituted values (from the arm joints) we have a value for 4 the RHS is completely known
41
Solving for 5 & 6 For 5 : Select (1,3) & (2,3) terms S5 = C4U 13 + S4U 23 C5 = U 33 Tan( 5 ) = S5/C5 = (C4U 13 + S4U 23 )/U 33 5 = Atan2(U 33, C4U 13 + S4U 23 ) For 6 : Select (3,1) & (3, 2) S6 = C4U 21 – S4U 11 C6 = C4U 22 – S4U 12 Tan( 6 ) = S6/C6 = ([C4U 21 – S4U 11 ],[C4U 22 – S4U 12 ]) 6 = Atan2 ([C4U 21 – S4U 11 ], [C4U 22 – S4U 12 ])
42
Summarizing: 4 = Atan2(U 13, U 23 ) 5 = Atan2(U 33, C4U 13 + S4U 23 ) 6 = Atan2 ([C4U 21 – S4U 11 ], [C4U 22 – S4U 12 ])
43
Lets Try One: Cylindrical Robot w/ Spherical Wrist Given a Target matrix (it’s an IKS after all!) The d 3 “constant” is 400mm; the d 6 offset (call it the ‘Hand Span’) is 150 mm. 1 = Atan2((d x – a x *150),(d y -a y *150)) d2 = (dz – a z *150) d3 = ((d x – a x *150) 2,(d y -a y *150) 2 ).5 - 400
44
The Frame Skeleton: Note “Dummy” Frame to account for Orientation problem with Spherical Wrist
45
Solving for U: NOTE: We needed a “Dummy Frame” to account for the Orientation issue at the end of the Arm
46
Simplifying:
47
Subbing U ij ’s Into Spherical Wrist Joint Models: 4 = Atan2(U 13, U 23 ) = Atan2((C1a x + S1a z ), (S1a x -C1a z )) 5 = Atan2(U 33, C4U 13 + S4U 23 ) = Atan2{a y, [C4(C1a x +S1a z ) + S4 (S1a x -C1a z )]} 6 = Atan2 ([C4U 21 - S4U 11 ], [C4U 22 - S4U 12 ]) = Atan2{[C4(S1n x -C1n z ) - S4(C1n x +S1n z )], [C4(S1s x -C1s z ) - S4(C1s x +S1s z )]}
Similar presentations
© 2025 SlidePlayer.com. Inc.
All rights reserved.