Báo cáo môn học Robotics

BÁO CÁO ROBOTICS

2016

NỘI DUNG CẦN ĐẠT ĐƯỢC
Nội dung:
1. Giải bài toán động học.
+ Cho một quy luật chuyển động của các khâu của robot.
+ Tính vận tốc điểm tác động cuối E, vận tốc góc khâu thao tác, vận tốc góc
các khâu.
2. Tính toán tĩnh học.
+ Cho lực vào khâu thao tác tại điểm E gồm vectơ lực F và mômen
M(F=[Fx ,Fy ,Fz]T,M=[Mx ,M y,Mz]T) tính lực (mô men) dẫn động tại các khớp
đảm bảo robot cân bằng tĩnh(bỏ qua ma sát)
3. Tính toán động lực học.
Coi các khâu của robot là đồng chất, tiết diện ngang không đáng kể
+ Xác định các tham số động lực học của robot
+ Tính động năng thế năng của robot
4. Điều khiển và mô phỏng
Chọn luật điều khiển phù hợp, thiết kế hệ thống điều khiển

CHƯƠNG 1: BÀI TOÁN
Nguyễn Duy Khánh
1


BÁO CÁO ROBOTICS

2016

ĐỘNG HỌC VỊ TRÍ – ĐỘNG HỌC VI PHÂN

1.Mô hình robot, thông số động học và bảng DH

θi
1
θ1
2
- π/2
3
θ3
+ Ma trận truyền biến đổi DH :
cos(1 )
�
�
sin( )
0
A1  � 1
� 0
�
� 0

di

ai

αi

d1
d2
0

0
0
a3

π/2
π/2
0

0 sin(1 )
0�
0  cos(1 ) 0 �
�
1
0
d1 �
�
0
0
1�

Nguyễn Duy Khánh
2


BÁO CÁO ROBOTICS

�0
�
1
1

A2  �
�0
�
�0

2016

0 1 0 �
0 0 0�
�
1 0 d2 �
�
0 0 1�

cos(3 )  sin(3 )
�
�
sin( ) cos( 3 )
2
A3  � 3
� 0
0
�
0
� 0

0 a3 cos( 3 ) �
0 a3 sin( 3 ) �
�
�

1
0
�
0
1
�

sin(1 )  cos(1 ) sin(1 ).d 2 �
�0
�0  cos( )  sin( )  cos( ).d �
1
1
1
2�
0
A2  0 A1 .1 A2  �
�
�
1
0
0
d1
�
�
0
0
1
�0
�

sin(1 ) cos( 3 )  cos(1 ) sin(1 )[ a3 sin( 3 )  d 2 ] �
�sin(1 ) sin(3 )
�
cos(1 ) sin(3 )  cos(1 ) cos(3 )  sin(1 )  cos(1 )[ a3 sin(3 )  d 2 ]�
0
0
2
�
�
A3  A2 . A3 
�  cos(3 )
�
sin( 3 )
0
 a3 cos( 3 )  d1
�
�
0
0
0
1
�
�

2. Vị trí điểm thao tác và hướng của khâu thao tác
+ Ma trận dưới đây là ma trận cosin chỉ hướng xác định theo một trong các phép
quay Roll-Pitch-Yaw, Cardan hoặc Euler:
c11 ( ,  , ) c12 ( ,  , ) c13 ( ,  , )
�
�

c ( ,  , ) c22 ( ,  , ) c23 ( ,  , )
0
A3 (t )  �21
�
c31 ( ,  , ) c32 ( ,  , ) c33 ( ,  , )
�
0
0
� 0

Xe �
Ye �
�
Ze �
�
1�

Với XE, YE, ZE là vị trí điểm tác động cuối ; α, β, η biểu diễn hướng của khâu thao tác.

Phương trình động học nhận được:

Từ đó ta xác định được vị trí điểm tác động cuối và hướng của khâu thao tác là
nghiệm hệ phương trình:
Nguyễn Duy Khánh
3


BÁO CÁO ROBOTICS

2016

�xE  sin(1 )[ a3 sin( 3 )  d 2 ]
�y   cos( )[a sin( )  d ]
1
3
3
2
�E
�
�z E  a3 cos(3 )  d1  0
�
sin(  )   cos(1 )
�
�
 sin( ) sin(  )sin( )  cos( )cos( )  cos( q1 ) cos( q3 )  0
�
cos(  ) sin( )  sin(1 )cos( 3 )
�

3. Động học vi phân
3.1. Vận tốc điểm tác động cuối
xE � �sin(1 )[ a3 sin(3 )  d 2 ] �
�
�
�
0
rE  �
yE �
 cos(1 )[ a3 sin(3 )  d 2 ]�
� �

�
�
�
�
�

a
cos(

)

d
z
3
3
1
�
�E � �



0

v

E



d(

0

r

dt

E

)

0



�(

r

�
q

E

)

.q&

&
&�

�
cos(q1 )[a3 sin(3 )  d 2 ]&
1  sin(1 ) d 2  sin(1 ).a3 .cos( 3 )3
�
&
&�
0 vE  �
sin(1 )[a3 sin(3 )  d 2 ]&
1  cos(1 ) d 2  cos(1 ).a3 .cos( 3 ) 3 �
�
�
a3 .sin( 3 )&
3
�
�
3.2. Vận tốc góc các khâu
* Khâu thao tác:
0
+ Từ ma trận A3 , rút ra ma trận cosin chỉ hướng:

0

sin(1 ) cos(3 )  cos(1 ) �
�sin(1 ) sin(3 )
�
R3  �
 cos(1 )sin(3 )  cos(1 ) cos(3 )  sin(1 ) �
�
�
�


cos(

)
sin(

)
0
3
3
�
�

�sin(1 ) sin(3 ) cos(1 )sin( 3 )  cos( 3 ) �
�
�
R �
sin(1 ) cos( 3 )  cos(1 ) cos(3 ) sin( 3 ) �
�  cos( )
�
 sin(1 )
0
�
1
�
&
&
&
�
�

1 cos(1 )sin(3 )  3 sin(1 ) cos( 3 ) 1 sin(1 ) sin(3 )  &
&
3 cos(1 ) cos( 3 )
3 sin( 3 )
�&
�
0 &T
&
&
&
R3  �
1 cos(1 ) cos( 3 )  &
3 sin(1 ) sin( 3 ) 1 sin(1 ) cos( 3 )   3 cos(1 ) sin( 3 )  3 cos( 3 ) �
�
�
&
&
0
1 sin(1 )
1 cos(1 )
�
�
0

T
3

Nguyễn Duy Khánh
4

BÁO CÁO ROBOTICS

2016

0 &0
T
0 %
3  R3 . R3

� 0
&
1
�
0
&
%


0
3
� 1
�
&
&
1 sin  3
1 cos  3
�
0

�
&
1 sin  3
�
&
1 cos 3 �
�
0
�

cos(q3 )q&1 �
�
�
3  �
sin(q3 )q&1 �
�
�
&
�
q
� 1
�

*Khâu 1:
+ ma trận cosin chỉ hướng:

0

R1 

0

R&1 

0 &0
T
 0 %
1  R1 . R1 

0 1 
*Khâu 2:
+ Ma trận cosin chỉ hướng:

0

R2 

Nguyễn Duy Khánh
5


BÁO CÁO ROBOTICS

0

2016

R&2 

0 &0

T
0 %
2  R2 . R2 

0 2 

3.3 Gia tôc điểm tác động cuối

+

&
&�
�
cos(q1 )[a3 sin(3 )  d 2 ]&
1  sin(1 ) d 2  sin(1 ).a3 .cos( 3 ) 3
�
0
&
&�
vE  �
sin(1 )[a3 sin( 3 )  d 2 ]&
1  cos(1 ) d 2  cos(1 ).a3 .cos( 3 ) 3 �
�
�
a3 .sin(3 )&
3
�
�

� 0 aE  0 v&E

 Ta sẽ tính vào 1 trường hợp cụ thể sau:
Để khảo sát chuyển động của điểm E, ta cho chuyển động của các biến khớp:
q1  sin(2t )
�
�
q2  3t  0.5
�
�
q3  sin(5t )  2t
�

Khi đó:
�sin(sin(2t ))(0,5sin(sin(5t )  2t )  3t  0,5) �
rE  �
cos(sin(2t ))(0,5sin(sin(5t )  2t )  3t  0,5) �
�
�
�
�
0,5sin(sin(5
t

2
t
))

0.5
�
�

Nguyễn Duy Khánh
6


BÁO CÁO ROBOTICS

2016

Quỹ đạo điểm thao tác cuối:

Hình 3.1 Đồ thì điểm tác động cuối E (với t = 0 -> Pi/2)
�x&E  2cos(sin(2t))cos(2t)(0,5sin(sin(5t)+2t)+3t+0,5)+sin(sin(2t))(0,5cos(sin(5t)+2t).(5cos(5t)+2)+3)
�
�y&E  2sin(sin(2t))cos(2t)(0,5sin(sin(5t)+2t)+3t+0,5)  cos(sin(2t))(0,5cos(sin(5t)+2t)(5cos(5t)+2)+3)
�z&  0,5sin(sin(5t)+2t).(5cos(5t)+2)
�E
0

vE  x&2  y&2E  z&2E

�
3 x  2cos(2t)cos(sin(5t)+2t)
�
3 y  2cos(2t)sin(sin(5t)+2t)
�
�
3 z  5cos(5t)+2
�
0

E  3 x 2  3 y 2  3 z 2

Nguyễn Duy Khánh
7


BÁO CÁO ROBOTICS

Hình 3.2 Đồ thị vận tốc điểm cuối E theo t
theo t

2016

Hình 3.3 Đồ thị vận tốc góc khâu 3

4. Bài toán động học ngược
+ Cho vị trí, vận tốc, gia tốc điểm tác động cuối, hướng, vận tốc góc, gia tốc góc khâu thao tác. Tính tọa
độ các biến khớp.
+ Đặt: q1 = θ1, q2 = d1, q3 = θ3

