TRƯỜNG ĐẠI HỌC BÁCH KHOA HÀ NỘI

VIỆN ĐIỆN
BỘ MÔN TỰ ĐỘNG HÓA CÔNG NGHIỆP

BÀI TẬP LỚN : TÍNH TOÁN THIẾT KẾ ROBOT MPL-80II

Giảng viên hướng dẫn : PGS.TS. Nguyễn Phạm Thục Anh

Nhóm sinh viên thực hiện MSSV Lớp

Vũ Trí Thành 20166751 CN ĐK&TĐH 02

Phạm Đức Long 20166386 CN ĐK&TĐH 01

Nguyễn Quang Linh 20166357 CN ĐK&TĐH 01

Lưu Long Nhật 20166544 CN ĐK&TĐH 01

Vũ Minh Chiến 20165811 CN ĐK&TĐH 01

Nguyễn Đình Dần 20160608 ĐK&TĐH 03

Phạm Quang Huy 20166197 CN ĐK&TĐH 02

Hồ Khánh Huy 20166185 CN ĐK&TĐH 02

Lương Xuân Phong 20166568 CN ĐK&TĐH 02

Đặng Tiến Đạt 20165947 CN ĐK&TĐH 02

Hà nội , tháng 6 năm 2020

1
 MỤC LỤC
CHƯƠNG 1. GIỚI THIỆU ROBOT MPL80II...................................................................................5
1.1 Lịch sử hình thành và phát triển robot.......................................................................................5
1.2 Robot MPL 80II.........................................................................................................................5
1.3 Ứng dụng công nghiệp...............................................................................................................6
1.4 Thông số kỹ thuật......................................................................................................................6
CHƯƠNG 2. XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG LỰC HỌC TRÊN MATLAB.......8
2.1 Xác định số khớp và các hệ trục tọa độ.....................................................................................8
2.2 Xác định các thông số động lực học của robot MPL-80II thông qua bảng DH........................9
2.3 Các ma trận A của robot MPL800II........................................................................................10
2.4 Tính toán ma trận T05...............................................................................................................11
2.5 Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab )................................................11
CHƯƠNG 3. VIẾT CHƯƠNG TRÌNH MATLAB TÍNH TOÁN MA TRẬN JACOBY THÔNG
QUA MA TRẬN JH..........................................................................................................................13
3.1 Ma trận Jacoby.........................................................................................................................13
3.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận JH.....................................16
CHƯƠNG 4. ĐỘNG HỌC NGƯỢC ROBOT..................................................................................18
4.1 Đặt vấn đề................................................................................................................................18
4.2 Tính động học ngược cho Robot MPL-80II............................................................................18
CHƯƠNG 5. THIẾT KẾ QUỸ ĐẠO CHUYỂN ĐỘNG CHO ROBOT..........................................22
5.1 Giới thiệu và cơ sở thiết kế quỹ đạo........................................................................................22
5.2 Tính toán thiết kế quỹ đạo chuyển động..................................................................................22
CHƯƠNG 6. MÔ PHỎNG ROBOT VÀ ĐIỀU KHIỂN CHUYỂN ĐỘNG BẰNG MATLAB
SIMSCAPE TOOLBOX....................................................................................................................26
6.1 Mô phỏng robot trên matlab simulink – simscape toolbox.....................................................26
6.2 Mô phỏng động lực học robot trên Simscape..........................................................................27
6.3 Điều khiển chuyển động của robot bằng thuật toán PID.........................................................32
CHƯƠNG 7. KẾT LUẬN.................................................................................................................38
CHƯƠNG 8. PHỤ LỤC....................................................................................................................39
8.1 Code tính động học thuận và Jacoby.......................................................................................39
8.2 Code GUIDE của động học thuận (chỉ hàm callback button).................................................40
8.3 Code GUIDE của Jacoby.........................................................................................................42
8.4 Code vẽ quỹ đạo chuyển động.................................................................................................43

1
 Danh mục hình ảnh

Hình 1.1: Robot MPL 80II...................................................................................................................5

Hình 1.2: Không gian làm việc của robot MPL 80II...........................................................................6
Hình 1.3: Một số mặt cắt của robot MPL - 80II..................................................................................7
Hình 1.4: Thông số kỹ thuật của robot MPL 80II................................................................................7
Y
Hình 2. 1:Gắn trục tọa độ cho robot.....................................................................................................8
Hình 2. 2 Giao diện............................................................................................................................12
Hình 2. 3 Chạy thử guide tính động học thuận..................................................................................12
Hình 3. 1 Giao diện............................................................................................................................17
Hình 3. 2 Chạy thử guide tính Jacoby................................................................................................17

Y
Hình 4. 1 Phương pháp phân li biến..................................................................................................19
Hình 4. 2 Biểu diễn q2 q3 trên trục tọa độ.........................................................................................20
Hình 5. 1 Đồ thị vị trí.........................................................................................................................24
Hình 5. 2 Đồ thị vận tốc.....................................................................................................................24
Hình 5. 3 Đồ thị gia tốc......................................................................................................................25

Y
Hình 6. 1 Sơ đồ mô phỏng.................................................................................................................26
Hình 6. 2 Mô hình 3D của đối tượng sau khi thiết kế........................................................................27
Hình 6. 3 Sơ đồ phương trình động lực học.......................................................................................32
Hình 6. 4 Mô phỏng trên simscape....................................................................................................33
Hình 6. 5 Trạng thái robot sau mô phỏng..........................................................................................33
Hình 6. 6 Đồ thị quỹ đạo vị trí, vận tốc của các khớp.......................................................................34
Hình 6. 7 Đồ thị quỹ đạo vị trí, vận tốc của khớp 1...........................................................................34
Hình 6. 8 Đồ thị quỹ đạo vị trí, vận tốc của khớp 2...........................................................................35
Hình 6. 9 Đồ thị quỹ đạo vị trí, vận tốc của khớp 3...........................................................................35
Hình 6. 10 Đồ thị quỹ đạo vị trí, vận tốc của khớp 4.........................................................................36
Hình 6. 11 Đồ thị quỹ đạo vị trí, vận tốc của khớp 5.........................................................................36
Hình 6.12 Sơ đồ thuật toán PID.........................................................................................................32
Hình 6.13 Sự ảnh hưởng của các tham số PID tới chất lượng điều khiển.........................................32
Hình 6.14 Thiết kế mô hình thuật toán PID trên simscape................................................................33
Hình 6.15 Hình 3D trạng thái robot sau khi mô phỏng......................................................................33
Hình 6.16 Quỹ đạo các khớp với các bộ PID tương ứng...................................................................34
2
Hình 6.17 Quỹ đạo của khớp 1 với bộ PID đã chọn..........................................................................34
Hình 6.18 Quỹ đạo của khớp 2 với bộ PID đã chọn..........................................................................35
Hình 6.19 Quỹ đạo của khớp 3 với bộ PID đã chọn..........................................................................35
Hình 6.20 Quỹ đạo của khớp 4 với bộ PID đã chọn..........................................................................36
Hình 6.21 Quỹ đạo của khớp 5 với bộ PID đã chọn..........................................................................36

3
 LỜI MỞ ĐẦU
Từ lâu , con người đã không còn xa lạ với Robot nữa . Bởi lẽ , Robot xuất hiện nhiều trong
cuộc sống của chúng ta và nếu chưa tận mắt chứng kiến những Robot này hoạt động thì cũng xem
được qua các phương tiện đại chúng thông tin .

Có thể thấy Robot đã đi vào mọi lĩnh vực đời sống con người . Vậy , Robot được ứng dụng
như thế nào ? Có thể nói , Robot được ứng dụng rất phong phú trong đời sống con người , cả trong
trường học và trong nông nghiệp . Đặc biệt trong công nghiệp , Robot được sử dụng rộng rãi để làm
một số công việc : vận chuyển nguyên vật liệu , sản phẩm , máy móc hay làm những công việc nặng ,
hoặc trong những môi trường có nhiệt độ cao , độc hại ,…

Robot được dùng trong một số ngành công nghiệp kỹ thuật cao như công nghiệp xe hơi , công
nghiệp máy bay … để làm những công việc như hàn thân xe , phun sơn … Những Robot này là những
cánh tay mô phỏng con người , mỗi tay máy bao gồm nhiều khâu liên kết với nhau bằng các khớp .
Các khâu này có thể chuyển động tương đối với nhau và làm thay đổi tầm với của Robot . Thông
thường những Robot này được đặt cố định một chỗ và chỉ thao tác được trong khả năng tầm với của
chúng .

