Tính toán thiết kế Robot sơn tường (thầy Phan Bùi Khôi)

Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

LỜI NÓI ĐẦU
Cơ điện tử đang trở thành một trong những ngành mũi nhọn trong những năm gần đây. Việc
phát triển và mở rộng quy mô đào tạo cơ điện tử đang dần cung cấp nguồn nhân lực dồi dào,
nâng cao chất lượng tự động hóa các quá trình sản xuất. Mà đây lại là một đòi hỏi vô cùng quan
trọng của sự nghiệp CNH-HDH mà chúng ta đang hướng tới.
Xuất phát từ yêu cầu đó, các tay máy Robot ra đời để đảm bảo sự tự động hóa trong quá trình
chế tạo, làm việc. Hơn thế, Robot ra đời còn giúp con người làm việc trong các môi trường độc
hại mà con người không thể tiếp xúc trực tiếp được.
Trong khuôn khổ học phần Tính toán thiết kế Robot, sau khi được trang bị các kiến thức cụ thể
về việc thiết kế một Robot hoàn chỉnh. Đây là cơ sở để tính toán, thiết kế cũng như điều khiển
các loại Robot trong công nghiệp phục vụ sản xuất sau này.
Cụ thể nhóm quyết định lựa chọn đề tài Tính toán thiết kế Robot ứng dụng trong sơn tường.

Tính toán thiết kế robot
Nội dung:
Đề xuất dự án và thực hiện tính toán thiết kế mô hình robot ứng dụng trong sơn tường.
1. Phân tích và lựa chọn cấu trúc
1.1. Phân tích mục đích ứng dụng của robot
1.2. Phân tích yêu cầu kỹ thuật thao tác
a. Đối tượng thao tác, dạng thao tác
b. Phân tích yêu cầu về vị trí
c. Yêu cầu về hướng của khâu thao tác
d. Yêu cầu về vận tốc, gia tốc khi thao tác
e. Yêu cầu về không gian thao tác
1.3. Xác định các đặc trưng kỹ thuật
a. Số bậc tự do cần thiết
b. Vùng làm việc có thể với tới của robot

c. Yêu cầu về tải trọng


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

1.4. Các phương án thiết kế cấu trúc robot, cấu trúc các khâu khớp, phân tích lựa chọn phương
án thực hiện
1.5. Thông số kỹ thuật: robot thiết kế, đối tượng và hệ thống thao tác
2. Thiết kế 3D mô hình robot
2.1. Thiết kế 3D
2.2. Lập bản vẽ 2D
2.3. Lập hồ sơ kỹ thuật
2.4 Xác định các thông số đặc trưng hình học-khối lượng
3. Thiết kế quỹ đạo chuyển động
3.1. Khảo sát động học thuận, khảo sát động học ngược
3.2. Thiết kế quỹ đạo chuyển động của robot theo mục đích ứng dụng
4. Phân tích trạng thái (tĩnh) yêu cầu lực/momen động cơ lớn nhất
5. Tính toán động lực học
5.1. Thiết lập phương trình vi phân chuyển động của robot bằng các phương pháp đã học
5.2. Tính động lực học ngược theo quy luật chuyển động được khảo sát ở câu 3.c
6. Thiết kế hệ dẫn động robot
6.1. Thiết kế hệ dẫn động (cho một khớp)
6.2. Chọn động cơ phù hợp
6.3. Tính chọn hộp giảm tốc
6.4. Thiết kế 3D và kiểm nghiệm bền các khâu của robot
7. Thiết kế hệ thống điều khiển
7.1. Chọn luật điều khiển phù hợp, thiết kế mô hình điều khiển
7.2. Mô phỏng bằng Matlab

8. Hiệu chỉnh thiết kế


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

1. Phân tích và lựa chọn cấu trúc
1.1. Phân tích mục đích ứng dụng robot
Robot sơn tường có nhiều ứng dụng trong đời sống, mục đích của robot là sơn tường theo
những hình dạng phức tạp, hình dạng đòi hỏi phức tạp có độ chính xác cao .

Hình 1. Minh họa kết quả làm việc của robot
1.2. Phân tích yêu cầu kỹ thuật thao tác
a. Đối tượng thao tác, dạng thao tác
Nhiệm vụ của robot là sơn tường và vẽ hình trên tường ( tường có 1000 x 1000 mm ) do
đó :
-

Đối tượng thao tác là bề mặt tường phẳng.
Dạng thao tác chuyển động được theo các cung cong và thằng.

b. Phân tích yêu cầu về vị trí.
Phải di chuyển được mọi vị trí trên bề mặt tường để phun sơn được toàn bộ mặt phẳng.
c. Yêu cầu về hướng: Đối tượng là mặt phẳng nên hướng thao tác là hướng vuông góc với
bề mặt của tường do đó hướng cố định và không thay đổi trong quá trình làm việc


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Về vận tốc thì Robot phải đáp ứng phun sơn với vận tốc ổn định để quá trình làm việc
không bị gián đoạn. Không có yêu cầu về gia tốc.
1.3. Xác định các đặc trưng kỹ thuật
a. Số bậc tự do cần thiết
Do quỹ đạo sơn bao gồm đường cong và đường thẳng trong mặt phẳng, để bảo đảm vị trí chính
xác của khâu thao tác, robot cần tối thiểu 2 bậc tự do. Nếu robot có nhiều hơn 2 bậc tự do (3,4,5,
hoặc 6 bậc tự do), chuyển động của robot sẽ linh hoạt hơn. Tuy nhiên việc có quá nhiều bậc tự
do sẽ khiến việc tính toán trở nên khó khăn, không thực sự cần thiết với yêu cầu đặt ra. Do đó,
nhóm đã quyết định lựa chọn số bậc tự do cần thiết cho robot là 2.
b. Vùng làm việc có thể với tới của robot
c. Yêu cầu về tải trọng
1.4. Các phương án thiết kế cấu trúc robot

Hình 2: Phương án thiết kế robot 2 bậc tự do
Để đảm bảo yêu cầu về các dạng thao tác và không gian thao tác thuận tiện thì phương án
robot 2 bậc tự do vừa đơn giản trong quá trình tính toán vừa dễ dàng trong quá trình thiết kế và
chế tạo.
1.5. Thông số kỹ thuật: robot thiết kế, đối tượng và hệ thống thao tác


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

*

Hình 3: Mô hình hóa robot
-

Bảng thông số các khâu

Khâu Chiều dài khâu (mm) Khối lượng(kg)
1
1200
15
2
250
5
3
1200
10
- Bảng thông số góc quay, tốc độ các khớp
Khớ Giới hạn chuyển động
p
1
0-1000 mm
2
0-1000 mm
- Lập bảng D-H .
Khâu
1
2

Tốc độ

a
0

0

0
0


Tính toán thiết kế robot

-

PGS.TS Phan Bùi Khôi

Tính các ma trận D-H :

�cos   / 2 
�
 sin   / 2 
0
T1  �
� 0
�
� 0
�cos   / 2 
�
 sin   / 2 
�
� 0
�
� 0

1
�
�
0
1
T2  �
�
0
�
0
�

sin   / 2 

cos   / 2 
0
0

sin   / 2 

cos   / 2 
0
0

0 0
1 0

0 ��
1
�

0�
0
��
0 1 q1 ��
0
��
0 0 1 ��
0

0 0 a�
1 0 0�
�
0 1 0�
�
0 0 1�

a cos   / 2  �
�
0 a sin   / 2  �
�
1
q1
�
0
1
�
0

0 0 q2 �
1 0 0�

�
0 1 0�
�
0 0 1�

�cos   / 2 
�
 sin   / 2 
0
0 1
� T2  T1 T2  �
� 0
�
� 0
�cos   / 2  sin   / 2 
�
 sin   / 2  cos   / 2 
�
� 0
0
�
0
� 0

-

1
0 0 ��
��
0

0 0 ��
0
1 0 ��
��
0
0 1 ��

sin   / 2 

cos   / 2 
0
0

1
a cos   / 2  ��
��
0
0  a sin   / 2  ��
��
0
1
q1
��
0
0
1
��

0

0 0 q2 �
1 0 0�
�
0 1 0�
�
0 0 1�

q2 cos   / 2   a cos   / 2  �
�
0  q2 sin   / 2   a sin   / 2  �
�
1
q1
�
0
1
�
0

Tìm vị trí điểm thao tác biểu diễn theo các tọa độ khớp. Xác định hướng của khâu thao tác


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Ta có :

�cos   / 2 
�

 sin   / 2 
