Ứng dụng Toolbox trong Matlab để khảo sát cánh tay robot ba bậc tự do RRR

TRƯỜNG ĐẠI HỌC SPKT HƯNG YÊN
KHOA ĐIỆN – ĐIỆN TỬ

ĐỒ ÁN TỐT NGHIỆP
Tên đề tài:
Ứng dụng Toolbox trong Matlab để khảo sát cánh tay robot ba bậc tự do RRR

Sinh viên thực hiện : Nguyễn Thị Thúy
Giảng viên hướng dẫn: Đào Minh Tuấn.
Hưng Yên, tháng 5, năm 2016.
02:00:33 PM

Slide 1


NỘI DUNG TRÌNH BÀY

1
2

Mô phỏng và xây dựng mô hình toán học

3

Điều khiển và mô phỏng pp PD bù trọng trường

4
02:00:34 PM

Giới thiệu tổng quan

Kết luận
Slide 2


1. GIỚI THIỆU TỔNG QUAN
Nội dung
 Khái niệm robot công nghiệp

ROBOT
CÔNG
NGHIỆP

ĐỘNG HỌC
THUẬN

 Lịch sử robot công nghiệp
 Bài toán động học thuận
 Bài toán động học ngược
 Bài toán động lực học

ĐỘNG HỌC
NGƯỢC

ĐỘNG LỰC
HỌC


2. BÀI TOÁN ĐỘNG HỌC THUẬN
Ta có sơ đồ đặt hệ tọa độ cho robot theo quy tắc DenavitHatenberg như sau :
Các tham số của cánh tay robot

m1 = 50
m2 = 20
m3 = 10
a1 = 5
a2 = 3
a3 = 2
g = 9,8


BẢNG D-H
Sau khi chọn trục tọa độ như trên ta có bảng DH:
Bảng 1.1: Bảng tham số động học của cánh tay robot RRR

02:00:39 PM

Khớp

di

θi

ai

αi

1

0

θ1

a1

π/2

2

0

θ2

a2

0

3

0

θ3

a3

0

5


CÁC MA TRẬN TRUYỀN DH
Dạng tổng quát của ma trân D-H cho

các khâu có dạng như sau:
�C i  S iC i S iS i aiC i �
�
�
S

i
C

iC

i

C

iS

i
aiS

i
�
Ai   �
�0
S i
C i
di �
�
�
0

0
1 �
�0
Ma trận D-H của khâu 2:
C2
S2
0 a2 C 2 �
�
�
�
S
C
0
a
S
2
2
2 �
A2    � 2
�0
0
1
0 �
�
�
0
0
0
1
�

�

Ma trận D-H của khâu 1:

C1 0
S1
a1C1 �
�
�
�
S
0

C
a
S
1
1 1 �
A1  � 1
�0 1
0
0 �
�
�
0
1 �
�0 0
Ma trận D-H của khâu 3:
C3  S3 0 a3C3 �
�

�
�
S
C
0
a
S
3
3 3 �
 A3   � 3
�0
0
1
0 �
�
�
0
0
1 �
�0


THIẾT LẬP CÁC PHƯƠNG TRÌNH ĐỘNG HỌC
Ma trận biến đổi tọa độ thuần nhât của khâu 3 so với khâu cố định là:
�N X
�
NY
�
T 3  A1 A2 A3   
�N Z

�
�0

OX
OY

AX
AY

OZ
0

AZ
0

Trong đó:

PX �
�
PY �
PZ �
�
1 �

Nx = C23C1

Ox = -S23C1

Ax = S1

Px = C1(a1 + a3C23+a2C2)

Ny = C23S1

Oy = -S23S1

Py = S1(a1 + a3C23+a2C2)

Nz = S23

Oz = C23

Ay = -C1
Az = 0

02:00:39 PM

Pz = a3S23 + a2S2
7


ĐỘNG HỌC THUẬN
Ta có tọa độ của khâu tác động cuối:

�PX � �C1 (a1  a3C23  a2C2 ) �
� ��
�
rE  �PY � �S1 (a1  a3C23  a2C2 ) �
�P � � a S  a S
�

� Z � � 3 23 2 2
�
Các thông số:
a3  5

a2  3
a3  2
02:00:39 PM

Chọn quy luật chuyển động bất kỳ:

q1  3t
q2  2t
q3  t
Thay các giá trị trên vào PT được:
� 2 �3t � �
� 2 �3t �
�
2
xE (t )  �
2C � �
 1�
4
C

6
C
(
t
)

� �2 �
�
2
�
�
�
�
�
�
�
�
� 2�
3t �
�
yE (t )  S (3t ) �
4C � �
 6C 2  t  �
�2 �
�
�

z E (t )  3S (2t )  2 S (3t )
8


MÔ PHỎNG CÁNH TAY ROBOT ĐỘNG HỌC THUẬN
Sử dụng Symbolic Math Toolbox và Robotics Toolbox
ta được quỹ đạo của cánh tay robot trong khoảng t = 0 – π/12 như sau:

02:00:40 PM

Slide 9


MÔ PHỎNG CÁNH TAY ROBOT ĐỘNG HỌC THUẬN
Sử dụng Symbolic Math Toolbox và Robotics Toolbox
ta được quỹ đạo của cánh tay robot trong khoảng t = 0 – π như sau:


MÔ PHỎNG CÁNH TAY ROBOT ĐỘNG HỌC THUẬN
Sử dụng Symbolic Math Toolbox và Robotics Toolbox
ta được quỹ đạo của cánh tay robot trong khoảng t = 0 – 2π như sau:

02:00:40 PM

11


BÀI TOÁN ĐỘNG HỌC NGƯỢC
Ta có ma trận cấu hình thao tác:
�a11  x  a12  x  a13  x  a14  x  �
�
�
a
x
a
x
a
x
a

x
  32   23   24   �
T3  x   �21
�a31  x  a32  x  a33  x  a34  x  �
�
�
0
0
1 �
� 0

Với tọa độ điểm thao tác cuối E:
a14(x) = xE(t)
a24(x)= yE(t)
a34(x) = yE(t)

Từ động học thuận ta có:
NX
�
�N
T3 (q)  A1 A2 A3   � Y
�N Z
�
�0

OX
OY
OZ
0

AX
AY
AZ
0

PX �
PY �
�
PZ �
�
1�

Dễ dàng giải ra được:
q1 = Atan2(a24,a14)

q2  Atan2( a31a3  a34 , � (a11a3  a14 )2  (a21a3  a24 )2 )
q3 = Atan2(a31,a32) - q2

02:00:40 PM

12


QŨY ĐẠO CÁNH TAY ROBOT ĐỘNG HỌC NGƯỢC
Sử dụng Symbolic Math Toolbox và Robotics Toolbox
ta được quỹ đạo của cánh tay robot trong khoảng t = 0 - 2π/6 như sau:

02:00:40 PM

13

XÂY DỰNG MÔ HÌNH TOÁN HỌC
Hàm Lagrange:

LK P
Momen đặt lên khớp 1:

d �
L �
L
M1 

dt �
q&
�
q1
1
Momen đặt lên khớp 2:

d �
L �
L
M2 

dt �
q&2 �
q2
Momen đặt lên khớp 3:

d �
L �
L
M3 

dt �
q&3 �
q3
02:00:40 PM

Phương trình Động lực học:
..

.

M  H ( q ) q  V ( q, q )  G ( q )
0 �
�H11 0
�
Trong đó: H  �0
H
H
22
23 �
�
�0 H H �
32
33 �
�
V1 �

�0 �
�
� �
�
�
V �
V2 �
G �
G2 �
�
�
�
�
V
G
�3 �
�3�

14


3. SƠ ĐỒ ĐIỀU KHIỂN
Sơ đồ khối điều khiển cánh tay robot RRR trong không gian khớp:

02:00:40 PM

15


3. KẾT QUẢ MÔ PHỎNG

Góc quay khớp 1:

02:00:40 PM

16


3. KẾT QUẢ MÔ PHỎNG
Góc quay khớp 2:

02:00:40 PM

17


3. KẾT QUẢ MÔ PHỎNG
Góc quay khớp 3:

02:00:40 PM

18


4. KẾT LUẬN
Các kết quả đạt được.
Qua thời gian nghiên cứu làm đồ án em đã thu được kết quả như sau:
 Giải được bài toán động học thuận, động học ngược.
 Mô phỏng được bài toán động học thuận, động học.
 Xây dựng thành công mô hình toán học.
Xây dựng được sơ đồ khối điều khiển trong không gian khớp.

Điều khiển được bằng phương pháp PD bù trọng trường.

02:00:40 PM

Slide 19


4. KẾT LUẬN
Hướng phát triển đề tài.
Thiết kế cánh tay robot.
Điều khiển thích nghi cánh tay robot.
Em mong rằng qua đề tài này sẽ giúp em hiểu rõ hơn về công việc
của ngành sau khi ra trường là bước đệm để em phát triển công việc
sau này .
02:00:40 PM

Slide 20


XIN CHÂN THÀNH
CẢM ƠN!

Họ tên: Nguyễn Thị Như
Gmail:

02:00:40 PM

21

VẬN TỐC KHÂU TÁC ĐỘNG CUỐI
Đạo hàm vị trí khâu tác động cuối ta được
.

vE  rE

�.
PX
�
�.
�
PY
�.
PZ
�
�

�Trong
�
�
�
�
�
�

.

đó:

.

.

.

PX   q2 C1 (a3 S 23  a2 S 2 )  q1 S1 (a1  a3C23  a2C 2 )  q3 a3 S 23C1
.

.

.

.

PY  q1 C1 (a1  a3 S 23  a2C2 )  q2 S1 (a3 S 23  a2 S 2 )  q3 a3 S 23 S1
.

.

.

PZ  q2 (a3C23  a2C2 )  q3 a3C23
02:00:40 PM

22

02:00:40 PM

22

Thành phần quán tính H:
1
1
�1 2
�2 1 2
2�
2�
H11  I1  a1 � m1  m2  m3 � a3 m3  a2 � m2  m3 �C2  a3 m3C2223
�4
�8
�4
� 8
1
1
 a1a3m3C23  a2 a3m3C223  a1a2  m2  2m3  C2  a2 a3m3
2
2

�1
�
H 22  I 2  a � m2  m3 � a2 a3m3C3
�4
�
2
2

1 2
H 32  a3 m3
4

02:00:41 PM

H 23

1 2
 a3 m3
4

1 2
H 33  I 3  a3 m3
4
23


Thành phần tương hỗ V:
.
.
.
.
.
�1
� . . 1 2 � .
�
�
�
V1  a � m2  m3 �
S 22 q1 q2  a3 m3 �
2 q2  2 q3 �
S2223 q1  a1a3m3 �
q2  q3 �

S 23 q1
8
�
�
�
�
�4
�
.
.
.
.
1
�.
�
 a2 a3 m3 �
2q2  q3 �
S 223 q1  a1a2  2m3  m2  q2 S 2 q1
2
�
�
.
.
.
.
1
1
1
1
�

�
V2   a2 a3 m3 S3 q2 q3  a22 � m2  m3 �
S 22 q12  a32 m3 S 2223 q12
2
8
2
8
�
�
2
2

.
.
.
1
1
1
�
�
 a1a3 m3 S 23 q12  a2 a3 m3 S 223 q12  a1a2 � m2  m3 �
S 2 q12
2
2
2
�
�
.
.
.

.
.
1
1 2
1
1
2
V3   a2 a3 m3 S3 q2 q3  a3 m3 S 2223 q1  a1a3 m3 S 23 q1  a2 a3m3 S 223 q12
2
8
2
4
1 .2 1 .2 1 . .
 a2 a3 m3 S3 ( q1  q2  q2 q3 )
4
2
2
02:00:41 PM
24


Thành phần trọng trường G:

1
1
�
�
G2  gm2 a2C2  gm3 �a2C3  a3C23 �
2
2

�
�
1
�
�
G3  gm3 �
a2C3  a3C23 �
2
�
�

02:00:41 PM

25