Qua môn học Kỹ Thuật Robot do cô PGS.TS. Nguyễn Phạm Thục Anh giảng dạy , chúng em
đã được nhận đề tài : “ Robot MPL80II ”

Xong với những hiểu biết còn hạn chế nên nhóm em không tránh khỏi những sai sót , chúng
em rất mong được sự chỉ bảo của cô và sự góp ý kiến của các bạn trong lớp để được hoàn thiện hơn .

Chúng em xin chân thành cảm ơn

4
 CHƯƠNG 1. GIỚI THIỆU ROBOT MPL80II
1.1 Lịch sử hình thành và phát triển robot
Kể từ khi Robot đầu tiên được chế tạo với sản phẩm đầu tiên có tên gọi người máy công nghiệp là
verstran của công ty Mỹ vào năm 1960 các sản phẩm liên quan đến tay máy công nghiệp bắt đầu được
quan tâm nghiên và nhờ đó kỹ thuật Robot bắt đầu có những bước phát triển đầu tiên.
Cùng theo bản quyền của Mỹ các nước trên thế giới đă chạy đua sản xuất robot công
nghiệp,các công ty về Robot được thành lập để nghiên cứu và phát triển ,ứng dụng Robot trong tự
động hoá công nghiệp.
Yaskawa Motoman là một công ty chuyên cung cấp các giải pháp tự động hóa sáng tạo cho
hầu như tất cả các ngành công nghiệp và ứng dụng robot như: hàn, lắp ráp, sơn, pha chế, cắt gọt vật
liệu,..với hơn 175 mô hình robot khác biệt và hơn 40 giải pháp công nghệ để giải quyết các công việc
một cách hoàn chỉnh như các hệ thống vận hành , thiết bị an toàn .

Yaskawa Motoman là công ty đi đầu trong lĩnh vực phát triển các Robot phục vụ cho các công
nghệ gia công và tự động, Robot Motoman chính là sản phẩm nghiên cứu sản xuất thành công
của hãng

1.2 Robot MPL 80II

Robot MPL80II là Robot tác động nhanh, linh hoạt, nhỏ gọn và đáng tin cậy.
Đây là một loại Robot hoạt động với nhiều cài đặt. Nó cũng cung cấp rất nhiều ứng dụng, với
hiệu suất sử dụng cao, đảm bảo những yêu cầu về chất lượng, thời gian hoàn vốn ngắn.

MPL 800II là một robot tác động tốc độ cao với 5 trục, mang lại hiệu suất cao và độ tin
cậy cho việc xử lý thực phẩm, chọn, đóng gói và các ứng dụng xử lý vật liệu tốc độ cao khác.

5
 Hình 1.: Robot MPL 80II
MPL 80II được thiết kế cứng và thẳng, điều này dẫn đến độ ồn làm việc thấp, thời gian
bảo trì lâu. Ngoài ra nó còn được thiết kế nhỏ gọn, cổ tay mỏng, hiệu suất hoạt động cao ngay
cả trong những vị trí khó.

1.3 Ứng dụng công nghiệp

Robot MPL80II được ứng dụng cho các dây chuyền sản xuất tự động, hiện nay các lĩnh vực
phổ biến nhất là:

 Các quá trình hàn và nhiệt luyện.

 Công nghệ gia công lắp ráp.
 Phun sơn, vận chuyển hàng hóa.

1.4 Thông số kỹ thuật

- Số bậc tự do : 5
- Kiểu khớp : quay
- Vùng không gian làm việc :

6
 Hình 1.: Không gian làm việc của robot MPL 80II

- Một số mặt cắt của robot :

Hình 1.: Một số mặt cắt của robot MPL - 80II

7
 Hình 1.: Thông số kỹ thuật của robot MPL 80II
CHƯƠNG 2. XÂY DỰNG PHẦN MỀM TÍNH TOÁN ĐỘNG LỰC
HỌC TRÊN MATLAB
2.1 Xác định số khớp và các hệ trục tọa độ
- Robot MPL80II là robot có 5 trục tự do .

- Ta sẽ lần lượt xác định các trục tọa độ như sau :

 Xác định trục Zi : vì tất cả các khớp đều là khớp quay nên Zi là trục mà khớp i+1 quay xung