0
T2  �
� 0
�
� 0


sin   / 2 
cos   / 2 
0
0

0 q2 cos   / 2   a cos   / 2  �
�
0 q2 sin   / 2   a sin   / 2  �
�
1
q1
�
0
1
�

Bài toán động học thuận robot

Nhiệm vụ của bài toán động học thuận là xác định vị trí của khâu thao tác, hay nói cách
khác là vị trí điểm tác động cuối và hướng của khâu thao tác đối với hệ tọa độ cố định với
điều kiện các biến khớp đã biết. Ở đây ta sẽ xác định từ ma trận DH của khâu thao tác .
1. Thiết lập phương trình động học

Phương trình động học robot có dạng ma trận sau:

T 2 (q )  0T 2 ( p )

0

Trong đó:

�cos   / 2 
�
 sin   / 2 
T20  �
� 0
�
� 0
Thay số ta được:

sin   / 2 

cos   / 2 
0
0

q2 cos   / 2   a cos   / 2  �
�
0 q2 sin   / 2   a sin   / 2  �
�
1
q1
�

0
1
�
0


Tính toán thiết kế robot

�0
�
1
0
T2  �
�0
�
�0

1
0
0
0

PGS.TS Phan Bùi Khôi

0
0
�
0 q2  250 �
�
�

1
q1
�
0
1
�(1)

Ma trận (1) là ma trận cosin chỉ hướng xác định theo một trong các phép quay Roll-PitchYaw, Cardan hoặc Euler.

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

xE �
yE �
�
zE �
�
1 �(2)

Phương trình (2) chính là phương trình động học dạng ma trận của robot. Nó bao gồm 16

phương trình vô hướng, trong đó có 4 phương trình tầm thường. Các ma trận góc trên
bên phải cho 3 phương trình độc lập, biểu diễn về vị trí của điểm thao tác thuộc khâu thao
tác. Các ma trận ở góc trên bên trái cho 9 phương trình, mô tả về hướng của khâu thao
tác. Tuy nhiên do các điều kiện trực dao nên trong 9 phương trình đó chỉ có 3 phương
trình độc lập.

-

Bài toán động học thuận về vị trí của robot

Như ta đã biết để xác định hướng và vị trí của vật rắn ta có thể sử dụng rất nhiều phương
pháp như: các góc Euler, các góc Cardan, các góc RPY.
Như trên đã trình bài ta có thể xác định ma trận biểu diễn vị trí và hướng của điểm tác
động cuối bằng phương pháp Denavit-Hartenberg như sau:
Trong trường hợp này ta dung các góc Cardan để biểu diễn hướng của khâu cuối robot ta
có phương trình động học dạng ma trận:


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

cos cos
-cos sin
sin
�
�sin sin cos +cos sin -sin sin sin +cos cos -sin cos
�
�
-cos sin cos +sin sin cos sin sin +sin cos cos cos

�
0
0
0
�
Ta có phương trình động học thuận về vị trí:

� XE  0
�
YE  q2  250
�
� Z q
E
1
�
Hay:

r0 p

�xE � � 0
�
� �
�
�
y

q

250
�E � � 2

�
�
�
�z E �
� �
� q1
�

=>

Hướng của khâu thao tác:

Sử dụng ma trận Cardan để xác định hướng của khâu thao tác ta có:
0

R 3  R Cd

Tức là :

XE �
YE �
�
ZE �
�
1 �


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

cos cosη
-cos sin
sin �
�
�
 �
sin

sin

cos

+cos

sin

-sin

sin

sin

+cos

cos

-sin

cos


�
�
�
-cos sin cos +sin sin cos sin sin +sin cos cos cos �
�
�

�0 1 0 �
�(3)
�

1
0
0
�
�
�
�0 0 1 �
�
Do đó ta có :




�
= 2

 = 0 hoặc 


�
2



=



Vận tốc điểm tác động cuối E:

0 �
�
���
d
vE  rE  �
 q2 �
dt
�� �
�
q1 �
�
�
 Vận tốc góc các khâu:


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Vì do các khâu là khâu tịnh tiến

� 1  0; 2  0
-

Tính gia tốc điểm tác động cuối. Tính gia tốc góc khâu thao tác:

 Gia tốc điểm tác động cuối E:

�0 �
� gg �
g
d
a E  vE  vE  �
 q2 �
dt
�gg �
�q �
�1 �
 Gia tốc góc các khâu:
Vì do các khâu là khâu tịnh tiến

� 1  0;  2  0
-

Bài toán động học ngược về vị trí của robot

� XE  0
q2  YE  250
�

�
YE   q2  250 � �
�
q1  Z E
�
� Z q
E
1
�
. Thiết kế quỹ đạo trong không gian thao tác
Trong trường hợp chuyển động liên tục trên đường dịch chuyển, kỹ thuật thiết kế quỹ
đạo trong không gian thao tác hay được sử dụng.
Đối với Robot này, ta chọn bài toán thiết kế quỹ đạo trong không gian thao tác sao
cho quỹ đạo của điểm tác động cuối là đường thẳng từ điểm A đến điểm B (bất kì trong


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

vùng làm việc) do khi sơn ta cần vận tốc phải giữ ổn định trong suốt quá trình nên ta áp
dụng quy luật vận tốc hình thang.
Quy luật vận tốc hình thang gồm có 3 giai đoạn:
-

Giai đoạn I: Vận tốc tăng từ 0 đến giá trị ổn định làm việc, gia tốc a1=a0.

-

Giai đoạn II: Vận tốc không đổi trong quá trình thao tác, a2=0.

-

Giai đoạn III: Vận tốc giảm dần về 0, gia tốc a3=ae.

>> A=[ 0 1000 0];
B=[1000 1000 0];
C=[1000 800 0];
D=[0 800 0];
E=[0 600 0];
F=[1000 600 0];


Tính toán thiết kế robot

G=[1000 400 0];
H=[0 400 0];
I=[0 200 0];
K=[1000 200 0];
L=[1000 0 0];
O=[ 0 0 0];
pts1=[A;B];
pts2=[B;C];
pts3=[C;D];
pts5=[E;F];
pts6=[F;G];
pts7=[G;H];
pts8=[H;I];
pts9=[I;K];
pts10=[K;L];

pts11=[L;O];
line(pts1(:,1), pts1(:,2), pts1(:,3));
line(pts2(:,1), pts2(:,2), pts2(:,3));
line(pts3(:,1), pts3(:,2), pts3(:,3));
line(pts4(:,1), pts4(:,2), pts4(:,3));
line(pts5(:,1), pts5(:,2), pts5(:,3));
line(pts6(:,1), pts6(:,2), pts6(:,3));
line(pts7(:,1), pts7(:,2), pts7(:,3));
line(pts8(:,1), pts8(:,2), pts8(:,3));
line(pts9(:,1), pts9(:,2), pts9(:,3));
line(pts10(:,1), pts10(:,2), pts10(:,3));
line(pts11(:,1), pts11(:,2), pts11(:,3));

PGS.TS Phan Bùi Khôi


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

plot3(pts1(:,1), pts1(:,2), pts1(:,3), 'r','linewidth',3);hold on;
plot3(pts2(:,1), pts2(:,2), pts2(:,3), 'r','linewidth',3);hold on;
plot3(pts3(:,1), pts3(:,2), pts3(:,3), 'r','linewidth',3);hold on;
plot3(pts4(:,1), pts4(:,2), pts4(:,3), 'r','linewidth',3);hold on;
plot3(pts5(:,1), pts5(:,2), pts5(:,3), 'r','linewidth',3);hold on;
plot3(pts6(:,1), pts6(:,2), pts6(:,3), 'r','linewidth',3);hold on;
plot3(pts7(:,1), pts7(:,2), pts7(:,3), 'r','linewidth',3);hold on;
plot3(pts8(:,1), pts8(:,2), pts8(:,3), 'r','linewidth',3);hold on;
plot3(pts9(:,1), pts9(:,2), pts9(:,3), 'r','linewidth',3);hold on;
plot3(pts10(:,1), pts10(:,2), pts10(:,3), 'r','linewidth',3);hold on;

plot3(pts11(:,1), pts11(:,2), pts11(:,3), 'r','linewidth',3);hold on;
xlabel('x(mm)','fontsize',10)
ylabel('y(mm)','fontsize',10)
zlabel('z(mm)','fontsize',10)
grid on
axis equal

 Đồ thị vận tốc :
Quỹ đạo của điểm tác động cuối theo đường thẳng từ A đến B trong te=4s

