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:

x
0
z
0
z
1
x
1

đặt tại trục khớp 1
Hệ toạ độ O
1
x
1
y
1
z
1
có gốc O
1
đặt tại trục khớp 2
Hệ toạ độ O
2
x
2
y
2
z
2
có gốc O
2
đặt tại trục khớp 3
Hệ toạ độ O
3
x
3
y
3
z

0

l
1

0
2
θ
2

0
l
2
0
3
0
d
1
-d
3

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

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





1
1
T
2
2
T
3

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
T
dd

     
     
     

     

     

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

3
, 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à d
3

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
T
n o a p d d

   
   

   






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

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
  

p p l l p p l l
l l ll

  

     
  

- 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
c
pp

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



     




    








12
1
2
2
3
1 12
12
0
x
x
x
p
l s l s
p
ls
p
d




  


p
d























3
1
3
2
3

.
.
1

2
.
.
x
y
z
p
pJ
d
p














1 2 2
1 2 2

l l l l
p l c p l p l s p l s p l p l c
ddp



     




    







Yêu cầu tay robot chuyển động từ điểm A( 0,8; 1,2; 0,2 ) đến điểm B( 0;
1; 0,4)
Xây dựng được quỹ đạo đường thẳng đi qua hai điểm trên trong không gian:
0.8 1.2 0.2
0 0.8 1 1.2 0.4 0.2
0.8 1.2 0.2
0.8 0.2 0.2
x y z
t
x y z
t

zi



    





Mô phỏng bằng matlab:
for i=0:1:1000
l1 = 1.5; l2 = 2;
x = 0.8 - 0.0032*i; y = 1+0.001*i; z = 0.2+0.0002*i;
theta2=atan2(sqrt(1-[(x*x+y*y-l1*l1-l2*l2)/(2*l1*l2)]),(x*x+y*y-l1*l1-
l2*l2)/(2*l1*l2));
theta1=atan2((y*l2*cos(theta2)+y*l1-x*l2*sin(theta2)),
(y*l2*sin(theta2)+x*l1+x*l2*cos(theta2)));
d3 = 1- z;
% vẽ đồ thị:
plot3(theta1,theta2,d3);
hold on;
end
grid on;






→ Mô hình đối tượng:

Mô hình simulink:

Creat subsystem

Khối vị trí ban đầu:

Creat subsystem

2. Thiết kế bộ điều khiển vị trí dạng PI cho khớp
Cho: tọa độ điểm ban đầu: A(0.8;1.2;0,2)
tọa độ điểm cuối: B(0;1;0,4)
Sử dụng động học ngược vị trí :
l1 = 1.5; l2 = 2;
x = ; y =; z =;
theta2=atan2(sqrt(1-[(x*x+y*y-l1*l1-l2*l2)/(2*l1*l2)]),(x*x+y*y-l1*l1-
l2*l2)/(2*l1*l2));
theta1=atan2((y*l2*cos(theta2)+y*l1-x*l2*sin(theta2)),
(y*l2*sin(theta2)+x*l1+x*l2*cos(theta2)));
d3 = 1- z;
Ta xác định được
- Góc ban đầu:
10
20

d












 Mô hình bộ điều khiển:

Thuật toán điều khiển:
0
. ( )
t
dk p I
U K e K e d

KK
KK
KK









Kết quả mô phỏng:
- Qũy đạo khớp 1:

- Qũy đạo khớp 2:

- Qũy đạo khớp 3:

- Quỹ đạo tay robot trong khong gian làm việc: