TRƯỜNG ĐẠI HỌC CÔNG NGHIỆP HÀ NỘI

KHOA ĐIỆN – BM TỰ ĐỘNG HÓA

ĐIỀU KHIỂN ROBOT

CHƯƠNG 2: ĐỘNG LỰC HỌC VỊ TRÍ

(Bản quyền thuộc bộ môn TĐH ĐHCN HN)

Chương 2: Động lực học vị trí robot

 13/03/2022
2.1. Biểu diễn ma trận
2.1.1. Biểu diễn 1 vector trong không gian

W=0 hoặc 1
- W=1: 3 thành phầnx,y,z biểu diễn 1
điểm
- W=0, khi đó 3 thành phần x,y,z biểu
diễn hướng của 1 vector. x,y,z là
thành phần của vector đơn vị

x
__ ^ ^ ^  y
P  ax i  by j  cz k
__ x y z
P  a x  ; by  ; c z 
z
  w w w
 w

Chương 2: Động lực học robot 2

 13/03/2022
2.1. Biểu diễn ma trận
2.1.2. Biểu diễn một khung tọa độ
Nếu khung tọa độ trùng với gốc tọa độ chuẩn

n x ox ax 0 
n oy a y 0 
F y
nz oz az 0 
 
0 0 0 1

Chương 2: Động lực học robot 3

 13/03/2022
2.1. Biểu diễn ma trận
2.1.2. Biểu diễn một khung tọa độ
Nếu khung tọa độ không trùng với gốc tọa độ chuẩn

n x ox ax Px 
n oy ay Py 
F y
nz oz az Pz 
 
0 0 0 1

Chương 2: Động lực học robot 4

 13/03/2022
2.1. Biểu diễn ma trận
2.1.2. Biểu diễn một khung tọa độ
VD: Khung tọa độ B đặt ở vị trí [3, 5, 7] với vector đơn vị iB
song song với trục x, vector jB tạo với trục y một góc 450,
vector kB tạo với trục z một góc 450 . Xác định khung tọa độ B.

Chương 2: Động lực học robot 5

 13/03/2022
2.2. Các phép biến đổi

2.2.1

Chương 2: Động lực học robot 6

 13/03/2022
2.2. Các phép biến đổi

Chương 2: Động lực học robot 7

 13/03/2022
2.2. Các phép biến đổi

ZB Z
Pzm
Pz P

yB
Py
Pym
y

xB

Chương 2: Động lực học robot 8

 13/03/2022
2.1. Các phép biến đổi

2.2.2.

Chương 2: Động lực học robot 9

 13/03/2022
2.2. Các phép biến đổi

2.2.2

2.2.3

Chương 2: Động lực học robot 10

 13/03/2022
2.2. Các phép biến đổi

2.2.3

Chương 2: Động lực học robot 11

 13/03/2022
2.2. Các phép biến đổi


VD1: Một điểm P [7,3,2]T đặt trong khung tọa độ B. xác định tọa độ
điểm P trong khung tọa độ gốc khi dịch chuyển khung tọa độ B bằng
các phép biến đổi so với khung tọa độ gốc theo thứ tự sau.
1. Quay xung quanh trục z một góc 900
2. Quay xung quanh trục y một góc 900
3. Tịnh tiến theo các trục x, y, z tương ứng với 4, -3, 7 đơn vị

Chương 2: Động lực học robot 12

 13/03/2022
2.2. Các phép biến đổi

Chương 2: Động lực học robot 13

 13/03/2022
2.2. Các phép biến đổi

2.2.4. phép biến đổi so với khung tọa độ di chuyển

Tọa độ của một điểm gắn trong khung tọa độ di chuyển được biểu
diễn so với khung tọa độ gốc được tính bằng phép nhân các phép
biến đổi theo thứ tự ngược (so với trường hợp các phép biến đổi
thực hiện trong khung tọa độ gốc)
VD2: thực hiện các phép biến đổi như VD1 nhưng thực hiện tương
đối so với khung tọa độ dịch chuyển. Xác định ma trận biểu diễn phép
biến đổi tổng hợp và tọa độ điểm P sau các phép biến đổi trên.