quanh nó .
 Xác định trục Xi , Yi : xác định như hình bên dưới .

Hình 2.:Gắn trục tọa độ cho robot

8
 2.2 Xác định các thông số động lực học của robot MPL-80II thông qua bảng DH
- Ta xác định các thông số của bảng D-H như sau :

 di : khoảng cách Xi-1 với Xi theo trục Zi-1.

 i : góc quay của thanh nối Xi-1 quanh trục Zi-1 theo quy tắc bàn tay phải để song song với Xi .
 ai : khoảng cách Zi-1 tơi Zi theo trục Xi .
 αi : góc vặn của thanh nối Zi-1 tới Zi quanh trục Xi theo quy tắc bàn tay phải .

- Như vậy ta có :

 a1 = 145 (mm) là khoảng cách giữa Z0 và Z1 .

 a2 = 870 (mm) là khoảng cách giữa Z1 và Z2 .
 a3 = 1046 (mm) là khoảng cách giữa Z2 và Z3 .
 a4 = 0 (mm) do Z3 với Z4 giao nhau .
 a5 = 0 do Z4 với Z5 trùng nhau .
π
 α1 = - (rad) là góc quay quanh trục X1 để biến Z0 thành Z1 .
2
 α2 = π (rad) là góc quay quanh trục X2 để biến Z1 thành Z2 .
 α3 = 0 là góc quay quanh trục X3 để biến Z2 thành Z3 .
π
 α4 = (rad) là góc quay quanh trục X4 để biến Z3 thành Z4 .
2
 α5 = 0 là góc quay quanh trục X5 để biến Z4 thành Z5 .
 d5 = 175 (mm) là khoảng cách giữa X4 và X5.

- Ta được bảng D-H như sau :

Khâu tác động αi ai i di

1 π 145 q1 0
-
2
2 π 870 q2 0
3 0 1046 q3 0
4 π 0 q4 0
2
5 0 0 q5 175

9
Với:

 q1=1
 q2=2-pi/2
 q3=3+β-pi/2
 q4=4- β
 q5=5

β = atan2(210,1025)≈ 0,2021 (rad)

(i là góc quay của các khớp i = 1÷5)

2.3 Các ma trận A của robot MPL800II

Ký hiệu :

C1 = cos(p1) , C2 = cos(p2), … , C5 = cos(p5)

S1 = sin(p1), S2 = sin(p2), … , S5 = sin(p5)

Các ma trận A được xác định bằng ma trận biến đổi tọa độ thuần nhất tổng quát :

Cα i −S i . Cα i S i Sα i a Ci

Ai=
[ Si
0
0
C i C α i −C i S α i a S α i
Sα i
0
C αi
0
di
1
]
C1 0 −S1 a1 C 1 C2 S 2 0 a2 C 2
A1=
[ S1 0

0 0
C 1 a1 S1
0 −1 0
0
0
1
] A2=
[ S 2 −C2 0 a 2 S2
0
0
0 −1
0 0
0
1
]
C3 −S 3 0 a3 C 3 C4 0 S4 0
A3=
[ S3 C 3
0
0
0
0
0 a3 S3
1
0
0
1
] A4=
[ S4
0
0
0 −C 4
1
0
0
0
0
0
1
]
10
 C5 −S 5 0 0
A5=
[ S5 C 5
0
0
0
0
0 0
1 175
0 1
]
2.4 Tính toán ma trận T05
Từ các ma trận chuyển đổi giữa các trục , ta sẽ xác định được hàm truyền 0T5

nx ox ax px
0

[
n oy ay
T5 = y
nz o z a z
0 0 0
py
pz
1
]
= A1. A2. A3. A4 . A5

Sử dụng công cụ matlab để nhập và nhân các ma trận . Sau khi dùng hàm simplify để rút gọn , ta
sẽ được thành phần của ma trận như sau :

nx = sin(q1)*sin(q5) + cos(q3 - q2 + q4)*cos(q1)*cos(q5)

ny = cos(q3 - q2 + q4)*cos(q5)*sin(q1) - cos(q1)*sin(q5)

nz = sin(q3 - q2 + q4)*cos(q5)

ox = cos(q5)*sin(q1) - cos(q3 - q2 + q4)*cos(q1)*sin(q5)

oy = - cos(q1)*cos(q5) - cos(q3 - q2 + q4)*sin(q1)*sin(q5)

