Bài tập lớn môn Robotics

BÀI TIỂN LUẬN ROBOTICS

MỤC LỤC
I. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot………...2
a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg………………………....2
b. Thiết lập hệ phương trình động học của robot………………………………...2
II. Giải bài toán động học…………………………………………………..…….3
2.1. Giải bài toán động học thuận……………..………………………….….....3
a, xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác ……........3
b, xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc
điểm E và vận tốc góc ……………………………………………………………..4
2.2. Giải bài toán động học ngược……………………………………………...6
a, giải bằng phương pháp giải tích……………………………………………..6
b, 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………………………………………...…7
III. Tính toán tĩnh học…………………………………………………………..11
3.1. Cho lực tác động vào khâu thao tác tìm lực dẫn động………………….11
3.2. Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng
lên đối tượng……………………………………………………………………..14
IV. Tính toán động lực học……………………………………………………..16
4.1 Xây dựng cấu trúc động lực học và các thành phần cần thiết để viết
phương trình động lực học……………………………………………………...16
4.2, thiết lập phương trình Lagrang của robot………………………………..19
V. Luật điều khiển………………………………………………………………22
1. Hệ thống điều khiển trong không gian khớp………………………………22
1.1. Hệ thống điều khiển phản hồi…………………………………………….22
1.2. Hệ thống điều khiển momen tính toán……………………………………23
2. Hệ thống điều khiển trong không gian làm việc…………………………..25
TÀI LIỆU THAM KHẢO ……………………………………………………...27
1

BÀI TIỂN LUẬN ROBOTICS

I. Xây dựng cấu trúc, thiết lập hệ phương trình động học của robot
Ta thiết lập phương trình động học theo phương pháp ma trận Denavit Hartenberg
a. Thiết lập hệ tọa độ theo quy tắc Denavit Hartenberg
Khâu 0: đế ta chọn hệ tọa độ XoYoZo có trục Zo chọn
trùng với khớp 1, trục Xo chọn tùy ý sao cho phù hợp nhất
như hình vẽ , trục Yo chọn theo quay tắc tam diện thuận.
Khâu 1: ta chọn hệ tọa độ X1Y1Z1 có trục Z1 trùng với
khớp 2, trục X1 ta chọn theo hướng Zo x Z1, trục Y1 chọn
theo quay tắc tam diện thuận.
Khâu 2: ta chọn hệ tọa độ X2Y2Z2 có trục Z2 trùng với
khớp 3,X2 chọn theo đường vuông góc chung Z1 và Z2, Y2
chọn theo quy tắc tam diện thuận.
Khâu 3: Ta chọn hẹ X3Y3Z3 có trục Z3 song song Z2 X2
chọn theo đường vuông góc chung Z2 và Z3, Y3 chọn theo
quy tắn tam diện thuận.
Hình 2.1. Hệ tọa độ Robot RRR
b. Thiết lập hệ phương trình động học của robot
Từ việc chọn hệ tọa độ ta có bảng DH sau:
Khâu
di
θi
ai
αi
1
d1
q1
0

π/2
2
0
q2
a2
0
3
0
q3
a3
0

0

A

1

=

 cos(q1 ) 0 sin( q1 )
sin( q ) 0 −cos( q )
1
1

 0
1
0

0

0
 0

0
0 
d1 

1 

1

,

A

2

=

 cos(q2 ) − sin( q2 )
 sin(q )
cos(q2 )
2

 0
0

0
 0

2

0 a2 cos( q2 ) 
0 a2 sin(q2 ) 

1
0

0
1



BÀI TIỂN LUẬN ROBOTICS

2

A

=>

=>

3

0

0

=

 cos(q3 ) − sin(q3 )
 sin(q )
cos( q3 )
3

 0
0

0
 0

A

0
2

1

=

A ( q)
3

0

.

2

=

1

A A
1

−C1 S2 S1 a2 C1C2

C1C2
S C
 1 2
 S2

 0

1

A A

0 a3 cos(q3 ) 
0 a3 sin(q3 ) 

1
0

0
1


2
2

A

− S1 S 2
C2
0

3


−C1 a2 S1C 2 
0 d1 + a2 S2 

0
1


C1C23
S C
 1 23
 S 23

 0

−C1 S 23 S1 C1 (a3C23 + a2 C2 ) 
− S1 S 23 −C1 S1 ( a3C23 + a2 C2 ) 
C23
0 d1 + a2 S 2 + a3 S 23 


