Đang tải... (xem toàn văn)
Vấn đề động học ngược cho tay máy là một lĩnh vực được rất nhiều người quan tâm bởi vì cho đến nay vẫn chưa có một phương pháp chung nào hiệu qủa nhất để giải quyết vấn đề này, đặc biệt là đối với tay máy có số bậc tự do lớn hơn 6. Do đó, tác giả đã bước đầu nghiên cứu, ứng dụng mạng nơron nhằm đề xuất một phương pháp mới có thể ứng dụng để giải bài toán động học ngược cho tay máy. Trong luận văn này, tác giả đã tiến hành giải bài toán động học ngược cho tay máy scorbot ER7 bằng mạng nơron và so sánh kết qủa của phương pháp này với phương pháp giải tích. Vì bài toán động học ngược là bài toán đa nghiệm, tức là cùng một vị trí của tay máy có thể có những lời giải khác nhau tương ứng với các cấu hình khác nhau. Do đó, để đảm bảo lời giải của hai phương pháp có cùng cấu hình và để đơn giản quá trình tính toán, tác giả chỉ xét trong trường hợp cụ thể, đó là hướng của trục z5 luôn ngược với trục z0 và hướng của trục x5 luôn cùng hướng với trục x0. Tác giả đã huấn luyện hai mạng: mạng Net1_1 tương ứng với vùng 1 (hình 3.2) và cấu hình 1(hình 3.3); mạng Net2_1 tương ứng với vùng 2 (hình 3.2) và cấu hình 1(hình 3.3). Kết quả huấn luyện như sau: Mạng net1_1: Sử dụng bộ mẫu gồm 1000 mẫu dùng để đánh giá khả năng tổng quát hóa của mạng. Kết quả cho thấy sai lệch của θ1 phân bố trong khoảng (0,290 0,290 ) , sai lệch của θ2 phân bố trong khoảng (0,40 0,60 ), sai lệch của θ3 phân bố trong khoảng (0,660 0,40 ), sai lệch của θ4 phân bố trong khoảng (0,20 0,30 ), sai lệch của θ5 phân bố trong khoảng (0,30 0,30 ). Trung bình bình phương sai lệch của các giá trị này tương ứng với các biến khớp lần lượt là 2,2x1004; 8,3x1004; 9,0x104; 7,8x1004; 2,3x1004 . Kết quả khi ứng dụng mạng Net1_1 để mô phỏng theo quỹ đạo đường tròn với 30 điểm nằm trong vùng huấn luyện cho thấy sai lệch vị trí của tay máy theo phương trục x phân bố trong khoảng (0,0754 0,0001), sai lệch theo phương trục y phân bố trong khoảng (0,1214 0,0695) và sai lệch theo phương trục z phân bố trong khoảng (0,0264 0,0285). Mạng Net2_1: Sử dụng bộ mẫu gồm 1000 mẫu dùng để đánh giá khả năng tổng quát hóa của mạng thì sai lệch của θ1 phân bố trong khoảng (0,10 0,150 ), sai lệch của θ2 phân bố trong khoảng (0,240 0,440 ), sai lệch của θ3 phân bố trong khoảng (0,440 0,580 ), sai lệch của θ4 phân bố trong khoảng (0,180 0,260 ), sai lệch của θ5 phân bố trong khoảng (0,10 0,150 ). Trung bình bình phương sai lệch của các giá trị này tương ứng với các biến khớp lần lượt là 3,7x1005; 2,7x1004; 4,7x1004; 1,9x1004 và 3,7x1005 . Kết quả khi ứng dụng mạng Net2_1 để mô phỏng theo quỹ đạo đường tròn với 15 điểm nằm trong vùng huấn luyện cho thấy sai lệch vị trí của tay máy theo phương trục x phân bố trong khoảng (0,0483 0,0139), sai lệch theo phương trục y phân bố trong khoảng (0,0852 0,1084) và sai lệch theo phương trục z phân bố trong khoảng (0,0646 0,0908). Như vậy, có thể khẳng định rằng việc ứng dụng mạng nơron để giải bài toán động học ngược cho tay máy là một giải pháp có hiệu quả cao. Phương pháp này có thể giải bài toán động học ngược cho các tay máy có các cấu hình khác nhau mà không phụ thuộc vào số bậc tự do của tay máy. Vì vậy nó thực sự có hiệu quả khi giải bài toán động học ngược cho các tay máy có số bậc tự do lớn mà sẽ gặp khó khăn khi giải bằng các pháp truyền thống.
Chương 1.1 Đặt vấn đề Ngày nay, với phát triển mạnh mẽ khoa học kỹ thuật, nước giới đẩy mạnh nghiệp cơng nghiệp hóa đại hóa vào trình sản xuất nhằm nâng cao suất, chất lượng, giảm giá thành sản phẩm giảm thiểu sức lao động người Đặc biệt mơi trường nặng nhọc, nguy hiểm như: nóng lò hơi, lây lan bệnh hiểm nghèo sở y tế, ô nhiễm khơng khí hầm mỏ, nguy hiểm duới đáy đại dương không gian vũ trụ… Để đáp ứng vấn đề trên, nước có sản xuất phát triển đưa robot công nghiệp vào dây chuyền sản xuất Robot ngày nâng cao tính lĩnh vực hoạt động Chúng trở nên thông minh hơn, linh hoạt hơn, xác đáp ứng nhanh Vì để khai thác, sử dụng cách hiệu robot trang bị, để tiến hành nghiên cứu, chế tạo robot đáp ứng nhu cầu địi hỏi ngày cao cơng nghiệp đại việc nghiên cứu Robot vấn đề sở sản xuất, nhà khoa học, trường học đại học, cao đẳng quan tâm Trong đó, giải tốn động học ngược cho tay máy điều kiện tiên để điều khiển tay máy theo quỹ đạo cho trước Hơn nữa, việc tìm phương pháp chung hiệu để giải cho tay máy n bậc tự thách thức nhà nghiên cứu tồn giới Nhiệm vụ tốn động học ngược xác định giá trị biến khớp qi, (i=1,…,n) biết trước vị trí hướng bàn kẹp tay máy Theo truyền thống, có ba phương pháp thường sử dụng để giải toán động học ngược cho tay máy là: Phương pháp giải tích, phương pháp hình học phương pháp lặp [10] Mỗi phương pháp có điểm hạn chế riêng Phương pháp giải tích khơng đảm bảo nhận nghiệm tường minh [10] Trường hợp giải theo phương pháp hình học, nghiệm tường minh cho ba khớp phải tồn phương diện hình học Bên cạnh đó, nghiệm tường minh loại tay máy khơng thể dùng cho loại tay máy có dạng hình học khác [10] Phương pháp lặp hội tụ tới lời giải nhất, phụ thuộc vào vị trí ban đầu [10] Nếu số bậc tự tay máy tăng lên việc giải tốn động học ngược phương pháp truyền thống tốn nhiều thời gian, không hội tụ đến lời giải cuối việc nghiên cứu đưa phương pháp chung sử dụng có hiệu để giải vấn đề động học ngược cho tay máy đề tài có giá trị thực tiễn cao 1.2 iới thiệu chung robot công nghiệp Robot công nghiệp hiểu thiết bị tự động linh hoạt, bắt chước chức lao động công nghiệp người [1] Những robot công nghiệp chế tạo vào năm 1956 công ty Unimation George Devol Joseph F Engelberger Mỹ [1] Các robot chủ yếu dùng để vận chuyển vật thể phạm vi nhỏ 1.2.1 Các phận cấu thành robot công nghiệp Cấu tạo robot công nghiệp thông thường hình 1.1 [1] gồm phận chủ yếu sau: Tay máy: cấu khí gồm khâu, khớp Chúng hình thành cánh tay để tạo chuyển động bàn kẹp để trực tiếp thao tác đối tượng Tay máy gồm có phận sau: Đế (1), thân (2), cánh tay (3), cánh tay (4), bàn kẹp (5) Hệ thống truyền dẫn động: phận chủ yếu tạo nên chuyển dịch khớp động Hệ thống điều khiển: đảm bảo hoạt động robot theo thông tin đặt trước nhận biết trình làm việc Hình 1.1 Các phận cấu thành robot cơng nghiệp Hệ thống cảm biến tín hiệu: thực việc nhận biết biến đổi thông tin hoạt động thân robot (cảm biến nội tín hiệu) mơi trường, đối tượng mà robot phục vụ (cảm biến ngoại tín hiệu) 1.2.2 Bậc tự tay máy Để định vị định hướng bàn kẹp cách tùy ý không gian ba chiều tay máy cần có bậc tự do, bậc tự để định vị ba bậc tự để định hướng Một số công việc nâng hạ, xếp dỡ, … cần số bậc tự Tay máy hàn, sơn thường có bậc tự Trong số trường hợp cần khéo léo, linh hoạt cần tối ưu hóa quỹ đạo,… người ta dùng tay máy có số bậc tự lớn Thông thường khâu cấu tay máy nối ghép với khớp quay khớp tịnh tiến, chúng thuộc khớp động học loại Trong cấu tay máy khâu nối liên tiếp với gọi cấu hở thông thường khâu gắn liền với nguồn lực riêng, loại cấu dùng khớp động loại số bậc tự cấu số khâu động Trong trường hợp chung tính tốn số bậc tự theo cơng thức thông dụng Nguyên lý máy [1]: n W 6n ipi (1.1) i 1 Trong đó: - n : số khâu động - P i : số khớp loại i 1.2.4 quan tình hình nghiên cứu toán động học ngược Ở nước ta, việc nghiên cứu giải toán động học ngược cho tay máy nhiều nhà nghiên cứu quan tâm [8], [9] Tuy nhiên chưa có cơng trình nghiên cứu ứng dụng mạng nơron để giải toán động học ngược cho tay máy Cho đến nay, giới có nhiều cơng trình nghiên cứu lĩnh vực đạt kết định công bố tài liệu tham khảo như: [10], [11], [12], [13], [14], [19] Điều mở hướng để giải vấn đề động học ngược cho tay máy nhiều bậc tự mà chưa có phương pháp chung thực hiệu Raşit Kokưr , Cemil Ưz, Tarik Çakar, Hüseyin Ekiz [11] ứng dụng mạng nơron để giải toán động học ngược cho tay máy bậc tự sử dụng mạng truyền thẳng với giải thuật lan truyền ngược (Back propagation) cập nhật trọng số theo qui tắc giảm dốc (gradient descent learning algorith) Bằng phương pháp kinh nghiệm, nhóm nghiên cứu thiết kế mơ hình mạng gồm 40 nơron lớp ẩn, nơron lớp vào nơron lớp Mơ hình có sai số 0,000121 với 5000 mẫu thực 3000000 bước lặp Alavandar S Nigam M J [12] xây dựng phương pháp để giải toán động học ngược cho tay máy dựa mạng nơron mờ Trong sử dụng hệ thống suy luận nơron mờ thích nghi (ANFIS) để huấn luyện cho tay máy bậc tự Kết mơ máy tính cho thấy phương pháp ứng dụng để giải vấn đề động học ngược cho tay máy Shah, Rattan Nakra [13] ứng dụng mạng nơron để giải toán động học ngược cho tay máy bậc tự phẳng Kết cho thấy, giải pháp hồn tồn sử dụng với sai số không Các tác giả khẳng định mạng nơron trở thành phương pháp thay để giải toán ánh xạ động học thuận nghịch Raşit Kokör [14] đề xuất phương pháp giải toán động học ngược phương pháp kết hợp mạng nơron với giải thuật di truyền Trong báo Ơng khẳng định mạng nơron thực với sai số chấp nhận Hơn nữa, kết hợp với giải thuật di truyền, sai số vị trí đạt tới mức micromet Panchanand Jha [19] giải toán động học ngược cho tay máy ba bậc tự với hai mạng khác Mạng nơron truyền thẳng nhiều lớp (MLP) sử dụng thuật toán back propagation, cập nhật trọng số theo quy tắc giảm dốc mạng nơron tiền xử lí đa thức(PPN) Kết cho thấy mạng MLP cho kết xác so với mạng PPN 1.3 Mục đích đề tài Nghiên cứu đưa phương pháp hiệu quả, có tính tổng qt cao ứng dụng mạng nơron để giải tốn động học ngược cho tay máy Phương pháp ứng dụng để giải toán động học ngược cho tay máy có số bậc tự lớn mà gặp khó khăn giải pháp truyền thống 1.4 hiệm vụ đề tài giới hạn đề tài - Nghiên cứu toán động học tay máy - Nghiên cứu phương pháp giải toán động học ngược - Nghiên cứu lý thuyết mạng nơron, nghiên cứu neural network toolbox phần mềm Matlab - Giải toán động học thuận động học ngược cho tay máy Scorbot ER7 - Xây dựng liệu để huấn luyện mạng - Xác định mạng nơron thích hợp để giải tốn động học ngược cho tay máy - Đánh giá kết nghiên cứu phương hướng phát triển đề tài 1.5 Phương pháp nghiên cứu Nghiên cứu lý thuyết, ứng dụng toolbox phần mềm Matlab để xác định cấu trúc trọng số mạng nơron để giải toán động học ngược cho tay máy Scorbot ER7 So sánh kết phương pháp giải toán động học ngược mạng nơron với phương pháp giải tích, từ rút kết luận KẾ L Ậ CHƯƠ Trong chương này, tác giả giới thiệu cách khái qt robot cơng nghiệp, cơng trình nghiên cứu thành tựu đạt nhà nghiên cứu giới lĩnh vực ứng dụng mạng nơron để giải toán động học ngược cho tay máy Trên sở khẳng định tầm quan trọng tính cấp thiết luận văn Chương CƠ L H Ế 2.1 Động học tay máy Tay máy thường cấu hở gồm chuỗi khâu nối với khớp quay khớp tịnh tiến Một đầu chuỗi gắn lên thân, đầu lại thường bàn kẹp khâu gắn liền với dụng cụ làm việc Điểm mút khâu tác động cuối điểm đáng quan tâm q trình làm việc đòi hỏi khâu phải định vị định hướng xác khơng gian Bài tốn động học tay máy giải dạng toán bản: Bài toán động học thuận toán động học ngược - Bài tốn động học thuận: Từ thơng số vị trí, vận tốc gia tốc khâu dẫn để xác định vị trí, hướng, vận tốc gia tốc điểm tác động cuối - Bài toán động học ngược: Từ yêu cầu vị trí, hướng, vận tốc gia tốc điểm tác động cuối, xác định giá trị thông số tương ứng khâu trước 2.1.1 Bài tốn động học thuận tay máy Trong đại đa số trường hợp, tay máy chuỗi động hở, bao gồm số khâu liên kết với nhờ khớp Mỗi khâu hình thành với khớp phía trước cặp khâu - khớp Tùy theo kết cấu mà loại khớp đảm bảo cho khâu nối sau khả chuyển động định Mỗi khớp (thực chất cặp khâu - khớp) đặc trưng loại thông số: - Các thông số khơng thay đổi q trình làm việc tay máy gọi tham số - Các thông số thay đổi trình làm việc gọi biến khớp Hai loại khớp thông dụng kỹ thuật tay máy khớp trượt khớp quay Chúng loại khớp có bậc tự Bài tốn thuận nhằm mơ tả vị trí hướng khâu tác động cuối dạng hàm số biến khớp Giả sử có tay máy với n+1 khâu n khớp Vị trí hướng khâu tác động cuối so với hệ tọa độ gốc O x0y0z0 mô tả vectơ định vị p vectơ phương n, s, a Theo [1] ma trận trạng thái khâu tác động cuối so với hệ tọa độ sở thể sau: n x n y TE = nz sx sy ax ay sz az px p y pz (2.1) Trong đó, p vectơ định vị; n, s, a vectơ phương khâu tác động cuối, vectơ đơn vị hệ trục tọa độ Nếu phần công tác bàn kẹp gốc tọa độ đặt vào tâm quay; vectơ a đặt theo phương tiến đến vật; s nằm mặt phẳng trượt hàm kẹp; n vng góc với s a theo quy tắc bàn tay phải Giải toán động học thuận theo phương pháp chung sử dụng quy tắc Denavit-Hartenberg, Denavit Hartenberg xây dựng vào năm 1955 [4] Đó quy tắc thiết lập hệ thống tọa độ cặp khâu - khớp tay máy Dựa hệ tọa độ mơ tả cặp hệ thống tham số, biến khớp áp dụng dạng phương trình tổng qt cho tốn động học tay máy 2.1.1.1 uy tắc Denavit - Hartenberg Giả sử chuỗi động học tay máy có n khâu, khâu thứ i nối khớp thứ i với khớp thứ i+1 Hình 2.1 trích từ [1] biểu diễn thông số động học theo quy tắc Denavit–Hartenberg trường hợp khớp động liên tiếp khớp quay Theo quy tắc Denavit–Hartenberg [1] hệ tọa độ gắn lên khâu, khớp sau: - Đặt trục tọa độ zi nằm dọc theo trục khớp động i+1 - Đặt gốc toạ độ Oi giao điểm Zi pháp tuyến chung nhỏ trục Zi Zi-1 - Đặt trục tọa độ xi theo phương pháp tuyến chung zi-1 zi, hướng từ khớp i đến khớp i+1 - Trục yi vuông góc với xi zi theo qui tắc bàn tay phải - Trong số trường hợp đặc biệt, quy tắc Denavit – Hartenberg cho phép xác định cách đơn giản hóa sau: + Đối với hệ tọa độ gốc có phương trục z0 xác định Gốc O0 trục x0 chọn tùy ý + Đối với hệ tọa độ thứ n, có phương trục xn xác định Trục zn chọn tùy ý + Khi khớp liền có trục song song, vị trí pháp tuyến chung lấy + Khi khớp thứ i khớp trượt có phương trục zi-1 xác định Hình 2.1 Các hệ toạ độ khâu động liên tiếp Sau thiết lập, vị trí hệ Oixiyizi so với hệ Oi-1xi-1yi-1 zi-1 hoàn toàn xác định nhờ thông số sau: - độ dài đường vuông góc chung trục khớp động i+1 i - αi góc quay quanh trục xi trục zi-1 zi - di khoảng cách đo dọc trục khớp động i từ đường vng góc chung trục khớp động i-1 với trục khớp động i đến đường vng góc chung khớp động i trục khớp động i+1 - θi góc quay quanh trục zi-1 trục xi-1 xi Bốn thông số ai, αi, di, θi gọi thông số Denavit – Hartenberg (DH) + Nếu khớp động i khớp quay biến khớp θ i + Nếu khớp động i khớp tịnh tiến di biến khớp 2.1.1.2 Mơ hình biến đổi Trên sở xây dựng hệ toạ độ, mơ tả phép chuyển tọa độ hệ i hệ i-1 theo phép biến đổi[1]: + Quay quanh trục zi-1 góc θi + Tịnh tiến dọc trục zi-1 đoạn di + Tịnh tiến dọc trục xi-1 (đã trùng với xi) đoạn + Quay quanh trục xi góc i Bốn phép biến đổi biểu thị tích ma trận [1]: Ai = R z,θi Tp 0,0,di Tp a i ,0,0 R(x,αi ) (2.2) Theo [1], sau thực phép nhân ma trận nói ta ma trận tổng hợp mơ tả vị trí hướng khâu thứ i so với khâu thứ i-1: cθi s θi Ai = 0 -sθi ci cθi ci sθisi -cθisi si ci Đối với khớp quay biến khớp khớp d i 10 a i cθi a isθi di 1 (2.3) , cịn khớp tịnh tiến biến L H M KH Ế Nguyễn Thiện Phúc, Robot công nghiệp, nhà xuất Khoa Học Kỹ Thuật, 2004 Phạm Hữu Đức Dục, Mạng nơron & ứng dụng điều khiển tự động, nhà xuất Khoa Học Kỹ Thuật, 2009 Nguyễn Đình Thúc, Trí tuệ nhân tạo - Mạng nơron phương pháp ứng dụng, nhà xuất Giáo dục, 2000 Đào Văn Hiệp, Kỹ thuật robot, nhà xuất Khoa Học Kỹ Thuật, 2003 Lương Hồng Sâm, Nâng cao độ xác định vị cho tay máy phương pháp hiệu chỉnh sai lệch vị trí, luận án tiến sĩ kỹ thuật, Đại học Bách khoa TP Hồ Chí Minh, năm 2006 Nguyễn Phùng Quang, Matlab/Simulink dành cho kỹ sư điều khiển tự động, Nhà xuất Khoa học Kỹ thuật, 2004 Lương Hồng Sâm, Nguyễn Việt Hùng, Ứng dụng mạng nơron để giải toán động học ngược tay máy, Tạp chí Cơ khí Việt Nam, số 3, năm 2014 Trần Hoàng Nam, Giải toán ngược động học, động lực học điều khiển trượt rơbốt dư dẫn động dựa thuật tốn hiệu chỉnh gia lượng véc tơ tọa độ suy rộng, luận án tiến sĩ kỹ thuật, Viện Cơ học, 2010 Lê Tiến Dũng, Đồn Quang Vinh, Phân tích động học cấu hình kì dị tay máy robot song song phẳng hai bậc tự do, chuyên san Kỹ thuật Điều khiển Tự động hóa số 8, 2013 Ế Ư C 10 Sreenivas Tejomurtula, Inverse kinematics in robotics using neural networks, M.S Thesis, Louisiana State University, 1998 74 11 Raşit Kokưr & Cemil Ưz, Tarik Çakar, Hüseyin Ekiz, A Study of Neural Network Based Inverse Kinematics Solution for a Planar Three Joint Robot , Sakarya University, 2003 12 Alavandar S and Nigam M J Neuro-Fuzzy based Approach for Inverse Kinematics Solution of Industrial Robot Manipulators, Int J of Computers, Communications & Control, ISSN 1841-9836, E-ISSN 1841-9844 Vol III ,2008, pp 224-234 13 Jolly Shah and S.S.Rattan and B.C.Nakra, Kinematic Analysis of a Planer Robot Using Artificial Neural Network, International Journal of Robotics and Automation (IJRA) Vol.1, 2012, pp 145 - 151 14 Raşit Kokör, A neuro-genetic approach to the inverse kinematics solution of robotic manipulators, Scientific Research and Essays Vol 6, 2011, pp 2784-2794 15 Bogdan M Wilamowski and J David Irwin, The Industrial lectronics Handbook, Second Edition, 2011, pp 12-1 – 12-16 16 Howard Demuth and Mark Beale , Neural Network Toolbox User’s Guide, Version 3.0, MathWorks, 1998 17 Martin T.Hagan and Howard B.Demuth and Mark Beale, Neural Network Design, PWS Publishing Company, 1996 18 Joe Zhizhong, Application of Force Control on a Robot, M.S Thesis, National Central University established Mechanical Engineering Research Institute, China, 2001 19 Panchanand Prediction of Jha, Novel Artificial Neural Network Application for Inverse Kinematics of Manipulator, M.S Thesis, Department of Mechanical Engineering National Institute of Technology Rourkela, India, 2009 20 Scorbot ER user’s manual, catalog #100017, Eshed Robotec,1998 21 C.S.G.Lee and M.Ziegler, A geometric approach in solving the inverse kinematics of puma robots, University of Michigan,1983 75 PH L C PH L C Chương trình lấy mẫu huấn luyện kiểm tra mạng Net1 format long fileID1 = fopen('teta_HL11.txt','a'); fileID= fopen('vitri_HL11.txt','a'); for i=1:3000 x1 = -125*pi/180.0; y1 =0 ; th1= x1 + (y1-x1) *rand; %lấy giá trị ngẫu nhiên khoảng x1- y1 x4 = -20*pi/180; y4 = 0; th4=x4 + (y4-x4) *rand; x2 = -90*pi/180 ; y2 =0 ; th2=x2 + (y2-x2) *rand; th3= -th4-th2; th5 = th1; an1=-pi/2; an2=0; an3=0; an4=-pi/2; an5=0; a1=50; a2=300; a3=350; a4=0; a5=0; d1=385.5; d2= -40; d3=0; d4=0; 76 d5=251; A1=[cos(th1) -sin(th1) a1*cos(th1); sin(th1) cos(th1) a1*sin(th1); -1 d1; 0 1]; A2=[cos(th2) -sin(th2) a2*cos(th2); sin(th2) cos(th2) a2*sin(th2); 0 d2; 0 1]; A3=[cos(th3) -sin(th3) a3*cos(th3); sin(th3) cos(th3) a3*sin(th3); 0 0; 0 1]; A4=[cos(th4) -sin(th4) 0; sin(th4) cos(th4) 0; -1 0; 0 1]; A5=[cos(th5) -sin(th5) 0; sin(th5) cos(th5) 0; 0 d5; 0 1]; T5=A1*A2*A3*A4*A5; C=[th1,th2,th3,th4,th5]; th1d=th1*180/pi; th2d=th2*180/pi; th3d=th3*180/pi; th4d=th4*180/pi; th5d=th5*180/pi; D=[th1d,th2d,th3d,th4d,th5d]; B=[T5(1,4), T5(2,4), T5(3,4)]; fprintf(fileID,'%2f %14f %16f\n',B); fprintf(fileID1,'%2f %12f %10f %14f %14f\n',D); end fclose(fileID); fclose(fileID1); 77 PH L C Chương trình lấy mẫu huấn luyện mạng Net2 format long fileID1 = fopen('teta_HL21.txt','a'); fileID= fopen('vitri_HL21.txt','a'); for i=1:3000 x1 =0; y1 =125*pi/180.0 ; th1= x1 + ((y1-x1) *i)/3000; x4 = -20*pi/180; y4 = 0; th4= x4 + (y4-x4) *rand; x2 = -90*pi/180 ; y2 =0 ; th2= x2 + (y2-x2) *rand; th3= -th4-th2; th5 = th1; an1=-pi/2; an2=0; an3=0; an4=-pi/2; an5=0; a1=50; a2=300; a3=350; a4=0; a5=0; d1=385.5; d2= -40; d3=0; d4=0; d5=251; A1=[cos(th1) -sin(th1) a1*cos(th1); sin(th1) cos(th1) a1*sin(th1); -1 d1; 0 1]; A2=[cos(th2) -sin(th2) a2*cos(th2); sin(th2) cos(th2) a2*sin(th2); 0 d2; 0 1]; A3=[cos(th3) -sin(th3) a3*cos(th3); sin(th3) cos(th3) a3*sin(th3); 0 0; 0 1]; A4=[cos(th4) -sin(th4) 0; sin(th4) cos(th4) 0; -1 0; 0 1]; A5=[cos(th5) -sin(th5) 0; sin(th5) cos(th5) 0; 0 d5; 0 1]; T5=A1*A2*A3*A4*A5; C=[th1,th2,th3,th4,th5]; th1d=th1*180/pi; th2d=th2*180/pi; th3d=th3*180/pi; th4d=th4*180/pi; th5d=th5*180/pi; 78 D=[th1d,th2d,th3d,th4d,th5d]; % tinh theo B=[T5(1,4), T5(2,4), T5(3,4)]; fprintf(fileID,'%2f %14f %16f\n',B); fprintf(fileID1,'%2f %12f %10f %14f %14f\n',D); end fclose(fileID); fclose(fileID1); PH L C Chương trình lấy mẫu kiểm tra mạng Net2 format long fileID1 = fopen('teta_KT21.txt','a'); fileID= fopen('vitri_KT21.txt','a'); for i=1:1000 x1 =0; y1 =125*pi/180.0 ; th1= x1 + ((y1-x1) *i)/1000; x4 = -20*pi/180; y4 = 0; th4= x4 + (y4-x4) *rand; x2 = -90*pi/180 ; y2 =0 ; th2= x2 + (y2-x2) *rand; th3= -th4-th2; th5 = th1; an1=-pi/2; an2=0; an3=0; an4=-pi/2; an5=0; a1=50; a2=300; a3=350; a4=0; a5=0; d1=385.5; d2= -40; d3=0; d4=0; d5=251; A1=[cos(th1) -sin(th1) a1*cos(th1); sin(th1) cos(th1) a1*sin(th1); -1 d1; 0 1]; A2=[cos(th2) -sin(th2) a2*cos(th2); sin(th2) cos(th2) a2*sin(th2); 0 d2; 0 1]; A3=[cos(th3) -sin(th3) a3*cos(th3); sin(th3) cos(th3) a3*sin(th3); 0 0; 0 1]; 79 A4=[cos(th4) -sin(th4) 0; sin(th4) cos(th4) 0; -1 0; 0 1]; A5=[cos(th5) -sin(th5) 0; sin(th5) cos(th5) 0; 0 d5; 0 1]; T5=A1*A2*A3*A4*A5; C=[th1,th2,th3,th4,th5]; th1d=th1*180/pi; th2d=th2*180/pi; th3d=th3*180/pi; th4d=th4*180/pi; th5d=th5*180/pi; D=[th1d,th2d,th3d,th4d,th5d]; % ttính theo độ B=[T5(1,4), T5(2,4), T5(3,4)]; fprintf(fileID,'%2f %14f %16f\n',B); fprintf(fileID1,'%2f %12f %10f %14f %14f\n',D); end fclose(fileID); fclose(fileID1); PH L C Chương trình giải tốn động học ngược cho vị trí đường trịn nằm vùng huấn luyện mạng Net1 a1=50; a2=300; a3=350; a4=0; a5=0; d1=385.5; d2=-40; d3=0; d4=0; d5=251; load dt1_1.txt %vị trí đường trịn fileID = fopen('teta_dt11.txt','a'); for i=1:30 px=dt1_1(i,1); py= dt1_1(i,2); pz= dt1_1(i,3); k= sqrt(px^2+py^2-(d2+d3)^2) ; th1 = atan2(py*k-px*(d2+d3), px*k + py*(d2+d3)) ; if th1>0 k= -sqrt(px^2+py^2-(d2+d3)^2) ; th1 = atan2(py*k-px*(d2+d3), px*k + py*(d2+d3)) ; th234=0 %(3.4); th5=th1; 80 th3=abs( acos(((k-a1)^2+(d1-pz- d5)^2- a2^2-a3^2)/(2*a2*a3 ))); sth2 = ( (- pz+d1 - d5)*(a2+a3*cos(th3))-(k - a1)*a3*sin(th3) )/((ka1)^2+(d1+d5-pz)^2 ); cth2= ( ((k - a1 )*(a2+a3*cos(th3))+ (- pz+d1- d5)*a3*sin(th3)))/((ka1)^2+(d1+d5-pz)^2 ); th2 = atan2(sth2,cth2); th4=th234-th2-th3; end th234=0 %(3.4); th5=th1; th3=abs(acos(((k-a1)^2+(d1-pz- d5)^2- a2^2-a3^2)/(2*a2*a3 ))); sth2 = ( (- pz+d1 - d5)*(a2+a3*cos(th3))-(k - a1)*a3*sin(th3) )/((ka1)^2+(d1+d5-pz)^2 ); cth2= ( ((k - a1 )*(a2+a3*cos(th3))+ (- pz+d1- d5)*a3*sin(th3)))/((ka1)^2+(d1+d5-pz)^2 ); th2 = atan2(sth2,cth2); th4=th234-th2-th3; B=[th1,th2,th3,th4,th5]; th1d=th1*180/pi; th2d=th2*180/pi; th3d=th3*180/pi; th4d=th4*180/pi; th5d=th5*180/pi; D=[th1d,th2d,th3d,th4d,th5d ]; % Tinh theo fprintf(fileID,'%2f %12f %10f %14f %14f\n',D); end fclose(fileID); PH L C : Chương trình giải tốn động học ngược cho vị trí đường trịn nằm vùng huấn luyện mạng Net2 a1=50; a2=300; a3=350; a4=0; a5=0; d1=385.5; d2=-40; d3=0; d4=0; d5=251; load dt2_1.txt fileID = fopen('teta_dt21.txt','a'); %vị trí đường trịn 81 for i=1:15 px=dt2_1(i,1); py= dt2_1(i,2); pz= dt2_1(i,3); k= sqrt(px^2+py^2-(d2+d3)^2) ; th1 = atan2(py*k-px*(d2+d3), px*k + py*(d2+d3)) ; if th1