NGHIÊN CỨU NÂNG CAO CHẤT LƯỢNG ĐIỀU KHIỂN ROBOT SCARA

LỜI NÓI ĐẦU Theo quá trình phát triển của xã hội , nhu cầu nâng cao sản xuất và chất lượng sản phẩm ngày càng đòi hỏi ứng dụng rộng rãi các phương tiện tự động hóa sản xuất . Xu hướng tạo ra những dây chuyền và thiết bị tự động có tính linh hoạt cao đã hình thành và phát triển mạnh mẽ…Vì thế ngày càng tăng nhanh nhu cầu ứng dụng người máy để tạo ra các hệ sản xuất tự động linh hoạt . Robot ứng dụng rộng rãi và đóng vai trò quan trọng trong sản xuất cũng như trong đời sống . Robot là cơ cấu đa chức năng có khả năng lập trình được dùng để di chuyển nguyên vật liệu , các chi tiết , các dụng cụ thông qua các truyền động được lập trình trước . Khoa học robot chủ yếu dựa vào các phép toán về đại số ma trận .Robot có thể thao tác như con người và có thể hợp tác với nhau một cánh thông minh .Robot có cánh tay với nhiều bậc tự do và có thể thực hiện được các chuyển động như tay người và điều khiển được bằng máy tính hoặc có thể điều khiển bằng chương trình được nạp sẵn trong chip trên bo mạch điều khiển robot . Để hệ điều khiển robot có độ tin cậy , độ chính xác cao , giá thành hạ và tiết kiệm năng lượng thì nhiệm vụ cơ bản là hệ điều khiển robot phải đảm bảo giá trị yêu cầu của các đại lượng điều chỉnh và điều khiển . Ngoài ra , hệ điều khiển robot phải đảm bảo ổn định động và tĩnh , chống được nhiễu trong và ngoài , đồng thời không gây tác hại cho môi trường như: tiếng ồn quá mức quy định , sóng hài của điện áp và dòng điện quá lớn cho lưới điện v .v . . . Khi thiết kế hệ điều khiển robot mà trong đó sử dụng các hệ điều chỉnh tự động truyền động , cần phải đảm bảo hệ thực hiện được tất cả các yêu cầu về công nghệ , các chỉ tiêu chất lượng và các yêu cầu kinh tế . Chất lượng của hệ thống được thể hiện trong trạng thái tĩnh và trạng thái động . Trạng thái tĩnh yêu cầu quan trọng là độ chính xác điều chỉnh . Trạng thái động thì có yêu cầu về độ ổn định và các chỉ tiêu về chất lượng động là độ quá điều chỉnh , tốc độ điều chỉnh , thời gian điều chỉnh và số lần dao động . Đối với hệ điều khiển robot , việc lựa chọn sử dụng các bộ biến đổi , các loại động cơ điện , các thiết bị đo lường , cảm biến , các bộ điều khiển và đặc biệt là phương pháp điều khiển có ảnh hưởng rất lớn đến chất lượng điều khiển bám chính xác quỹ đạo của hệ . Các công trình nghiên cứu về hệ thống điều khiển robot tập trung chủ yếu theo hai hướng là sử dụng các mô hình có đặc tính phi tuyến có thể ước lượng được để đơn giản việc phân tích và thiết kế hoặc đề ra các thuật toán điều khiển mới nhằm nâng cao chất lượng đáp ứng của robot . Đặc điểm cơ bản của hệ thống điều khiển robot là thực hiện được điều khiển bám theo một quỹ đạo phức tạp đặt trước trong không gian , tuy nhiên khi dịch chuyển thì trọng tâm của các chuyển động thành phần và mômen quán tính của hệ sẽ thay đổi , điều đó dẫn đến thông số động học của hệ cũng thay đổi theo quỹ đạo chuyển động và đồng thời xuất hiện những lực tác động qua lại , xuyên chéo giữa các chuyển động thành phần trong hệ với nhau . Các yếu tố trên tác động sẽ làm cho hệ điều khiển robot mang tính phi tuyến mạnh , gây cản trở rất lớn cho việc mô tả và nhận dạng chính xác hệ thống điều khiển robot . Do vậy , khi điều khiển robot bám theo quỹ đạo đặt trước phải giải quyết được những vấn đề sau:Khắc phục các lực tương tác phụ thuộc vào vận tốc , gia tốc của quỹ đạo riêng các chuyển động thành phần và quỹ đạo chung của cả hệ như: lực quán tính , lực ly tâm , lực ma sát v .v . . .Khi trọng tâm của các chuyển động thành phần và của cả hệ thay đổi theo quỹ đạo riêng và chung kéo theo sự thay đổi của các thông số động học của hệ , điều đó đòi hỏi phải có sự biến thiên các tham số đưa vào bộ điều khiển tương ứng để vẫn đảm bảo sự cân bằng , ổn định và bền vững đồng thời vẫn bám theo được quỹ đạo đặt . Với công cụ toán vi phân người ta đã có thể phân tích tính điều khiển được , tính quan sát được cho hệ phi tuyến . Nội dung của đề tài nghiên cứu như sau:1 . Tên đề tài: Nâng cao chất lượng điều khiển

