Robot manipulator and control

Động lực học và

điều khiển robot
TS. Ngô Quang Hiếu
Bộ môn Kỹ Thuật Cơ Khí
Khoa Công Nghệ - Đại học Cần Thơ
 Động học ngược

Robot manipulator and control Ngo Quang Hieu

 Bài toán động học ngược
 Find the values of joint parameters that will put the tool frame at a
desired position and orientation (within the workspace)
 Given H:
 R o
H    SE3
 0 1
 Find all solutions to:

Tn0 q1,..., qn   H

 Noting that:

Tn0 q1,..., qn   A1q1     An qn 

 This gives 12 (nontrivial) equations with n unknowns

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.1: Robot tọa độ cầu
 For a given H:
0 1 0  0.154 c1c2 c4c5c6  s4 s6   s2 s5c6   d 2 s4c5c6  c4 s6   0
0 0 1 0.763  s1c2 c4c5c6  s4 s6   s2 s5c6   c1s4c5c6  c4 s6   0
H  
1 0 0 0   s2 c4c5c6  s4 s6   c2 s5c6  1
  c1 c2 c4c5s6  s4c6   s2 s5s6   s1 s4c5s6  c4c6   1
0 0 0 1 
 s1 c2 c4c5s6  s4c6   s2 s5s6   c1 s4c5s6  c4 s6   0
s2 c4c5s6  s4c6   c2 s5s6  0
 Find q1, q2, d3, q4, q5, q6: c1c2c4 s5  s2c5   s1s4 s5  0
s1c2c4 s5  s2c5   c1s4 s5  1
 s2c4 s5  c2c5  0
c1s2d3  s1d 2  d6 c1c2c4 s5  c1c5s2  s1s4 s5   0.154
s1s2d3  c1d 2  d6 c1s4 s5  c2c4 s1s5  c5s1s2   0.763
c2d3  d6 c2c5  c4 s2 s5   0

 One solution: q1 = p/2, q2 = p/2, d3 = 0.5, q4 = p/2, q5 = 0, q6 = p/2

Robot manipulator and control Ngo Quang Hieu
 Động học ngược
 The previous example shows how difficult it would be to obtain a
closed-form solution to the 12 equations
 Instead, we develop systematic methods based upon the
manipulator configuration
 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!

Robot manipulator and control Ngo Quang Hieu

 Tách chuỗi động học
• 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 two
parts:
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
R60 q1,..., q6   R
o60 q1,..., q6   o

• Use the position of the wrist center to determine the first three joint
angles…

Robot manipulator and control Ngo Quang Hieu

 Tách chuỗi động học
• Now, origin of tool frame, o6, is a distance d6 translated along z5 (since
z5 and z6 are collinear)
– Thus, the third column of R is the direction of z6 (w/ respect to the
base frame) and we can write: 0
o  o60  oco  d6 R 0
 
1
– Rearranging: 0 
oco  o  d6 R 0
 
1
– Calling o = [ox oy oz]T, oc0 = [xc yc zc]T

 xc   ox  d6r13 
 y   o  d r 
 c  y 6 23 
 zc   oz  d6r33 

Robot manipulator and control Ngo Quang Hieu

 Tách chuỗi động học
• Since [xc yc zc]T are determined from the first three joint angles, our
forward kinematics expression now allows us to solve for the first
three joint angles decoupled from the final three.
– Thus we now have R30
– Note that: R  R30 R63

– To solve for the final three joint angles:

3
 
0 1
 
0T
R6  R3 R  R3 R

– Since the last three joints for a

spherical wrist, we can use a set of
Euler angles to solve for them

Robot manipulator and control Ngo Quang Hieu

 Động học ngược vị trí
• Now that we have [xc yc zc]T we need to find q1, q2, q3
– Solve for qi by 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 q1, project the
arm onto the x0, y0 plane
• Note: We use atan2(·) instead of atan(·) to account for the full range of
angular solutions
─ Called ‘four-quadrant’ arctan
 atan 2 y, x  y0
  y
  atan   y  0, x  0
  x