oz = -sin(q3 - q2 + q4)*sin(q5)

ax = sin(q3 - q2 + q4)*cos(q1)

ay = sin(q3 - q2 + q4)*sin(q1)

az = -cos(q3 - q2 + q4)

px = cos(q1)*(1046*cos(q2 - q3) + 175*sin(q3 - q2 + q4) + 870*cos(q2) + 145)

py = sin(q1)*(1046*cos(q2 - q3) + 175*sin(q3 - q2 + q4) + 870*cos(q2) + 145)

pz = - 1046*sin(q2 - q3) - 175*cos(q3 - q2 + q4) - 870*sin(q2)

2.5 Xây dựng phần mềm tính toán ( sử dụng GUI trong Matlab )
11
 Giao diện phần mềm tính toán động học thuận vị trí

- Từ ma trận hàm truyền ta có thể thấy rằng các giá trị của các thành phần phụ thuộc vào các giá trị
của các tham số đầu vào là :

 Các góc : 1, 2, 3, 4, 5 .

- Sau khi thiết kế giao diện và viết code ta sẽ được như hình dưới :

Hình 2.: Giao diện guide tính động học thuận.

- Ta sẽ chạy thử một ví dụ và kết quả như sau :

12
 Hình 2.: Chạy thử guide tính động học thuận.

Chương trình code : tham khảo phụ lục 1 và 2

CHƯƠNG 3. VIẾT CHƯƠNG TRÌNH MATLAB TÍNH TOÁN MA

TRẬN JACOBY THÔNG QUA MA TRẬN JH
3.1 Ma trận Jacoby

nx ox ax 0 0 0

[ ]
ny oy ay 0 0 0
nz o z a z 0 0 0 H
J= . J
0 0 0 nx o x a x
0 0 0 n y o y ay
0 0 0 n z o z az

nx ox ax 0 0 0 JH 11 JH 12 JH 13 JH 14 JH 15

[ ][ ]
ny oy ay 0 0 0 JH 21 JH 22 JH 23 JH 24 JH 25
nz o z a z 0 0 0 JH 31 JH 32 JH 33 JH 34 JH 35
J= .
0 0 0 nx o x a x JH 41 JH 42 JH 43 JH 44 JH 45
0 0 0 n y o y ay JH 51 JH 52 JH 53 JH 54 JH 55
0 0 0 n z o z az JH 61 JH 62 JH 63 JH 64 JH 65

- Ta xác định các ma trận iTn theo các hệ tọa độ lần lượt từ khâu cuối trở về gốc :

cos( q 5) −sin( q 5) 0 0
4
T5 =
[
sin( q 5) cos(q 5)
0
0
0
0
0 0
1 175
0 1
]
cos( q 4)∗cos (q 5) −cos (q 4)∗sin (q 5) sin(q 4) 175∗sin (q 4 )
3
T5 =
[
cos (q 5)∗sin (q 4 ) −sin (q 4)∗sin(q 5) −cos (q 4) −175∗cos (q 4)
sin(q 5)
0
cos (q 5)
0
0
0
0
1
]
cos( q 3+q 4)∗cos(q 5) −cos(q 3+q 4)∗sin (q 5) sin (q 3+ q 4) Px
2
T5 =
[ sin(q 3+q 4)∗cos (q 5) −sin(q 3+q 4 )∗sin(q 5) −cos (q 3+ q 4 ) Py
sin(q 5)
0
cos (q 5)
0
0
0
0
1
]
13
 Với: Px=175∗sin(q 3+ q 4 )+1046∗cos (q 3)

Py=1046∗sin(q 3)−175∗cos (q 3+ q 4 )

cos(q 3−q 2+ q 4 )∗cos (q 5) −cos (q 3−q 2+q 4)∗sin(q 5) sin(q 3−q 2+q 4) Px
1
T5 =
[−sin( q 3−q 2+ q 4)∗cos (q 5) sin (q 3−q 2+q 4)∗sin (q 5) cos (q 3−q 2+ q 4) Py
−sin(q 5)
0
−cos( q 5)
0
0
0
0
1
]
Với: Px= 1046*cos(q2 - q3) + 175*sin(q3 - q2 + q4) + 870*cos(q2)

Py= 1046*sin(q2 - q3) + 175*cos(q3 - q2 + q4) + 870*sin(q2)

