Luận văn tốt nghiệp.

Chương 5

CHƯƠNG 5: KẾT QUẢ TÍNH TOÁN

5.1 Mô hình:
Ngày nay yêu cầu sản xuất những sản phẩm chất lượng cao với sản lượng tối đa và
giá thành thấp nhất có thể đang là động lực thúc đẩy các nhà sản xuất đưa robot hàn vào
sàn máy. Mô hình robot hàn cố định, hàn được trong mặt phẳng nằm ngang là đối tượng
nghiên cứu chính của đề tài. Hình 5.1 mô tả hình dáng robot hàn trong đề tài.

Hinh 5.1 Mô hình robot Scara đang hàn.

5.2. Giải bài toán động học robot Scara.

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

5.2.1.1 Giới thiệu.

Bài toán động học thuận nhằm mô tả vị trí và hướng của phần công tác dưới dạng
hàm số của các biến khớp. Bài toán này được giải khi nhập vào các thông số là các góc
quay hay tịnh tiến của tay máy.

Một trong những phương pháp giải bài toán động học thuận là dùng trực tiếp hình
học giải tích. Phương pháp tính toán trực tiếp này chỉ áp dụng được cho các cơ cấu đơn
giản. Để có thể giải quyết các bài toán tổng quát cần có một giải thuật chung. Một trong
những giải thuật như vậy xuất phát từ quy tắc Denavit-Hartenberg (mục 2.3.2). Đó là quy
tắc thiết lập hệ thống tọa độ trên các cặp khâu – khớp trên tay máy. Dựa trên hệ tọa độ

GVTH: TS. Trần Huy Long 59 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

này có thể mô tả các cặp bằng hệ thống các tham số, biến khớp và áp dụng một dạng
phương trình tổng quát cho bài toán động học tay máy.

5.2.1.2 Giải bài toán động học thuận cho robot Scara.

Bây giờ chúng ta xét tay máy 4 bậc tự do của đề tài, gắn hệ trục tọa độ như hình 4.4

+ Đặt các hệ trục tọa độ zi ( = 0,1,2,3,4) trùng với phương của trục khớp động
(1,2,3,4).

+ Trục Xi của hệ tọa độ thứ i nằm dọc theo đường vuông góc chung hướng từ khớp i
đến i +1.

Khi gắn các hệ tọa độ lên các khâu phải tuân theo các phép biến đổi của ma trận Ai. Đó
là các phép biến đổi:

R(z,θi) , Tp(0,0,di) , Tp(a,0,0) , R(x,αi)

+ Khớp 1: Ta đặt trục Z0 trùng với trục quay của khâu 1 để được phép biến đổi
R(z,θ1), quay quanh trục Z0 một góc θ1.

+ Khớp 2: Tương tự khớp 1 ta có được phép biến đổi R(z,θ2), quay quanh trục Z1
một góc θ2.

+ Khớp 3: Ta đặt trục Z2 theo phương và hướng tay kẹp để có được phép biến đổi
Tp(0,0,d3), tịnh tiến dọc trục Z2 một khoảng d3.

+ Khớp 4: Tương tự khớp 1 và 2 ta đặt trục Z3 theo hướng và phương của bàn kẹp
để được phép biến đổi R(z,θ3), quay quanh trục Z3 một góc θ4.

GVTH: TS. Trần Huy Long 60 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.2 Mô hình robot Scara.

Với việc gắn khung tọa độ như hình 4.4 ta có bảng thông số D-H như sau:

Bảng 5.1 thông số Denavit & Hartenberg.

Khâu ai di

1 0 a1 0

2 180 a2 0

3 0 0 0 d3
4 0 0 d4

Xác định các ma trận Ai theo các thông số DH từ bảng trên, ta có:
Với ma trận Ai:

(5.1)

Từ đó ta có các ma trận Ai như sau:

(5.2)

(5.3)

GVTH: TS. Trần Huy Long 61 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

(5.4)

(5.5)

Các ma trận biến đổi đồng nhất:

(5.6)

(5.7)

(5.8)

(5.9)

GVTH: TS. Trần Huy Long 62 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Trong đó ma trận 0T4 thể hiện vị trí và hướng của khâu tác động cuối với hệ tọa độ
cố định.

TE = = 0T4. (5.10)

Phương trình (5.9) là phương trình động học thuận cho robot của đề tài.

5.2.1.3 Lập trình Matlab để giải bài toán động học thuận robot Scara.

Lập trình Matlab với giao diện như hình 5.3, yêu cầu người dùng nhập các thông
số kích thước của robot Scara vào là a1, a2 và d4.

Sau đó là chọn bài toán cần giải ,ở đây là chọn bài toán thuận.

Hình 5.3 Nhập thông số robot Scara.

GVTH: TS. Trần Huy Long 63 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Các kích thước của robot Scara được ghi trong hình vẽ của chương trình cùng với
bảng thông số Denavit & Hartenberg.

Tiếp theo nhập các thông số góc quay của các khâu và độ dài tịnh tiến của khớp
tịnh tiến như trên hình 5.4

Hình 5.4 Nhập các giá trị biến khớp

Khi đó kết quả đươc thể hiện trong các bảng ở bên phải màn hình, gồm có các ma
trận biến đổi thuần nhất. Dựa vào các ma trận này ta có thể tính được vị trí của khâu tác
động cuối so với gốc tọa độ cố định.

GVTH: TS. Trần Huy Long 64 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.5 Xem kết quả.

Hình 5.5 thể hiện quá trình xem và lưu kết quả. Kết quả có thể xem lại sau khi lưu

5.2.2 Bài toán động học ngược robot Scara.

5.2.2.1 Giới thiệu.

Bài toán ngược nhằm xác định bộ thông số động học để đảm bảo chuyển động cho
trước của phần công tác. Thông thường bài toán động học ngược tay máy luôn có nhiều
nghiệm. Để giải người ta đưa ra các ràng buộc về mặt động học, hoặc đưa ra vấn đề phải
tối ưu hóa hoat động của tay máy theo một hàm mục tiêu nào đó.

5.2.2.2 Giải bài toán động học ngược cho robot Scara.

Hệ phương trình động học của robot Scara đã được thiết lập trong phần trước.

Nhân lần lượt bên trái ma trận T4 với các ma trận nghịch đảo Ai-1ta có:

A1-1T4 =1T4. (5.11)

GVTH: TS. Trần Huy Long 65 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

A2-1A1-1T4 =2T4. (5.12)

A3-1 A2-1A1-1T4 =3T4. (5.13)

Tính d3, ta có:

(5.14)

(5.15)

So sánh các phần tử ở hàng 3 cột 4 của (4.14) và ma trận (4.15) ta có:

Pz = -(d3 + d4) (5.16)

Từ đó ta có: d3 = - (d4 +pz). (5.17)

Tính θ1:

Tiếp tục tách và cân bằng các phần tử ở hàng 1 và cột 4 và hàng 2 cột 4 của
(4.14) và ma trận (4.15) ta được hệ 2 phương trình lượng giác sau:

(5.18)

Bình phương hai vế 2 phương trình rồi rút gọn ta được:

Px2 +py2 -2a1(C1px +S1py) +a12 = a22

Đặt C1px + S1py =A. (5.19)

Suy ra: A= (5.20)

GVTH: TS. Trần Huy Long 66 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Đặt px = rcos và py = rsin

Với

Từ (4.19) ta được:

Với

Hay là:

Với

Từ đó: (5.21)

Trong đó dấu trừ phù hợp với góc quay trái và dấu cộng phù hợp với góc quay
phải của robot. Kết quả:

(5.22)

Tính θ2:

Từ (4.18) ta có: (5.23)

Vậy: (5.24)

Tính θ4:

Từ hệ phương trình động học của robot ở trên ta có hai phương trình:

(5.25)

GVTH: TS. Trần Huy Long 67 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Do đó: (5.26)