atan 2 y, x    atan  
y
y  0, x  0
 x
 
 y  0, x  0
2
 undefined y  0, x  0

Robot manipulator and control Ngo Quang Hieu
 Ví dụ 4.2: Robot toàn khớp quay (RRR)
1. To solve for 1, project the arm onto the x0, y0 plane
1  atan 2xc , yc 

1    atan 2xc , yc 
– Can also have:
• This will of course change the solutions for 2 and 3

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.2: Điểm kỳ dị
 If xc=yc=0, 1 is undefined
 i.e. any value of 1 will
work
Robot manipulator and control
 If there is an offset, then we
will have two solutions for 1:
left arm and right arm
 However, wrist centers
cannot intersect z0
Ngo Quang Hieu
 Ví dụ 4.2: Tay trái và tay phải
 Left arm:  Right arm:

1    
1   - 
  atan 2 xc , yc 
  atan 2xc , yc 
    atan 2 xc 2  yc 2  d 2 , d 
  atan 2 xc 2  yc 2  d 2 , d   
 
 atan 2  xc 2  yc 2  d 2 ,d 
 

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.2: Khuỷu tay
 Therefore there are in general two solutions for 1
 Finding 2 and 3 is identical to the planar two-link manipulator we
have seen previously:

r 2  s 2  a22  a32
cos3 
2a2a3
r 2  xc 2  yc 2  d 2
s  zc  d1
xc 2  yc 2  d 2   zc  d1 2  a2 2  a32
 cos3  D
2a2a3

 Therefore we can find two solutions for 3:

3  atan 2 D, 1  D 2 

 

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.2: Khuỷu tay
 The two solutions for 3 correspond to the elbow-down and elbow-up
positions respectively
 Now solve for 2:
 2  atan 2r , s   atan 2a2  a3c3 , a3s3 

 atan 2 xc 2  yc 2  d 2 , zc  d1   atan 2a2  a3c3 , a3s3 

 
 Thus there are two solutions for the pair (2, 3)

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.2: Robot RRR (4 lời giải)
 In general, there will be a maximum of four solutions to the inverse
position kinematics of an elbow manipulator
 Ex: PUMA

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.3: Robot tọa độ cầu (RRP)
 Spherical configuration
 Solve for 1 using same method as with RRR

1  atan2 xc , yc 
 Again, if there is an offset, there will be left-arm and right-arm solutions
 Solve for 2:

 2  atan2s, r 
r 2  xc 2  yc 2
s  zc  d1
 Solve for d3:
d3  r 2  s 2

 xc 2  yc 2   zc  d1 2
Robot manipulator and control Ngo Quang Hieu
 Ví dụ 4.4: Robot tọa độ trụ (RPP)
 Cylindrical configuration
 Solve for 1 using same method as with RRR and RRP

1  atan2 xc , yc 
 Again, if there is an offset, there will be left-arm and right-arm solutions
 Solve for d2:

d 2  zc  d1
 Solve for d3:

d3  xc 2  yc 2

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.5: Robot SCARA (RRP)
 SCARA configuration
xc2  yc2  a12  a2 2
 Solve for 2 cos 2 
2a1a2
 Solve for 1: 1  atan2 xc , yc   atan2a1  a2c2 , a2 s2 
 Solve for d3: d3  d1  d 4  zc

d3  xc 2  yc 2

Robot manipulator and control Ngo Quang Hieu

 Động học orientation
Inverse ngược hướngkinematics

 Now that we can solve for the position of the wrist center (given
kinematic decoupling), we can use the desired orientation of the end-
effector to solve for the last three joint angles
 Finding a set of Euler angles corresponding to a desired rotation matrix R
 We want the final three joint angles that give the orientation of the tool
frame with respect to O3 (i.e. R63)

Robot manipulator and control Ngo Quang Hieu

 Động học ngược hướng: cổ tay khớp cầu
• Previously, we said that the forward kinematics of the spherical wrist
were identical to a ZYZ Euler angle transformation:

c4 c5 c6  s4 s6  c4 c5 s6  s4 c6 c4 s5 c4 s5 d 6 
s c c  c s  s4 c5 s6  c4 c6 s4 s5 s4 s5 d 6 
T63  A4 A5 A6   4 5 6 4 6 
  s5 c6 s5 s6 c5 c5 d 6 
 
 0 0 0 1 

Robot manipulator and control Ngo Quang Hieu

 Động học ngược hướng: cổ tay khớp cầu
• The inverse orientation problem reduces to finding a set of Euler
angles (4, 5, 6) that satisfy:

c4c5c6  s4 s6  c4c5s6  s4c6 c4 s5 

R63   s4c5c6  c4 s6  s4c5s6  c4c6 s4 s5 
 
  s5c6 s5s6 c5 
• to solve this, take two cases:
1. Both r13 and r23 are not zero (i.e. 5 ≠ 0)… nonsingular
2. 5 = 0, thus r13 = r23 = 0… singular
• Nonsingular case
– If 5 ≠ 0, then r33 ≠ ±1 and:

c5  r33 , s5   1  r33 2

5  atan2 r33 , 1  r33 2 

 
Robot manipulator and control Ngo Quang Hieu
 Động học ngược hướng: cổ tay khớp cầu
• Thus there are two values for 5. Using the first (s5 > 0):
 4  atan2r13 , r23 
 6  atan2 r31 , r32 
• Using the second value for 5 (s5 < 0):

 4  atan2 r13 ,r23 

 6  atan2r31 ,r32 
• Thus for the nonsingular case, there are two solutions for the inverse
orientation kinematics

Robot manipulator and control Ngo Quang Hieu

 Động học ngược hướng: cổ tay khớp cầu
• In the singular case, 5 = 0 thus s5 = 0 and r13 = r23 = r31 = r32 = 0
• Therefore, R63 has the form:

c4c6  s4 s6  c4 s6  s4c6 0 c46  s46 0  r11 r12 0

R63   s4c6  c4 s6  s4 s6  c4c6 0   s46 c46 0  r21 r22 0
     
 0 0 1  0 0 1  0 0 1

• So we can find the sum 4 + 6 as follows:

4  6  atan2r11, r21   atan2r11,r12 

• Since we can only find the sum, there is an infinite number of solutions
(singular configuration)

Robot manipulator and control Ngo Quang Hieu

 Động học ngược hướng: phương pháp tổng quát

1. Find q1, q2, q3 such that the position of the wrist center is:

0 
oco  o  d6 R 0 inverse position
  kinematics
1
2. Using q1, q2, q3, determine R30
3. Find Euler angles corresponding to the rotation matrix:

3
 0 1 0T
R6  R3 R  R3 R  inverse orientation
kinematics

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.6: Robot RRR với khớp cầu cổ tay
• For the DH parameters below, we can derive R30 from the forward kinematics:
c1c23  c1s23 s1 
R30   s1c23  s1s23  c1  link ai i di i
  1 0 90 d1 1
 s23 c23 0 
2 a2 0 0 2
• We know that R6 is given as follows:
3
3 a3 0 0 3
c4c5c6  s4 s6  c4c5s6  s4c6 c4 s5 
R63   s4c5c6  c4 s6  s4c5s6  c4c6 s4 s5 
 
  s5c6 s5s6 c5 
• To solve the inverse orientation kinematics:
3 0T
R6  R3 R  
– For a given desired R

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.6: Robot RRR với khớp cầu cổ tay

• Euler angle solutions can be applied. Taking the third column of (R30)TR
c 4s5  c1c23 r13  s1c 23 r23  s23 r33
s4s5  c1s23 r13  s1s23 r23  c23 r33
c5  s1r13  c1r23

• Again, if 5 ≠ 0, we can solve for 5:

5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 
• Finally, we can solve for the two remaining angles as follows:
 4  atan2c1c23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c23 r33 
 6  atan2 s1r11  c1r21, s1r12  c1r22 
