Date post: | 05-Apr-2018 |
Category: |
Documents |
Upload: | heba-mohammed-fadhil |
View: | 224 times |
Download: | 0 times |
of 33
7/31/2019 Lec04 Forward InverseKinematicsII
1/33
ES159/259
Ch. 3: Forward and Inverse Kinematics
7/31/2019 Lec04 Forward InverseKinematicsII
2/33
ES159/259
Recap: The Denavit-Hartenberg (DH) Convention
Representing each individual homogeneous transformation as theproduct of four basic transformations:
1000
0
1000
00
00
0001
1000
0100
0010
001
1000
100
0010
0001
1000
0100
00
00
,,,,
i
i
i
i
i
xaxdzzi
dcs
sascccs
casscsc
cs
sc
a
d
cs
sc
A
ii
iiiiii
iiiiii
ii
iiii
ii
iiii
RotTransTransRot
7/31/2019 Lec04 Forward InverseKinematicsII
3/33
ES159/259
Recap: the physical basis for DH parameters
ai: link length, distance between the o0and o1 (projected along x1)
i: link twist, angle between z0and z1 (measured around x1)
di: link offset, distance between o0and o1 (projected along z0)
i: joint angle, angle between x0and x1 (measured around z0)
7/31/2019 Lec04 Forward InverseKinematicsII
4/33
ES159/259
General procedure for determining forward kinematics
1. Label joint axes as z0, , zn-1 (axis zi is joint axis for joint i+1)
2. Choose base frame: set o0on z0and choose x0and y0using right-handed convention
3. For i=1:n-1,
i. Place oiwhere the normal to zi and zi-1 intersects zi. If zi intersects zi-1, putoiat intersection. If zi and zi-1 are parallel, place oialong zi such that di=0
ii. xi is the common normal through oi, or normal to the plane formed by zi-1and zi if the two intersect
iii. Determine yiusing right-handed convention
4. Place the tool frame: set znparallel to zn-1
5. For i=1:n, fill in the table of DH parameters6. Form homogeneous transformation matrices, Ai7. Create Tn
0that gives the position and orientation of the end-effector inthe inertial frame
7/31/2019 Lec04 Forward InverseKinematicsII
5/33
ES159/259
Example 2: three-link cylindrical robot
3DOF: need to assign four coordinate frames
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1 axis (axis of translation for joint 2)
3. Choose z2 axis (axis of translation for joint 3)
4. Choose z3 axis (tool frame) This is again arbitrary for this case since we have described no wrist/gripper
Instead, define z3as parallel to z2
7/31/2019 Lec04 Forward InverseKinematicsII
6/33
ES159/259
Example 2: three-link cylindrical robot
Now define DH parameters
First, define the constant parameters ai, i
Second, define the variable parameters i, di
link ai i di i
1 0 0 d1 1
2 0 -90 d2 0
3 0 0 d3 0
1000
100
0010
0001
1000
010
0100
0001
1000
100
00
00
3
3
2
2
1
11
11
1d
Ad
Ad
cs
sc
A ,,
1000
010
0
0
21
3111
3111
321
0
3dd
dccs
dssc
AAAT
7/31/2019 Lec04 Forward InverseKinematicsII
7/33
ES159/259
Example 3: spherical wrist
3DOF: need to assign four coordinate frames
yaw, pitch, roll (4, 5, 6) all intersecting at one point o(wrist center)
1. Choose z3 axis (axis of rotation for joint 4)
2. Choose z4 axis (axis of rotation for joint 5)
3. Choose z5 axis (axis of rotation for joint 6)4. Choose tool frame:
z6 (a) is collinear with z5 y6(s) is in the direction the gripper closes
x6(n) is chosen with a right-handed convention
7/31/2019 Lec04 Forward InverseKinematicsII
8/33
ES159/259
Example 3: spherical wrist
link ai i di i
4 0 -90 0 4
5 0 90 0 5
6 0 0 d6
6
Now define DH parameters
First, define the constant parameters ai, i
Second, define the variable parameters i, di
1000
100
00
00
1000
0010
00
00
1000
0010
00
00
6
66
66
6
55
55
5
44
44
4d
cs
sc
Acs
sc
Acs
sc
A ,,
1000
6556565
654546465464654
654546465464654
65436
dcccscsdssssccscsscccs
dscsccssccssccc
AAAT
7/31/2019 Lec04 Forward InverseKinematicsII
9/33
ES159/259
Example 4: cylindrical robot with spherical wrist
6DOF: need to assign seven coordinate frames
But we already did this for the previous two examples, so we can fill in thetable of DH parameters:
link ai i di i
1 0 0 d1 1
2 0 -90 d2 0
3 0 0 d3 0
4 0 -90 0 4
5 0 90 0 5
6 0 0 d6 6
o3, o4, o5are all atthe same point oc
7/31/2019 Lec04 Forward InverseKinematicsII
10/33
ES159/259
Example 4: cylindrical robot with spherical wrist
Note that z3 (axis for joint 4) is collinear with z2 (axis for joint 3), thus wecan make the following combination:
1000
333231
232221
131211
3
6
0
3
0
6
z
y
x
drrr
drrr
drrr
TTT
21654
316516541
316516541
5433
5154123
5154113
6465432
651641654122
651641654112
6465431
651641654121
651641654111
dddssd
dcdccdscsd
dsdcsdsccd
ssr
ccscsr
cssccr
ccccsr
cscssssccsr
csscscscccrscccsr
cscssscccsr
csssscccccr
z
y
x
7/31/2019 Lec04 Forward InverseKinematicsII
11/33
ES159/259
Example 5: the Stanford manipulator
6DOF: need to assign seven coordinate frames:
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1-z5 axes (axes of rotation/translation for joints 2-6)
3. Choose xiaxes
4. Choose tool frame5. Fill in table of DH parameters:
link ai i di i
1 0 -90 0 1
2 0 90 d2 2
3 0 0 d3 0
4 0 -90 0 4
5 0 90 0 5
6 0 0 d6 6
Suggested insertion:photo of the Stanfordmanipulator
7/31/2019 Lec04 Forward InverseKinematicsII
12/33
ES159/259
Example 5: the Stanford manipulator
Now determine the individual homogeneous transformations:
1000
100
00
00
1000
0010
00
00
1000
0010
00
00
1000
100
0010
0001
1000
010
00
00
1000
0010
00
00
6
66
66
6
55
55
5
44
44
4
33
2
22
22
2
11
11
1
d
cs
sc
Acs
sc
Acs
sc
A
dAd
cs
sc
A
cs
sc
A
,,
,,
7/31/2019 Lec04 Forward InverseKinematicsII
13/33
ES159/259
Example 5: the Stanford manipulator
Finally, combine to give the complete description of the forwardkinematics:
1000
333231
232221
131211
61
0
6
z
y
x
drrr
drrr
drrr
AAT
52452632
2155142541621321
5412515421621321
5254233
54152542123
54152542113
65264654232
646541652646542122
646541652646542112
65264654231
646541652646542121
646542652646542111
sscccddcd
sscssccsscddcdssd
ssssccscccddsdscd
ccscsr
ssccssccsr
ssscsscccr
ssccssccsr
scscscssscsscccsr
ccscssssscssccccrcscsscccsr
scccsccssssccccsr
scccsdcsssscccccr
z
y
x
7/31/2019 Lec04 Forward InverseKinematicsII
14/33
ES159/259
Example 6: the SCARA manipulator
4DOF: need to assign five coordinate frames:
1. Choose z0 axis (axis of rotation for joint 1, base frame)
2. Choose z1-z3 axes (axes of rotation/translation for joints 2-4)
3. Choose xiaxes
4. Choose tool frame5. Fill in table of DH parameters:
link ai i di i
1 a1 0 0 1
2 a2 180 0 2
3 0 0 d3 0
4 0 0 d4 4
Suggestedinsertion: photoof SCARAmanipulator
7/31/2019 Lec04 Forward InverseKinematicsII
15/33
ES159/259
Example 6: the SCARA manipulator
Now determine the individual homogeneous transformations:
1000
100
00
00
1000
100
0010
0001
1000
0100
0
0
1000
0100
0
0
4
44
44
4
3
3
2222
2222
2
1111
1111
1
d
cs
sc
A
d
Asacs
casc
Asacs
casc
A ,,,
1000100
0
0
43
12211412412412412
12211412412412412
41
0
4
dd
sasaccsssccs
cacacsscsscc
AAT
7/31/2019 Lec04 Forward InverseKinematicsII
16/33
ES159/259
Forward kinematics of parallel manipulators
Parallel manipulator: two or more series chains connect the end-effector to the base (closed-chain)
# of DOF for a parallel manipulator determined by taking the total DOFsfor all links and subtracting the number of constraints imposed by the
closed-chain configuration
Grueblers formula (3D):
jn
iijL fnn
1
6DOF#
number of links*
*excluding ground
number of joints
#DOF for joint i
7/31/2019 Lec04 Forward InverseKinematicsII
17/33
ES159/259
Forward kinematics of parallel manipulators
Grueblers formula (2D):
Example (2D):
Planar four-bar, nL = 3, nj= 4, fi= 1(for all joints)
3(3-4)+4 = 1DOF
Forward kinematics:
jn
i
ijL fnn1
3DOF#
2tan
2
22cos
1
21
2
2
2
1
2
2
21
L
L
LL
L
7/31/2019 Lec04 Forward InverseKinematicsII
18/33
ES159/259
Inverse Kinematics
Find the values of joint parameters that will put the tool frame at adesired position and orientation (within the workspace)
Given H:
Find allsolutions to:
Noting that:
This gives 12 (nontrivial) equations with nunknowns
310
SEoR
H
HqqT nn ,...,10
nnnn qAqAqqT 1110
,...,
7/31/2019 Lec04 Forward InverseKinematicsII
19/33
ES159/259
For a given H:
Find 1, 2, d3, 4, 5, 6:
One solution: 1 = /2, 2= /2, d3= 0.5, 4= /2, 5= 0, 6= /2
1000
0001
763.0100
154.0010
H
Example: the Stanford manipulator
0
763.0
154.0
0
1
0
0
0
1
1
0
0
52452632
2155142541621321
5412515421621321
52542
541525421
541525421
652646542
6465416526465421
6465416526465421
652646542
6465416526465421
6465426526465421
sscccddcsscssccsscddcdss
ssssccscccddsdsc
ccscs
ssccssccs
ssscssccc
ssccssccs
scscscssscsscccs
ccscssssscsscccc
cscsscccs
scccsccssssccccs
scccsdcssssccccc
7/31/2019 Lec04 Forward InverseKinematicsII
20/33
ES159/259
Inverse Kinematics
The previous example shows how difficult it would be to obtain aclosed-form solution to the 12 equations
Instead, we develop systematic methods based upon the manipulatorconfiguration
For the forward kinematics there is always a unique solution
Potentially complex nonlinear functions
The inverse kinematics may or may not have a solution
Solutions may or may not be unique
Solutions may violate joint limits
Closed-form solutions are ideal!
7/31/2019 Lec04 Forward InverseKinematicsII
21/33
ES159/259
Overview: kinematic decoupling
Appropriate for systems that have an arm a wrist
Such that the wrist joint axes are aligned at a point
For such systems, we can split the inverse kinematics problem into twoparts:
1. Inverse position kinematics: position of the wrist center
2. Inverse orientation kinematics: orientation of the wrist
First, assume 6DOF, the last three intersecting at oc
Use the position of the wrist center to determine the first three jointangles
oqqo
RqqR
61
0
6
61
0
6
,...,
,...,
7/31/2019 Lec04 Forward InverseKinematicsII
22/33
ES159/259
Overview: kinematic decoupling
Now, origin of tool frame, o6, is a distance d6 translated along z5 (sincez5and z6are collinear)
Thus, the third column of Ris the direction of z6 (w/ respect to the baseframe) and we can write:
Rearranging:
Calling o= [oxoyoz]
T
, oc0
= [xcyczc]
T
1
0
0
606 Rdooo
oc
1
0
0
6Rdoo
o
c
336
236
136
rdo
rdo
rdo
z
y
x
z
y
x
c
c
c
7/31/2019 Lec04 Forward InverseKinematicsII
23/33
ES159/259
Overview: kinematic decoupling
Since [xcyczc]Tare determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first threejoint angles decoupled from the final three.
Thus we now have R30
Note that:
To solve for the final three joint angles:
Since the last three joints for aspherical wrist, we can use a set of
Euler angles to solve for them
3
6
0
3RRR
RRRRR T0310
3
3
6
7/31/2019 Lec04 Forward InverseKinematicsII
24/33
ES159/259
Inverse position
Now that we have [xcyczc]T we need to find q1, q2, q3
Solve for qiby projecting onto the xi-1, yi-1 plane, solve trig problem
Two examples: elbow (RRR) and spherical (RRP) manipulators
For example, for an elbow manipulator, to solve for 1, project the arm onto
the x0, y0plane
7/31/2019 Lec04 Forward InverseKinematicsII
25/33
ES159/259
Background: two argument atan
We use atan2() instead of atan() to account for the full range ofangular solutions
Called four-quadrant arctan
0,0
0,02
0,0
0,0
0,2
,2
xy
xy
xyx
y
xyx
y
yxy
xy
undefined
atan
atan
atan
atan
7/31/2019 Lec04 Forward InverseKinematicsII
26/33
ES159/259
Example: RRR manipulator
1. To solve for 1, project the arm onto the x0, y0plane
Can also have:
This will of course change the solutions for 2and 3
cc yx ,21 atan
cc yx ,21 atan
7/31/2019 Lec04 Forward InverseKinematicsII
27/33
ES159/259
If there is an offset, then we willhave two solutions for 1: left armand right arm
However, wrist centers cannotintersect z0
If xc=yc=0, 1 is undefined
i.e. any value of 1 will work
Caveats: singular configurations, offsets
7/31/2019 Lec04 Forward InverseKinematicsII
28/33
ES159/259
Left arm: Right arm:
Left arm and right arm solutions
ddyx
yx
cc
cc
,2
,2
222
1
atan
atan
-
ddyx
ddyx
yx
cc
cc
cc
,2
,2
,2
222
222
1
atan
atan
atan
7/31/2019 Lec04 Forward InverseKinematicsII
29/33
ES159/259
Therefore there are in general two solutions for 1 Finding 2and 3 is identical to the planar two-link manipulator we have
seen previously:
Therefore we can find two solutions for 3:
Left arm and right arm solutions
D
aa
aadzdyx
dzs
dyxr
aa
aasr
ccc
c
cc
32
2
3
2
2
2
1
222
3
1
2222
32
2
3
2
2
22
3
2cos
2cos
2
3 1,2 DD atan
7/31/2019 Lec04 Forward InverseKinematicsII
30/33
ES159/259
The two solutions for 3correspond to the elbow-down and elbow-uppositions respectively
Now solve for 2:
Thus there are two solutions for the pair (2, 3)
Left arm and right arm solutions
333321222
333322
,2,2
,2,2
sacaadzdyx
sacaasr
ccc
atanatan
atanatan
7/31/2019 Lec04 Forward InverseKinematicsII
31/33
ES159/259
In general, there will be a maximum of four solutions to the inversepositionkinematics of an elbow manipulator
Ex: PUMA
RRR: Four total solutions
7/31/2019 Lec04 Forward InverseKinematicsII
32/33
ES159/259
Spherical configuration
Solve for 1 using same method as with RRR
Again, if there is an offset, there
will be left-arm and right-arm solutions
Solve for 2:
Solve for d3:
Example: RRP manipulator
cc yx ,21 atan
1
222
2 ,2
dzs
yxr
rs
c
cc
atan
21
22
22
3
dzyx
srd
ccc
7/31/2019 Lec04 Forward InverseKinematicsII
33/33
ES159/259
Next class
Complete the discussion of inverse kinematics
Inverse orientation
Introduction to other methods
Introduction to velocity kinematics and the Jacobian