0
0
1


=
.
.
=
Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=sin(q2+q3),
C23=cos(q2+q3), q=[q1, q2, q3]T
Mặt khác ta lại mô tả được hướng và vị trí qua ma trận sau thông qua vector
p=[xE, yE, zE, α, β, η]T α, β, η là 3 góc Cardan.

0

A ( p)
3

=

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

0

0

A (q) A ( p)
3

So sánh hai ma trận

xE 
yE 
zE 

1 

3

,

ta thiết lập được hệ phương trình động học sau:

 f1 = 0 A ( p )[1, 4] − 0 A ( q)[1, 4] = xE − cos(q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] = 0
3
3

0

0
 f 2 = A ( p )[2, 4] − A ( q)[2, 4] = yE − sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ] = 0
3
3

0
0
 f 3 = A ( p )[3, 4] − A ( q)[3, 4] = z E − [ d1 + a2 sin( q2 ) + a3 sin( q2 + q3 ) ] = 0
3
3

0
0
 f 4 = A3( p )[1,1] − A3( q)[1,1] = cos(α )cos(η ) − cos( q1 )cos(q2 + q3 ) = 0

0
0
 f 5 = A3( p )[2, 2] − A3( q)[2, 2] = − sin(α ) sin( β ) sin(η ) + cos(α )cos(η ) + sin(q1 )sin(q2 + q3 ) = 0

0
0
 f 6 = A3( p )[3,3] − A3( q)[3,3] = cos(α )cos( β ) = 0

II. Giải bài toán động học

3


BÀI TIỂN LUẬN ROBOTICS

2.1. Giải bài toán động học thuận
Để tính toán ta chọn d1=1 (m), a2=0.5 (m), a3=0.4 (m).
a, xác định vận tốc điểm tác động cuối và vận tốc góc khâu thao tác
Từ phương trình động ở trên ta rút ra ( với q=[q1, q2, q3]T )

0

r

E

 xE  cos( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] 
 y  = sin( q ) a cos(q + q ) + a cos( q ) 
1 [ 3
2
3
2
2 ] 
 E 


 z E 
 d1 + a2 sin(q2 ) + a3 sin( q2 + q3 ) 

=

d(
0



=>

r

=

∂(

0

r

E

)

∂q

.q& = J E .q&

⇒

 − S1 (a3C23 + a2 C2 ) −C1 (a3 S 23 + a2 S2 )
C ( a C + a C ) − S (a S + a S )
2 2
1
3 23
2 2
 1 3 23


0
a3C23 + a2 C2

− a3 C1 S 23 
− a3 S1 S23 
a3 C23 

E

J =

v

 x& E  −S1 (a3 C23 + a2 C2 )q& 1 − C1 (a3 S 23 + a2 S2 ) q& 2 − a3C1 S 23 q& 3 
 y&  =  C (a C + a C )q& − S (a S + a S )q& − a S S q& 
2 2
1
1
3 23
2 2
2
3 1 23 3 
 E   1 3 23
 z& E  

&
&
( a3 C23 + a2 C2 )q2 + a3 C23 q3

E

=
Từ ma trận

R

)

dt

E

0

0

E

=



0

v

0

3

=

C1C23
S C
 1 23
 S23

A

3

ta rút trân cosin chỉ hướng

−C1 S23 S1 
− S1 S 23 −C1 
C23
0 

0

=>

ω%

3

=

0

 sin(q1 )(q& 2 + q& 3 ) 
 −cos( q )( q& + q& ) 
1
2
3 



q&1

0

R& . RT
2

2

=

b, xây dựng quy luật chuyển động từng khâu từ đó vẽ quỹ đạo điểm E, vận tốc
điểm E và vận tốc góc.

4


BÀI TIỂN LUẬN ROBOTICS

Để khảo sát kết quả trên ta xây dựng quy luật chuyển động của các biến khớp
q1 = sin(3t )


q2 = 2t + 3
q = cos(5t ) + 3t
 3

q như sau

0

r

E

=

Với t=0-> 2*Pi/5
ta vẽ được đồ thị điểm
tác động cuối bằng
phân mềm maple

Hình 2.2. Đồ thị điểm tác động cuối

x& E

=

5


BÀI TIỂN LUẬN ROBOTICS

y& E
z& E

=
=
vE = x& 2E + y& 2E + z& 2E


ωy =

ωx =

ωy =

,
w3 = ω

