Áp dụng phương pháp hoạch định quỹ đạo dùng phép nội suy đường đa thức bậc 3 trong không gian tọa độ khớp để hoạch định định quỹ đạo gắp vật. Ta xét trong trường hợp chung. Giả sử khớp thứ n quay góc 𝜃𝑖 và 𝜃𝑖+ trong các khoảng thời gian 𝑡𝑘 và 𝑡𝑘+ tương ứng, quỹ đạo đa thức bậc ba của biến khớp n có dạng:
i(t) ai3t3 ai2t2 ai1t ai0 (4.7) Và:
i 1(t) a(i 1)3t3 a(i 1)2t2 a(i 1)1t a(i 1)0 (4.8) Vận tốc:
̇i(t) 3ai3t2 2ai2t ai1 (4.9) Việc xác định các tham số của các đa thức dựa trên các điều kiện ràng buộc sao cho đảm bảo điều kiện liên tục về vị trí, vận tốc và gia tốc.
Ràng buộc vị trí: i(0) k 1 i(tk) i 1(0) k i 1(tk 1) k 1 Ràng buộc vận tốc: ̇i(0) ̇k 1 ̇i(tk) ̇i 1(0) ̇k ̇i 1(tk 1) ̇k 1
Ràng buộc gia tốc:
̈i(tk) ̈i 1(0) ̈k
Các khoảng thời gian được chia đều nên tk tk 1 ∆t. Giải các phương trình trên ta được: ai3 2( k k 1) ( ̇k ̇k 1)∆t ∆t3 (4.10) ai2 6( k k 1) 2( ̇k 2 ̇k 1)∆t 2∆t2 (4.11) ai1 ̇k 1 (4.12) ai0 k 1 (4.13)
Quỹ đạo đa thức bậc ba:
i(t) 2( k k 1) ( ̇k ̇k 1)∆t
∆t3 t3 6( k k 1) 2( ̇k 2 ̇k 1)∆t
2∆t2 t2 ̇k 1t k 1 (4.14) Ta xét một quỹ đạo:
Giả sử cánh tay robot di chuyển từ điểm tựa A đến điểm tựa B. Tọa độ 2 điểm tựa này được hoạch định sẵn và cánh tay robot phải di chuyển qua. Để đưa đầu công tác di chuyển từ điểm tựa A đến điểm tựa B, khớp 1 sẽ di chuyển một góc từ 30o đến 60o
thời gian hoạch định là 5s. Tốc độ tại A và B là 0.
(0) a0 30
(5) a0 5a1 52a2 53a3 60
̇(0) a1 0
Ta thu được hệ số của quỹ đạo
a0 30; a1 0/s; a2 3,6/s2; a3 0,48/s3
Phương trình biểu diễn vị trí, tốc độ và gia tốc của khớp có dạng:
(t) 30 3,6.t2 0,48.t3 (4.15)
̇(t) 7,2.t 1,44.t2 (4.16)
̈(t) 7,2 2,88.t (4.17)
Hình 4.1: Đường biểu diễn vị trí, tốc độ, gia tốc của khớp 1 theo thời gian Tương tự đối với các góc khớp còn lại ta có thể sử dụng phương pháp đường bậc 3 để tính toán quỹ đạo của robot trong không gian khớp.
4.4 Phƣơng pháp điều khiển
Để điều khiển đầu công tác của robot di chuyển từ A đến B, ta điều khiển từng khớp bám theo quỹ đạo mong muốn. Có 2 phương pháp tiếp cận vấn đề điều khiển các khớp: điều khiển khớp độc lập và điều khiển dựa trên mô hình đối tượng.
Điều khiển khớp độc lập, mỗi khớp trong cánh tay robot được xem là một hệ độc lập, điều khiển khớp theo quỹ đạo được hoạch định bằng các phương pháp đã nêu trên. Phương pháp này được áp dụng rộng rãi trong công nghiệp và thường có hai vòng điều khiển , vòng điều khiển vận tốc và vòng điều khiển vị trí. Luật điều khiển được sử dụng là PID có nhược điểm là phải sử dụng khâu tích phân để bù sai số điều khiển, điều này dẫn đến hệ thống cần có thời gian xác lập và độ vọt lố.
Điều khiển dựa trên mô hình hướng đối tượng: từ bài toán động lực học ta có thể tính được lực hoặc moment cần thiết để điều khiển khớp theo quĩ đạo,đây là phương pháp điều khiển feedforward. Tuy nhiên, điều khiển feedforward phụ thuộc vào độ chính xác của mô hình đối tượng được tính toán, chính vì thế nhiễu sẽ ảnh hưởng lớn đến sai số hành trình. Ví dụ, khi tải thay đổi, các thông số lực trong mô hình sẽ thay đổi theo ,vòng điều khiển feedback được thêm vào để bù sai số trong các trường hợp này.
Ta có thể thấy phương pháp điều khiển theo hướng đối tượng là tốt hơn vì đảm bảo được độ chính xác với cả 2 vòng điều khiển feedback và feedforward. Tuy nhiên, độ phức tạp sẽ cao hơn và số lượng phép tính lớn. Vì vậy, sẽ không phù hợp với vi điều khiển có tốc độ tính toán và tài nguyên giới hạn. Do việc áp dụng hệ thống điều khiển mới vào điều khiển cánh tay robot này và bước đầu phát triển đề tài nên cần một hệ thống điều khiển đơn giản hơn và dễ áp dụng vào thực tiễn. Đề tài chọn phương pháp điều khiển khớp độc lập để áp dụng vào điều khiển cánh tay robot lắp vật.
Trong phương pháp này, mỗi khớp được xem như một hệ thống độc lập nhằm điều khiển khớp theo quỹ đạo cho trước. Thường có hai vòng điều khiển , vòng điều khiển vận tốc và vòng điều khiển vị trí. Do thời gian luận văn có giới hạn nên ở đây ta chỉ sử dụng vòng điều khiển vị trí và luật điều khiển được sử dụng là PID (proportional- integral – derivative controller).
Kết luận: Phương án điều khiển khớp độc lập bám theo quỹ đạo thường có hai vòng điều khiển. Vòng điều khiển vận tốc và vòng điều khiển vị trí , do đề tài chỉ dừng ở mức ban đầu và thời gian giới hạn nên đề tài đã sử dụng Driver DCS3T- 25 để thực hiện giải thuật PID. Đề tài sẽ xây dựng quỹ đạo các điểm tựa cho cánh tay robot và sau đó đưa các tọa độ vị trí điểm tựa vào phương trình động học ngược được lập trình bằng phần mềm C# tính ra các góc quay tương ứng của các khớp. Từ góc quay các khớp, phần mềm sẽ tính ra số xung cần quay cho 5 động cơ tương ứng với số góc quay cần di chuyển từ điểm tựa này đến điểm tựa khác và gửi dữ liệu này xuống cho vi điều khiển. Theo lý thuyết, ta dựa vào phương trình nội suy đường bậc 3, có vị trí điểm mong muốn và thời gian quy định thì ta sẽ tính ra được vận tốc mong muốn của mỗi khớp. Ứng với thời gian bắt đầu di chuyển trong 1 đoạn là thời gian gửi dữ liệu vị trí và vận tốc từ máy tính xuống vi điều khiển. Dữ liệu vị trí bây giờ là số xung và dữ liệu vận tốc bây giờ là tốc độ bắn xung của vi điều khiển điều khiển driver. Thời gian di chuyển giữa các quỹ đạo là như nhau, số điểm tựa trong quỹ đạo do ta hoạch định, do đó thời gian gi chuyển toàn bộ quỹ đạo ta sẽ tính được. Lúc này ta chỉ cần đồng bộ thời gian di chuyển từ vị trí cảm biến của bảng
làm việc đến vị trí lắp vật bằng với thời gian di chuyển của cánh tay robot thì có thể lắp vật chính xác vào vị trí mong muốn.
4.5 Giải thuật điều khiển
Để có thể lắp vật chính xác vào vị trí cần lắp ráp khi bảng làm việc di chuyển trên băng tải đòi hỏi hệ thống phải có cảm biến xác định vị trí của bảng làm việc so với vị trí cần gắp. Từ yêu cầu đó, ta sử dụng cảm biến tiệm cận để thực hiện công việc này. Khoảng cách đặt cảm biến là cố định, vận tốc băng tải cố định được điều khiển bởi Driver DCS3T-25 do đó thời gian di chuyển của bảng làm việc đến vị trí lắp ráp là xác định được.
S Đ S Đ S S Đ S Đ
Hình 4.4: Lưu đồ giải thuật điều khiển trên Transmit khi tiến hành lắp vật
Start Nhận dữ liệu
từ máy tính
End
Return Khai báo cấu hình hoạt động
cho các chức năng : Can, Uart
Khai các biến sự kiện ngắt
Kích hoạt ngắt
Truyền tín hiệu lên máy tính
Nhận dữ liệu từ máy tính
Gởi dữ liệu nhận được cho 6 mạch Receive , qua mạng Can
và tín hiệu output
Đọc các sai số từ mạch Receive điều khiển khớp tay máy.
Lưu dữ liệu nhận được
Bật cớ ngắt
Truyền tín hiệu lên máy tính Tín hiệu cảm biến tiệm cận ? Ngắt Rs232 ? Dữ liệu nhận = 32 byte ? Err1 = 0 Err2 = 0 Err3 = 0 Err4 = 0 Err5 = 0
S S Đ S Đ S Đ S Đ
Hình 4.5: Lưu đồ giải thuật điều khiển cho Receive khớp tay máy
Start
Khai báo cấu hình hoạt động cho các chức năng : Can, Timer
Khai báo các biến, sự kiện ngắt.
Kích hoạt ngắt
Ngắt nhận tin nhắn Can.
Bắn xung và kích chân chiều vào driver để điều khiển góc quay động cơ.
Gởi tin nhắn cho Transmit Error = 0 ?
End
Ngắt nhận tin nhắn Can.
Kiểm tra địa chỉ tin nhắn ?
Dữ liệu nhận – 8 byte ?
Lưu dữ liệu data trong tin nhắn. Bật cờ ngắt Gởi tin nhắn Error cho Transmit Gởi tin nhắn hoàn thành cho Transmit Error = 0 ? Return
Start
Khai báo cấu hình hoạt động cho các chức năng input và Timer1
Khai báo các biến, sự kiện ngắt
Kích hoạt ngắt
Bắn xung điều khiển động cơ tay kẹp nhả
End
Nhận tín hiệu output của mạch Transmit
THỰC NGHIỆM, ĐÁNH GIÁ KẾT QUẢ CHƢƠNG 5:
5.1 Giới thiệu
5.1.1Mục tiêu, yêu cầu thực nghiệm
Kiểm tra hoạt động, độ ổn định của cơ cấu tay kẹp Kiểm tra hoạt động của các mạch điện.
Kiểm tra hoạt động của giải thuật gắp vật và chương trình điều khiển. Hiệu chỉnh.
5.1.2Các thành phần thí nghiệm
Mô hình khối lập phương mô phỏng thùng hàng cần gắp.
Hình 5.1: Mô hình khối lập phương
Đề tài được thực nghiệm trên mô hình cánh tay robot 5 bậc tự do.
5.2 Quá trình thực nghiệm
5.2.1Kiểm tra hoạt động của mạch điện, bài toán động học và di chuyển của cánh tay theo quỹ đạo.
Bằng cách sử dụng giao diện chương trình trên máy tính, đề tài tiến hành thực hiện việc điều khiển vị trí cho từng khớp, sau đó điều khiển cánh tay robot di chuyển theo quỹ đạo lắp ráp. Lúc này, ta đưa bảng làm việc đến vị trí lắp ráp rồi cho dừng lại để kiểm tra cánh tay robot có di chuyển đúng quỹ đạo, lắp ráp có chính xác và đúng thời gian mong muốn không.
Hình 5.3: Ảnh chụp trong quá trình robot gắp khối lập phương.
Theo quỹ đạo hoạch định cánh tay robot sẽ di chuyển từ tọa độ (250,0,-85) và kết thúc quỹ đạo lắp vật tại tọa độ (0,-250,-105) thời gian di chuyển là 1 phút 5 giây. Quỹ đạo có 56 điểm và thời gian di chuyển giữa các điểm là 1,2 giây tương ứng với thời gian mỗi lần gửi dữ liệu của máy tính xuống Transmit.
Hình 5.4: Kết quả gắp khối lập phương
Vùng gạch màu đen là vùng đặt mong muốn (kích thước 30mm X 30mm). Đường màu vàng là đường kẽ vị trí khối lập phương chiếm chỗ khi được robot gắp. Sai lệnh cho phép để đảm bảo việc sắp xếp đạt yêu cầu là sai lệch so với biên chuẩn nhỏ hơn 4mm .
Thực nghiệm được tiến hành 5 lần và cho kết quả: Bảng 5.1: Bảng khảo sát
Số lần
Sai số so với vị trí mong muốn (mm)
Thời gian thực hiện (s)
1 2,4 65
2 3,1 65
3 2,7 64
4 2,4 64
Thực nghiệm kiểm tra sai số lặp lại 5 lần liên tiếp.
Hình 5.5: Kết quả gắp khối lập phương lặp lại 5 lần liên tục.
5.2.2Kết quả
Mạch điện robot hoạt động tốt, mạch điều khiển đáp ứng được yêu cầu đặt ra. Robot di chuyển theo quỹ đạo mong muốn nhưng còn sai số. Việc còn lại là đồng bộ tốc độ di chuyển của bảng làm việc từ vị trí cảm biến tiệm cận đến vị trí lắp vật.
5.3 Đánh giá sai số
Nhìn chung kết quả lắp ráp là khá chính xác có thể gắp vật đúng vị trí mong muốn nhưng còn có sai số (sai số cao nhất là 3,1 mm tuy nhiên vẫn đảm bảo gắp khối lập phương vào đúng vị trí mong muốn, sai số nhỏ nhất là 2,4 mm). Sai số vị trí so với tọa độ cần tới tương đối lớn nhưng so với quỹ đạo di chuyển là nhỏ và do cánh tay di chuyển qua nhiều điểm tựa thuộc quỹ đạo nên sai số này chấp nhận được, vẫn đảm bảo gắp vật vào vị trí mong muốn thành công.
5.4 Kết luận
Những kết quả trên là những kết quả bước đầu nên còn thiếu sót và còn sai số trong quá trình điều khiển. Việc điều khiển robot vẫn chưa toàn diện cả vận tốc và vị trí nên cánh tay lúc di chuyển theo quỹ đạo vẫn còn khựng chưa đều và êm, ngoài ra chưa khử được sai số do độ rơ của các kết cấu cơ khí.
TỔNG KẾT VÀ HƢỚNG PHÁT TRIỂN ĐỀ TÀI CHƢƠNG 6:
6.1 Đánh giá kết quả đạt đƣợc 6.1.1Nội dung hoàn thành
Đề tài đã thực hiện được các nội dung đề ra trong phần nhiệm vụ, cánh tay robot có thể hoạt động và gắp khối lập phương di chuyển đúng với yêu cầu đề ra. Trong nội dung đề tài đã thực hiện được việc tổng quan về tình hình nghiên cứu và ứng dụng tay máy robot trong việc sắp xếp các thùng carton lên pallet, thiết kế mô hình cơ khí của băng tải, tay gắp và tiến hành gia công chết tạo, thiết kế mạch điện điều khiển, giao diện giao tiếp giữa máy tính và robot, xây dựng được mạng giao tiếp CAN ổn định và ít sai số, giải thuật điều khiển cánh tay robot lắp vật khối hộp lập phương, lắp mô hình, chạy thực nghiệm hệ thống và đánh giá kêt quả thu được.
6.1.2Hạn chế của đề tài
Trong quá trình thực hiện đề tài, nhận thấy có những vấn đề phát sinh và chưa được khắc phục triệt đó là sai số do độ rơ cơ khí, sai số chế tạo tay kẹp dẫn đến kết quả lắp ráp có sai số. Trong thực nghiệm chỉ dừng lại ở việc điều khiển vị trí chưa thực hiện được điều khiển vận tốc do đó cánh tay robot di chuyển không thể êm mượt mà còn bị khựng lại giữa 2 đoạn điểm tựa trong quĩ đạo.
6.2 Hƣớng phát triển của đề tài
Dựa trên những hạn chế của đề tài, ta có được những định hướng phát triển trong tương lai như sử dụng vi điều khiển có tốc độ cao để xử lý cao hơn để đọc chính xác số liệu encoder. Từ đó có thể kiểm tra driver có điều khiển vị trí chính xác như nhận xung điều khiển từ vi điều khiển không . Sử dụng phương pháp khử rơ về cả cơ khí lẫn mạch điện nhằm tăng độ chính xác cho việc điều khiển hệ thống. Áp dụng vòng điều khiển vận tốc để cánh tay robot có thể hoạt động êm và ổn định hơn.
TÀI LIỆU THAM KHẢO
[1] John J.Craig. (2005). Introduction to Robotics, Mechanical and Control, third edition, Pearson Education International.
[2] Nguyễn Mạnh Tiến. (2007). Điều khiển robot công nghiệp. Nhà xuất bản Khoa học và kỹ thuật.
[3] Nguyễn Thiện Phúc. (2006). Robot công nghiệp. Nhà xuất bản Khoa học và kỹ thuật.
[4] Nguyễn Hữu Lộc. (2012). Cơ sở thiết kế máy. Nhà xuất bản Đại Học Quốc Gia Thành Phố Hồ Chí Minh.
[5] Peter Corke,Robotics. (2008). Vision and Control,Fundamental Algorithms in Matlab.
[6] Trịnh Chất, Tính toán thiết kế Hệ dẫn động Cơ khí Tập 1. Nhà xuất bản Giáo Dục Việt Nam,
[7] Trịnh Chất. (2010) Tính toán thiết kế Hệ dẫn động Cơ khí Tập 2. Nhà xuất bản Giáo Dục Việt Nam.
[8] Phạm Đăng Phước. (2007). Giáo trình Robot Công Nghiệp. NXB Xây Dựng Hà Nội.
[9] Reza N. Jazar, Theory of Applied Robotics,Kinematics,Dynamics,and Control Second Edition.
[10] Advisors, Dr. Hatem Elaydi and Dr.Iyad Abuhadrous. (2011). Simulation and Interfacing of 5 DOF Educational Robot Arm, A master thesis.