ĐỒ ÁN TỐT NGHIỆP NGHIÊN CỨU NÂNG CAO CHẤT LƯỢNG ĐIỀU KHIỂN ROBOT SCARA LỜI NÓI ĐẦU Theo trình phát triển xã hội , nhu cầu nâng cao sản xuất chất lượng sản phẩm ngày đòi hỏi ứng dụng rộng rãi phương tiện tự động hóa sản xuất Xu hướng tạo dây chuyền thiết bị tự động có tính linh hoạt cao hình thành phát triển mạnh mẽ…Vì ngày tăng nhanh nhu cầu ứng dụng người máy để tạo hệ sản xuất tự động linh hoạt Robot ứng dụng rộng rãi đóng vai trị quan trọng sản xuất đời sống Robot cấu đa chức có khả lập trình dùng để di chuyển nguyên vật liệu , chi tiết , dụng cụ thơng qua truyền động lập trình trước Khoa học robot chủ yếu dựa vào phép toán đại số ma trận  Robot thao tác người hợp tác với cánh thông minh  Robot có cánh tay với nhiều bậc tự thực chuyển động tay người điều khiển máy tính điều khiển chương trình nạp sẵn chip bo mạch điều khiển robot Để hệ điều khiển robot có độ tin cậy , độ xác cao , giá thành hạ tiết kiệm lượng nhiệm vụ hệ điều khiển robot phải đảm bảo giá trị yêu cầu đại lượng điều chỉnh điều khiển Ngoài , hệ điều khiển robot phải đảm bảo ổn định động tĩnh , chống nhiễu , đồng thời không gây tác hại cho môi trường như: tiếng ồn mức quy định , sóng hài điện áp dòng điện lớn cho lưới điện v v Khi thiết kế hệ điều khiển robot mà sử dụng hệ điều chỉnh tự động truyền động , cần phải đảm bảo hệ thực tất yêu cầu công nghệ , tiêu chất lượng yêu cầu kinh tế Chất lượng hệ thống thể trạng thái tĩnh trạng thái động Trạng thái tĩnh yêu cầu quan trọng độ xác điều chỉnh Trạng thái động có u cầu độ ổn định tiêu chất lượng động độ điều chỉnh , tốc độ điều chỉnh , thời gian điều chỉnh số lần dao động Đối với hệ điều khiển robot , việc lựa chọn sử dụng biến đổi , loại động điện , thiết bị đo lường , cảm biến , điều khiển đặc biệt phương pháp điều khiển có ảnh hưởng lớn đến chất lượng điều khiển bám xác quỹ đạo hệ Các cơng trình nghiên cứu hệ thống điều khiển robot tập trung chủ yếu theo hai hướng sử dụng mơ hình có đặc tính phi tuyến ước lượng để đơn giản việc phân tích thiết kế đề thuật toán điều khiển nhằm nâng cao chất lượng đáp ứng robot Đặc điểm hệ thống điều khiển robot thực điều khiển bám theo quỹ đạo phức tạp đặt trước không gian , nhiên dịch chuyển trọng tâm chuyển động thành phần mơmen qn tính hệ thay đổi , điều dẫn đến thơng số động học hệ thay đổi theo quỹ đạo chuyển động đồng thời xuất lực tác động qua lại , xuyên chéo chuyển động thành phần hệ với Các yếu tố tác động làm cho hệ điều khiển robot mang tính phi tuyến mạnh , gây cản trở lớn cho việc mơ tả nhận dạng xác hệ thống điều khiển robot Do , điều khiển robot bám theo quỹ đạo đặt trước phải giải vấn đề sau:  Khắc phục lực tương tác phụ thuộc vào vận tốc , gia tốc quỹ đạo riêng chuyển động thành phần quỹ đạo chung hệ như: lực quán tính , lực ly tâm , lực ma sát v v  Khi trọng tâm chuyển động thành phần hệ thay đổi theo quỹ đạo riêng chung kéo theo thay đổi thông số động học hệ , điều địi hỏi phải có biến thiên tham số đưa vào điều khiển tương ứng để đảm bảo cân , ổn định bền vững đồng thời bám theo quỹ đạo đặt Với cơng cụ tốn vi phân người ta phân tích tính điều khiển , tính quan sát cho hệ phi tuyến Nội dung đề tài nghiên cứu sau: Tên đề tài: Nâng cao chất lượng điều khiển cho robot Scara Cơ sở khoa học thực tiễn đề tài: Ứng dụng phương pháp điều khiển động lực học ngược cho điều khiển bền vững quỹ đạo robot Mục đích đề tài: Xây dựng cấu trúc thuật toán điều khiển robot Scara Nội dung đề tài , vấn đề cần giải quyết: - Xây dựng mơ hình tốn học cho robot Scara Serpent bậc tự - Xây dựng hệ thống điều khiển quỹ đạo đạt độ xác cao - Đánh giá chất lượng hệ thống mô Nội dung luận văn đề cập tới vấn đề “Nâng cao chất lượng điều khiển robot Scara” với mục tiêu điều khiển bền vững bám xác quỹ đạo chuyển động Luận văn trình bày thành chương với nội dung chương tóm tắt sau: Chương I – Tổng quan robot điều khiển robot: Mô tả tổng quan robot Phân tích ưu , nhược điểm số phương pháp điều khiển robot áp dụng thực tiễn để nâng cao độ xác điều khiển quỹ đạo robot Chương II – Mơ tả tốn học đối tượng robot Scara: Nghiên cứu số robot họ Scara sâu vào phân tích mơ hình Robot Scara Serpent để phục vụ việc nghiên cứu kiểm chứng sở lý thuyết phương pháp điều khiển lựa chọn Chương III – Xây dựng mơ hình hệ thống điều khiển: Thiết kế điều khiển để nâng cao chất lượng hệ thống điều khiển quỹ đạo cho robot Xây dựng mơ hình hệ thống điều khiển cho robot Scara Serpent , xây dựng mơ hình mô sử dụng phần mềm Matlab-Sumulink Chương IV – Mơ với mơ hình robot Scara Serpent: Định hình kiểm chứng mặt lý thuyết sở thực tiễn đề tài tính khả thi phương pháp điều khiển lựa chọn áp dụng cho hoạt động bền vững điều khiển quỹ đạo robot Scara Serpent bậc tự CHƯƠNG I TỔNG QUAN VỀ ROBOT VÀ ĐIỀU KHIỂN ROBOT 1 Robot robot công nghiệp 1 Vài nét lịch sử phát triển robot robot cơng nghiệp Nhìn ngược dịng thời gian nhận thấy từ “Robot” xuất từ lâu Năm 1921 nhà viết kịch Karelcapek người Séc viết kịch với tựa đề R U R (Rossums Universal Robot) mô tả loạn cỗ máy phục dịch Từ “Robot” có nghĩa máy móc biết làm việc người Có lẽ gợi ý cho nhà sáng chế kỹ thuật thực mơ ước cỗ máy bắt chước thao tác lao động bắp người Thời gian sau cấu điều khiển từ xa (Teleoperator) đời ngày phát triển hoàn thiện Teleoperator cấu sinh học , bao gồm khâu , khớp với dây chằng gắn liền với hệ điều hành cánh tay người điều khiển thông qua cấu khuếch đại khí Teleoperator cầm nắm , nâng hạ , dịch chuyển , xoay lật đối tượng không gian hoạt động định Tuy thao tác tinh vi , khéo léo tốc độ hoạt động chậm , lực tác dụng hạn chế hệ điều khiển tuý khí Từ thập kỷ 50 , phát triển đầy hứa hẹn kỹ thuật điều khiển theo chương trình số cứng ngành vật liệu làm chỗ dựa vững cho đời cấu điều khiển vô cấp (servo mechanism) hệ điện toán (computation) Ngay ý tưởng kết hợp hệ điều khiển NC (Numerical control) với cấu điều khiển từ xa (Teleoperator) hình thành triển khai nghiên cứu Sự phối hợp tuyệt vời khả linh hoạt khéo léo Teleoperator với độ thông minh nhạy bén hệ điều khiển NC đưa kết hệ máy móc tự động cao cấp với tên gọi “Robot” Năm 1961 người máy công nghiệp (IR- industrial Robot) đưa thị trường Tiếp theo nước khác bắt đầu sản xuất robot công nghiệp theo quyền Mỹ , Anh (1967) , Thụy Điển , Nhật (1968) , Đức (1971) Ngày , giới có khoảng 200 cơng ty sản xuất IR , Nhật có 70 , nước Tây âu có 90 , Mỹ có 30 Nhờ áp dụng rộng rãi tiến khoa học kỹ thuật vi xử lý , tin học vật liệu nên số lượng robot công nghiệp tăng lên nhanh chóng , giá thành giảm rõ rệt , tính có nhiều cải tiến Robot cơng nghiệp phát huy mạnh lĩnh vực hàn hồ quang , đúc , lắp ráp , sơn phủ , hệ thống tự động điều khiển liên hợp 1 Robot công nghệ cao Robot công nghệ cao khái niệm sản xuất tự động hoá đại Các nước công nghiệp phát triển đưa chiến lược dùng tự động hoá đại (IR+High Tech) kéo xí nghiệp cơng nghiệp đầu tư nước ngồi (trước lý lương thợ rẻ mạt) trở quốc (dùng lao động robot cơng nghiệp) Chính phủ nước áp dụng biện pháp hỗ trợ hữu hiệu như: coi robot công nghiệp ngành công nghiệp quan trọng , xây dựng nhiều chương trình nhà nước áp dụng tiến khoa học kỹ thuật vào sản xuất robot Khuyến khích cách ưu tiên thuế đặt quy chế có lợi cho người sản xuất người sử dụng robot công nghiệp Nhờ sau thời gian ngắn sử dụng robot công nghiệp trở nên rộng lớn đa dạng với sở nguồn động lực phát triển “lực đẩy” công nghệ “lực kéo” thị trường Một đặc điểm quan trọng robot công nghiệp chúng cho phép dễ dàng kết hợp việc phụ trình sản xuất thành dây chuyền tự động So với phương tiện tự động hoá khác , dây chuyền tự động dùng robot có nhiều ưu điểm dễ dàng thay đổi chương trình làm việc , có khả tạo dây chuyền tự động từ máy vạn , tự động hố tồn phần Khi sử dụng robot vào dây chuyền tự động , khâu chuẩn bị kỹ thuật rút ngắn Trong với thời gian từ lúc định phương án đến lúc thiết kế xong dây chuyền máy tự động , mặt hàng quy trình cơng nghệ trở thành lạc hậu Theo số liệu chuyên gia Mỹ nghiên cứu vấn đề khảo sát 70 đề án thiết kế với nửa số phương án dùng máy tự động chuyên dụng phải tốn năm Vì , phương án dùng robot Unimate với máy tự động vạn đưa vào sử dụng phát huy hiệu to lớn Kỹ thuật robot có ưu điểm quan trọng tạo nên khả linh hoạt hoá sản xuất Việc sử dụng máy tính điện tử , robot máy điều khiển theo chương trình cho phép tìm phương thức mẻ để tạo nên dây chuyền tự động cho sản xuất hàng loạt với nhiều mẫu , loại sản phẩm Dây chuyền tự động “cứng” gồm nhiều thiết bị tự động chuyên dùng đòi hỏi vốn đầu tư lớn , tốn nhiều thời gian để thiết kế chế tạo , lúc quy trình cơng nghệ luôn cải tiến , nhu cầu chất lượng quy cách sản phẩm luôn thay đổi Bởi , nhu cầu “mềm” hoá hay linh hoạt hóa dây chuyền sản xuất ngày tăng Kỹ thuật robot cơng nghiệp máy tính đóng vai trị quan trọng cơng việc tạo dây chuyền tự động linh hoạt Xuất phát từ nhu cầu khả linh hoạt hoá sản xuất , năm gần nhà khoa học mà nhà sản xuất tập trung ý vào việc hình thành áp dụng hệ sản xuất tự động linh hoạt , gọi tắt hệ sản xuất linh hoạt Hệ sản xuất linh hoạt FMS (Flexible Manufacturing System) ngày thường bao gồm thiết bị gia công điều khiển chương trình số , phương tiện vận chuyển kho chứa phân xưởng tự động hố nhóm robot cơng nghiệp vị trí trực tiếp với thiết bị gia công thực nguyên công phụ Việc điều khiển kiểm tra hoạt động toàn hệ sản xuất linh hoạt thực máy tính Ưu điểm bật hệ sản xuất linh hoạt thích hợp với quy mơ nhỏ vừa , thích hợp với u cầu ln ln thay đổi chất lượng sản phẩm quy trình cơng nghệ Bởi , ngày hệ sản xuất linh hoạt thu hút ý nước phát triển mà nước phát triển Trong số tài liệu nước FMS hệ sản xuất tương lai (Future Manufactring System) 1 Định nghĩa robot công nghiệp Robot công nghiệp máy tự động linh hoạt thay phần tồn hoạt động bắp hoạt động trí tuệ người nhiều khả thích nghi khác Về mặt khí điều khiển điện tử , robot công nghiệp tổ hợp khả hoạt động linh hoạt cấu điều khiển từ xa với tốc độ phát triển ngày cao hệ thống điều khiển theo chương trình số kỹ thuật chế tạo cảm biến , cơng nghệ lập trình phát triển trí tuệ nhân tạo , hệ chuyên gia Robot cơng nghiệp có khả chương trình hố linh hoạt nhiều trục chuyển động , biểu thị cho số bậc tự chúng Robot công nghiệp trang bị bàn tay máy cấu chấp hành giải nhiệm vụ xác định q trình cơng nghệ: trực tiếp tham gia nguyên công (sơn , hàn , lắp ráp cụm thiết bị ) phục vụ q trình tổ chức dịng lưu thơng vật chất (chi tiết , dao cụ , gá lắp ) với thao tác cầm nắm , vận chuyển trao đổi đối tượng vật chất với trạm công nghệ hệ thống máy tự động linh hoạt Ta điểm qua vài định nghĩa robot công nghiệp sau: - Định nghĩa theo tiêu chuẩn AFNOR Fracais: Robot công nghiệp cấu chuyển đổi tự động chương trình hố , lặp lại chương trình , tổng hợp chương trình đặt trục toạ độ , có khả định vị , định hướng , di chuyển đối tượng vật chất theo chương trình thay đổi chương trình hố nhằm thực nhiệm vụ công nghệ khác - Định nghĩa theo tiêu chuẩn VDI 2806/BRD: Robot công nghiệp thiết bị có nhiều trục , thực chuyển động tự động chương trình hố nối ghép chuyển động chúng khoảng cách tuyến tính hay phi tuyến động trình Chúng điều khiển hợp ghép nối với , có khả học nhớ chương trình , chúng trang bị dụng cụ phương tiện công nghệ khác để thực nhiệm vụ sản xuất trực tiếp gián tiếp - Định nghĩa theo IOTC - 1980: Robot công nghiệp máy tự động liên kết tay máy cụm điều khiển chương trình hố , thực chu trình cơng nghệ cách chủ động với điều khiển thay chức tương tự người Bản chất định nghĩa cho ta thấy ý nghĩa quan trọng: robot công nghiệp phải liên hệ chặt chẽ với máy móc , cơng cụ thiết bị công nghệ tự động khác hệ thống tự động tổng hợp Trong q trình phân tích thiết kế khơng thể quan niệm robot đơn vị cấu trúc biệt lập , trái lại phải thiết kế tổng thể “hệ thống tự động linh hoạt robot hoá” cho phép thích ứng nhanh đơn giản nhiệm vụ sản xuất thay đổi Theo , mẫu hình robot phải đảm bảo có: - Thủ pháp cầm nắm , chuyển đổi tối ưu - Trình độ hành nghề khôn khéo , linh hoạt - Kết cấu phải tn theo ngun tắc mơdun hố Các phương pháp điều khiển robot Cho đến thực tế , nhiều phương pháp hệ thống điều khiển robot thiết kế sử dụng , phương pháp điều khiển chủ yếu là: - Điều khiển động lực học ngược - Điều khiển phản hồi phân ly phi tuyến - Các phương pháp điều khiển thích nghi  Điều khiển thích nghi theo sai lệch  Điều khiển thích nghi theo mơ hình mẫu (MRAC)  Điều khiển động lực học ngược thích nghi  Điều khiển trượt Chúng ta tìm hiểu phương pháp điều khiển robot để biết ưu nhược điểm phương pháp 10 Sơ đồ khối khâu điều chỉnh Sơ đồ khối điều khiển PID cho khớp 88 Sơ đồ khâu robot Scara Serpent Sơ đồ khối tín hiệu 89 Nhập giá trị tham số cho mơ hình 90 91 PHỤ LỤC 2: CÁC CHƯƠNG TRÌNH M FILES (chạy Matlab/Simulink) Tính tốn Quỹ đạo chuyển động chuẩn % Tinh toan Quy dao chuyen dong chuan % Xay dung quy dao chuyen dong chuan cho khop quay cua Robot Serpent % qi0 , qic: goc quay dau va cuoi cua khop quay thu i function qdc=qdcdc(u) % Toa diem dau: DDau=[X0;Y0] X0=u(1); Y0=u(2); % Toa diem cuoi: DCuoi=[Xc;Yc] Xc=u(3); Yc=u(4); % td: Dat thoi gian chuyen dong td=u(5); % Thoi gian gia toc (hoac giam toc) ta=u(6); % Thong so cua Robot: a1 ,a2 a1=u(7); %(m) a2=u(8); %(m) t=u(9); % thoi gian thuc nx=u(10); % CHUONG TRINH CHINH % Dong hoc nguoc va tinh cac goc quay cua tung khop tai diem dau va diem cuoi q20=Gocquay2(X0 ,Y0 ,a1 ,a2); q2c=Gocquay2(Xc ,Yc ,a1 ,a2); q10=Gocquay1(X0 ,Y0 ,q20 ,a1 ,a2); 92 q1c=Gocquay1(Xc ,Yc ,q2c ,a1 ,a2); q40=Gocquay4(q10 ,q20 ,nx); q4c=Gocquay4(q1c ,q2c ,nx); %Khoi tao cac he so cua quy dao chuan cua khop h1=Init(q10 ,q1c ,ta ,td); h2=Init(q20 ,q2c ,ta ,td); h4=Init(q40 ,q4c ,ta ,td); %Quy dao chuyen dong chuan khop quay cua Robot Serpent qdc1= Quydaochuan(t ,h1 ,ta ,td); qdc2= Quydaochuan(t ,h2 ,ta ,td); qdc4= Quydaochuan(t ,h4 ,ta ,td); qdc=[qdc1;qdc2;qdc4]; %KET THUC CHUONG TRINH CHINH %CHUONG TRINH TINH GIA TRI %Tinh cac gia tri tuong ung goc quay 2: q20 ,q2c function gq2=Gocquay2(x ,y ,a1 ,a2) C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); gq2=atan2(S2 ,C2); %Tinh cac gia tri tuong ung goc quay 1: q10 ,q1c function gq1=Gocquay1(x ,y ,q2 ,a1 ,a2) C1=((a1+a2*cos(q2))*x+a2*sin(q2)*y)/(x^2+y^2); S1=((a1+a2*cos(q2))*y-a2*sin(q2)*x)/(x^2+y^2); gq1=atan2(S1 ,C1); 93 %Tinh cac gia tri tuong ung goc quay 4: q40 ,q4c function gq4=Gocquay4(q1 ,q2 ,nx) KK=sqrt(1-nx^2); tetaphu=atan2(KK ,nx); gq4=q1+q2-tetaphu; %Xay dung quy dao chuyen dong chuan %qac=a0ac+a3ac*t^3+a4ac*t^4 %qcd=a0cd+a1cd*(t-2ta) %qdf=a0df+a1df*t+a3df*t^3+a4df*t^4 function quydao=Quydaochuan(t ,h ,ta ,td) a0ac=h(1);a3ac=h(2);a4ac=h(3); a0cd=h(4);a1cd=h(5); a0df=h(6);a1df=h(7);a3df=h(8);a4df=h(9);qcuoi=h(10); if td0)&&(td=0)&&(ttd/2)&&(ttd quydao=qcuoi; else 94 quydao=-1; %Error end else %if td>(4*ta) %Gom giai doan: gia toc , chay deu , giam toc if (t>=0)&&(t=2*ta)&&(t(td-2*ta))&&(ttd quydao=qcuoi; else quydao=-1;%Error end end %Tinh cac he so cua quy dao chuan ung voi tung khop function heso=Init(qdau ,qcuoi ,ta ,td) a0ac=0;a3ac=0;a4ac=0;a0cd=0;a1cd=0;a0df=0;a1df=0;a3df=0;a4df=0; if (td>0)&&(td(4*ta) %Quy dao doan ac: a0ac=qdau; a3ac=(qcuoi-qdau)/(4*ta^2*(td-2*ta)); a4ac=-(qcuoi-qdau)/(16*ta^3*(td-2*ta)); %Quy dao doan cd: a0cd=qdau+(qcuoi-qdau)*ta/(td-2*ta); a1cd=(qcuoi-qdau)/(td-2*ta); %Quy dao doan df: a0df=qcuoi-(qcuoi-qdau)*ta/(td-2*ta); a1df=(qcuoi-qdau)/(td-2*ta); a4df=-(3*qcuoi-3*a0df-4*a1df*ta)/(16*ta^4); a3df=-(a1df+32*a4df*ta^3)/(12*ta^2); end heso=[a0ac ,a3ac ,a4ac ,a0cd ,a1cd ,a0df ,a1df ,a3df ,a4df ,qcuoi]; 96 Tính momem chuẩn % Tinh toan momem chuan function momem=momemchuan(u) q2d=u(1); dq1d=u(2);ddq1d=u(3); dq2d=u(4);ddq2d=u(5); dq4d=u(6);ddq4d=u(7); mt=u(8);Jt=u(9); % Thong so cua robot J1=0 021; J2=0 0034; J3=0 006; J40=0 001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1 5; m3=2; m40=0 6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; l1=0 25; l2=0 15; % Tinh T T=-(m234*l1*l2*sin(q2d)); 97 % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2d); H12=m234*l2^2+J24+m234*l1*l2*cos(q2d); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Dau % Tinh momem chuan quy dao dat u1m=H11*ddq1d+H12*ddq2d+H13*ddq4d+T*dq2d^2+2*T*dq1d*dq2d; u2m=H21*ddq1d+H22*ddq2d+H23*ddq4d-T*dq1d^2; u4m=H31*ddq1d+H32*ddq2d+H33*ddq4d; momem =[u1m;u2m;u4m]; Tính góc quay ban đầu %Tinh toan goc quay ban dau function init=gocquaybd(u) x=u(1);%m y=u(2);%m a1=u(3);%(m) a2=u(4);%(m) nx=u(5); %q20=Gocquay2(X0 ,Y0 ,a1 ,a2) 98 %q10=Gocquay1(X0 ,Y0 ,q20 ,a1 ,a2); %q40=Gocquay4(q10 ,q20 ,nx); %Tinh cac gia tri tuong ung goc quay 2: q20 C2=(x^2+y^2-a1^2-a2^2)/(2*a1*a2); S2=sqrt(1-C2^2); q20=atan2(S2 ,C2); %Tinh cac gia tri tuong ung goc quay 1: q10 C1=((a1+a2*cos(q20))*x+a2*sin(q20)*y)/(x^2+y^2); S1=((a1+a2*cos(q20))*y-a2*sin(q20)*x)/(x^2+y^2); q10=atan2(S1 ,C1); %Tinh cac gia tri tuong ung goc quay 4: q40 KK=sqrt(1-nx^2); thetaphu=atan2(KK ,nx); q40=q10+q20-thetaphu; %Dau init=[q10;q20;q40]; Tính góc điều khiển thực % Tinh toan goc dieu khien thuc function ddtheta=tinh_ddtheta(u) % Momem dieu khien u1=u(1); u2=u(2); u4=u(3); % Thong so cua tai mt=u(4); Jt=u(5); % Goc quay theta2 thuc te q1tt=u(6);q2tt=u(7);q4tt=u(8); 99 % Dao ham goc quay thuc te dq1tt=u(9);dq2tt=u(10); % Goc quay ban dau q10=u(11);q20=u(12);q40=u(13); % CHUONG TRINH CHINH % Tinh cac he so heso=tinh_heso(q2tt ,dq1tt ,dq2tt ,mt ,Jt); % Tinh dao ham cap hai cua goc quay thuc ddtheta=tinh_ddthetathuc(heso ,u1 ,u2 ,u4 ,q10 ,q20 ,q40); % Chuong trinh tinh cac he so function HS=tinh_heso(q2tt ,dq1tt ,dq2tt ,mt ,Jt) % Thong so cua robot J1=0 021; J2=0 0034; J3=0 006; J40=0 001; J4=J40+Jt; J124=J1+J2+J4; J24=J2+J4; m1=4; m2=1 5; m3=2; m40=0 6; m4=m40+mt; m1234=m1+m2+m3+m4; m234=m2+m3+m4; m34=m3+m4; 100 l1=0 25; l2=0 15; % Tinh T T=-(m234*l1*l2*sin(q2tt)); % Tinh toan cac gia tri Hij H11=m1234*l1^2+m234*l2^2+J124+2*m234*l1*l2*cos(q2tt); H12=m234*l2^2+J24+m234*l1*l2*cos(q2tt); H13=J4; H21=H12; H22=m234*l2^2+J24; H23=J4; H31=J4; H32=J4; H33=J4; DH=J4^2*(2*H12-H11-H22)+J4*(H11*H22-H12^2); % Cac gia tri dau bij b11=DH^-1*(H22*H33-H23*H32); b12=DH^-1*(H13*H32-H12*H33); b13=DH^-1*(H12*H23-H13*H22); b21=DH^-1*(H23*H31-H21*H33); b22=DH^-1*(H11*H33-H13*H31); b23=DH^-1*(H13*H21-H11*H23); b31=DH^-1*(H21*H32-H31*H22); b32=DH^-1*(H12*H31-H11*H32); b33=DH^-1*(H11*H22-H12^2); % Cac gia tri h1 ,h2 h1=T*dq2tt^2+2*T*dq1tt*dq2tt; h2=-(T*dq1tt^2); 101 % Cac gia tri a1=-(b11*h1+b12*h2); a2=-(b21*h1+b22*h2); a4=-(b31*h1+b32*h2); HS =[b11 ,b12 ,b13 ,b21 ,b22 ,b23 ,b31 ,b32 ,b33 ,a1 ,a2 ,a4]; % Chuong trinh tinh dao ham cap hai cua goc quay thuc function d2theta=tinh_ddthetathuc(heso ,u1 ,u2 ,u4 ,q10 ,q20 ,q40) b11=heso(1);b12=heso(2);b13=heso(3); b21=heso(4);b22=heso(5);b23=heso(6); b31=heso(7);b32=heso(8);b33=heso(9); a1=heso(10);a2=heso(11);a4=heso(12); % Tinh dao ham cap hai cua goc quay thuc d2theta1=a1+b11*u1+b12*u2+b13*u4; d2theta2=a2+b21*u1+b22*u2+b23*u4; d2theta4=a4+b31*u1+b32*u2+b33*u4; d2theta=[d2theta1;q10;d2theta2;q20;d2theta4;q40]; 102 ... mạch điều khiển robot Để hệ điều khiển robot có độ tin cậy , độ xác cao , giá thành hạ tiết kiệm lượng nhiệm vụ hệ điều khiển robot phải đảm bảo giá trị yêu cầu đại lượng điều chỉnh điều khiển. .. dựng hệ thống điều khiển quỹ đạo đạt độ xác cao - Đánh giá chất lượng hệ thống mô Nội dung luận văn đề cập tới vấn đề ? ?Nâng cao chất lượng điều khiển robot Scara? ?? với mục tiêu điều khiển bền vững... Xây dựng mơ hình hệ thống điều khiển: Thiết kế điều khiển để nâng cao chất lượng hệ thống điều khiển quỹ đạo cho robot Xây dựng mơ hình hệ thống điều khiển cho robot Scara Serpent , xây dựng