2
x

+ω

2
y

+ω

,

2
z



Ta dùng maple để vẽ với khoảng t=0..3*Pi

Hình 2.3. Đồ thị vận tốc điểm E theo t Hình 2.4. Đồ thị vận tốc góc khâu 3 theo
t

Để tính toán ta sử dụng hai thủ tuc đó là hai hàm con viết bằng maple như sau:
Hàm doitm có tác dụng đổi các tham số các ma trận có các thành phần là hàm
theo thời gian

6


BÀI TIỂN LUẬN ROBOTICS

Hàm wmo có tác dụng tính vector vân tốc góc tuyệt đối chiếu trên hệ tọa độ
i

khâu

ω%

i

2.2. Giải bài toán động học ngược
a, giải bằng phương pháp giải tích

từ hệ phương

 xE = cos(q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ]

 yE = sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos( q2 ) ]
 z = d + a sin( q ) + a sin( q + q )
1
2
2
3
2
3
 E

q1 = a tan 2( y E , xE )
Từ phương trình 1, 2 trong hệ ta rút ra :

(2.1)

7


BÀI TIỂN LUẬN ROBOTICS

xE2 + yE2 + ( zE − d1 ) 2 = a22 + a32 + 2a2 a3 (C2 C23 + S2 S23 ) = a22 + a32 + 2a2 a3 C3

xE2 + yE2 + ( zE − d1 )2 − a22 − a32
 cos(q3 ) =
2a2 a3



2
sin(q3 ) = ± 1 − cos (q2 )


q3 = a tan 2(sin(q3 ), cos( q3 ))



(2.2)

Ta viết lại hệ như sau :

 xE = (a2 + a3C3 )C1C2 − a3 S3C1 S 2

 z E − d1 = a3 S3C2 + (a2 + a3 C3 ) S 2

∆=

(a2 + a3C3 )C1

∆1 =

∆2 =

a3 S3
xE
zE − d

− a3 S3C1

(a2 + a3 C3 )

−a3 S3C1
a2 + a3C3

(a2 + a3C3 )C1 x p
a3 S3

zE − d1

= (a22 + a32 + 2a2 a3 C3 )C1 = [ xE2 + yE2 + ( zE − d1 )2 ) ] C1

= a2 xE + a3 xE C3 + a3 ( z E − d ) S3C1

= a2 ( zE − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ]

a2 xE + a3 xE C3 + a3 ( z E − d ) S3 C1

cos(
q
)
=
2

[ xE2 + yE2 + ( zE − d1 )2 ) ] C1


sin(q ) = a2 ( z E − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ]
2


[ xE2 + yE2 + ( zE − d1 )2 ) ] C1


8


BÀI TIỂN LUẬN ROBOTICS

Tính atan2 triệt tiêu được C1 ở dưới mẫu hệ trên

=>

 a x + a x C + a ( z − d )S C a ( z − d1 ) + a3 [ ( z E − d )C3C1 − xE S3 ] 
q1 = a tan 2  2 E 2 3 E 2 3 3 E 2 3 1 , 2 E
÷
xE + yE + ( zE − d1 ) )
xE2 + yE2 + ( z E − d1 )2 )



(2.3)
Từ (2.1), (2.2), (2.3) ta xác định được dưới dạng giải thức của các biến khớp
liên hệ với điểm tác động cuối, khi xác định được các biến khớp ngược lại ta tìm
được các góc cardan
b, 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.
 f1 = xE − cos( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] = 0

 f 2 = y E − sin( q1 ) [ a3 cos( q2 + q3 ) + a2 cos(q2 ) ] = 0


 f 3 = z E − [ d1 + a2 sin( q2 ) + a3 sin( q2 + q3 ) ] = 0
 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.
Dưới đây là 1 doạn chương trình viết bằng maple sử dụng phương pháp
Newton-Raphson lưu kết quả tính toán ra file txt và 1 đoạn vẽ đồ thị từ file txt
được viết bằng matlab
Với quỹ đạo điểm tác động cuối là đường thẳng có phương trình như sau

9


BÀI TIỂN LUẬN ROBOTICS

 xE = − t + 0.6

 yE = 0.4t + 0.3
 z = 0.5t + 0.4
 E
Đoạn chương trình viết bằng maple
>
>

>

>

>

>
>

>

10


BÀI TIỂN LUẬN ROBOTICS

>
>

Flile vẽ đồ thị từ file txt được xuất ra từ chương trình trên viết bằng matlab.
%---------------------------------------------------------------------fid = fopen('D:/manh3.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)');

11


BÀI TIỂN LUẬN ROBOTICS
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
%----------------------------------------------------------------------

Ta thu được các đồ thị sau:

Hình 2.5. Đồ thị q1, q2 theo t

12


BÀI TIỂN LUẬN ROBOTICS

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

III. Tính toán tĩnh học
3.1. Cho lực tác động vào khâu thao tác tìm lực dẫn động
Ta biết lực tác động vào khâu cuối đề bài ra

 0 F3 E = [ FX , FY , Fz ]T
 0 FE 3 = [ -FX , − FY , − Fz ]T
=>  0
0
T
T
 M E 3 = [ M X , M Y , M z ]
 M E 3 = [ -M X , − M Y , − M z ]
Từ phần động học ta xác định được các ma trận cosin chỉ
hướng sau:

0

0

R

R

1

3

=

=

C1
S
 1
 0

C1C23
S C
 1 23
 S23

S1 
0 −C1 
1 0 

0

0

R

2

=

C1C2
S C
 1 2
 S 2

−C1 S 2 S1 
− S1 S2 −C1 
C2
0 

−C1 S23 S1 
− S1 S 23 −C1 
C23
0 

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.
 Xét khâu 3:

13


BÀI TIỂN LUẬN ROBOTICS


 −c3 C1C23
0
0


1
 rc1 = R 3. rc1 =  − c3 S1C23

 −c3 S23
 3 rc 3 = [ -c3 , 0, 0]T
=>
3

T
 a3C1C23 

 r3 = [ a 3 , 0, 0]
0
 0r =
. 3 r3 =  a3 S1C23 
3
R
3

 a3 S 23








, và

0

P

3

= [ 0, 0, − m3 g]T

Thay vào công thức đã được đã được thiết lập trong bài giảng robot
0
0
0

 F32 = FE 3 − P3
0
0
0
0
0
0

 M 32 = M E 3 + r%3 F32 − r%c 3 P3

Ta tính được

 −Fx 
0



 F32 =  −Fy 

m g − F 
z 
 3


 −M x + S 23 [(a 3 − c3 ) S1m3 g + a3 Fy − a3 S1 Fz ] 

 0 M =  −M − a S F + C C [-(a − c ) m g + a F ]

32
y
3 23 x
1 23
3
3
3
3 z 




−M z + S1 S 23 a3 Fx − C1C23 a3 Fy



 Xét khâu 2 ( lấy kết quả của khâu 3 ):



 − c2 C1C2
0
0

2
 rc 2 = R 2. rc 2 =  − c2 S1C2

 2 rc 2 = [ -c2 , 0, 0]T
 −c2 S2
=>
2

T
 a2 C1C2 
 r2 = [ a 2 , 0, 0]

0
0
2
 r =
. r2 =  a2 S1C2 
2
R
2

 a3 S 2



14






, và

0

P

2

= [ 0, 0, − m2 g]T


BÀI TIỂN LUẬN ROBOTICS

Thay vào công thức:

0
0
0

 F21 = F32 − P2
0
0
0
0

0
0

 M 21 = M 32 + r%2 F21 − r%c 2 P2

Ta tính được:




− Fx
0


− Fy
 F21 = 


 (m + m ) g − F 
z
 3 2


 − M x + S23 [(a 3 − c3 ) S1m3 g + a3 Fy − a3 S1Fz ]+a 2 S1C2 m3 g + (a2 − c2 )S1C2 m2 g + a2 S 2 Fy − a2 S1C2 Fz 

 0 M =  − M − a S F + C C [-(a − c )m g + a F ]-a C C m g − (a − c )C C m g − a S F + a C C F 
3 z
2 1 2 3
2
2

1 2 2
2 2 x
2 1 2 z
 21  y 3 23 x 1 23 3 3 3



− M z + S1 S23 a3 Fx − C1C23 a3 Fy + a2 S1C2 Fx − a2C1C2 Fy



 Xét khâu 1 ( lấy kết quả của khâu 2 ):


0 
0
0
 
1
 rc1 = R 1. rc1 =  0 
 d1 
 1rc1 = [ 0, d1 , 0]T

0
=>
, và P 1 = [ 0, 0, − m1g]T
1

T
0 

 r1 = [ 0, −c1 , 0]

0
0
1
 r =