• For the singular configuration (5 = 0), we can only find 4 + 6 thus it is
common to arbitrarily set 4 and solve for 6

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.6: Robot RRR với khớp cầu cổ tay

• Derive complete inverse kinematics solution

link ai i di i
1 0 90 d1 1
2 a2 0 0 2
3 a3 0 0 3
4 0 -90 0 4
5 0 0 0 5
6 0 0 d6 3
• we are given H = T60 such that: ox  r11 r12 r13 
o  oy , R  r21 r22 r23 
oz  r31 r32 r33 

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.6: Robot RRR với khớp cầu cổ tay

• First, we find the wrist center:

 xc  o x  d 6 r13 
 y   o  d r 
 c  y 6 23 

 zc  oz  d 6 r33 

• Inverse position kinematics:

1  atan2xc , y c 
 2  atan2 xc  y c  d 2 , zc  d1   atan2a2  a3c3 , a3s3 
2 2

 

 3  atan2 D, 1  D 2 
• Where d is the shoulder offset (if any) and D is given by:
xc  y c  d 2  zc  d1   a2  a3
2 2 2 2 2

D
2a2a3

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.6: Robot RRR với khớp cầu cổ tay

• Inverse orientation kinematics:

– Now that we know 1, 2, 3, we know R30. need to find R36:
 
R63  R30 R
T

• Solve for 4, 5, 6, Euler angles:

 4  atan2c1c 23 r13  s1c23 r23  s23 r33 ,c1s23 r13  s1s23 r23  c 23 r33 
 5  atan2 s1r13  c1r23 , 1  s1r13  c1r23  
2

 
 6  atan2 s1r11  c1r21, s1r12  c1r22 

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.7: Động học ngược robot SCARA

R o 
• We are given T40: T40   
 0 1
c12 c 4  s12 s4 s12 c 4  c12 s4 0 a1c1  a2c12 
s c  c s  c12 c 4  s12 s4 0 a1s1  a2s12 
  12 4 12 4

 0 0  1  d3  d 4 
 
 0 0 0 1 

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

Robot manipulator and control Ngo Quang Hieu

 Ví dụ 4.7: Động học ngược robot SCARA

• Thus, given the form of T40, R must have the following form:
c  s 0
R  s c 0
 0 0 1

• Where  is defined as:   1   2   4  atan2r11, r12 

• To solve for 1 and 2 we project the manipulator onto the x0-y0 plane:
ox  oy  a1  a2
2 2 2 2

c2 
2a1a2
• This gives two solutions for 2:  2  atan2 c2 , 1  c22 
 
• Once 2 is known, we can solve for 1:
1  atan2ox , oy  - atan2a1  a2c2,a2s2 
• 4 is now give as:  4  1   2  atan2r11, r12 
• Finally, it is trivial to see that d3 = oz + d4

Robot manipulator and control Ngo Quang Hieu

 Số lời giải

• How many solutions to the inverse position kinematics of a planar 3-link

arm?
– given a desired d=[dx dy]T, the forward kinematics can be written as:
d x  a1c1  a2c12  a3c123
d y  a1s1  a2s12  a3s123
– Therefore the inverse kinematics problem is under-constrained (two
equations and three unknowns)

∞ solutions is d is inside the workspace

1 solution if d is on the workspace boundary
0 solutions else

Robot manipulator and control Ngo Quang Hieu

 Số lời giải

• What if now we describe the desired position and orientation of the end
effector?
– given a desired d=[dx dy]T, we can now call the position of o2 the ‘wrist
center’. This position is given as: w x  d x  a3 cosd 
w y  d y  a3 sind 
– Now we have reduced the problem to finding the joint angles that will give
the desired position of the wrist center (we have done this for a 2D planar
manipulator).
– Finally, 3 is given as: 3  d  1  2 

∞ solutions if the wrist center is on the origin

2 solutions if wrist center is inside the 2-link workspace
1 solution if wrist center is on the 2-link workspace boundary
0 solutions else

Robot manipulator and control Ngo Quang Hieu

Robot manipulator and control