4.1 Giải bằng phương pháp giải tích
Từ hệ phương trình:
�xE  sin(q1 )[a3 sin(q3 )  q2 ]
�
�yE   cos(q1 )[a3 sin(q3 )  q2 ]
�z  a cos(q )  d
3
3
1

�E

Từ phương trình (3):
�d  z �
q3  a cos � 1 E �
� a3 �

Kết hợp phương trình (1) và (2):
q1  a tan  xE yE 

Từ phương trình (1):
q2 

�
�d1  z E
xE
xE
 a3 sin(q3 ) 
 a3 sin �
a
cos
�
�
sin( q1)
sin(a tan( xE yE ))
� a3
�

�
�

�
�
�
�
�

4.2 Xây dựng quy luật chuyển động khâu thao tác E và giải động học ngược
bằng phương pháp số Newton-Raphson.
�f 1  xE  sin(q1 )[ a3 sin( q3 )  q2 ]  0
�
�f 2  yE  cos(q1 )[a3 sin(q3 )  q2 ]  0
�f 3  z  a cos(q )  d  0
E
3
3
1
�
�f1 �
�
F �
�f 2 � 0
�
�f 3 �
�

Bài toán khi biết được xE(t), yE(t), zE(t) tại mỗi thời điểm t ta sẽ tìm được vector
q=[q1, q2, q3]T tại mỗi thời điểm đó.
Ta lấy giá trị sát giá trị đầu để tiến hành quá trình lặp Newton-Raphson

Quá trình lặp dừng lại khi sai số ở lần k+1 với lần k nhỏ hơn giá trị cho phép.

Chương trình viết bằng phần mềm Maple
Nguyễn Duy Khánh
8


BÁO CÁO ROBOTICS

2016

Với quỹ đạo điểm tác động cuối là đường thẳng có phương trình như sau
�xE  t  1
�
�yE  0, 4t  0.3
�z  0,5t  0.1
�E

Đoạn chương trình viết bằng maple
>
>
>
>
>
>
>

>
>
>
Vẽ đồ thị trên Matlab dựa vào file data.txt .
%---------------------------------------------------------------------Nguyễn Duy Khánh

9


BÁO CÁO ROBOTICS

2016

fid = fopen('D:/data.txt', 'r');
a = fscanf(fid, '%g %g %g %g %g %g %g', [7 inf])
fclose(fid);
d1=a(1,:);%t
d2=a(2,:);%q1
d3=a(3,:);%q2
d4=a(4,:);%q3
d5=a(5,:);%x
d6=a(6,:);%y
d7=a(7,:);%z
figure(1);
plot(d1,d2,'c','LineWidth',2);xlabel('t');ylabel('q1(rad)');
legend('q1');
set(gcf,'Color',[1.0,1.0,1.0]);
grid on
figure(2);
plot(d1,d3,'c','LineWidth',2);xlabel('t');ylabel('q2(rad)')
legend('q2');
set(gcf,'Color',[1.0,1.0,1.0]);
grid on
figure(3);
plot(d1,d4,'c','LineWidth',2);xlabel('t');ylabel('q3(rad)');
legend('q3');

set(gcf,'Color',[1.0,1.0,1.0]);
grid on
figure(4);
plot3(d5,d6,d7,'c','LineWidth',2);
legend('quy dao diem tac dong cuoi E');
set(gcf,'Color',[1.0,1.0,1.0]);
grid on
%----------------------------------------------------------------------

Nguyễn Duy Khánh
10


BÁO CÁO ROBOTICS

2016

Đồ thị q1, q2 theo t

Đồ thị q và điểm tác động cuối được vẽ từ q1, q2, q3

Nguyễn Duy Khánh
11


BÁO CÁO ROBOTICS

2016

CHƯƠNG 2: TĨNH HỌC ROBOT

+ Dựa vào hệ phương trình đệ quy để tính lực trong các khớp:
0
0
0
�
�Fi ,i 1  Fi 1,i  Pi
�0
0
0 i 0
0 i 0
%
�M i ,i 1  M i 1,i  r%
i 1 Fi ,i 1  r ci Pi
+ Từ các ma trận truyền DH ta có các ma trận cosin chỉ hướng:

cos(q1 ) 0 sin(q1 ) �
�
0
R1  �
sin( q1 ) 0 cos( q1 ) �
�
�
�
�
0
1
0
�
�
0

0

sin(q1 ) cos(q1 ) �
�0
�
R2  �0 cos(q1 )  sin(q1 ) �
�
�
�

1
0
0
�
�
sin( q1 ) cos( q3 )  cos( q1 ) �
�sin( q1 )sin( q3 )
�
R3  �
cos(q1 ) sin(q3 )  cos( q1 ) cos(q3 )  sin(q1 ) �
�
�
�

cos(
q
)
sin(
q

)
0
3
3
�
�

+ Gọi c1, c2, c3 lần lượt là khoảng cách từ gốc tọa độ của khâu 1, 2, 3 đến khối tâm
của từng khâu 1, 2, 3.
*Tính toán khâu 3:

Nguyễn Duy Khánh
12


BÁO CÁO ROBOTICS

3 3

r

c3

 [  c3 , 0, 0]T

� r

r

c3

sin( q1 ) cos( q3 )  cos( q1 ) �
�sin(q1 ) sin(q3 )
�
T
 R 3. rc 3  �
cos( q1 ) sin( q3 )  cos( q1 ) cos( q3 )  sin( q1 ) �
[

c
,
0,
0]
3
�
�
�

cos(
q
)
sin(
q
)
0
3
3
�
�
0

0 3

0 3

2016

c3

3

sin(q1 )sin(q 3 )c3 �
�
�
 �cos(q1 )sin(q1 )c3 �
�
�
�
cos(q
)c
3 3
�
�

r 3  [  a 3 , 0, 0]T

3 3

sin( q1 ) cos( q3 )  cos( q1 ) �
�sin( q1 )sin( q3 )

0 3
r 2  R 3. 3 r 33  �
cos(q1 ) sin(q3 )  cos( q1 ) cos( q3 )  sin( q1 ) �
[  a , 0, 0]T
�
� 3
�
sin( q3 )
0 �
�  cos(q3 )
�
0

   sin(q1 )a 3sin(q3 )   �
�
�
�
0 3
r 2 �
   cos(q1)a 3sin(q 3 )  �
�
�
    a 3cos(q 3 )  
�
�
+ Các ma trận đối xứng lệch được thiết lập:

0
a 3cos(q 3 ) 
cos(q1)a 3sin(q3 ) �

�
�
�
r%  � a 3cos(q 3 )
0
sin(q1 )a 3sin(q3 ) �
�
�
cos(q1)a 3sin(q 3 )  sin(q1 )a 3sin(q 3 )
0
�
�

0 3
2

0
cos(q 3 )c3
cos(q1 )sin(q1 )c3  �
�
�
�
r%  � cos(q 3 )c3
0
sin(q1 )sin(q 3 )c3 �
�cos(q )sin(q )c   sin(q )sin(q )c
�
0
1
1 3

1
3
3
�
�
T
P 3  [ 0, 0, m3g]

0 3
c3

0
0
0
�
�F3,2  F4,3  P3
�0
0
0 30
0 3 0
%
�M 3,2  M 4,3  r%
2 F3,2  r c 3 P3

Mặt khác:
0
T
�
�F3  [  Fx ,  Fy ,  Fz ]
�0

T
�M 3  [  M x ,  M y ,  M z ]

Thay vào công thưc trên ta tính được lực momen khâu 3:

Nguyễn Duy Khánh
13


BÁO CÁO ROBOTICS

2016

�
�  Fx �
�0
�
�
� F32  �  Fy �
�
�
 Fz  m3 g �
�
�
�
�
�
M x +a 3cos(q3 )Fy  cos(q1 )a 3sin(q 3 )Fz +cos(q1 )sin(q1 )c3m 3g �
�
�

0
�M  M  a cos(q )F  sin(q )a sin(q )Fz+sin(q )sin(q )c m g �
3
3
x
1
3
3
1
3
3 3 �
� 32 � y
�
�
M
+cos(q
)a3sin(q
)F
+sin(q
)a
sin(q
)F
�
z
1
3
x
1
3
3

y
�
�
�
* Khâu 2:

sin(q1 )c 2 �
�
0
2 2
r c 2  [  c 2 , 0, 0]T � 0 r 2 c 2  R 2. 2 rc 2  � cos(q1 )c 2 �
�
�
�
�
� 0 
�
0 
� 0
�
r% � 0
0
�cos(q )c   sin(q )c
1 2
1 2
�

cos(q1 )c 2  �
�
sin(q1 )c 2 �

�
0
�

0 2
1

2

sin(q1 )q 2 �
�
�
r  [ q 2 , 0, 0] � r2  R 2. r  � cos(q1 )q 2 �
�
�
�
0 
�
�
2
1

T

0

0

2

0 
� 0
�
r% � 0
0
�cos(q )q   sin(q )q
1
2
1
2
�

0 2
1

0

P

2

2
1

cos(q1 )q 2  �
�
sin(q1 )q 2 �
�
0
�

 [ 0, 0, m2 g]T

0
0
0
�
�F2,1  F3,2  P2
�0
0
0 20
0 2 0
%
�M 2,1  M 3,2  r%
1 F2,1  r c 2 P2

Thay vào công thưc trên ta tính được lực momen khâu 2:

�
 Fx
�
�
�0
�
�
 Fy
�F21  �
�
�
�

�

F

m
g

m
g
3
2 �
� z
�
�
�M x +a 3cos(q3 )Fy +(  q 2  a 3sin(q3 ))cos(q1 )Fz+(q 2 m3g+sin(q 3 )c3 m3 g+c2 m2 g)cos(q1 ) �
�
�
�0 M  �
M y  a 3cos(q 3 )Fx +(  q 2  a 3sin(q 3 ))sin(q1 )Fz +(q 2 m 3g 3 +sin(q 3 )c 3 m3g+c2 m 2 g)sin(q1 ) �
21
�
�
�
�
M z +(a 3sin(q 3 )+q 2 )cos(q1 )Fx +(a 3sin(q 3 )+q 2 )sin(q1 )Fy
�
�
�
�
*Khâu 1:

Nguyễn Duy Khánh
14


BÁO CÁO ROBOTICS

2016

 0   �
�
0 1
r c1  [ 0, c1 , 0]T � 0 r1c1  R1.1r1c1  �
 0   �
�
�
�
 

c
 
� 1�
�
0

�0 c1   0 �
�
�
0 2
r%

c1 0 0 �
1 �
�0 0 0 �
�
�
0    �
�
0
1 1
r 0  [ 0, d1 , 0]T � 0 r 10  R1.1r10  �
0   �
�
�
�
d1 �
�
�
�0 d1   0 �
�
�
r% � d1 0 0 �
�0  0 0 �
�
�

0 2
1

1.
0

P

1

 [ 0, 0, m1g]T

0
0
0
�
�F10  F21  P1
�0
0
0 0
0
%0
�M 10  M 21  r%
1 F10  rc1 P1

Từ đó ta có tại khâu 1:

�
 Fx
�
�
�0
�
�
 Fy

�F10  �
�
�
�
�

F

m
g

m
g

m
g
3
2
1 �
� z
�
�
�  M x +(a 3C3  d1 )Fy +(  q 2  a 3 S3 )C1Fz +(q 2 m 3 +S3c3m 3 +c 2 m 2 )gC1 �
�
�
0
�M  �
 M y +(  a 3C3 +d1 )Fx +(  q 2  a 3Sq 3 )S1Fz +(q 2 m 3 +S3c 3m 3 +c 2m 2 )gSq1 �
10
�

�
�
�
 M z +(a 3S3 +q 2 )C1Fx +(a 3S3 +q 2 )sS1Fy
�
�
�
�

CHƯƠNG 3: ĐỘNG LỰC HỌC
1.Tính ma trận jacobi tịnh tiến và quay
Gọi lc1, lc2, lc3 lần lượt là khoảng cách từ gốc tọa độ O0, O1, O2 đến khối tâm của
từng khâu 1, 2, 3 và gọi

i

Cj

là tọa độ trọng tâm của khâu thứ j trên hệ Ri

Nguyễn Duy Khánh
15


BÁO CÁO ROBOTICS

2016

Các tọa độ và vận tốc góc các khâu:
� 0

�
�0 �
�
�
�0 �
0 0 0�
( d  l )
�
1
rc1  � 1 c1 � 0 rc1 0 A1.1 rc1  � �
�
0 0 0�
� 0
�
�
lc1 � J T 1  �
�
�
�
��
�
�
0
0
0
1
1
�
�
�

�=>
� �=>
sin(q1 )(lc 2  q 2 ) �
�
�0 �
�cos(q )(l  q ) �
�
�
l
1
c2
2 �
0
2
rc 2  0 A2 .2 rc 2  �
rc 2  � c 2 �
�
�
d1
�0 �
�
�
� �
1
�
�=>
�1 �=>
cos(q1 )(lc 2  q 2 ) sin(q1 ) 0 �
�
�

JT 2  �
sin(q1 )(lc 2  q 2 ) cos(q1 ) 0 �
�
�
0
0
0�
�
�
( a3  lc 3 ) �
�
�sin(q1 )(sin(q 3 )l c 3 +q 2 ) �
� 0
�
�
cos(q1 )(sin(q 3 )lc 3 +q 2 ) �
3
� 0 rc 3 0 A3 .3 rc 3  �
�
rc 3  �
� 0
�
� cos(q 3 )lc 3 +d1
�
�
�
�
�
1
� 1

�=>
�
�=>
JT 3

cos(q1 )(sin(q 3 )lc 3 +q 2 ) sin(q1 )
sin(q1 )cos(q 3 )lc 3 �
�
�
sin(q1 )(sin(q 3 )lc3 +q 2 ) cos(q1 ) cos(q1 )cos(q 3 )l c3 �
�
�
�
�
0
0
sin(q
)l
3 c3
�
�

0�
0 0 0�
�
�
�
�
�
%

R&  1  �
q&1 � J R1  �
1 0 0�
1  R
�
�
�
�
�
0
0
0
0
��
�
�
�q&1 �
�1 0 0�
�
�
2
0 T0&
2
%2  R2 R2  2  �0 � J R 2  �
0 0 0�
�
�
�
�
�

�
0
0
0
0
� �
�
�

1

0

T 0
1
1

1

�q&1cos(q3 ) �
�cos(q 3 ) 0 0 �
�
�
�
%3  R R&  3  �q&1 sin(q3 ) � J R3  �
sin(q3)
0
0
�
�

�
&
�
�
�
q
0
0
1
� 3 �
�
�

3

0 T 0
3
3

3

2.Tính tenxo quán tính các khâu của robot