. r =  0 
1
R
1 1

 −c1 


Thay vào công thức:

0
0
0

 F10 = F21 − P1
0
0
0
0
0
0

 M 10 = M 21 + r%1 F10 − r%c1 P1

Ta tính được :

15


BÀI TIỂN LUẬN ROBOTICS




− Fx
0


− Fy
 F10 = 


 (m + m + m ) g − F 
z
 3 2 1


 − M x + S23 [(a 3 − c3 )S1m3 g + a3 Fy − a3 S1 Fz ]+a 2 S1C2 m3 g + (a2 − c2 )S1C2 m2 g + a2 S 2 Fy − a2 S1C2 Fz + d1 Fy 

 0M =  − M − a S F + C C [-(a − c )m g + a F ]-a C C m g − (a − c )C C m g − a S F + a C C F − d F 
3 z
2 1 2 3
2
2 1 2 2

2 2 x
2 1 2 z
1 x
 10  y 3 23 x 1 23 3 3 3



− M z + S1 S23 a3 Fx − C1C23 a3 Fy + a2 S1C2 Fx − a2 C1C2 Fy



Để có kết quả tính toán trên ta sử dụng các hàm con để tím lực và momen viết
bằng maple như sau :

3.2. Cho lực ( mô men ) dẫn động tính lực ( mô men ) khâu thao tác tác dụng
lên đối tượng.

16


BÀI TIỂN LUẬN ROBOTICS

Nhận xét : robot ở đây là 3 bậc RRR không gian tương ứng 3 momen dẫn
U = [U1 ,U 2 ,U 3 ]T

động
như vậy bài toán tính lực ( mô men ) khâu thao tác tác dụng
lên đối tượng khi giải ra chỉ cho ta ba thành phần hoặc 3 lực hoặc 3 mô men hoặc
2 lực và 1 mô men hoặc 2 mô men và 1 lực. Ở đây ta tính 3 lực đối tượng tác dụng
0

lên khâu cuối
0

F3 E = [ FX , FY , Fz ]T

thì lực khâu cuối tác dụng lên đối tượng

FE 3 = [ -FX , − FY , − Fz ]

T

còn 3 momen bằng không.
Như vậy từ kết quả phần 3.1 thay mô men Mx, My, Mz băng không.

 S23 [(a 3 − c3 ) S1 m3 g + a3 Fy − a3 S1 Fz ]+a 2 S1C2 m3 g + (a2 − c2 )S1C2 m2 g + a2 S 2 Fy − a2 S1C2 Fz + d1 Fy 


0
M 10 =  − a3 S23 Fx + C1C23 [-(a 3 − c3 ) m3 g + a3 Fz ]-a 2 C1C2 m3 g − (a2 − c2 )C1C2 m2 g − a2 S2 Fx + a2 C1C2 Fz − d1 Fx 


S1 S23 a3 Fx − C1C23 a3 Fy + a2 S1C2 Fx − a2 C1C2 Fy


 S23 [(a 3 − c3 )S1m3 g + a3 Fy − a3 S1 Fz ]+a 2 S1C2 m3 g + (a2 − c2 )S1C2 m2 g + a2 S2 Fy − a2 S1C2 Fz 


0
M 21 =  − a3 S23 Fx + C1C23 [-(a 3 − c3 )m3 g + a3 Fz ]-a 2 C1C2 m3 g − (a2 − c2 )C1C2 m2 g − a2 S2 Fx + a2 C1C2 Fz 



S1S 23a3 Fx − C1C23 a3 Fy + a2 S1C 2 Fx − a2C1C2 Fy



0

M 32

 S23 [(a 3 − c3 ) S1 m3 g + a3 Fy − a3 S1 Fz ] 


=  −a3 S 23 Fx + C1C23 [-(a 3 − c3 )m3 g + a3 Fz ]


S1 S23 a3 Fx − C1C23 a3 Fy



Bài toán đặt ngược lại đó là khi biết các lực dẫn
động
2

U = [U1 ,U 2 ,U 3 ]T

M 21 [3]

3

M 32 [3]

1

tương ứng bằng

,
ta tìm được
Momen dẫn động các khớp


0

M 10 [2]

,

FE 3 = [ -FX , − FY , − Fz ]T

Với khớp 1:

Từ hình ta thấy momen dẫn động cho khâu 1 tại khớp 1 dọc theo trục Y
nên ta có

U1 = 1M 10 [2]

17

1