X A   0 1000 0 ;
�

X A   0 0 0 ;

X B   1000 1000 0 ;
�

X B   0 0 0 ;

t A  0( s)

t1  0.2( s)


Tính toán thiết kế robot

t B  4( s )
a0 

PGS.TS Phan Bùi Khôi

t2  3.8( s)

2( Se  S0 )
 1316(mm / s 2 )
(t1  t0 )(te  t0  t1  t2 )

ae 

a0 (t1  t0 )
 1316( mm / s 2 )
t 2  te

�a0 � a0 �
2
1 �
 t1  t0   a0  te  t0   t1  t0   S0  0
� �
�2 � ae �
�
v(t )  a0 (t1  t0 )  ae (t  t2 )
�
�
a (t )  a
�
�
�
t=(0:0.02:4);
a_0=1316;

a_e=-1316;
t_1=0.2;
t_2=3.8;
t_0=0;
t_e=4;
s_0=1000;
t1=(0:0.02:0.2);
s1=s_0+a_0/2*t1.^2;
v1=a_0.*t1;
a1=a_0+0*t1.^2;


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

plot(t1, s1, 'r-', 'LineWidth', 2); hold on;
plot(t1, v1, 'g-', 'LineWidth', 2);hold on;
plot(t1, a1, 'b-', 'LineWidth', 2);hold on;

t2=(0.2:0.02:3.8);
s2=s_0+a_0/2*t_1^2+a_0.*t_1*(t2-t_1);
v2=a_0.*t_1+0.*(t2-t_1);
a2=0+0*t2.^2;
plot(t2, s2, 'r-', 'LineWidth', 2); hold on;
plot(t2, v2, 'g-', 'LineWidth', 2); hold on;
plot(t2, a2, 'b-', 'LineWidth', 2); hold on;
t3=(3.8:0.02:4);
s3=s_0+a_0/2*t_1^2+a_0.*t_1*(t_2-t_1)+a_e/2*(t3-t_2).^2+(a_0.*t_1).*(t3-t_2);
v3=a_0.*t_1+a_e*(t3-t_2);

a3=a_e+0*t3.^2;
plot(t3, s3, 'r-', 'LineWidth', 2);
plot(t3, v3, 'g-', 'LineWidth', 2);
plot(t3, a3, 'b-', 'LineWidth', 2);
grid on;

xlabel('t [s]');
ylabel('y[mm]')
legend('x [mm]', 'v mm/s', 'a [mm/s^2]');


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Đồ thị quỹ đạo, vận tốc, gia tốc AB

-

Quỹ đạo của điểm tác động cuối theo đường thẳng từ B đến C trong te=0.8s

X B   1000 1000 0 ;
�

X B   0 0 0 ;

X C   1000 800 0 ;
�

X C   0 0 0 ;

t B  4( s )

t1  4.2( s )


Tính toán thiết kế robot

tC  4.8( s)

PGS.TS Phan Bùi Khôi

t2  4.6( s)

a0 

2( Se  S0 )
 384.6( mm / s 2 )
(t1  t0 )(te  t0  t1  t2 )

ae 

a0 (t1  t0 )
 384.6(mm / s 2 )
t 2  te

�a0 � a0 �
2
1 �
 t1  t0   a0  te  t0   t1  t0   S0  0

� �
�2 � ae �
�
v(t )  a0 (t1  t0 )  ae (t  t2 )
�
�
a (t )  a
�
�
�
t=(4:0.02:4.8);
a_0=-384.6;
a_e=384.6;
t_1=4.2;
t_2=4.6;
t_0=4;
t_e=4.8;
s_0=1000;

t1=(4:0.02:4.2);


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

s1=s_0+a_0/2*t1.^2;
v1=a_0.*t1;
a1=a_0+0*t1.^2;
plot(t1, s1, 'r-', 'LineWidth', 2); hold on;

plot(t1, v1, 'g-', 'LineWidth', 2);hold on;
plot(t1, a1, 'b-', 'LineWidth', 2);hold on;