Vậy tập nghiệm bài toán động học ngược của robot Scara 4 bậc tự do là:

(5.27)

5.2.2.3 Giải bài toán động học ngược cho robot Scara dùng chương trình Matlab.

Tương tự trước tiên ta nhập các thông số kích thước của robot Scara, sau đó là
chọn bài toán ngược như hình 5.8

Hình 5.6 Bài toán động học ngược.

Tiếp theo ta sẽ nhập vào các thông số của vị trí tác động cuối, để khách quan ta
dùng kết quả của bài toán thuận để nhập vào hình 5.7

GVTH: TS. Trần Huy Long 68 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.7 Giải bài toán động học ngược.

Khi đó các giá trị biến khớp được giải ra và bằng giá trị mà ta đã nhập vào trong bài
toán động học thuận. Kết quả được in ra màn hình và ta có thể lưu lại để xem kết quả về
sau.
5.3 Ứng dụng kiểm chứng
Thuật toán tối ưu và phương pháp biến đổi ngược các ma trận thuần nhất 4x4, áp
dụng cho tất cả các loại robot có n bậc tự do. Nhưng đối với mỗi loại robot riêng biệt thì
có các chương trình tính riêng.
Ta áp dụng thuật toán tối ưu và phương pháp biến đổi ngược các ma trận thuần nhất
4x4 cho tay máy SCARA : có 4 khâu và có cơ cấu kiểu RRTR.

GVTH: TS. Trần Huy Long 69 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.8. Robot SCARA và các hệ tọa độ (vị trí ban đầu)

Khâu

1 0 0

2 180 0

3 0 0 0

4 0 0

Hình 5.9 Bảng thông số DH của robot SCARA

Bằng ngôn ngữ lập trình MATLAB, ta viết chương trình Prog_main.m để xây dựng
thuật toán tối ưu.
clear all
disp('SO DIEM TREN DUONG DAN:')
sd=input('sd = ')
% DIEU KIEN BIEN chuyen dong khop:
theta0 = pi/2;
d0 = 800;
% CAC THONG SO DH KHOI TAO: xet tay may SCARA 4 BTD, 1,2,4 la khop ban
le, 3 la khop tinh tien

GVTH: TS. Trần Huy Long 70 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

%CAC bien khop KHOI TAO:')

theta1 =0;
theta2 =0;
theta4 =0;
d3 = 100;
eps = input('Do chinh xac cho phep, eps = ')
for k=1:sd
disp('TOA DO CUA DIEM KHAO SAT THU:')
k
disp('Enter to continue')
pause
px0=input('Toa do cua Ak tren Ox0 (0<px0<1000),px0 (mm)= ')
py0=input('Toa do cua Ak tren Oy0 (0<py0<1000),py0 (mm)= ')
pz0=input('Toa do cua Ak tren Oz0 (-800<pz0<-250),pz0 (mm)= ')
j_max=5;
j=1;
stopp=0;
while stopp==0&j<=j_max
prog_Optimal; %tiem can diem cuc tri
prog_bind; %kiem tra dieu kien rang buoc
j=j+1;
end
disp('KET QUA (don vi goc: RADIAN, tinh tien: mm) ')
theta=[];
sso
theta1(k,1)=teta1%=(theta1/pi)*180
theta2(k,1)=teta2%=(theta2/pi)*180
theta4(k,1)=teta4%=(theta4/pi)*180
d3(k,1)=d3
save rresult theta1 theta2 theta4 d3 sso
end
%plot3(ppx0,ppy0,ppz0)
%grid on
%axis square

Bằng phần mềm EASY – ROB, ta mô phỏng và kiểm chứng chuyển động của tay máy.
5.3.1 Kiểm chứng thuật toán bằng đường dẫn thực tế là mối hàn

GVTH: TS. Trần Huy Long 71 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

- Ta có 2 tấm kim loại ghép lại với nhau :