BÀI TIỂN LUẬN ROBOTICS
M 10 = 0 R1T . 0 M 10

1

=> 1M 10 [2] = [ a3 S1 S 23 + a2 S1C2 ] Fx − [ a3C1C23 + a2 C1C2 ] Fy = U1


Với khớp 2:

Từ hình ta thấy momen dẫn động cho khâu 2 tại khớp 2 dọc theo trục Z
nên ta có
2

2

U 2 = 2 M 21 [3]

M 21 = 0 R2T 0 M 21

=> 2 M 21 [3] = C1 ( a3 S 23 + a2 S2 ) Fx + S1 ( a3 S 23 + a2 S2 ) Fy −  (a3 S 23 + a2 C2 )S12 + (a3C23 + a2 C2 )C12  Fz

+ { [ m3 ga2 + (a2 − c2 )m2 g ] C2 + ( a3 − c3 )m3 gS23 } S12 + { [ m3 ga2 + (a2 − c2 ) m2 g ] C2 + ( a3 − c3 ) m3 gC23 } C12
= U2


Với khớp 3:

Từ hình ta thấy momen dẫn động cho khâu 3 tại khớp 3 dọc theo trục Z

nên ta có
3

3

U 3 = 3M 32 [3]
M 32 = 0 R3T 0 M 32

=> 3M 32 [3] = a3C1 S23 Fx + a3 S1 S 23 Fy − a3 ( S12 S23 + C12 C23 ) Fz + (a3 − c3 )m3 gS 23 S12
+ (a3 − c3 )m3 gC23C12 = U 2

Như vậy ta có hệ phương trình 3 ẩn Fx, Fy, Fz giải ta được :

18


BÀI TIỂN LUẬN ROBOTICS

19


BÀI TIỂN LUẬN ROBOTICS

IV. Tính toán động lực học
4.1 Xây dựng cấu trúc động lực học và các thành phần cần
thiết để viết phương trình động lực học
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.
 cos( q1 )
 sin(q )

1
0
C1 = 0 A1 1C1 = 
 0

 0

sin( q1 ) 0  
0 


0 − cos( q1 ) 0   − (d1 − lc1 )
=
1
0
d1  
0 


1 
0
0
1 
0

0 
0 
l 
 c1  => 0 r =  l 
c1

 c1 
0 
 0 
 
1 

 C1C2 − C1 S2 S1 a2 C1C2   − (a2 − lc 2 ) 
S C − S S −C a S C  
0 
1 2
1 2
1
2 1 2
0
0
2



C2 = A2 C2 =
=
 S2
C2 0 d1 + a2 S2  
0 



0
0
1  1 

 0

 lc 2 C1C2 
l S C 
 c 2 1 2  => 0 r
c2
 d1 + lc 2 S2 


 1


 C1C23 − C1 S23 S1 C1 (a3C23 + a2 C2 )   − (a3 − lc3 ) 
 S C − S S − C S (a C + a C )  
0 
1 23
1
1 3 23
2 2 
0
0
3
C3 = A3 C3 =  1 23
=
 S23
C23 0 d1 + a2 S2 + a3 S23   0 



0

0
1
 0
 1 

0 
1
0 T 0 &
ω%1 = R1 R1 =  q& 1 
 0 

 q&1 S 2 
2
ω% 2 = 0 R2T 0 R& 2 =  q&1C2 
 q& 2 

 C1 (lc3C23 + a2 C2 )
 S (l C + a C ) 
 1 c 3 23 2 2  => 0 r =
c3
 d1 + a2 S2 + lc3 S23 


1




3
ω% 3 = 0 R3T 0 R& 3 = 



20

 lc 2 C1C2 
=  lc 2 S1C2 
 d1 + lc 2 S2 

q&1 S 23 
q&1C23 
q& 2 + q& 3 

 C1 (lc3 C23 + a2 C2 )
 S (l C + a C ) 
 1 c 3 23 2 2 
 d1 + a2 S 2 + lc3 S23 


BÀI TIỂN LUẬN ROBOTICS

Từ các vị trí trọng tâm và vận tốc góc ta tính được các ma trân Jacobi tịnh tiến
và quay sau
0 0 0 
JT 1 =  0 0 0 
 0 0 0 

JT 2

1

J R1

JT 2

 −lc 2 S1C2 −lc 2 C1 S 2
=  lc 2 C1C2 −lc 2 S1 S2

0
lc 2 C2