t2=(4.2:0.02:4.6);
s2=s_0+a_0/2*t_1^2+a_0.*t_1*(t2-t_1);
v2=a_0.*t_1+0.*(t2-t_1);
a2=0+0*t2.^2;
plot(t2, s2, 'r-', 'LineWidth', 2); hold on;
plot(t2, v2, 'g-', 'LineWidth', 2); hold on;
plot(t2, a2, 'b-', 'LineWidth', 2); hold on;
t3=(4.6:0.02:4.8);
s3=s_0+a_0/2*t_1^2+a_0.*t_1*(t_2-t_1)+a_e/2*(t3-t_2).^2+(a_0.*t_1).*(t3-t_2);
v3=a_0.*t_1+a_e*(t3-t_2);
a3=a_e+0*t3.^2;
plot(t3, s3, 'r-', 'LineWidth', 2);
plot(t3, v3, 'g-', 'LineWidth', 2);
plot(t3, a3, 'b-', 'LineWidth', 2);
grid on;

xlabel('t [s]');


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

ylabel('y[mm]')
legend('x [mm]', 'v mm/s', 'a [mm/s^2]');

KHẢO SÁT TĨNH HỌC

Bài toán đặt ra là: Giả thiết ngoại lực tác động vào khâu thao tác tại điểm E gồm véc tơ
lực FE. Tính lực (và mô men) tác động vào các khớp để đảm bảo robot cân bằng tĩnh.
Ta có hệ phương trình cân bằng lực trong hệ tọa độ cơ sở:
0
�
Fi ,i 1  0 Fi 1,i  0 Pi
�
�
0
0
0
i 0
0 i 0
%
M

M

r
F

r%
�
i 1,i
i 1
i ,i 1
ci Pi
� i ,i 1

Lực tác dụng từ Robot lên đối tượng công nghệ F, M:

T
0
T
�
�
F

[
F
,
F
,
F
]
F

[
F
,
F
,
F
]
x
y
z
x
y
z

�
� 3,2

�
�0
T
T
M

[
M
,
M
,
M
]
M

[
M
,
M
,
M
]
� 3,2
x
y
z
x

y
z
�

Khảo sát lực khâu 3
Biểu diễn
….

0 2
1

r

và

0 2
c2

r

trong hệ cơ sở:


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

CHƯƠNG 4
TÍNH TOÁN ĐỘNG LỰC HỌC ROBOT
4.1 Xây dựng cấu trúc động học

4.1.1 Phương trình Lagrange dạng ma trận
Để tính toán động lực học Robot, ta sẽ đi thiết lập phương trình vi phân chuyển động của
Robot. Phương trình vi phân chuyển động của Robot được xây dựng theo phương trình
Lagrange loại II dạng ma trận có dạng tổng quát như sau:
T

T

T

��
d ��
T � ��
T�
� *



� &� � �
� � Q
dt ��
q � ��
q�
q�
��
Với: T- động năng của Robot.

 - thế năng của Robot.
Q*- véctơ lực suy rộng không thế.
n- số bậc tự do của Robot.

4.1.2 Động năng của Robot
Động năng của Robot được tính theo CT sau:
n

T  T1  T2  ...  Tn  �Ti
i 1

Với Ti là động năng khâu i, tính theo CT sau:

1 T
1i T i
Ti  vCi
mV
i I i i
i Ci 
2
2
Với:
mi – khối lượng khâu i
vci – vận tốc dài của khối tâm khâu i trong hệ tọa độ cố định
i

ωi – vận tốc góc khâu i tính trong hệ tọa độ động


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Ii – ma trận tenxơ quán tính của khâu i đối với khối tâm của nó trong hệ tọa độ động.

r

+) Đặt Ci là véc tơ xác định vị trí khối tâm của khâu i trong hệ tọa độ cố định
theo công thức:

rCi tính

rCi  o rOi  o Ri i rCi
Trong đó:
o

-

rOi là vị trí điểm gốc hệ tọa độ động Oi đối với hệ tọa độ cố định
o
Ri

là ma trận cosin chỉ hướng của hệ tọa độ động (Oxyz)i đối với hệ tọa độ cố định

i

-

rCi là véc tơ xác định vị trí khối tâm của khâu i trong hệ tọa độ (Oxyz)

Khi đó vận tốc dài của khối tâm khâu i :

�

�
xCi
�
�
q
x&Ci � � 1
�
�
yCi
�
� �
&
vCi  r&

y
�
Ci
Ci
� � �
� q1
�
&
�
z
�Ci � �
�
z
� Ci
q1
��

�
xCi
�
q2
�
yCi
�
q2
�
zCi
�
q2

q&
... �
xCi ��
1�
�
��
q&
�
qn ��
1�
. � �
... �
xCi ��
xCi
q&
�� �

. � �
�
qn ��
q
. �
... �
xCi ��
�� �
�
qn ��
q&n �

Vận tốc góc của khâu i :

i

i  �
x  y
�

Với i =1,2,…,n.

�
x
�
��
q&
� 1
�
�

y
T
z �

�
�
q&
1
��
�
�

� z
q&
1
��

�
x
�
q&2
�
y
�
q&2
�
z
�
q&2

q&
... �
x ��
1�
�
�
�
�
q&n q&
��1 �
... �
 y ��
. � �ii
q&
�� �
. � �
�
q&n ��
q&
. �
... �
z ��
�� �
�
q&n ��
q&n �

i

Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Quy ước :

�iCi
�
rCi
J Ri 
J Ti 
�
q&
�
q và
Thì :

�vCi  J Ti q&
�i
�i  J Ri q&
Với:
JTi – ma trận Jacobi tịnh tiến của khâu i
JRi – ma trận Jacobi quay của khâu i
Do đó :
Ti 

1
q&( J TiT mi J Ti  J RiT I i J Ri )q&
2

Suy ra Động năng của Robot :
n
1 �n T
�
T  �Ti  q&T �
( J Ti mi J Ti  J RiT I i J Ri ) �q&
�
2 �i 1
i 1



1 T
q&M (q )q&
2

Với :
m11 (q ) m12 (q )
�
�
n
m ( q) m22 (q)
M (q)  �( J TiT mi J Ti  J RiT I i J Ri )  � 21
� ..
..
i 1
�
mn1 (q ) mn 2 (q )
�

M(q) là ma trận khối lượng suy rộng.
4.1.3 Thế năng của Robot
n

�  �mi g 0T rCi
i 1

Trong đó:
mi – Khối lượng khâu i

.. m1n �
.. m2 n �
�
.. .. �
�
.. mnn �
nxn


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

g 0T  [0,  g , 0]
2
g – gia tốc trọng trường, g = 9.81 m / s

Đặt :

g (q )  [ g1 (q ) g 2 (q ) ... g i (q)]T

g i (q ) 

��
�
qi

4.1.4 Thiết lập phương trình vi phân chuyển động của Robot
Xét biểu thức:
n

T  �Ti 
i 1



1 T �n T
�
q& �
( J Ti mi J Ti  J RiT I i J Ri ) �q&
�
2 �i 1

1 T
q& M (q) q&
2

Đặt:

M (q)  �
mij �

�
�

nxn

Ta có:

T

1 T
1 n n
q&M (q )q& ��m jk (q )q&&
j qk
2
2 k 1 j 1

Suy ra:
n
�
T
 �mij (q ).q&j
�
q&i k 1


Tính toán thiết kế robot

PGS.TS Phan Bùi Khôi

Mặt khác:

n dm ( q )
n
n �
n
mij
d ��
T� n
ij
&j  �
&j
� � � �mij (q )q&
q&j  �� q&&
mij (q )q&
k ql  �
dt ��
q&i � j 1
dt
qk
j 1
j 1 k 1 �
j 1

m (q )
�
T 1 n n �
 �� jk
q&&
j qk
�
qi 2 j 1 k 1 �

qi

Vậy ta có:
n
n
mij 1 �
m jk ( q) �
��
&
&
m
(
q
)
q

q&&
� 
�
�
�
�
ij
j
k q j  g i (q)   i
&
�
q
2
�

q
j 1
j 1 k 1 � n
i
�
n

Ta đưa vào ký hiệu:

hijk  q  

�
mij (q )
�
qk



m jk ( q )
1�
1
 mij ,k  q   m jk ,i (q )
2 �
qi
2

Do M(q) là ma trận đối xứng nên mik,k = mjk,i = mkl,i. Từ đó suy ra:

hijk  q  

1
1
mij , k  mik , j  mkj ,i    mik , j  m ji , k  mkj ,i 

2
2

Suy ra:
n

n

n

j 1

j 1 k 1

�mij  q  q&&j  ��hijk  q  q&&
k q j  gi  q    i
Đặt:
n

cij  q, q&  �hijk  q  q&k
k 1