nx ox ax px
0

[n oy ay
T5 = y
nz o z a z
0 0 0
py
pz
1
]
Các hệ số của 0T5 được xác định ở phần động lực thuận vị trí .

- Tiếp theo ta sẽ tính toán ma trận HJ bằng việc tính các ma trận theo từng cột , tương ứng với
từng khớp của robot.
H
J11 = 1ny. 1px – 1nx. 1py
H
J21 = 1oy. 1px – 1ox. 1py
H
J31 = 1ay. 1px – 1ax. 1py
H
J41= 1nz
H
J51= 1oz
H
J61= 1az

H
J12 = 2ny. 2px – 2nx. 2py
H
J22 = 2oy. 2px – 2ox. 2py
H
J32 = 2ay. 2px – 2ax. 2py
H
J42= 2nz
H
J52= 2oz
H
J62= 2az

H
J13 = 3ny. 3px – 3nx. 3py
H
J23 = 3oy. 3px – 3ox. 3py
H
J33 = 3ay. 3px – 3ax. 3py
H
J43= 3nz
H
J53= 3oz
14
 H
J63= 3az

H
J14 = 4ny. 4px – 4nx. 4py
H
J24 = 4oy. 4px – 4ox. 4py
H
J34 = 4ay. 4px – 4ax. 4py
H
J44= 4nz
H
J54= 4oz
H
J64= 4az

H
J15 = 5ny. 5px – 5nx. 5py
H
J25 = 5oy. 5px – 5ox. 5py
H
J35 = 5ay. 5px – 5ax. 5py
H
J45= 5nz
H
J55= 5oz
H
J65= 5az

Sau khi tính được HJ ta tính được ma trận Jacobi của Robot bằng công thức:

R0n 0 H
J=
[ ]
0 R0n
x J

Ta được kết quả như sau:

J 11 J 12 J 13 J 14 J 15

[ ]
J 21 J 22 J 23 J 24 J 25
J 31 J 32 J 33 J 34 J 35
J=
J 41 J 42 J 43 J 44 J 45
J 51 J 52 J 53 J 54 J 55
J 61 J 62 J 63 J 64 J 65

Với:

J11 = -S1*(870*C2 + 1046*C23 + 175*S234 + 145)

J12 = C1*(870*C2 + 1046*C23 + 175*S234 + 145)

J13 = J14 = J15 = 0

J16 = 1

J21 = -C1*(175*C234 + 870*S2 + 1046*S23)

J22 = -S1*(175*C234 + 870*S2 + 1046*S23)

15
J23 = - 870*C2 - 1046*C23 - 175*S234

J24 = - S1

J25 = C1

J26 = 0

J31 = (175*cos(q1 + q2 - q3 - q4))/2 + 523*sin(q1 + q2 - q3) - 523*sin(q1 - q2 + q3) + (175*cos(q1

- q2 + q3 + q4))/2

J32 = (175*sin(q1 + q2 - q3 - q4))/2 - 523*cos(q1 + q2 - q3) + 523*cos(q1 - q2 + q3) + (175*sin(q1

- q2 + q3 + q4))/2

J33 = 1046*C23 + 175*S234

J34 = S1

J35 = - C1

J36 = 0

J41 = 175*C1*C234

J42 = 175*C234*S1

J43 = 175*S234

J44 = S1

J45 = -C1

J46 = 0

J51 = J52 = J53 = 0

J54 = C1*S234

J55 = S1*S234

J56 = -C234

Với: C234 = cos(-q2+q3+q4), S234 = sin(-q2+q3+q4)

C23 = cos(q2-q3), S23 = sin(q2-q3)

Vì kết quả quá dài nên các bước tính toán và kết quả sẽ được thực hiện trên matlab, tham khảo
thêm code matlab ở phụ lục để biết thêm chi tiết.

16
 3.2 Viết chương trình Matlab tính ma trận Jacoby thông qua ma trận JH
a) Thiết kế giao diện trên GUIDE

Hình 3.: Giao diện gui tính Jacoby

- Nhập thử số ta sẽ được kết quả như sau :

17
 Hình 3.: Chạy thử guide tính Jacoby robot.

Chương trình code : tham khảo phụ lục 1 và 3

CHƯƠNG 4. ĐỘNG HỌC NGƯỢC ROBOT