Hình 5.10 Hình ảnh mối ghép hàn theo đường thẳng.
- Yêu cầu đặt ra là quỹ đạo tay máy chuyển động sao cho trùng với lại mối ghép của
tấm kim loại.
+ Đầu tiên ta gắn đường hàn vào hệ trục tọa độ tuyệt đối của tay máy
+ Trích mẫu tọa độ của một số điểm rời rạc.
X Y Z
150 300 -450
150 330 -450
150 360 -450
150 390 -450
150 420 -450
150 450 -450
150 480 -450
150 510 -450
150 540 -450
150 570 -450
150 600 -450
180 600 -450
210 600 -450
240 600 -450
270 600 -450
300 600 -450

GVTH: TS. Trần Huy Long 72 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

330 600 -450

360 600 -450
390 600 -450
420 600 -450
450 600 -450
480 600 -450
510 600 -450
540 600 -450
570 600 -450
600 600 -450
630 600 -450
660 600 -450
690 600 -450
720 600 -450

Bảng 4.5 Tọa độ các điểm trên đường hàn theo đường thẳng.
- Chạy chương trình Pro_main.m, ta có được bảng tọa độ suy rộng.
Sai số (10-4)
-0.1216 2.4575 199.9998 0 9.7457
-0.0557 2.3997 199.9998 0 9.6203
0.0058 2.3403 199.9998 0 9.8733
0.0639 2.2794 199.9998 0 9.8309
0.1192 2.2170 199.9998 0 8.8406
0.1725 2.1532 199.9998 0 9.8874
0.2241 2.0877 199.9998 0 9.1025
0.2744 2.0206 199.9998 0 9.8214
0.3240 1.9517 199.9998 0 9.4996
0.3730 1.8809 199.9998 0 9.3320
0.4218 1.8080 199.9998 0 9.3612
0.3855 1.7877 199.9998 0 9.9373
0.3522 1.7638 199.9998 0 9.9849
0.3221 1.7363 199.9998 0 9.3589
0.2952 1.7054 199.9998 0 9.2894
0.2717 1.6710 199.9998 0 9.8073
0.2514 1.6330 199.9998 0 9.9936
0.2346 1.5916 199.9998 0 9.9755

GVTH: TS. Trần Huy Long 73 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

0.2211 1.5466 199.9998 0 9.9195

0.2111 1.4979 199.9998 0 9.9195
0.2046 1.4455 199.9998 0 9.9196
0.2016 1.3890 199.9998 0 9.9206
0.2022 1.3282 199.9998 0 9.9337
0.2066 1.2627 199.9998 0 9.7116
0.2150 1.1920 199.9998 0 9.6659
0.2278 1.1152 199.9998 0 9.6240
0.2454 1.0312 199.9998 0 9.9225
0.2687 0.9382 199.9998 0 9.9454
0.2989 0.8336 199.9998 0 9.8214
0.3385 0.7124 199.9998 0 9.9870

Bảng 4.6 : Tọa độ suy rộng của đường hàn

- Từ bảng tọa độ suy rộng, xây dựng chương trình mô phỏng.
PROGRAMFILE
! prgfln C:\Documents and Settings\Jerry\Desktop\EASY_ROB\EASY_ROB\proj\
duongdanthucteA11.prg
ERC TRACK ON
PTP_AX -6.9671 140.8043 0.20000 0.0000
PTP_AX -3.1913 137.4926 0.20000 0.0000
PTP_AX 0.3323 134.0893 0.20000 0.0000
PTP_AX 3.6612 130.5999 0.20000 0.0000
PTP_AX 6.8296 127.0247 0.20000 0.0000
PTP_AX 9.8835 123.3692 0.20000 0.0000
PTP_AX 12.8399 119.6163 0.20000 0.0000
PTP_AX 15.7219 115.7718 0.20000 0.0000
PTP_AX 18.5638 111.8241 0.20000 0.0000
PTP_AX 21.3713 107.7676 0.20000 0.0000
PTP_AX 24.1673 103.5907 0.20000 0.0000
PTP_AX 22.0875 102.4276 0.20000 0.0000
PTP_AX 20.1795 101.0582 0.20000 0.0000
PTP_AX 18.4549 99.4826 0.20000 0.0000
PTP_AX 16.9137 97.7122 0.20000 0.0000
PTP_AX 15.5672 95.7412 0.20000 0.0000
PTP_AX 14.4041 93.5640 0.20000 0.0000
PTP_AX 13.4415 91.1919 0.20000 0.0000
PTP_AX 12.6680 88.6136 0.20000 0.0000
PTP_AX 12.0951 85.8233 0.20000 0.0000
PTP_AX 11.7227 82.8210 0.20000 0.0000