Chương 2: Động lực học robot 14

 13/03/2022

2.2. Các phép biến đổi

Chương 2: Động lực học robot 15

 13/03/2022
2.2. Các phép biến đổi

VD3: Một khung tọa độ B được quay xung quanh trục x một góc
90 tiếp theo tịnh tiến theo trục z một đoạn 3 đơn vị, tiếp theo quay
xung quanh trục z của khung tọa độ chuẩn một góc 90. cuối cùng
tịnh tiến theo trục y hiện tại 1 đoạn 5 đơn vị.
Viết phương trình và tính ma trận biểu diễn phép biến đổi
Xác định vị trí cuối cùng của điểm P [1,5,4]T gắn ở khung tọa độ B

Chương 2: Động lực học robot 16

 13/03/2022
2.2. Các phép biến đổi

Chương 2: Động lực học robot 17

 13/03/2022
2.2. Các phép biến đổi

Chương 2: Động lực học robot 18

 2.3. Nghịch đảo của ma trận phép biến13/03/2022
đổi

Chương 2: Động lực học robot 19

 2.4. Khung tạo độ trong không gian làm
13/03/2022

việc của robot

Ma trận biểu diễn khung tọa độ
E so với khung tọa độ gốc:

Chương 2: Động lực học robot 20

 13/03/2022
2.5. Các cấu hình robot điển hình
Robot có 4 cấu hình điển hình: Tọa độ Đecac, tọa độ hình trụ, tọa độ
hình cầu, tạo độ khớp nối.
1. cấu hình tọa độ Đề các
Robot cấu hình tọa độ Đề các tay robot có thể chuyển động tịnh tiến
theo 3 trục x, y, z
Ma trận biến đổi biểu thị phương trình động học thuận của vị trí tay
robot trong hệ tạo độ Đecac

Chương 2: Động lực học robot 21

 13/03/2022
2.5. Các cấu hình robot điển hình
2. Cơ cấu tọa độ trụ
Cơ cấu tạo độ trụ gồm 2 chuyển động tịnh tiến và 1 chuyển động quay.
Chuyển động tịnh tiến theo trục x một đoạn r và quay theo trục z một
góc  và tịnh tiến theo trục z một đoạn lz

VD: Tính toán biến khớp của robot

cơ cấu trụ để điểm đặt vị trí khung
tọa độ tay ở vị trí [3,4,7]T

Chương 2: Động lực học robot 22

 13/03/2022

2.5. Các cấu hình robot điển hình

Chương 2: Động lực học robot 23

 13/03/2022

2.5. Các cấu hình robot điển hình

VD: Xác định các biến khớp để có thể đặt gốc của tay robot cầu ở
vị trí [3,4,7]T

Chương 2: Động lực học robot 24

 13/03/2022

2.5. Các cấu hình robot điển hình

4. Phép quay Roll, Pitch, Yaw (RPY)
Định hướng tay robot bằng 3 phép quay xung quanh các trục hiện tại
a,o,n của khung tọa độ định hướng tay robot.
Nếu giải thiết, khung tọa độ hiện tại của tay robot xong xong với
khung tọa độ chuẩn, khung tọa độ robot và khung tọa độ chuẩn sẽ
như nhau trước khi tay robot chuyển động.

Chương 2: Động lực học robot 25

 13/03/2022
2.5. Các cấu hình robot điển hình

Chương 2: Động lực học robot 26

 13/03/2022
2.5. Các cấu hình robot điển hình
5. Phép biến đổi Euler

Chương 2: Động lực học robot 27

 13/03/2022