4.1 Đặt vấn đề
Trong bài toán động học ngược ta cần xác định giá trị các biến khớp của Robot khi đã biết vị trí
và hướng của điểm tác động cuối. Vị trí và hướng của khâu tác động cuối được xác định từ quỹ đạo
chuyển đông cho trước của robot, dưới dạng ma trận:

nx ox ax px
0

[n oy ay
Tn = y
nz o z a z
0 0 0
py
pz
1
]
Việc giải bài toán động học ngược đặc biệt có ý nghĩa nhất là trong việc điều khiển Robot. Bởi
trong thực tế điều ta biết là vị trí và hướng của cơ cấu tác động cuối mà ta muốn dịch chuyển tới và
ta cần phải xác định giá trị của các biến khớp để đáp ứng được yêu cầu dịch chuyển đó. Tuy nhiên
bài toán động học ngược thông thường khó giải và không có lời giải tổng quát cho mọi Robot.
Trong quá trình giải bài toán động học ngược ta có thể gặp các trường hợp:

Các phương trình có dạng phi tuyến và siêu việt thường không cho lời giải đúng.
Có thể có nhiều lời giải.
Có thể có nhiều nghiệm tìm được bằng toán học lại không chấp nhận được
về mặt vật lý, do các ràng buộc về kết cấu robot.

Tính đa nghiệm của bài toán động học vào số bậc tự do của Robot, và vị trí mà
cơ cấu tác động cuối cần thực hiện. Khi số bậc tự do của Robot càng lớn thì số lời
giải chấp nhận được càng nhiều. Do đó việc tìm kiếm một nghiệm phù hợp đòi hỏi
có một trực giác về toán học và về kết cấu để dự đoán được những điểm hoặc khu
vực khả dĩ mà lựa chọn nghiệm cho thích hợp.

4.2 Tính động học ngược cho Robot MPL-80II

Vì robot MPL 80II có tới 5 bậc tự do nên ta phải dùng phương pháp phân ly biến để giải bài
toán động học ngược:

nx ox ax px
0

[n oy ay
T5 = y
nz o z a z
0 0 0
py
pz
1
]
= A1. A2. A3. A4 . A5

18
 Hình 4.: Phương pháp phân li biến
Đầu tiên ta có:
0
T5 (robot) = 0T5 (bài toán đặt ra)

Ta thấy từ đây không thể tìm ra được bất cứ góc nào bằng cách cân bằng phần tử hai ma trận, vì
vậy ta nhân A1-1 cho cả hai vế trên được:

C 234∗C 5 −C 234∗S 5 S 234 1046∗C 23+175∗S 234+870∗C 2

VT=1T5 =
[
−S 234∗C 5 S 234∗S 5 C 234 175∗C 234 +1046∗S 23+ 870∗S 2
−S 5
0
−C 5
0
0
0
0
1
]
C 1∗nx+ S 1∗ny C 1∗ox + S 1∗oy C 1∗ax + S 1∗ay C 1∗px + S 1∗py−145
VP =
[ −nz −oz −az
C 1∗ny −S 1∗nx C 1∗oy−S 1∗ox C 1∗ay−S 1∗ax
0 0 0
− pz
C 1∗py−S 1∗px
1
]
Cân bằng các phần tử của ma trận VT và VP ta được:

 VT(3,3) = VP(3,3)  C 1∗ay−S 1∗ax = 0

 q1 = ATAN2 (ay,ax)  1 = q1

 VT(3,1) = VP(3,1)  S5 =−C 1∗ny +S 1∗nx

VT(3,2) = VP(3,2)  C5 = −C 1∗oy +S 1∗ox

 q5 = ATAN2 (S5,C5)  5 = q5

 VT(1,3) = VP(1,3)  S234 = C 1∗ax+ S 1∗ay

VT(2,3) = VP(2,3)  C234 = - az

19
 q234 = ATAN2 (S234,C234)

 VT(1,4) = VP(1,4)

 1046∗C 23+870∗C 2= C 1∗px + S 1∗py−145−175∗S 234

VT(2,4) = VP(2,4)

 1046∗S 23+870∗S 2= - pz−175∗C 234

Đặt: a3 = 1046 / a2 = 870 / p1 = C 1∗px + S 1∗py−145−175∗S 234 /

p2 = - pz−175∗C 234

20