CHƯƠNG 4 : THIẾT KẾ CƠ KHÍ VÀ MẠCH ĐIỆN
4.2 Thiết kế mạch điện
4.2.2 Mạch Arduino Uno R3
Đối với vấn đề lựa chọn vi điều khiển cho bất cứ đề tài nào, trước hết người ta thường phân vân chọn vi điều khiển của hãng nào. Thực tế việc lựa chọn phụ thuộc vào mức độ am hiểu để sử dụng vi điều khiển của người thiết kế, tiếp theo đó là lựa chọn dịng vi điều khiển có khả năng đáp ứng nhu cầu của hệ thống.
Trong đề tài đồ án hệ thống yêu cầu vi điều khiển thực hiện chức năng nhận và xử lý tín hiệu từ 3 cảm biến và cấp xung điều khiển vị trí cho 3 động cơ bước. Dựa vào yêu cầu sử dụng trên, đã chọn Arduino Uno R3 với các tính năng đáp ứng đầy đủ yêu cầu của hệ thống và giá thành tương đối thấp .
Để giảm thiểu khối lượng thiết kế, đề tài sử dụng Arduino Uno R3 tích hợp mạch nguồn 3.3V cấp cho vi điều khiển, mạch nạp và debug đồng thời cấp nguồn cho vi điều khiển thơng qua 1 cổng USB.
Các tính năng Arduino Uno R3:
• Clock Speed: 16MHz
• Chíp điều khiển chính: Atmega328P
• Số chân Digital I/O: 14.
• Số chân PWN Digital I/O: 6.
• Số chân Analog Input: 6.
• EEPROM: 1KB ( Atmega328P ). .
• SRAM: 2KB ( Atmega328P ).
• Dịng điện DC Curent chân 3.3V: 50mA.
• Nguồn ni mạch: 5VDC từ cổng USB hoặc nguồn ngồi cắm từ giắc trịn DC.
Hình 4.6: Kit Arduino Uno R3 4.2.3 Driver và động cơ bước 4.2.3 Driver và động cơ bước
Động cơ bước thực chất là một động cơ đồng bộ dung để biến đổi các tín hiệu điều khiển dưới dạng các xung điện rời rạc kế tiếp nhau thành các chuyển động góc quay hoặc các chuyển động của roto và có khả năng cố định roto vào những vị trí cần thiết. Động cơ bước làm việc được là nhờ có bộ chuyển mạch điện tử đưa các tín hiệu điều khiển vào stato theo một thứ tự và một tần số nhất định. Tổng số góc quay của roto tương ứng với số lần chuyển mạch, cũng như chiều quay và tốc độ quay của roto, phụ thuộc vào thứ tự chuyển đổi và tần số chuyển đổi. Khi một xung điện áp đặt vào cuộn dây stato (phần ứng) của động cơ bước thì roto (phần cảm) của động cơ sẽ quay đi một góc nhất định, góc ấy là một bước quay của động cơ. Khi các xung điện áp đặt vào các cuộn dây phần ứng thay đổi liên tục thì roto sẽ quay liên tục. (Nhưng thực chất chuyển động đó vẫn là theo các bước rời rạc). Động cơ bước có thể hoạt động ở chế độ một bước, nửa bước hay vi bước. Số vi bước của động cơ càng nhỏ thì động cơ hoạt động càng mượt mà, không bị rung động và cho tốc độ và momen lớn hơn.
Để điều khiển động cơ ở chế độ vi bước ta cần có các driver chuyên dung. Driver được chọn phải cung cấp đủ dòng cho động cơ hoạt động. Một điều lưu ý là các động cơ step điều khiển theo dịng chứ khơng phải theo áp.
Từ những thông số hoạt động của động cơ, ta cần lựa chọn Driver sao cho khả năng chịu dòng hoạt động cao hơn 3A. Do đó, đề tài quyết định lựa chọn Driver điều khiển động cơ bước Leadshine MD542 – 05 với đầy đủ các tính năng phù hợp với mục tiêu đề tài đặt ra.
Thiết lập chế độ Driver Leadshine
DM542 - 05 Nguồn
Mạch cách li
Hình 4.7: Sơ đồ khối Driver Leashine DM542 - 05
Chức năng các khối:
- Khối nguồn: Sử dụng nguồn 12 – 40V DC cấp cho động cơ và biến đổi thành điện áp 5V cung cấp cho các khối còn lại.
- Khối thiết lập chế độ: người dùng sử dụng các switch để thiết lập các chế độ tùy chọn như: chọn dòng điện cấp cho động cơ, điều chỉnh chế độ vi bước.
- Khối cách li: cách li tín hiệu điều khiển với mạch động lực.
- Khối tín hiệu điều khiển: gồm có 6 chân lần lượt là Enable +, Enable – là chân tín hiệu cho phép module được kích hoạt hay khơng kích hoạt, Pulse +, Pulse – là chân tín hiệu xung điều khiển động cơ bước, Dir +, Dir – là chân tín hiệu xung điều khiển chiều quay động cơ.
- Khối động cơ: gồm 4 chân A-, A+, B-, B+ kết nối với 4 dây của động cơ bước. - Khối driver động cơ sử dụng IC DM542-05 điều khiển hoạt động động cơ.
Để điều khiển động cơ hoạt động cần phải cấp xung vào chân PUL+. Mỗi xung tương ứng với động cơ sẽ quay một bước. Chiều quay của động cơ sẽ quyết định bởi tín hiệu cấp vào chân DIR+.
Giả sử độ rộng xung cấp cho driver là τ thì tốc độ của động cơ sẽ xác định theo công thức:
n =
vịng/phút 60m
Trong đó:
− α là bước góc của động cơ (thường là 1,8𝑜).
− m là số xung cần cấp để động cơ quay hết một vịng (với driver Leashine DM542-05, m có thể lên đến 25600 xung).
4.2.4 Cảm biến xác định vị trí Home
Vị trí home là vị trí ban đầu xuất phát trước khi thực hiện các tác vụ. Đây là vị trí định vị tọa độ cho robot nên cần đảm bảo vị trí này có sai số nhỏ khi lặp lại thao tác nhiều lần. Do đặc điểm cơ cấu delta robot ở đây là dạng khớp xoay, khâu 1 liên kết trực tiếp với động cơ. Do đó, để xác định vị trí home cho delta robot ta cần xác định giá trị góc 𝜃𝑖 tại thời điểm đó. Vì vậy đề tài lựa chọn cảm biến MPU - 6050 của hãng InvenSense để xác định vị trí home cho robot.
Cảm biến MPU - 6050 là loại cảm biến gia tốc phổ biến nhất trên thị trường hiện nay được sử dụng để đo 6 thơng số: 3 trục góc quay (Gyroscope) và 3 trục gia tốc hướng (Accelerometer). MPU - 6050 có ba bộ chuyển đổi ADC (analog-to-digital) 16-bit để số hóa các đầu ra con quay hồi chuyển và ba ADC 16-bit để số hóa các đầu ra gia tốc kế. Độ phân giải góc là ± 250, ± 500, ± 1000, và ± 2000 °/s (dps) và độ phân giải gia tốc là ± 2g, ± 4g, ± 8g, và ± 16g.
Để giảm thiểu khối lượng cơng việc thì đề tài sử dụng module GY – 521 MPU - 6050 có tích hợp sẵn cảm biến MPU - 6050 có thể giao tiếp với nhiều loại vi điều khiển thơng qua giao tiếp I2C.
Hình 4.8: Sơ đồ nguyên lý module GY – 521 MPU – 6050
4.2.5 Nguồn điện
Khối nguồn của hệ thống có nhiệm vụ cung cấp năng lượng cho động cơ và driver.
Động cơ step trong hệ thống sử dụng điện áp 24V và dòng điện cấp tối đa 3A. Hệ thống sử dụng ba động cơ, do đó dịng điện yêu cầu là 9A. Ta sử dụng nguồn tổ ong 24V-15A cho khối driver và động cơ step.
Vi điều khiển và cảm biến sử dụng nguồn tổ ông 5V – 5A.
Bảng 4.2: Thông số điện áp nguồn cấp cho hệ thống.
Tên nguồn điện Điện áp ngõ vào Điện áp ngõ ra Dòng điện mức
Nguồn 24VDC 220VAC 24VDC 15A
Nguồn 5VDC 220VAC 5VDC 5A
4.2.6 Tổng kết phần điện
Nội dung phần điện đã trình bày đầy đủ các yêu cầu cần thiết để xây dựng một hệ thống điện phục vụ cho việc điều khiển robot delta. Hệ thống điện bao gồm các module: khối ngoại vi, khối công suất và khối nguồn.
CHƯƠNG 5: MÔ PHỎNG ĐỘNG HỌC VÀ GIẢI THUẬT ĐIỀU KHIỂN GIẢI THUẬT ĐIỀU KHIỂN
Chương 5 sẽ trình bày về phần mô phỏng động học trên phần mềm Matlab Similink và lưu đồ giải thuật điều khiển Robot Delta di chuyển từ điểm đến điểm.
5.1 Mô phỏng động học trên phần mềm Matlab Similink
5.1.1 Xây dựng mơ hình mơ phỏng
Mục đích của việc mơ phỏng hoạt động của mơ hình robot trên phần mềm Matlab là kiểm chứng sai số động học giữa thiết kế cơ khí và mơ hình robot lý tưởng. Mơ hình 3D được xây dựng trên phần mềm Solidwork, sau đó sử dụng cơng cụ SimMechanics Link để xuất các file có đi “.STL” từ khối 3D. Liên kết động học giữa các khối được thể hiện bằng sơ đồ liên kết các khối trong Matlab Simulink.
Để tiến hành mô phỏng ta cần thiết lập các thơng số cho các khối:
• Machine Environment: thơng số gia tốc trọng trường, chế độ phân tích, sai số, những điều kiện ràng buộc.
• Ground: tọa độ gốc (world frame).
• Khối Body (các khâu ): khối lượng, ma trận moment quán tính, hệ trục tọa độ của khâu đó (các thơng số này thu được ở phần thiết kế trên SolidWork sau khi lựu chọn vật liệu và thơng số).
• Khối Joint (khớp nối): sử dụng các joint theo thiết kế của robot delta và hiệu chỉnh phần trục quay quanh phù hợp với hệ trục tọa độ nối từ khâu trước đó.
Hình 5.1 Phần động học thuận trong phần mềm matlab
- Động học thuận là xác định vị trí của khâu tác động cuối của tay máy khi biết các biến khớp của tay máy
- Động học ngược là tìm các biến khớp của tây máy khi biết vị trị khâu tác động cuối của tay máy . Có 3 phương pháp cơ bản để xác định mơ hình động học ngược của tay máy là phép đảo hướng, phép đảo vị trí và phép đảo kết hợp .
5.1.2 Kết quả mô phỏng
Để vẽ ra quỹ đạo đáp ứng của cụm robot delta đã thiết kế, ta cho thông số đầu vào của mô phỏng động học là một đường tròn nằm trong mặt phẳng Oxy.
Quỹ đạo đường tròn này được đưa vào hàm chia điểm trong matlab, thơng qua bài tốn động học ngược đã được trình bày ở chương 3, ta tính ra tập hợp góc mà khâu 1 cần quay để đạt được quỹ đạo đường trịn. Tập hợp góc này sẽ được truyền vào trong các khối mô phỏng Simulink. Để đo được quỹ đạo đáp ứng của mô phỏng, ta sử dụng khối đồ thị Graph XY để vẽ ra quỹ đạo đầu ra và đầu vào của robot.
Nhận xét kết quả:
Ta nhận thấy rằng sai số động học của thiết kế cơ khí so với cơng thức động học ngược đã được tính ra ở chương 3 là khá nhỏ.
5.2 Thiết kế giải thuật điều khiển
Phần này trình bày lưu đồ giải thuật điều khiển Robot Delta di chuyển từ điểm đến điểm.
5.2.1 Yêu cầu thiết kế
Để điều khiển delta robot di chuyển từ điểm đến điểm, ta đưa về giải quyết bài tốn điều khiển vị trí cho 3 động cơ bước. Từ tập hợp điểm đó, thơng qua bài tốn động học ngược ta tính được tập hợp góc mà động cơ cần quay. Từ đó, ta tính được số xung cấp tương ứng với từng động cơ. Sau đó, vi điều khiển bắn xung xuống driver để động cơ đi đến vị trí mong muốn. Cơng việc này thực hiện tuần tự và liên tục cho đến khi kết thúc.
5.2.2 Chương trình điều khiển robot delta
Việc đầu tiên của chương trình điều khiển khi vi điều khiển vừa mới được bật lên đó là khởi tạo các ngoại vi cho vi điều khiển, bao gồm:
− Cấp xung clock cho ngoại vi cần sử dụng.
− Ngõ ra cấp xung, chiều cho động cơ.
− Ngõ vào tín hiệu cảm biến.
− Khởi tạo các Timer, chương trình ngắt.
− Khởi tạo module giao tiếp I2C.
Bus I2C (dây giao tiếp) chỉ gồm hai dây và được đặt tên là Serial Clock Line (SCL) và Serial Data Line (SDA). Dữ liệu được truyền đi được gửi qua dây SDA và được đồng bộ với tín hiệu đồng hồ (clock) từ SCL. Tất cả các thiết bị / IC trên mạng I2C được kết nối với cùng đường SCL và SDA như sau:
Hình 5.3 Sơ đồ giao tiếp I2C
Cả hai đường bus I2C (SDA, SCL) đều hoạt động như các bộ lái cực máng hở (open drain). Nó có nghĩa là bất kỳ thiết bị / IC trên mạng I2C có thể lái SDA và SCL xuống mức thấp, nhưng khơng thể lái chúng lên mức cao. Vì vậy, một điện trở kéo lên (khoảng 1 kΩ đến 4,7 kΩ) được sử dụng cho mỗi đường bus, để giữ cho chúng ở mức cao (ở điện áp dương) theo mặc định.
Các thiết bị kết nối với bus I2C được phân loại hoặc là thiết bị Chủ (Master) hoặc là thiết bị Tớ (Slave). Ở bất cứ thời điểm nào thì chỉ có duy nhất một thiết bị Master ở trang thái hoạt động trên bus I2C. Nó điều khiển đường tín hiệu đồng hồ SCL và quyết định hoạt động nào sẽ được thực hiện trên đường dữ liệu SDA. Dữ liệu được truyền giữa thiết bị Master và các thiết bị Slave thông qua một đường dữ liệu SDA duy nhất, thơng qua các chuỗi có cấu trúc gồm các số 0 và 1 (bit). Mỗi chuỗi số 0 và 1 được gọi là giao dịch (transaction).
Sau khi khởi tạo hệ thống. Việc quan trọng tiếp theo đó là đưa robot delta về vị trí Home. Vị trí này được xác định nhờ vào ba cảm biến đo góc nghiêng MPU6050 –
GY521 đặt trên ba cánh tay trên của robot. Sau khi thực hiện set Home cho robot, ta tiến hành điều khiển vị trí cho 3 động cơ.
Đưa về vị trí Home N Cảm biến 1 Set góc Ɵi = a N Cảm biến 2 Set góc Ɵi= a N Cảm biến 3 Set góc Ɵi = a Y Y Y Động cơ 3 dừng Return Động cơ 2 dừng Động cơ 1 dừng
Động cơ 3 quay lên Động cơ 2 quay lên
Động cơ 1 quay lên
5.2.3 Giải thuật điều khiển đưa robot về vị trí Home
Vị trí Home được định nghĩa là vị trí mà ở đó 3 cánh tay trên của robot cùng lệch một góc bằng nhau so với phương ngang. Khi chương trình điều khiển được bật lên nó cần xác định xem robot đã được đưa về vị trí Home hay chưa bằng cách đọc về giá trị 3 góc trên 3 cảm biến. Nếu giá trị 3 cảm biến chưa bằng nhau tại một góc định trước, chương trình sẽ bật động cơ tương ứng điều khiển 3 cánh tay robot di chuyển lên để cảm biến đạt giá trị mong muốn. Chương trình ngắt có nhiệm vụ tắt động cơ sẽ được bật lên khi cơ cấu đạt vị trí Home.
Điều khiển vị trí
goc[i] = new_goc[i] – old_goc[i]
Giá trị Timer[i] = clock/[(2500.goc[i])/9]
Sau khi đưa robot về vị trí Home, ta cần nhập vào điểm mà robot cần di chuyển. Việc tiếp theo chúng cần làm đó là thực hiện bài tốn nội suy để tính ra giá trị các góc và xuất ra số xung tương ứng để điều khiển động cơ.
5.2.4 Giải thuật điều khiển vị trí
Hình 5.5: Lưu đồ giải thuật điều khiển vị trí
Set giá trị Timer; n[i] = 0; N If n[i] < xung[i] Y End N Ngắt Timer[i] Y n[i] = n[i] + 1
Ta chia điểm di chuyển của robot thành một tập hợp điểm, thơng qua bài tốn động học nghịch ta tính được giá trị các góc mà động cơ cần quay thơng qua tập hợp điểm đó. Trong lưu đồ giải thuật, biến goc[i] = new_goc[i] – old_goc[i] chính là giá
trị góc mà động cơ cần tiếp tục quay để đạt được vị trí mong muốn. Biến
𝑇𝑖𝑚𝑒𝑟[𝑖] chính là khoảng thời gian giữa hai lần timer đếm.
Để robot di chuyển mịn hơn, driver điều khiển động cơ được cài đặt số 25600 xung/vòng. Ta tính được số xung cần cấp cho mỗi động cơ là 𝑥𝑢𝑛𝑔[𝑖] = (25600. 𝑔𝑜𝑐[𝑖])/360 = (1250. 𝑔𝑜𝑐[𝑖])/9 .Từ đó, ta tính được giá trị giữa hai lần đếm là 𝑇𝑖𝑚𝑒𝑟[𝑖]
= 𝑐𝑙𝑜𝑐𝑘/[(1250. 𝑔𝑜𝑐[𝑖])/9]. Trong đó, biến 𝑐𝑙𝑜𝑐𝑘 là giá trị chu kì xung clock của
hệ thống. Sau đó ta cài đặt giá trị cần nạp vào cho timer để khi tràn timer nhảy vào chương trình ngắt.
Tiếp đến, nếu số xung hiện tại nhỏ hơn số xung cần cấp thì chương trình sẽ nhảy vào hàm ngắt để bật động cơ quay, đồng thời giá trị biến đếm n[i] sẽ tăng lên 1 đơn vị sau mỗi lần nhảy vào hàm ngắt. Công việc này được lặp đi lặp lại cho đến khi thực hiện đủ số xung cần cấp, động cơ quay đúng giá trị góc mong muốn thì chương trình dừng lại.
CHƯƠNG 6: THỰC NGHIỆM MÔ PHỎNG VÀ ĐÁNH GIÁ ĐÁNH GIÁ
6.1 Mục đích thực nghiệm
- Kiểm nghiệm tính đúng đắn của phương trình động học và giải thuật điều khiển đã xây dựng.
- Đánh giá sai số dựa trên kết quả thực nghiệm.
- Đánh giá các nguyên nhân gây ra sai số và hướng phát triển đề tài.
6.2 Bố trí thí nghiệm kiểm tra
Solidworks Motion là một công cụ tạo mẫu ảo cung cấp khả năng mô phỏng chuyển