2.6. Động học vị trí thuận Robot
2.6.1 Bài toán động học vị trí thuận
- Nhiệm vụ của bài toán thuận là khi cho trước các biến khớp
phải xác định vị trí và hướng của tất cả các khâu trên cánh tay,
thường chỉ xác định vị trí và hướng của khâu sau cùng.
- Thông số của các khâu và các khớp gồm 2 loại:
+Thông số không thay đổi (chiều dài khâu) gọi là tham số.
+ Thông số thay đổi (góc quay của khâu, lượng di chuyển dài
của khâu tịnh tiến) gọi là biến khớp.
-Trong kĩ thuật robot sử dụng phổ biến hai loại khớp là quay và
tịnh tiến

Chương 2: Động lực học robot 28

 13/03/2022
2.6. Động học vị trí thuận Robot
Quy tắc bàn tay phải
Trong kỹ thuật robot, chúng ta sử dụng quy tắc bàn tay phải để
xác định hướng các trục tọa độ như sau:

Quy tác bàn tay phải được sử dụng

để hoàn thành việc xác định hệ tọa
độ khi biết trước 2 trục tọa độ cần
tìm trục thứ 3

Chương 2: Động lực học robot 29

 13/03/2022
2.6. Động học vị trí thuận Robot
Quy tắc bàn tay phải để xác định chiều của góc quay quanh 1
trục tọa độ:
Để ngón tay cái chỉ theo chiều dương của trục xoay, khi đó các
ngón tay còn lại nằm lại tạo thành đường cong và chỉ chiều
dương của góc quay.
Với quy tắc này, chúng ta xác định được chiều dương của góc
quay nếu biết trước chiều dương của trục tọa độ. Ngược lại nếu
biết chiều dương của góc quay, chúng ta sẽ xác định được chiều
dương của trục tọa độ.

Chương 2: Động lực học robot 30

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 31

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 32

 13/03/2022
2.6. Động học vị trí thuận Robot

- Quy ước đặt tên các khâu:

+ Một tay máy robot có N khớp được đánh số từ 1 đến N
+ Trong đó có N+1 thanh nối (links) được đánh số từ 0 đến N
+ Khớp i (joint i) có chức năng liên kết thanh thứ i-1 với thanh thứ i
+ Thanh thứ i (link i) có chức năng kết nối khớp thứ i đến khớp i+1.
+Thanh số 0 (link 0) là đế của robot, thường được gắn cố định.
+ Thanh số N (link N) là thanh cuối cùng của robot, được gắn với
khâu chấp hành hoặc dụng cụ.

Chương 2: Động lực học robot 33

 13/03/2022
2.6. Động học vị trí thuận Robot
2.6.2 Nguyên lý Denavit-Hartenberg (D-H)
 Theo quy tắc DH các hệ tọa độ
được xác định theo quy ước
sau:
+ Trục zi đặt theo phương của
trục khớp i+1
+ Gốc khung tọa độ của thanh i là
giao điểm của pháp tuyến
chung của trục khớp i và i+1
với trục zi và nằm trên trục
khớp i+1.
+ Trục xi đặt theo phương pháp
tuyến chung của trục khớp i và
i+1 theo hướng từ trục i đến
i+1
Chương 2: Động lực học robot 34
 13/03/2022
2.6. Động học vị trí thuận Robot
Quy ước các góc và khoảng cách trên lược đồ như sau:

+ ai là khoảng cách giữa

Zi-1 và Zi dọc theo trục Xi
+ αi là góc quay quanh trục
Xi để Zi-1 đến trùng với Zi.
+ di là khoảng cách giữa
Xi-1 và Xi dọc theo trục Zi-1
+ θi là góc quay quanh
trục Zi-1 để Xi-1 đến trùng
với Xi

Chương 2: Động lực học robot 35

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 36

 13/03/2022