Nguyễn Duy Khánh
16


BÁO CÁO ROBOTICS

2016

Ma trận tenxơ quán tính của hai khâu 1, 2 và 3 với trục gắn vào khối tâm song song với hệ trục của
khâu cũng tương ứng là hệ quán tính chính:

=
1

�
�
m.l 2 c1
    0       0 �
�
�12
�
� m.l 2 c1
�
0      
   0 �
�
12
�
�
0      0      0 �
�
�
�
�
�,



2

=

�
�
m.l 2 c 2
      0       0 �
�
� 12
�
� m.l 2 c 2
�
0      
   0 �
�
12
�
�
0      0      0
�
�
�
�
�
�,



3

=

�
�
�
�
0    0      0
�
�
2
m.l c 3
�
0      
   0 �
�
�
12
�
�
m.l 2c 3 �
�
0      0      
�
12 �

3.Tính động năng, thế năng của robot
3.1. Động năng robot

Ti 

0

1 T
1
v ci .mi . v ci  . i i T .I i i .i
2
2

Ac1  0 A1.1 Ac1 

cos(1 )
�
�
sin(1 )
�
� 0
�
� 0

� r � vc1  r& , v
0

1
c1

0

1

c1

1
�
0 sin(1 )
0 ��
0
�
0  cos(1 ) 0 �
�
.�
1
0
d1 ��
0
��
0
0
1�
0
�

0 0
1 0
0 1
0 0

0 �
0 �
�

lc1 �
�
2 �
1 �

T
c1

1
�
sin(1 )  cos(1 ) sin(1 ).d 2 ��
�0
0
�0  cos( )  sin( )  cos( ).d ��
1
1
1
2�
�
.�
�
��
1
0
0
d1
0
�
��
0

0
0
1
0
�0
Ac 2  0 A2 . 2 Ac 2  �
�

0 0
1 0
0 1
0 0

0 �
0 �
�
lc 2 �
�
2 �
1 �

2
T
� 0 rc 2 2 � vc 2  0 r&
c2 , v c2

sin(1 ) cos( 3 )  cos(1 ) sin(1 )[ a3 sin( 3 )  d 2 ] �
�sin(1 ) sin(3 )
�
cos(1 )sin( 3 )  cos(1 ) cos( 3 )  sin(1 )  cos(1 )[ a3 sin( 3 )  d 2 ]�

�
�
�  cos(3 )
�
sin(3 )
0
 a3 cos(3 )  d1
�
�
0
0
3
Ac 3  A3 . Ac 3  �
0
0
0
1
�.
1 0 0
0 �
�
�
0 1 0
0 �
�
�
�
lc 2 �
0 0 1
�

�
2 �
�
0 0 0
1 �
�
3
T
� 0 rc33 � vc3  0 r&
c3 , v c3

Nguyễn Duy Khánh
17


2016

BÁO CÁO ROBOTICS

+ Mặt khác theo trên ta đã tính được các

J T 1 , J T 2 , J T 3 , J R1 J R 2 , J R 3 , 1

,

 
2

,

3

ở

các phần trên. Ta thay vào công thức dưới đây và tìm ra kết quả động năng

Ti 

1 T
1
v ci .mi . v ci  . i i T .I i i .i
2
2

3.2. Thế năng robot
2

T 0

  �mi. g .
0

r

Ci

i 1

T
g


[0,
0,

g]
0
với
�   m1lc1g+m 2d1g+m3 (  cos(q 3 ).l c 3 +d1 )g

3.3 Tính lực suy rộng
+ Ta có lực suy rộng:
3

Q j  � J Tj Ti Fi  J Tj Ri M i 
n 1

J T 1   0

T

cos(q1 )(lc 2  q 2 ) sin(q1 ) 0 �
�
�
JT 2  �
sin(q1 )(lc 2  q 2 ) cos(q1 ) 0 �
�
�
�
0
0

0
�
�
cos(q1 )(lc 2  q 2 ) sin(q1 )(lc 2  q 2 ) 0 �
�
�
T
� J T 2  � sin(q1 )
cos(q1 )
0�
�
�
�
0
0
0
�
�

JT 3

cos(q1 )(sin(q 3 )lc 3 +q 2 ) sin(q1 )
sin(q1 )cos(q 3 )l c 3 �
�
�
�
sin(q1 )(sin(q 3 )l c 3 +q 2 ) cos(q1 ) cos(q1 )cos(q 3 )l c3 �
�
�
�

0
0
sin(q
)l
3 c3
�
�

�J

T
T3

cos(q1 )(sin(q3 )lc3 +q 2 ) sin(q1 )(sin(q 3 )lc 3 +q 2 )
0
�
�
�
�
�
sin(q1 )
cos(q1 )
0
�
�
�
sin(q
)cos(q
)l


cos(q
)cos(q
)l
sin(q
)l
1
3
c
3
1
3
c
3
3
c
3
�
�

Nguyễn Duy Khánh
18


BÁO CÁO ROBOTICS

0 0 0�
0 1
�
�
�

�
�
T
J R1  �
1 0 0 �� J R1  �
0 0
�
�
0 0 0�
0 0
�
�
�
1 0 0 �
1
�
�
�
�
�
T
J R 2  �0 0 0 �� J R 2  �0
�
�
�0 0 0 �
�
�0
J R3

2016

0�
0�
�
0�
�
0 0�
0 0�
�
0 0�
�

cos(q 3 ) 0 0 �
cos(q 3 ) sin(q3) 0 �
�
�
�
�
�
 �sin(q3) 0 0 �� J R 3  � 0
0
0�
�
�
�
�
�
0
0
1

0
0
1
�
�
�
�

Thay vào công thức tổng quát ta sẽ tìm được lực suy rộng.
3

Q j  � J Tj Ti Fi  J Tj Ri M i 
n 1

CHƯƠNG 4: ĐIỀU KHIỂN ROBOT
Tất cả các hệ thống điều khiển nêu dưới đây đều theo luật điều khiển PD. Khi
thiết kế hệ thống điều khiển ta bỏ qua động học của cơ cấu chấp hành, quán tính
động cơ. Như vậy chức năng của bộ điều khiển là tạo ra một moomen cần thiết để
truyền động khớp robot đảm bảo khớp robot luôn bám theo vị trí đặt.
4.1. Hệ thống điều khiển trong không gian khớp
Tín hiệu đặt đó là quỹ đạo bậc 3 của các biến khớp
4.1.1. Hệ thống điều khiển phản hồi
Luật điều khiển

Nguyễn Duy Khánh
19


BÁO CÁO ROBOTICS

2016

Hình5.1.Sơ đồ cấu trúc hệ thống điều khiển robot với bộ điều khiển
PD

& C (q, q&)q& G (q)  U  J ET F
M (q)q&

Ta có phương trình động lực học :
, gọi
&
&
&
V
(
q
,
q
)

C
(
q
,
q
)
q
H(q)=M(q)tránh nhầm với M là vector momen và
, M  U tiếp đến
ta coi robot không chịu tác dụng của ngoại lực vì luật điều khiển bám quỹ đạo

F  0 như vậy phương trình động lực học được rút gọn như sau:

& V ( q, q&)  G ( q)
M  H ( q).q&

Luật điều khiển :

M dk  K p (qd  q)  K d (q&d  q&)  K p   K d &

K  diag ( K , K ,..., K )

p1
p2
pn
Trong đó : p
- ma trận đường chéo các hệ số khuếch
đại của từng khớp riêng biệt.
K d  diag ( K d 1 , K d 2 ,..., K dn )
-ma trận đường chéo các hệ số

khuếch đại đạo hàm của từng khớp riêng biệt.
Với luật điều khiển này đã giả thiết thành phần momen trọng lực G(p) đã được
bù hoàn toàn.
Hệ thống điều khiển với cấu trúc bộ điều khiển như trên, ổn định tuyệt đối
toàn cục. Thực vậy chọn hàm Liapunov có dạng như sau:
1
T
V  .( T .Kp.  q&.H .q&)
2
Hàm VL biểu thị

tổng năng lượng của hệ thống robot: Thành phần chứa Kp tỷ lệ với năng lượng đầu
L

Nguyễn Duy Khánh
20


BÁO CÁO ROBOTICS

2016