GVTH: TS. Trần Huy Long 74 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

PTP_AX 11.5508 79.5838 0.20000 0.0000

PTP_AX 11.5852 76.1002 0.20000 0.0000
PTP_AX 11.8373 72.3473 0.20000 0.0000
PTP_AX 12.3185 68.2965 0.20000 0.0000
PTP_AX 13.0519 63.8962 0.20000 0.0000
PTP_AX 14.0603 59.0834 0.20000 0.0000
PTP_AX 15.3953 53.7549 0.20000 0.0000
PTP_AX 17.1257 47.7617 0.20000 0.0000
PTP_AX 19.3946 40.8175 0.20000 0.0000
ENDPROGRAMFILE
- Chạy mô phỏng chương trình :

Hình 5.11: Hình ảnh tay máy đang hàn

Bằng ngôn ngữ lập trình MATLAB, ta thiết kế giao diện guide để giải bài toán
động học ngược theo phương pháp biến đổi ngược các ma trận thuần nhất 4x4.
Từ Bảng 4.5 tọa độ các điểm trên đường hàn theo đường thẳng ta nhập các thông số
tọa độ các điểm vào chương trình Matlab áp dụng cho bài toán giải động học ngược ta
được các tọa độ suy rộng như sau:

-6.9676 140.805 200 0

-3.107 137.4934 200 0
0.33463 134.091 200 0

GVTH: TS. Trần Huy Long 75 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

3.6615 130.6019 200 0

6.8324 127.0276 200 0
9.8815 123.367 200 0
12.8375 119.617 200 0
15.7242 115.7726 200 0
18.5626 111.8267 200 0
21.3713 107.7702 200 0
24.1679 103,5916 200 0
22.0817 102.4273 200 0
20.181 101.0578 200 0
18.4558 99.4856 200 0
16.9161 97.7124 200 0
15.5654 95.7392 200 0
14.4062 93.5661 200 0
13.4403 91.1918 200 0
12.6695 88.6133 200 0
12.0954 85.8252 200 0
11.7205 82.8192 200 0
11.5484 79.5836 200 0
11.5846 76.1017 200 0
11.8377 72.3501 200 0
12.3204 68.2967 200 0
13.0519 63.89611 200 0
14.0615 59.0827 200 0
15.3948 53.7578 200 0
17.1276 47.7629 200 0
19.397 40.8171 200 0

Bảng 4.7 Tọa độ suy rộng của đường hàn.

Từ bảng tọa độ suy rộng trên, xây dựng chương trình mô phỏng.

PROGRAMFILE
! prgfln C:\Documents and
Settings\Jerry\Desktop\EASY_ROB\EASY_ROB\proj\DuongdanthucteA12.prg
ERC TRACK ON
PTP_AX -6.9676 140.805 0.20000 0.0000
PTP_AX -3.1907 137.4934 0.20000 0.0000

GVTH: TS. Trần Huy Long 76 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

PTP_AX 0.33463 134.091 0.20000 0.0000

