View
15
Download
0
Category
Preview:
Citation preview
M.E. 530.646UR5 Inverse Kinematics
Ryan Keating · 2014Updated by Noah J. Cowan · 2016
Johns Hopkins University
Introduction
Figure refFrames below illustrates a common assignment of Denavit-Hartenberg conventionto the UR5 robot (shown with all joint angles at 0).
y0
x0z0
x1
y0
y1 y2y3
y4
y5
y6z1 z2x2
x3
x4
x5
x6
d1
a2 a3
d4
d5 d6
z3
z4
z5
z6
Figure 1: D-H Convention Frame Assignment
Joint a α d θ1 0 π/2 0.089159 θ12 −0.425 0 0 θ23 −0.39225 0 0 θ34 0 π/2 0.10915 θ45 0 −π/2 0.09465 θ56 0 0 0.0823 θ6
Table 1: D-H Parameters
1
As with any 6-DOF robot, the homogeneous transformation from the base frame to thegripper can be defined as follows:
T 06 (θ1, θ2, θ3, θ4, θ5, θ6) = T 0
1 (θ1) T12 (θ2) T
23 (θ3) T
34 (θ4) T
45 (θ5) T
56 (θ6) (1)
Also remember that a homogenous transformation T ij has the following form:
T ij =
[Rij
~P ij
0 1
]=
xx yx zx (P i
j )xxy yy zy (P i
j )yxz yz zz (P i
j )z0 0 0 1
(2)
where ~P ij is the translation from frame i to frame j and each column of Ri
j is the projectionof one of the axes of frame j onto the axes of frame i (i.e. [xx, xy, xz]
T ≡ xij).
Inverse Kinematics
The first step to solving the inverse kinematics is to find θ1. To do so, we must first find thelocation of the 5th coordinate frame with respect to the base frame, P 0
5 . As illustrated inFigure 2 below, we can do so by translating by d6 in the negative z direction from the 6th
frame.
x0
x6
y0
y6
z0
z6
z5d6
Figure 2: Finding the Origin of the 5th Frame.
This is equivalent to the following operation:
~P 05 = T 0
6
00−d6
1
−
0001
(3)
2
x0x1
y0z1
z2
z3
z5
z4
θ1
φ
ψ
d4
Frame 5 origin
Figure 3: Finding θ1. Note that the rotations around the axes z1, z2, z3, and z4 do not change the factthat the origin of Frame 5 is in the plane that is parallel to the x1 axis, offset by d4 as shown.
The key to this observation is that T 60 is known because it is simply the desired transfor-
mation! That is, given the desired transformation, T 60 , we can calculate the vector from the
origin of Frame 0 to the origin of Frame 5.
With the location of the 5th frame, we can draw an overhead view of the robot as shown inFigure 3. From this, we can see that θ1 = ψ + φ+ π
2where
ψ = atan2((P 0
5 )y, (P05 )x)
(4)
φ = ± arccos
(d4
(P 05 )xy
)= ± arccos
(d4√
(P 05 )x 2 + (P 0
5 )y 2
)(5)
The two solutions for θ1 above correspond to the shoulder being either “left” or “right,”.Note that Equation 5 has a solution in all cases except that d4 > (P 0
5 )xy. You can see fromFigure 3 this happens when the origin of the 3rd frame is close to the z axis of frame 0. Thisforms an unreachable cylinder in the otherwise spherical workspace of the UR5 (as shown inFigure 4 taken from the UR5 manual).
3
Figure 4: The Workspace of the UR5
Knowing θ1, we can now solve for θ5. Once again we draw an overhead view of the robot,but this time we consider location of the 6th frame with respect to the 1st (Figure 5.
We can see that (P 16 )z = d6 cos(θ5)+d4, where (P 1
6 )z = (P 06 )x sin(θ1)− (P 0
6 )y cos(θ1). Solvingfor θ5,
θ5 = ± arccos
((P 1
6 )z − d4d6
)(6)
Once again, there are two solutions. These solutions correspond to the wrist being “down”and “up.”
Figure 6 illustrates that, ignoring translations between frames, z1 can be represented withrespect to frame 6 as a unit vector defined with spherical coordinates. We can find the xand y components of of this vector by projecting it onto the x-y plane and then onto the xor y axes.
Next we find transformation from frame 6 to frame 1,
T 61 = ((T 0
1 )−1 T 06 )−1 (7)
Remembering the structure the of the first three columns of the homogenous transformationT 61 (see Equation 2), we can form the following equalities:
− sin(θ6) sin(θ5) = zy (8)
cos(θ6) sin(θ5) = zx (9)
4
x0
x1
y0z1
θ1
z2
z3
z5
z6
x4
x5
y4
θ5
d6c5
d4
Figure 5: Finding θ5
5
Solving for θ6,
θ6 = atan2
(−zy
sin(θ5),
zxsin(θ5)
)(10)
Equation 10 shows that θ6 is not well-defined when sin(θ5) = 0 or when zx, zy = 0. We cansee from Figure 6 that these conditions are actually the same. In this configuration joints 2,3, 4, and 6 are parallel. As a result, there four degrees of freedom to determine the positionand rotation of the end-effector in the plane, resulting in an infinite number of solutions. Inthis case, a desired value for q6 can be chosen to reduce the number of degrees of freedomto three.
x4,x5,x6
z4 y5,y6
z5,z6,y4
x5,x6
z4 y5,y6
z5,z6z5,z6 z5,z6
θ5
θ5 c5θ5
-θ6
y6
x6
y4
x4
y4,z1
c6s5
-s6s5
Figure 6: Finding θ6
We can now consider the three remaining joints as forming a planar 3R manipulator. Firstwe will find the location of frame 3 with respect to frame 1. This is done as follows:
T 14 = T 1
6 T64 = T 1
6 (T 45 T
56 )−1 (11)
~P 13 = T 1
4
0−d4
01
−
0001
(12)
Now we can draw the plane containing frames 1-3, as shown in Figure 7. We can seethat
cos(ξ) =|| ~P 1
3 ||2 − a22 − a232a2a3
(13)
6
with use of the law of cosines.
cos(ξ) = − cos(π − ξ)= − cos(−θ3)
= cos(θ3)(14)
Combining 13 and 14
θ3 = ± arccos
(|| ~P 1
3 ||2 − a22 − a232a2a3
)(15)
Equation 15 has a solution as long as the argument to arccos is ∈ [−1, 1]. We can see that
large values of || ~P 13 || will cause this the argument to exceed 1. The physical interpretation
here is that the robot can only reach so far out in any direction (creating the spherical boundto the workspace as seen in 4).
Figure 7 also shows thatθ2 = −(δ − ε) (16)
where δ = atan2((P 13 )y,−(P 1
3 )x) and ε can be found via law of sines:
sin(ξ)
|| ~P 13 ||
=sin(ε)
a3(17)
Combining 16 and 17
θ2 = − atan2((P 13 )y,−(P 1
3 )x) + arcsin
(a3 sin(θ3)
|| ~P 13 ||
)(18)
Notice that there are two solutions for θ2 and θ3. These solutions are known as “elbow up”and “elbow down.”
The final step to solving the inverse kinematics is to solve for θ4. First we want to findT 34 :
T 34 = T 3
1 T14 = (T 1
2 T23 )−1T 1
4 (19)
Using the first column of T 34 ,
θ4 = atan2(xy, xx) (20)
Below are figures which show the eight solutions for one end-effector position/orientation.
This solution was adapted from an existing solution written by Kelsey P. Hawkins
7
x1
x2
x3
y1z1
y2 z2
δ-θ2
-θ3
a2
a3
Figure 7: Finding θ2 and θ3
8
(a) Shoulder Left, Elbow Up, Wrist Down (b) Shoulder Left, Elbow Up, Wrist Up
(c) Shoulder Left, Elbow Down, Wrist Down (d) Shoulder Left, Elbow Down, Wrist Up
(e) Shoulder Right, Elbow Down, Wrist Up (f) Shoulder Right, Elbow Down, Wrist Down
(g) Shoulder Right, Elbow Up, Wrist Up (h) Shoulder Right, Elbow Up, Wrist Down
9
Recommended