Mục đích của bài toán động học ngược, là từ việc cho trƣớc quy luật chuyển
động của khâu tác động cuối ( ở đây là tâm P của bàn máy động), cần phải tìm các quy luật chuyển động của các khớp dẫn động: tọa độ, vận tốc và gia tốc. Với các robot song song và các robot chuỗi phức tạp, ngừơi ta sử dụng phương pháp số cho kết quả chính xác tùy ý. Phương pháp giải tích thường gặp khó khăn và liên quan đến bài toán chọn nghiệm từ các bộ nghiệm, bởi hệ phương trình động học liên quan đến các hàm lượng giác. Ở đây, ta sẽ trình bày phương pháp số NewtonRaphson cải tiến (hiệu chỉnh gia lượng) để giải bài toán động học ngược. Việc giải bài toán có
thể tách riêng cho từng chân, nghĩa là ở mỗi chân, ta sẽ có 3 ph ƣơng trình 3 ẩn, bài
toán sẽ đơn giản hơn nhưng ta phải thực hiện ba lần, bởi có 3 chân. Ở đây, ta nhóm chúng lại thành 9 phương trình 9 ẩn. Cách giải là hoàn toàn giống nhau, nhưng ta sẽ phải tính toán các ma trận Jacobian 9x9, thay vì việc giải 3 bài toán với 3 ma trận Jacobian 3x3 độc lập.
Phương trình chuyển động tổng quát của robot có dạng như sau: (3.12)
Bài toán động lực học ngược là bài toán cho trước quy luật biến đổi của các tọa độ, vận tốc và gia tốc khâu thao tác cần tìm lực hoặc momen dẫn động. Như vậy,
63
qua bài toán động học ngược với quỹ đạo cho trước của tâm bàn máy độngta đã tìm được các véctơ . Từ đó các biểu thức về ma trận khối lượng,ma trận quán tính ly tâm và Coriolis, ma trận Φs , cũng như véctơ g(s) đã xác định hoàn toàn. Như vậy, phương trình (3.3) là phương trình đại số tuyến tính với ẩn là các véc tơ momen dẫn động và các nhân tử Lagrange λ với số phương trình bằng số ẩn. Vì vậy, ta có thể giải trực tiếp hệ phương trình này sau đó tách lấy kết quả là các mô men dẫn động .
Trong đồ án này sẽ không giải trực tiếp phương trình (3.12) mà tìm cách khử nhân tử Lagrange λ , biến đổi hệ phương trình vi phân – đại số (3.12) về hệ phương trình chỉ có các ẩn là mô men khớp chủ động .
Ta phân tập các toạ độ vật lý thành hai tập con: Tập các toạ độ suy rộng độc lập và tập các toạ độ suy rộng phụ thuộc . Chú ý rằng f + r = n. Số lượng các toạ độ suy rộng phụ thuộc bằng số lượng các phương trình liên kết bổ sung. Ma trận Φs có dạng:
(3.13) Để biến đổi, ta sẽ sử dụng các ký hiệu:
64
(3.13*) Bây giờ ta đưa vào ký hiệu:
(3.14) trong đó E là ma trận đơn vị cỡ fxf.
Các bước giải bài toán động lực học ngược theo phương pháp này: Bước 1: Giải bài toán động học ngược. Cho biết x(t) và f(q,x)=0.
Tính
Bước 2: Tính các ma trận trong công thức 3.3
Bước 3: Tính các mô men (lực) các khâu dẫn theo công thức (3.12).
Tổng động năng của cơ hệ bao gồm động năng chuyển động của các khâu chủ động, tay nối và bàn máy động trong đó:
Động năng của khâu chủ động với chuyển động quay xung quanh trục khớp cố định:
Động năng của tay dẫn với hai khối lượng tập trung ở hai đầu khớp :
Động năng của bàn máy động chuyển động quay trong không gian:
65
Như vậy tổng động năng của Robot là: (3.2)
Tổng thế năng của cơ hệ: (3.15) Trong đó:
là thế năng của khâu chủ động. là thế năng của tay nối.
là thế năng của bàn tay kẹp.
Các phương trình liên kết thể hiện quan hệ ràng buộc giữa các tọa độ suy rộng dư chính là các phương trình động học (2.13):
Trong đó:
Thay các biểu thức vào phương trình Lagrange nhân tử ta thành lập được các phương trình sau:
Phương trình vi phân chuyển động của bàn máy động : Tính toán các số hạng ta có:
Tương tự đối với các thành phần và ta có :
66
Các thành phần và bằng 0, thành phần
Xác đinh các thành phần :
Các lực suy rộng không có thế đối với các tọa độ suy rộng (x,y,z) đều bằng 0 (giả thiết bỏ qua lực cản). Thay các số hạng tính toán ở trên vào phương trình Lagrange dạng nhân tử ta thu được các phương trình vi phân chuyển động của bàn máy động như sau:
(3.16) (3.17) (3.18)
Phương trình vi phân chuyển động của khâu chủ động: Tính các số hạng ta có:
Với .
Tính các số hạng ta có:
Tính số hạng ta có:
Nhân ra và thu gọn các biểu thức ta sẽ thu được:
67
Lực suy rộng không thế ứng với các tọa độ suy rộng chính là các momen tác động từ động cơ . Thay các số hạng đã tính toán vào phương trình Lagrange dạng nhân tử ta thu được phương trình vi phân chuyển động của khâu chủ động như sau:
(3.19) Với.
Như vậy ta có hệ gồm 6 phương trình vi phân chuyển động (3.14)-(3.19) mô tả cho bàn máy động và khâu chủ động như sau:
Với các quỹ đạo thiết kế từ trước, các tọa độ của bàn máy động là hoàn toàn xác định theo thời gian. Bằng việc giải bài toán động học ngược ta xác định được giá trị các biến khớp theo thời gian. Thay các giá trị tọa độ và và các đạo hàm cấp hai của chúng theo thời gian vào ba phương trình vi phân chuyển động của bàn máy động ta giải và tìm được giá trị của các nhân tử Lagrange . Thay các giá trị của nhân tử Lagrange và các đạo hàm cấp hai của biến khớp vào ba phương trình
vi phân chuyển động của khâu chủ động ta tính toán được các giá trị momen tác
động vào các khâu chủ động.
68
CHƯƠNG 4: THIẾT KẾ HỆ THỐNG ĐIỀU KHIỂN 4.1 - Cấu trúc hệ thống điều khiển
Hình 4.52- Sơ đồ cấu trúc hệ thống điều khiển
Bộ điều khiển nhận dữ liệu từ các cảm biến để tiến hành xử lý ảnh tìm ra tọa độ ban đầu của vật thể để giải phương trình bắt điểm tìm ra tọa độ của vật thể sau khoảng thời gian t từ đó tự động tạo ra mã điều khiển, đồng thời cũng nhận các tín hiệu và chương trình từ người vận hành, các chương trình này thể hiện vị trí làm việc mong muốn cho robot được viết theo mã mà hệ thống quy định.
69
Bộ điều khiển thực hiện việc truyền nhận dữ liệu điều khiển và giải mã tín hiệu đồng thời thực hiện tính toán động học ngược robot truyền xung điều khiển đến các driver điều khiển động cơ để thực hiện điều khiển góc quay cho động cơ và gửi xung điều khiển servo. Bộ điều khiển còn thực hiện nhận các tín hiện phản hồi của các công tắc hành trình, nút nhấn để thực hiện các chức năng được đề ra cho bộ điều khiển.
4.2 - Các thành phần chính trong hệ thống điều khiển4.2.1– Vi điều khiển Atemga328P 4.2.1– Vi điều khiển Atemga328P
Vi điều khiển là một máy tính được tích hợp trên một chip, nó thường được sử dụng để điều khiển các thiết bị điện tử. Vi điều khiển, thực chất, là một hệ thống bao gồm một vi xử lý có hiệu suất đủ dùng và giá thành thấp (khác với các bộ vi xử lý đa năng dùng trong máy tính) kết hợp với các khối ngoại vi như bộ nhớ, các module vào/ra, các module biến đổi số sang tương tự và tương tự sang số,... Ở máy tính thì các module thường được xây dựng bởi các chip và mạch ngoài.
Vi điều khiển thường được dùng để xây dựng các hệ thống nhúng. Nó xuất hiện khá nhiều trong các thiết bị điện, điện tử, máy giặt, lò vi sóng, điện thoại, đầu đọc DVD, thiết bị đa phương tiện, dây chuyền tự động,...
Để hệ thống đọc được dữ liệu thu thập từ cảm biến và gửi lệnh điều khiển đến các cơ cấu chấp hành thì cần có một vi điều khiển thực hiện xử lí các thông tin trên. Áp dụng vào đồ án, nhóm lựa chọn dòng kit điều khiển Arduino được tích hợp vi điều khiển Atemega328P nhằm dễ dàng thực hiện các hoạt động như mong muốn của người lập trình.
ATmega328P là một bộ vi điều khiển tiên tiến và nhiều tính năng. Nó là một trong những vi điều khiển nổi tiếng của Atmel vì nó được sử dụng trong bo mạch arduino UNO. Nó là một bộ vi điều khiển thuộc họ vi điều khiển megaMVR của
70
Atmel (Cuối năm 2016, Atmel được Microchip Technology Inc mua lại). Các vi điều khiển được sản xuất trong họ megaMVR được thiết kế để xử lý các bộ nhớ chương trình lớn và mỗi vi điều khiển trong họ này chứa lượng ROM, RAM, các chân I / O và các tính năng khác nhau và được sản xuất với các chân đầu ra khác nhau, từ 8 chân đến hàng trăm chân.
Hình 4.53- Vi điều khiển Atemega328P
Hình 4.54- Vi điều khiển Atemega328P Pinout
71
Mạch bên trong của ATmega328P được thiết kế với tính năng tiêu thụ dòng điện thấp. Con chip này chứa 32 kilobyte bộ nhớ flash trong, 1 kilobyte EEPROM và 2 kilobyte SRAM. EEPROM và bộ nhớ flash là bộ nhớ lưu thông tin và thông tin đó vẫn thoát ra mỗi khi nguồn điện bị ngắt nhưng SRAM là bộ nhớ chỉ lưu thông tin cho đến khi có điện và khi ngắt nguồn điện tất cả thông tin được lưu trong SRAM sẽ bị xóa.
Đặc tính / Thông số kỹ thuật của ATmega328P:
Thiết kế hiệu suất cao Tiêu thụ ít điện năng
Tổng số chân ngõ vào Analog là 6 Chứa 32 kilobyte bộ nhớ flash Chứa 2 kilobyte SRAM
Chứa 1 kilobyte EEPROM Tốc độ xung nhịp 16 megahertz
Nhiệt độ tối thiểu và tối đa -40 độ C đến 105 độ C. Tổng số chân I / O kỹ thuật số là 14 chân
RISC tiên tiến
Khóa chức năng chương trình để bảo mật mã lập trình Chứa tổng cộng ba bộ định thời, hai 8 bit và một 16 bit Tổng số chân I / O là 23 chân
Tổng số kênh PWM là 6
Điện áp hoạt động tối thiểu và tối đa từ 1.8V DC đến 5.5V DC Số thứ tự chân
1
2 3 PD1 4 PD2 5 PD3 6 PD4 7 8 GND 9 10 download by : skknchat@gmail.com
11 12 13 14 15 16 PB2 17 18 PB4 19 PB5 download by : skknchat@gmail.com
20 AVcc 21 AREF 22 23 PC0 24 PC1 25 PC2 26 PC3 27 PC4 download by : skknchat@gmail.com
sử dụng làm kết nối giao diện nối tiếp cho dữ liệu.
28 PC5 Đầu vào analog Đầu vào analog
giá trị kỹ thuật số kênh 5. Chân này
cũng được sử
dụng như dòng xung nhịp giao diện nối tiếp.
4.2.2- Driver A4988
Hình 4.55- Driver A4988
Thông số kỹ thuật:
Điều khiển đơn giản
Điều khiển được động cơ hoạt động với điện áp lên tới 35V dòng lên tới 2A. Có 5 chế độ: full bước, 1/2 bước, 1/4 bước, 1/8 bước, 1/16 bước.
Điểu chỉnh dòng ra bằng triết áp (bé xíu) nằm bên trên Current Limit = VREF × 2.5
Tự động Shutdown khi quá nóng.
76
Lựa chọn chế độ full hay 1/2 hay 1/4.. sẽ được thông qua 3 pin MS1 MS2 MS3.
Hình 4.56- Bảng chế độ hoạt động của A4988
- Bật tắt động cơ thì thông qua pin ENABLE, mức LOW là bật module, mức HIGH
là tắt module
- Điều khiển chiều quay của động cơ thông qua pin DIR
- Điều khiển bước của động cơ thông qua pin STEP, mỗi xung là tương ứng với 1
bước (hoặc vi bước)
- Hai chân Sleep với Reset nối với nhau.
77
4.2.3- Cảm biến hồng ngoại
Hình 4.57- Cảm biến hống ngoại
Hình 4.58- Sơ đồ cấu tạo cảm biến hống ngoại
78
Tính năng:
Module Thu Phát Hồng Ngoại V1 là một loại cảm biến thông dụng được dùng rất nhiều trong các hệ thống cửa tự động thông minh, cảm biến an toàn của cổng tự động cũng như barrie tự động, cổng co giãn inox tự động đó là cảm biến phát hiện vật cản hồng ngoại hay cảm biến IR (IR detector).
Ứng dụng:
Cửa tự động thông minh, bộ chống trộm, phát hiện vật cản, đếm sản phẩm, đếm số lượng người,...
Thông số kỹ thuật:
Module phát hiện vật cản trong khoảng cách từ 2 - 30cm Góc phát hiện: 35°
Khi phát hiện vật cản, tín hiệu đầu ra OUT ở mức thấp và đèn led màu xanh sáng.
Có thể điều chỉnh khoảng cách bằng biến trở. Chỉnh chiết áp để tăng khoảng cách theo chiều kim đồng hồ, và ngược lại để giảm khoảng cách.
Cổng ra OUT có thể điều khiển trực tiếp 1 Rơ le 5V hoặc cổng IO của MCU. Điện áp cung cấp: 3 - 5V DC.
Dòng điện tiêu thụ: 23 mA (3,3V), 43 mA (5V)
79
Nguyên lý hoạt động: Module Thu Phát Hồng Ngoại V1 được tích hợp bộ phát hồng ngoại và bộ thu hồng ngoại. Bộ phát hồng ngoại là một diode phát sáng (LED) phát ra các tia hồng ngoại. Do đó, chúng được gọi là IR LED. Mặc dù LED IR trông giống như một đèn LED bình thường, bức xạ phát ra từ IR LED là sóng hồng ngoại nên con người không thể nhìn thấy bằng mắt thường được. Bộ thu hồng ngoại cũng được gọi là cảm biến hồng ngoại khi chúng phát hiện các tia từ bộ phát hồng ngoại. Bộ thu hồng ngoại có dạng photodiode và phototransistors. Photodiode hồng ngoại khác với điốt thông thường vì chúng chỉ phát hiện ra bức xạ hồng ngoại. Khi led phát hồng ngoại phát ra bức xạ, nó đến được vật thể và một số bức xạ phản xạ lại led thu hồng ngoại. Dựa trên cường độ thu của led thu hồng ngoại, đầu ra của cảm biến sẽ được xác định là mức cao hoặc thấp.
4.3 - Lập trình điều khiển
80
Hình 4.59- Sơ đồ mạch nguyên lí của hệ thống điều khiển
81
Hình 4.60- Sơ đồ thuật toán điều khiển
4.3.1- Phần mềm lập trình điều khiển
Phần mềm Arduino IDE
Arduino IDE là một phần mềm mã nguồn mở chủ yếu được sử dụng để viết và biên dịch mã vào module Arduino.
Đây là một phần mềm Arduino chính thức, giúp cho việc biên dịch mã trở nên dễ dàng. Nó có các phiên bản cho các hệ điều hành như MAC, Windows, Linux và chạy trên nền tảng Java đi kèm với các chức năng và lệnh có sẵn đóng vai trò quan trọng để gỡ lỗi, chỉnh sửa và biên dịch mã trong môi trường.
Có rất nhiều các module Arduino như Arduino Uno, Arduino Mega, Arduino Leonardo, Arduino Micro và nhiều module khác. Trong phạm vi đồ án này, nhóm sử dụng module Arduino Nano. Mỗi module chứa một bộ vi điều khiển trên bo
82
mạch được lập trình và chấp nhận thông tin dưới dạng mã. Mã chính, còn được gọi là sketch, được tạo trên nền tảng IDE sẽ tạo ra một file Hex, sau đó được chuyển và tải lên trong bộ điều khiển trên bo. Môi trường IDE chủ yếu chứa hai phần cơ bản: Trình chỉnh sửa và Trình biên dịch, phần đầu sử dụng để viết mã được yêu cầu và phần sau được sử dụng để biên dịch và tải mã lên module Arduino.
Môi trường này hỗ trợ cả ngôn ngữ C và C ++.
Khi người dùng viết mã và biên dịch, IDE sẽ tạo file Hex cho mã. File Hex là các file thập phân Hexa được Arduino hiểu và sau đó được gửi đến bo mạch bằng cáp USB. Mỗi bo Arduino đều được tích hợp một bộ vi điều khiển, bộ vi điều khiển sẽ nhận file hex và chạy theo mã được viết.
Arduino IDE bao gồm các phần khác nhau
1. Window bar 2. Menu bar 3. Phím tắt 4. Text Editor 5. Output Panel 83 download by : skknchat@gmail.com
Hình 4.61- Giao diện lập trình trên phần mềm Arduino IDE
4.3.2- Code điều khiển
(Phụ lục 1)
84
LỜI KẾT
Trên đây là bản báo cáo đồ án tốt nghiệp Thiết kế, chế tạo robot song song
Delta của chúng em. Đây là một đề tài có tính thực tế cao, trong thời đại công nghiệp hóa hiện đại hóa đất nước, ngày càng phát triển với sự cạnh tranh không ngừng từ các nước trên thế giới. Đòi hỏi năng suất và chất lượng phải được cải thiện