PTP_AX 3.6615 130.6019 0.20000 0.0000
PTP_AX 6.8324 127.0276 0.20000 0.0000
PTP_AX 9.8815 123.367 0.20000 0.0000
PTP_AX 12.8375 119.617 0.20000 0.0000
PTP_AX 15.7242 115.7726 0.20000 0.0000
PTP_AX 18.5626 111.8267 0.20000 0.0000
PTP_AX 21.3713 107.7702 0.20000 0.0000
PTP_AX 24.1679 103.5916 0.20000 0.0000
PTP_AX 22.0871 102.4273 0.20000 0.0000
PTP_AX 20.181 101.0578 0.20000 0.0000
PTP_AX 18.4558 99.4856 0.20000 0.0000
PTP_AX 16.9161 97.7124 0.20000 0.0000
PTP_AX 15.5654 95.7392 0.20000 0.0000
PTP_AX 14.4062 93.5661 0.20000 0.0000
PTP_AX 13.4403 91.1918 0.20000 0.0000
PTP_AX 12.6695 88.6133 0.20000 0.0000
PTP_AX 12.0954 85.8252 0.20000 0.0000
PTP_AX 11.7205 82.8192 0.20000 0.0000
PTP_AX 11.5484 79.5836 0.20000 0.0000
PTP_AX 11.5846 76.1017 0.20000 0.0000
PTP_AX 11.8377 72.3501 0.20000 0.0000
PTP_AX 12.3204 68.2967 0.20000 0.0000
PTP_AX 13.0519 63.8961 0.20000 0.0000
PTP_AX 14.0615 59.0827 0.20000 0.0000
PTP_AX 15.3948 53.7578 0.20000 0.0000
PTP_AX 17.1276 47.7629 0.20000 0.0000
PTP_AX 19.397 40.8171 0.20000 0.0000
PTP_AX 0.0000 0.0000 0.0000 0.0000
ENDPROGRAMFILE

- Chạy mô phỏng chương trình :

GVTH: TS. Trần Huy Long 77 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.12 : Hình ảnh tay máy đã hàn xong.

Kết quả thu được của 2 phương pháp là hoàn toàn gần giống nhau.
Vậy: “thuật toán tối ưu và phương pháp biến đổi ngược các ma trận thuần nhất
4x4 làm tốt công việc hoạch định đường dẫn liên tục cho tay máy”.

* Tương tự ta cũng có được hình ảnh tay máy hàn cắt theo đương tròn như sau.
- Ta có 1 tấm kim loại :

Hình 5.13 : Hình ảnh mối hàn cắt theo đường tròn
Làm các bước tương tự như trên ta được .
* Theo thuật toán tối ưu ta được.
PROGRAMFILE

GVTH: TS. Trần Huy Long 78 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

! prgfln C:\Documents and

Settings\Jerry\Desktop\EASY_ROB\EASY_ROB\proj\DuongdanthucteA22.prg
ERC TRACK ON
PTP_AX 4.7326 129.4197 0.20000 0.0000
PTP_AX 10.5138 121.5587 0.20000 0.0000
PTP_AX 14.7995 113.1477 0.20000 0.0000
PTP_AX 17.9851 104.4617 0.20000 0.0000
PTP_AX 20.3343 95.7183 0.20000 0.0000
PTP_AX 22.0016 87.0953 0.20000 0.0000
PTP_AX 23.0787 78.7759 0.20000 0.0000
PTP_AX 23.5715 70.9780 0.20000 0.0000
PTP_AX 23.4339 63.9707 0.20000 0.0000
PTP_AX 22.5745 58.1037 0.20000 0.0000
PTP_AX 20.8213 53.7950 0.20000 0.0000
PTP_AX 17.9221 51.6006 0.20000 0.0000
PTP_AX 14.0661 51.3885 0.20000 0.0000
PTP_AX 8.9954 53.5887 0.20000 0.0000
PTP_AX 2.9851 57.7943 0.20000 0.0000
PTP_AX -3.7185 63.5754 0.20000 0.0000
PTP_AX -10.8404 70.5254 0.20000 0.0000
PTP_AX -18.1398 78.2832 0.20000 0.0000
PTP_AX -25.3935 86.5739 0.20000 0.0000
PTP_AX -32.3721 95.1855 0.20000 0.0000
PTP_AX -38.8122 103.9288 0.20000 0.0000
PTP_AX -44.3813 112.6206 0.20000 0.0000
PTP_AX -48.6040 121.0603 0.20000 0.0000
PTP_AX -50.8328 128.9613 0.20000 0.0000
PTP_AX -50.1625 135.9228 0.20000 0.0000
PTP_AX -45.6132 141.3257 0.20000 0.0000
PTP_AX -36.8068 144.3968 0.20000 0.0000
PTP_AX -25.0669 144.4885 0.20000 0.0000
PTP_AX -13.1207 141.5893 0.20000 0.0000
PTP_AX -3.0252 136.3009 0.20000 0.0000
PTP_AX 4.7326 129.4197 0.20000 0.0000
PTP_AX 0.0000 0.0000 0.0000 0.0000
ENDPROGRAMFILE