vào, thành phần sau là động năng của robot mà Kp và H là các ma trận có hệ số
dương .Nên hàm VL> 0 với q khác qd.
Tính đạo hàm cấp 1 của VL ta nhận được:
1 T
T
T
&
&
&
&
.H .q& q&T .H&.q& q&T .H .q
)
V&  .(&.Kp.  .Kp.& q
2
L

T

.H .q&

Do tính chất đối xứng của các thành phần  .Kp. , q&
, ta rút gọn được
1 T
T
T
&
&
V&  &.Kp.  .q&.H&.q& q&.H .q
2
Từ phương trình động lực học với giả thiết không có thành phần momen trọng
lực G(q), nhận được phương trình sau :
1 T
T
T
)]
V&  &.Kp.  .q&.H&.q& q&.[M V (q,q&
2
Sử dụng thuộc tính của phương trình động lực học và áp dụng luật điều khiển
1 T
T
T
T
T 1
V&  q&.Kd .& q&.C (q, q&).q& .q&.H&(q).q& q&.Kd .& q&.( .H&C )q&
2
2
ta có
Trong đó: V (q, q&)  C (q, q&).q&.
T

L

L

L

1 &
1
.H  C
q&T .( .H& C )q& 0
Do ma trận 2
là ma trận đối xứng ngược => 2
T
.Kd .q&�0
&
&

q
V
L




Từ trên cho thấy rằng, mức độ dương của VL phụ thuộc vào Kp;

&

mức độ âm của V L phụ thuộc vào Kd . Do đó tăng tốc độ hội tụ bằng

tăng giá trị Kd. Nâng cao độ chính xác tinh của hệ thống điều khiển đạt
được bằng tăng hệ số Kp của khâu khuếch đại. Tuy nhiên ,Kp và Kd quá
lớn sẽ làm giảm độ ổn định và chất lượng quá trình quá độ như độ quá
điều chỉnh , thời gian quá độ tăng.
4.1.2. Hệ thống điều khiển momen tính toán
Luật điều khiển .

Nguyễn Duy Khánh
21


BÁO CÁO ROBOTICS

2016

Hình 5.2. Sơ đồ điều khiển
Phươngpháp cơ bản của luật điều khiển là lựa chọn luật điều khiển sao cho
khử được các thành phần phi tuyến của phương trình động lực học và phân li đặc
tính động lực của thanh nối. Kết quả sẽ nhận được một hệ thống tuyến tính đảm
bảo độ chính xác chuyển động yêu cầu.
Dựa trên phương trình động lực học :

& V (q, q&)  G ( q)
M  H (q).q&

 Phương trình mô tả Luật điều khiển có dạng như sau :
M dk  H ( q )U dk  V ( q, q&)  G (q )
Cân bằng hai phương trình trên và đựa trên tính chất H(q) là ma trận thực
dương nên có thể lấy nghịch đảo, ta nhận được phương trình vi phân tuyến tính cấp
& U dk

q&
hai như sau:
đây là phương trình vi phân tuyến tính cấp 2 độc lập giữa
các khớp. Do đó có thể thiết kế các bộ điều khiển độc lập có cấu chúc PD cho từng
khớp như sau :

&
&
U dk  q&
d  K p  Kd 

Từ đó ta rút ra được phương trình vi phân sai số vị trí của hệ thống kín có dạng
như sau :

& K d & K p   0
&
s2 I  Kd s  K p  0

Phương trình đặc tính ở dạng toán tử Laplace là :
Viết cho từng khớp riêng lẻ (đó là khâu quán tính bậc hai):
Nguyễn Duy Khánh

22


BÁO CÁO ROBOTICS

2016

s 2  K di s  K pi  0

Các hệ số Kdi, Kpi được chọn luôn dương nên đảm bảo ổn định, và chúng được
tính toán theo yêu cấu về chất lượng điều khiển như độ quá điều chỉnh σ , Thời
than quá độ Tqd
K di  2n K pi  n2
,
(5.7)
Để đạt được độ quá điều chỉnh  �20% , hệ số suy giảm ξ = 0,5-0,7 . Tần số
dao động được tính theo thời gian quá độ ( Tqd) và yêu cầu về hệ số suy giảm ξ:


n



4
 .T

qd

 100.e

 . n
2
1



4.2. Hệ thống điều khiển trong không gian làm việc
Trong hệ thống điều khiển không gian làm việc tín hiệu đặt trực tiếp là quỹ

đạo chuyển động mong muốn của tay máy robot trong không gian làm việc , lương
phản hồi về sẽ được tính từ vị trí của khớp thông qua khâu động học thuận. Khâu
tính toán động học ngược được đặt trong mạch vòng điều khiển phản hồi sẽ tính
đổi các biến về không gian khớp.
Hệ thống điều khiển trong không gian làm việc sử dụng hiệu quả khi thực hiện
tương tác giữa tay máy và môi trường
Hệ thống điều khiển ma trận Jacobien chuyển vị
Luật điều khiển
Lực cần thiết để di chuyển tay máy theo quỹ đạo đặt trong không gian làm
việc được xác định từ sai lệch vị trí và sai lệch tốc độ trong không gian làm việc
tương ứng với luật điều khiển phản hồi PD đinh điển:

Gdk  K p ( Sd  S )  K d ( S&d  S&)

Trong đó: Gdk - là vector lực cần thiết để tay robot di chuyển theo quỹ đạo và
tốc độ đặt trước.

Sd , S

: tương ứng là vector vị trí đặt và vector vị trí thực của tay robot.

S&d , S&

: tương ứng là vector tốc độ đặt và vector tốc độ thực

K p  diag ( K p1 , K p 2 ,..., K pn )

K d  diag ( K d 1 , K d 2 ,..., K dn )

- ma trận đường chéo các hệ số khuếch đại

-ma trận đường chéo các hệ số khuếch đại đạo hàm .

Nguyễn Duy Khánh
23


BÁO CÁO ROBOTICS

2016

Vector lực ở tay robot được biến đổi về lực hoặc momen khớp robot thông qua
ma trận Jacobien chuyển vị. Như vậy vector momen truyền động khớp robot được
xác định như sau:
M dk  J T �
K p ( S d  S )  K d ( S&d  S&) �
�
�
Sơ đồ khối của hệ thống như sau :

Hình 5.3. Sơ đồ điều khiển

Nguyễn Duy Khánh
24


BÁO CÁO ROBOTICS

2016

TÀI LIỆU THAM KHẢO

[1] Bài giảng Robotics - PGS. TS. Phan Bùi Khôi.
[2] Động lực học hệ nhiều vật - GS. TSKH. Nguyễn Văn Khang.
[3] Cơ sở Robot công nghiệp -GS.TSKH Nguyễn Văn Khang .

Nguyễn Duy Khánh
25