Bài số 1:
Cho robot 3 DOF như hình vẽ:
Cho các thông số của robot như sau: d1 = 1m; l1 = 1,5 m ; l2 = 2m ; d3 = 0,5 m
θ1 = л/3 ; θ2 = л/6
1. Tính toán động học thuận, động học ngược vị trí, ma trận jacoby
2. Yêu cầu tay robot chuyển động theo quỹ đạo là 1 đường thẳng trong không
gian từ A(0.8;1.2;0,2) đến B(0;1;0,4) trong thòi gian 4s. Tính toán quỹ đạo các
khớp
3. Cho mô hình động học khớp robot:
(s)
u(s) 1
i
i
K
Ts
Thiết kế bộ điều khiển vị trí dạng PI cho khớp:
x0
z0
z1
x1
z2
z3
x2
x3
l1
d1
d3
Cho quỹ đạo đặt các khớp như tính toán tại phần 2. Vẽ quỹ đạo tay robot
trong không gian làm việc
Bài làm
I. Tính toán động học thuận, động học ngược vị trí và ma trận Jacoby
1. Động học thuận vị trí
a. Thiết lập hệ toạ độ
Hệ toạ độ cố định O0x0y0z0 đặt tại trục khớp 1
Hệ toạ độ O1x1y1z1 có gốc O1 đặt tại trục khớp 2
Hệ toạ độ O2x2y2z2 có gốc O2 đặt tại trục khớp 3
Hệ toạ độ O3x3y3z3 có gốc O3 đặt tại bàn tay robot.
Bốn trục z0, z1, z2, z3 cùng nằm trên mặt phẳng tờ giấy.
b. Bảng D-H
Căn cứ vào các thông số và hệ toạ độ đã được thiết lập ta có bảng thông số
DH của robot như sau:
i
θi
di
ai
αi
1
θ1
0
l1
0
2
θ2
0
l2
0
3
0
d1-d3
0
0
Phép biến đổi tổng quát từ hệ tọa độ (i-1) sang hệ tọa độ i:
1
0
0 0 0 1
i i i i i i i
i i i i i i i
i
i
i i i
c s c s s a c
s c c c s a s
Ts c d







Áp dụng vào robot 3DOF ta có:
1
1
0
1
1 1 0 1
1 1 0 1
0 0 1 0
0 0 0 1
c s l c
s c l s
T






2
2
1
2
2 2 0 2
2 2 0 2
0 0 1 0
0 0 0 1
c s l c
s c l s
T






2
3
13
1 0 0 0
0 1 0 0
0 0 1
0 0 0 1
Tdd






Phương trình động học thuận được xác định như sau:
0T3=0T1
1T2
2T3
12
12
0
3
13
1 1 0 1 2 2 0 2 1 0 0 0
1 1 0 1 2 2 0 2 0 1 0 0
..
0 0 1 0 0 0 1 0 0 0 1
0 0 0 1 0 0 0 1 0 0 0 1
c s l c c s l c
s c l s s c l s
Tdd

12
12
0
3
13
12 12 0 1 12
12 12 0 1 12
0 0 1
0 0 0 1
c s l c l c
s c l s l s
Tdd








Trong đó ta ký hiệu:
11
22
12
12
1 ; 1
2 ; 2
12 ( )
12 ( )
c c s s
c c s s
cc
ss








Với:
1 2 1 2 1 3
; ; 1,5 ; 2 ; 1 ; 0,5
36
l m l m d m d m


0
3
0 0,75
1 0 0 2 0,75 3
0 0 1 0,5
0 0 0 1
10
T






2. Động học ngược vị trí robot
Biết ma trận 0T3 , tức là biết vị trí và hướng của khung toạ độ tay robot, cần
xác định giá trị các biến khớp 1, 2 và d3
12
12
0
3
13
12 12 0 1 12
12 12 0 1 12
0 0 1
0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
n o a p c s l c l c
n o a p s c l s l s
Tn o a p d d


Theo phương pháp phân ly biến ta có:
0 1 0 1 2
1 3 2 3
..T T T T
Với
1
1
0
1
1 1 0 1
1 1 0 1
0 0 1 0
0 0 0 1
c s l c
s c l s
T






1
01
1
1 1 0
1 1 0 0
0 0 1 0
0 0 0 1
c s l
sc
T







1
0 1 0 1
1 3 3
1 1 0
1 1 0 0
..
0 0 1 0
0 0 0 1 0 0 0 1
x x x x
y y y y
z z z z
c s l n o a p
s c n o a p
T T T
n o a p
Mặt khác:
22
22
1 1 2
3 2 3
1 3 1 3
2 2 0 2 1 0 0 0 2 2 0 2
2 2 0 2 0 1 0 0 2 2 0 2
..
0 0 1 0 0 0 1 0 0 1
0 0 0 1 0 0 0 1 0 0 0 1
c s l c c s l c
s c l s s c l s
T T T d d d d


Cân bằng hệ số 2 ma trận :
- Xét thành phần ở hàng 1,2 cột 3:
2
21
1. 1. 2
1. 1 2
xy
xy
s p c p l s
c p s p l c l
Bình phương 2 vế và cộng lại ta được phương trình sau:
2 2 2 2
1 2 1 2
22
xy
p p l l l l c
2 2 2 2
12
12
2 2 2 2 2 2 2 2
1 2 1 2
2
1 2 1 2
22
atan2( 1- , )
22
xy
x y x y
p p l l
cll
p p l l p p l l
l l l l

- Cũng theo trên ta có :
2 1 2
22
2
2 1 2 1 2
22
22
1
. 1 . 1 2
. 1 . 1 2 2 2
1
y y x
x y x y
x y y x x
xy
p l c p l p l s
s
p s p c l s p p
p c p s l c l p l s p l p l c
cpp



1 2 1 2 2 1 2
atan2( 2 2; 2 2)
y y x y x x
p l c p l p l s p l s p l p l c
- Xét thành phần ở hàng 3 cột 4 ma trận 0T3:
1 3 3 1zz
p d d d d p
Vậy:
2 2 2 2 2 2 2 2
1 2 1 2
2
1 2 1 2
1 2 1 2 2 1 2
31
atan2( 1- , )
22
atan2( 2 2, 2 2)
x y x y
y y x y x x
z
p p l l p p l l
l l l l
p l c p l p l s p l s p l p l c
ddp


3. Ma trận Jacoby
Ta có :
12
12
13
1 12
1 12
x
y
z
pl c l c
p l s l s
dd
p