0
0 
0 

 − S1 (lc3C23 + a2 C2 ) − C1 (lc3 C23 + a2 C2 )
=  C1 (lc 3C23 + a2 C2 ) − S1 (lc3 C23 + a2 C2 )

0
a2 C2 + lc3C23
0 0 0
= 1 0 0
 0 0 0






2

J R2

 S2 0 0 
=  C2 0 0 
 0 1 0 

1

J R1

− lc3 C1 S23 
− lc 3 S1 S23 
lc 3C23 
 S 23
= C23
 0

0 0
0 0 
1 1 

Ma trận ten xơ 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:

I

1

=

 I1x 0 0 


0 I1 y 0 
0 0 I 

1z 

,

I

2

=

I2x
0 0


0 I 2 y 0 
0 0 I 

2z 

,

I

3

=

 I3x 0 0 


0 I3 y 0 
0 0 I 
3z 


 Động năng, ma trân khối lượng M(q):

1 T 3
1

T
T = q& ∑ ( J TiT mi J Ti + J Ri
I i J Ri )  q& = q& T M ( q ) q&
2  i =1
2

m11 m12 m13 
T
M (q ) = ∑( J TiT mi J Ti + J Ri
I i J Ri ) = m21 m22 m23 
i =1
m31 m32 m33 
3

21


BÀI TIỂN LUẬN ROBOTICS

Với các thành phần như sau :
m11 = I1 y + I 3 y cos(q2 + q3 ) 2 + I 3 x sin(q2 + q3 ) 2 + I 2 x sin( q2 ) 2 + [m3 lc23 cos(2q3 ) + 2m3 a2 lc3 cos(q3 )
1
+ I 2 y + m2 lc22 + m3 a22 ]cos(q2 )2 + m3lc23 sin(q3 )2 − [m3lc 3 a2 sin(q3 ) + m3lc23 sin(2q3 )]sin(2q2 )
2
m12 = m21 = m13 = m31 = 0
m22 = I 2 z + I 3 z + m2 lc22 + m3 [a22 + lc23 + 2a2 lc 3 cos(q3 )]
m23 = m32 = I 3 z + m3 lc23 + m3 a2 lc 3 cos(q3 )
m33 = I 3 z + m3lc23
2

Π = −∑ mi. g .

T 0
0

r

Ci

i =1

Biểu thức thế năng của hệ :
với go = [ 0 ,0 ,-g ]T
Π = m1lc1 g + m2 g[d1 + lc 2 sin(q2 )] + m3 g[d1 + a2 sin(q2 ) + lc 3 sin(q2 + q3 )]



Công ảo của các lực suy rộng không có thế ở đây giả tại điểm tác động



FE = [-Fx , −Fy , −Fz ]T

cuối robot chịu 1 lực
:
T
0
δ A = τ 1δ q1 + τ 2δ q2 + τ 3δ q3 + F δ ( rE ) = U1δ q1 + U 2δ q2 + U 3δ q3 + F T ( J E δ q )


Vector lực suy rộng viết dạng cột được lấy từ tổng công ảo có dang như
sau:
Q np = [Q1 , Q2 , Q3 ]T =U + J ET F
(4.1)

1

2

3

T

E

Trong đó U =[U , U , U ] là vector momen dẫn động, J được tính từ phần động

 − S1 (a3C23 + a2 C2 ) − C1 (a3 S 23 + a2 S 2 )
 C (a C + a C ) − S (a S + a S )
1 3 23
2 2
 1 3 23 2 2

0
a3C23 + a2 C2
E

học J =
4.2, thiết lập phương trình Lagrang của robot
22

− a3C1 S 23 
− a3 S1 S23 
a3C23 


BÀI TIỂN LUẬN ROBOTICS

Chúng ta đã biết phương trình lagrang loại 2 viết từng tọa độ suy rộng như sau:

d  ∂T  ∂T
∂Π
=−
+ Q np


÷−
dt  ∂q& i  ∂qi
∂qi
Để tránh dài dòng ta không đi chứng mình dạng ma trận của phương trình
Lagrang áp dụng luôn ( có thể tham khảo cách chứng minh trong quyển Robot
công nghiệp của GS. TSKH. Nguyễn Văn Khang )

&& + C ( q, q& )q& + G (q ) = Q np
M (q)q
Từ (4.1) viết lại dạng sau :

