Professional Documents
Culture Documents
Bai Tap Dai Robot Cong Nghiep Co Loi Giai Bai So 1
Bai Tap Dai Robot Cong Nghiep Co Loi Giai Bai So 1
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
z0 z1 z2
x2
x1
x0 z3
l1 l2 d3
d1
x3
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
(s) Ki
u(s) Ti s 1
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
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
c1 s1 0 l1c1 c 2 s 2 0 l2 c 2 1 0 0 0
s1 c1 0 l1s1 1 s2 c2 0 l2 s 2 0 1 0 0
0
T1 T2 2
T3
0 0 1 0 0 0 1 0 0 0 1 d1 d3
0 0 0 1 0 0 0 1 0 0 0 1
Phương trình động học thuận được xác định như sau:
0
T3=0T11T22T3
c1 s1 0 l1c1 c 2 s 2 0 l2 c 2 1 0 0 0
s1 c1 0 l1s1 s 2 c 2 0 l2 s 2 0 1 0 0
0
T3 . .
0 0 1 0 0 0 1 0 0 0 1 d1 d3
0 0 0 1 0 0 0 1 0 0 0 1
Với:
1 ;2 ; l1 1,5m; l2 2m; d1 1m; d3 0,5m
3 6
0 1 0 0, 75
2 0, 75 3
T3
0 1 0 0
0 0 1 0,5
0 0 0 1
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
c1 s1 0 l1 nx ox ax px
s1 c1 0 0 n y oy ay py 1
T1 . T3
0 1 0 . T3
0 0 1 0 nz oz az pz
0 0 0 1 0 0 0 1
Mặt khác:
c 2 s 2 0 l2c 2 1 0 0 c 2 s 2
0 0 l2 c 2
s2 c2 0 l2 s 2 0 1 0 0 s2 c2 0 l2 s 2
1
T3 T2 . T3
1 2 .
0 0 1 0 0 0 1 d1 d3 0 0 1 d1 d3
0 0 0 1 0 0 0 1 0 0 0 1
s1. px c1. p y l2 s 2
c1. px s1 p y l2c 2 l1
Bình phương 2 vế và cộng lại ta được phương trình sau:
px2 p y2 l12 l22 2l1l2c 2
3. Ma trận Jacoby
px l1c1 l2c12
Ta có : p y l1s1 l2 s12
p d1 d3
z
px p y d 3
l1s1 l2 s12 l1c1 l2c12 0
1 1
1
p p y d3
x l2 s12 l2c12 0
2 2 2
px p y d 3
0 0 1
d3 d3 d 3
p. x 1.
. .
p y J 2
. d .
pz 3
l1s1 l2 s12 l2 s12 0
J l1c1 l2c12 l2c12 0
0 0 1
II. Tính toán quỹ đạo các khớp
Khi cho tọa độ vị trí tay robot trong không gian làm việc (x,y,z) ta có thể
xác định được các biến khớp thông qua động học ngược vị trí:
px2 p y2 l12 l22 px2 p 2y l12 l22
2 atan2( 1- , )
2l1l2 2l1l2
1 atan2(p yl2c 2 p yl1 pxl2 s 2,p yl2 s 2 p xl1 p xl2c 2)
d3 d1 pz
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:
x 0.8 y 1.2 z 0.2
t
0 0.8 1 1.2 0.4 0.2
x 0.8 y 1.2 z 0.2
t
0.8 0.2 0.2
Hay :
x 0.8 0.8t
y 1.2 0.2t
z 0.2 0.2t
Với thời gian chuyển động trên quỹ đạo là t = 4s, gián đoạn hóa quỹ đạo
thành 1000 điểm, chu kỳ trích mẫu : T = 4/1000 = 4ms
x 0.8 0.0032i
y 1.2 0.0008i i 0 1000
z 0.2 0.0008i
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;
Kết quả :
U : 10 10V
(s) Ki
: 5 5rad / s
u(s) Ti s 1 T 0.01( s)
I
→ Mô hình đối tượng:
Mô hình simulink:
Creat subsystem
10 0.28
- Góc ban đầu: 20 2.06
d30 0.8
1d 0.01
1.9
- Góc đặt: 2d
d3d 0.6
Mô hình bộ điều khiển:
Xác định các tham số của bộ điều khiển sao cho không có quá điều chỉnh:
Ta chọn:
K P1 10 K I 1 0
K P 2 10 K I 2 0
K 15 K 0
P1 I3