2.6. Động học vị trí thuận Robot
Một số trường hợp đặc biệt:
+ Khi 2 trục z cắt nhau: khi đó gốc của khung tọa độ là giao
điểm của hai trục và trục x được đặt dọc theo đường vuông
góc với mặt phẳng chứa 2 trục z đó.
+ Hai trục song song, khi đó chọn pháp tuyến chung trùng với
pháp tuyến chung của khớp trước, gốc khung tọa độ chọn sao
cho di là nhỏ nhất.
+ Đối với khớp tịnh tiến: khoảng cách di là biến khớp. Hướng
của trục khớp trùng với hướng di chuyển của khớp, đặt ai=0.
Gốc tọa độ đặt trùng với gốc thanh nối tiếp theo.

Chương 2: Động lực học robot 37

 13/03/2022
2.6. Động học vị trí thuận Robot

X3

O3

X1

Z3

O2 X2
Z1

Z2

X0

Z0

Chương 2: Động lực học robot 38

 13/03/2022
2.6. Động học vị trí thuận Robot

X1
Link 3
Joint 2
Link 2 X2
Link 1 Z3
Joint 3
Joint 1
Link 0 Z1
Z2

link ai
Z0
1 l1 0 0
2 l2 0 0
3 l3 0 0

Chương 2: Động lực học robot 39

 13/03/2022
2.6. Động học vị trí thuận Robot
Z1

d3

Z2 Z3
z0
X2
d2
X3

a1 X1

d1
x0



Chương 2: Động lực học robot 40

 2.6. Động học vị trí thuận Robot

d3


a2
d1



Chương
41 2: Động lực học robot
 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 42

 13/03/2022
2.6. Động học vị trí thuận Robot
2.6.3 Một số ví dụ ứng dụng quy tắc DH:
Ví dụ 1 : Robot 2 khâu phẳng

Hình 2. Kết cấu robot 2 thanh phẳng

Chương 2: Động lực học robot 43

 13/03/2022
2.3. Động học vị trí thuận Robot
 Lập bảng thông số D-H

Chương 2: Động lực học robot 44

 13/03/2022
2.3. Động học vị trí thuận Robot
2.6.3 Một số ví dụ ứng dụng quy tắc DH:
Ví dụ 2: Tay máy ba khâu phẳng:

Chương 2: Động lực học robot 45

 13/03/2022
2.6. Động học vị trí thuận Robot
 Lập bảng thông số D-H

ci  si 0 a i ci 
s ci 0 ai si 
Aii 1 (i )   i
0 0 1 0 
 
0 0 0 1 

c123  s123 0 a1c1  a2 c12  a3c123 

s c123 0 a1 s1  a2 s12  a3 s123 
T30 (q)  A10 A21 A32   123
 0 0 1 0 
 
 0 0 0 1 

c123  cos(1  2  3 )

Chương 2: Động lực học robot 46

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 47

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 48

 13/03/2022
2.6. Động học vị trí thuận Robot

Chương 2: Động lực học robot 49

 2.6. Động học vị trí thuận Robot
Joint 3
Y1 O3
d2
Joint 1
O0 O1 O2
Joint 2

a0 a1

Chương 2: Động lực học robot

 Z6
Z5 Z3
X6 Z4

X5 X3
X2
X4
Z2 Z1
i ai di i i
Z0
1 0 d1 90° 1 X1
2 0 d2 90° 2

3 0 d3 90° 90°
(var)
4 0 0 90° 4

5 0 d5 0° 5
6 0 d6 0 6
X0 Y0

Chương 2: Động lực học robot

 13/03/2022
Cấu hình và khung tọa độ robot Stanford

Chương 2: Động lực học robot 52

 thanh  d a 
1 1 0 0 -90

2 2 d2 0 90

3 0 d3 0 0

4 4 0 0 -90

5 5 0 0 90

6 6 0 0 0

Fig. 2.31 The frames of the Stanford Arm.

Chương 2: Động lực học robot
 Cấu hình và khung tọa độ robot Stanford

Chương
54 2: Động lực học robot
 Cấu hình và khung tọa độ robot Stanford

Chương
55 2: Động lực học robot
Robot Scara

Khâu i i Li Di

1  1* 0 L1 H

2  2* 1800 L2 0

3 0 0 0 d 3*

4  4* 0 0 d4
Chương 2: Động lực học robot
 Bài tập 1
Cho cơ cấu robot như hình vẽ
 Xây dựng khung toạ độ thanh nối theo Denavit-Hartenberg.
 Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
 Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, a2 =
40cm, 2=0o, a3 = 25cm, 3=30o,a4 = 35cm, 4=60o ?

Chương 2: Động lực học robot

 Bài tập 2
 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số
theo Denavit-Hartenberg.
 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
 3. Xác định vị trí và hướng tay khi :
 d1=30 cm, 1=60o, a1 = 40cm, d2=30cm, d3 = 40cm ?
d3

d2

a1
d1



Chương 2: Động lực học robot

 Bài tập 3
 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số
theo Denavit-Hartenberg.
 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
 3. Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, a2 =
40cm, 2=90o, d3 = 20 cm ?

d3


a2
d1


Chương 2: Động lực học robot
 Bài tập 4
 1. Xây dựng khung toạ độ thanh nối và lập bảng thông số
theo Denavit-Hartenberg.
 2. Tìm ma trận xác định hướng và vị trí điểm cuối của tay
Robot. Giải thích ý nghĩa của ma trận này ?
 3. Xác định vị trí và hướng tay khi : d1=30 cm, 1=60o, d2 =
40cm, 2=90o, d3 = 20 cm ?

Chương 2: Động lực học robot

 13/03/2022
2.7. Động học ngược Robot
2.7.1 Bài toán động học ngược của tay máy:
Bài toán thuận nhằm xác định vị trí và hướng của phần công tác khi cho
trước các biến khớp. Bài toán ngược cho trước vị trí và hướng của khâu tác
động sau cùng yêu cầu xác định các biến khớp tương ứng.
Bài toán động lực học ngược thông thường khó giải và không có lời giải
tổng quát cho mọi robot.

Chương 2: Động lực học robot 61

 13/03/2022
2.7. Động học ngược Robot
2.7.2 Động học ngược robot ba khâu phẳng:
Dựa trên kết quả đã triển khai ở bài toán thuận, ta đã có phương
trình động học của tay máy này dưới dạng ma trận đồng nhất (4.4):

Ma trận định vị và định hướng phần tác động sau cùng trên cánh tay
được cho trước trong bài toán ngược dưới dạng như sau:
 a11 a12 a13 a14 
a a 22 a 23 a 24 
A   21
 a31 a32 a33 a34 
 
a 41 a 42 a 43 a 44 

Chương 2: Động lực học robot 62

 13/03/2022
2.7.2 Động học ngược robot ba khâu phẳng:

A
o
s2   1  c2
2

Chương 2: Động lực học robot 63

 13/03/2022
2.7.2 Động học ngược robot ba khâu phẳng:

Chương 2: Động lực học robot 64

 13/03/2022
2.7.2 Động học ngược robot ba khâu phẳng:

Chương 2: Động lực học robot 65

 13/03/2022
2.7. Động học ngược Robot
2.7.3 Động học ngược robot 6 bậc tự do:

Phương trình động lực học thuận của robot có dạng:

Chương 2: Động lực học robot 66

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:
Nhân 2 vế của phương trình 2-38 với ma trận nghịch đảo A1:

Khi đó ta có phương trình sau:

Chương 2: Động lực học robot 67

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:

Chương 2: Động lực học robot 68

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:

Chương 2: Động lực học robot 69

 2.4.3 Động học ngược robot 6 bậc tự 13/03/2022
do:

Chương 2: Động lực học robot 70

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:
Tiếp tục nhân ma trận nghịch đảo của A1-A4 ta có phương trình:

Chương 2: Động lực học robot 71

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:

Chương 2: Động lực học robot 72

 2.7.3 Động học ngược robot 6 bậc tự 13/03/2022
do:

Chương 2: Động lực học robot 73