- Chạy mô phỏng chương trình :

GVTH: TS. Trần Huy Long 79 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

Hình 5.14 : Hình ảnh tay máy đang hàn

* Theo phương pháp biến đổi ngược các ma trận thuần nhất 4x4 ta được như sau:
PROGRAMFILE
! prgfln C:\Documents and
Settings\Jerry\Desktop\EASY_ROB\EASY_ROB\proj\DuongdanthucteA22.prg
ERC TRACK ON
PTP_AX 4.734 129.42 0.20000 0.0000
PTP_AX 10.514 121.5607 0.20000 0.0000
PTP_AX 14.8015 113.1463 0.20000 0.0000
PTP_AX 17.9867 104.4617 0.20000 0.0000
PTP_AX 20.3333 95.7174 0.20000 0.0000
PTP_AX 22.0026 87.0943 0.20000 0.0000
PTP_AX 23.0773 78.7748 0.20000 0.0000
PTP_AX 23.5695 70.9784 0.20000 0.0000
PTP_AX 23.4367 63.9716 0.20000 0.0000
PTP_AX 22.5721 58.1042 0.20000 0.0000
PTP_AX 20.8211 53.7339 0.20000 0.0000
PTP_AX 17.9812 51.6023 0.20000 0.0000
PTP_AX 14.0653 51.3907 0.20000 0.0000
PTP_AX 8.9947 53.5908 0.20000 0.0000
PTP_AX 2.983 57.7918 0.20000 0.0000
PTP_AX -3.7185 63.5767 0.20000 0.0000
PTP_AX -10.8377 70.5246 0.20000 0.0000
PTP_AX -18.1369 78.2828 0.20000 0.0000
PTP_AX -25.3933 86.5756 0.20000 0.0000
PTP_AX -32.3718 95.1854 0.20000 0.0000
PTP_AX -38.8123 103.9274 0.20000 0.0000
PTP_AX -44.3786 112.6212 0.20000 0.0000
PTP_AX -48.6063 121.0593 0.20000 0.0000

GVTH: TS. Trần Huy Long 80 SVTH: Phạm Minh Quân

Luận văn tốt nghiệp. Chương 5

PTP_AX -50.8344 128.9628 0.20000 0.0000

PTP_AX -50.1649 135.9206 0.20000 0.0000
PTP_AX -45.6149 141.324 0.20000 0.0000
PTP_AX -36.8098 144.394 0.20000 0.0000
PTP_AX -25.0693 144.4892 0.20000 0.0000
PTP_AX -13.1235 141.5866 0.20000 0.0000
PTP_AX -3.0282 136.3025 0.20000 0.0000
PTP_AX 4.734 129.42 0.20000 0.0000
PTP_AX 0.0000 0.0000 0.0000 0.0000
ENDPROGRAMFILE
- Chạy mô phỏng chương trình :

Hình 5.15 : Hình ảnh tay máy đã hàn xong.

Kết quả thu được của 2 phương pháp là hoàn toàn gần giống nhau.
Vậy: “thuật toán tối ưu và phương pháp biến đổi ngược các ma trận thuần nhất
4x4 làm tốt công việc hoạch định đường dẫn liên tục cho tay máy”.

GVTH: TS. Trần Huy Long 81 SVTH: Phạm Minh Quân