&& + C (q, q& ) q& + G (q ) = U + J ET F
M (q)q
Ta tính ma trân Clioris từ ma trận M(q) bởi công thức tính như sau

& 3x3
C (q, q& ) = [cij (q,q)]
 ∂ mij (q) 1 ∂ m jk (q) 
cij = ∑ 
−
÷
2 ∂ qi 
k = 1  ∂ qk
3

Vector G(q) momen trọng lực tính như sau :

G (q) = [g1 (q), g 2 (q), g3 (q)]T
gi (q ) =

∂Π
∂ qi

Như vậy phương trình vi phân chuyển động của robot cấu hình RRR không gian
có dạng ma trân như sau: ta xác định được

&& + C (q, q& ) q& + G (q ) = U + J ET F
M (q)q
•

Ma trận khối lương M(q) và ma trận quán tính clitoris C
23


BÀI TIỂN LUẬN ROBOTICS

m11 m12 m13 
T
⊕ M ( q) = ∑( J TiT mi J Ti + J Ri
I i J Ri ) = m21 m22 m23 
i =1
m31 m32 m33 
3

Với các thành phần như sau :
m11 = I1 y + I 3 y cos(q2 + q3 ) 2 + I 3 x sin( q2 + q3 ) 2 + I 2 x sin( q2 ) 2 + [m 3lc23 cos(2q3 ) + 2m3 a2 lc3 cos( q3 )
1
+ I 2 y + m2lc22 + m3 a22 ]cos(q2 )2 + m3lc23 sin(q3 ) 2 − [m3lc3 a2 sin(q3 ) + m3lc23 sin(2q3 )]sin(2q2 )
2

m12 = m21 = m13 = m31 = 0
m22 = I 2 z + I 3 z + m2 lc22 + m3 [a22 + lc23 + 2a2 lc3 cos(q3 )]
m23 = m32 = I 3 z + m3 lc23 + m3 a2 lc3 cos(q3 )
m33 = I3 z + m3 lc23

c11
⊕ C ( q, q& ) = 
c21

c31

c12
c22
c32

c13 
c23 

c33 


24


BÀI TIỂN LUẬN ROBOTICS

Với các thành phần như sau :

c11 =  ( I 3 x − I 3 y − m3lc23 )sin(2q2 + 2q3 ) + ( I 2 x − I 2 y − m3 a22 − m2 lc22 )sin(2q2 ) − 2m3lc 3 a2 sin( q3 + 2q2 )  q& 2
+  ( I 3 x − I 3 y − m3lc23 )sin(2q2 + 2q3 ) − m3lc3 a2 sin( q3 + 2q2 ) − m3lc3 a2 sin q3  q& 3

c12 = c13 = 0
1
c21 =  ( I 2 y − I 2 x + m2 lc22 + m3 a22 )sin(2q2 ) + ( I 3 y − I 3 x + m3lc23 )sin(2q2 + 2q3 ) + 2m3lc 3 a2 sin(q3 + 2q2 )  q&1
2
c22 = − 2m3 lc3 a2 sin q3 q& 3
c23 = − m3lc 3 a2 sin q3 q& 3
1
c31 =  ( I 3 y − I 3 x − m3lc23 )sin(2q2 + 2q3 ) + m3lc 3 a2 sin( q3 + 2q2 ) + m3lc 3 a2 sin q3  q&1
2
1 

c32 = m3lc 3 a2 sin q3  q& 2 − q& 3 ÷
2 

1
c33 = m3 lc3 a2 q& 2
2
•

E

Vector G(q) và ma trận J (q)
⊕ G ( q) = [g1 , g 2 , g 3 ]T
gi = 0

g 2 = m2 glc 2 cos q2 + m3 g [ lc 3 cos(q2 + q3 ) + a2 cosq2 ]
g 3 = m3 glc 3 cos(q2 + q3 )

⊕

 − S1 ( a3C23 + a2 C2 ) −C1 (a3 S 23 + a2 S2 )
J E = C1 (a3C23 + a2 C2 ) − S1 (a3 S23 + a2 S2 )

0
a3 C23 + a2 C2

−a3C1 S 23 
−a3 S1 S23 
a3C23 

Với C1=cos(q1), S1=sin(q1), C2=cos(q2), S2=sin(q2), S23=sin(q2+q3),
C23=cos(q2+q3), q=[q1, q2, q3]T
FE = [-Fx , −Fy , −Fz ]T

Điểm tác động cuối robot chịu 1 lực

:

25