CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
CHƯƠNG 5: THIẾT KẾ BỘ ĐIỀU KHIỂN
Với kết quả đạt được ở chương 4, cấu trúc phần cứng của thiết bị đã cơ bản được hồn thành, kết cấu cơ khí và hệ thống điện đã được thiết kế. Trong chương này, sẽ tập trung thiết kế bộ điều khiển cho robot bao gồm: thiết kế ứng dụng điều khiển bằng ngơn ngữ C++, thiết kế bộ điều khiển PID cho động cơ với chế độ tải thay đổi, giải thuật xác định vị trí line sử dụng cảm biến Photo-Transistor.
5.1. Thiết kế ứng dụng điều khiển bằng ngơn ngữ C++.
Như phần trước đã trình bày, AGV và máy tính sẽ giao tiếp được với nhau khi đủ các điều kiện sau:
- ESP32 và máy tính cùng kết nối chung một mạng WIFI
- Thiết lập ESP32 bằng lệnh AT như bảng 4.4
- Máy tính phải tạo được một Server với địa chỉ IP và Port, cung cấp địa chỉ IP và
Port cho ESP32 để mơ-đun này tự động kết nối vào với vai trị là Client.
Yêu cầu thiết kế ứng dụng:
- Cĩ khả năng thiết lập một Server và cho phép các Client kết nối vào.
- Cĩ khả năng truyền – nhận dữ liệu thơng qua giao thức TCP
Phần mềm Visual Studio được sử dụng để thiết kế giao diện thơng qua ngơn ngữ C++. Ứng dụng được thiết kế cho phép tạo một Server và các thiết bị cĩ thể kết nối vào, giao thức TCP được sử dụng để truyền nhận dữ liệu. Trên ứng dụng hiển thị bản đồ mơ phỏng hoạt động của AGV trong nhà kho, cho phép hiển thị tốc độ hai bánh và sai số dị line. Ngồi ra cịn được thiết kế các nút nhấn và các chế độ cho phép điều khiển robot bằng tay hoặc tự động.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Hình 5. 1: Giao diện ứng dụng điều khiển AGV bằng máy tính Ở mục Connection, yêu cầu nhập 2 thơng số đĩ là: Ở mục Connection, yêu cầu nhập 2 thơng số đĩ là:
- Server: Địa chỉ IP của máy tính trong mạng đang kết nối (kiểm tra IP thơng qua
máy tính).
- Port: Cổng giao tiếp đăng kí mở (lựa chọn tùy ý).
Sau khi nhập 2 thơng số trên và nhấn nút Connect, một Server đã được mở ở cho phép các Client kết nối và giao tiếp.
Cĩ ba chế độ được đề nghị bao gồm: Auto Mode, Manual Mode và Lift Cargo, tương ứng với chế độ tự động, bằng tay và nâng hạ kệ hàng.
Trong chế độ Auto Mode: Sau khi nhập 3 điểm yêu cầu gồm Start Node, End Node và Cargo Node, robot sẽ tự động xác định đường đi ngắn nhất thơng qua giải thuật tìm đường ngắn nhất ASTAR.
Trong chế độ Manual Mode: Robot được điều khiển di chuyển bám line thẳng, quay trái, quay phải 900.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Bắt đầu Nhập địa chỉ IP Nhập địa chỉ PORT Nhấn Connect S Đ Robot đã kết nối S Đ Hiển thị đã kết nối Hiển thị sai số bám line, vận tốc 2 bánh, vị trí hiện tại
Gửi danh sách các điểm tìm được từ giải thuật
ASTAR Đọc tín hiệu từ Robot gửi lên Đ Đ Chọn Auto Nhập Start Node, Cargo Node, End
Node S Chọn Manual Đ Nhấn Đ Move S S S Đ Chọn Stop Nhấn Up Đ S Đ Kết thúc Down Nhấn Gửi: Cylinder_Do wn S Đ
Nhấn Stop Gửi: Stop Gửi: Cylinder_Up Gửi: Move Giải thuật ASTAR tìm được ngắn nhất
Chế độ Lift Cargo: Cho phép người sử dụng điều khiển cơ cấu nâng hạ, nâng lên, hạ xuống, dừng ở vị trí bất kì.
Lưu đồ giải thuật:
Hình 5. 2: Lưu đồ giải thuật chương trình điều khiển trên máy tính Chương trình điều khiển trên máy tính được viết bằng ngơn ngữ C++, cĩ chức Chương trình điều khiển trên máy tính được viết bằng ngơn ngữ C++, cĩ chức năng điều khiển, giám sát hoạt động của robot. Khi bắt đầu chạy chương trình, yêu cầu nhập địa chỉ IP của máy tính đang kết nối mạng, thiết lập cổng PORT, nhấn nút
Connect. Lúc này máy chủ đang ở chế độ chờ, đợi robot kết nối. Khi robot kết nối, thơng báo kết nối được hiển thị, lúc này người sử dụng lựa chọn các chức năng điều
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN ENCODER DC SERVO MOTOR DCS3T-28 PID Controller u(t)
khiển robot như đã trình bày ở trên. Trong quá trình robot di chuyển, các sai số dị line, vận tốc 2 bánh, vị trí hiện tại của robot được gửi lên liên tục.
5.2. Thiết kế bộ điều khiển PID cho chế độ tải trọng thay đổi.
Trong quá trình hoạt động của thiết bị tự hành, với nhiệm vụ nâng và di chuyển kệ hàng với khối lượng dao động từ 0 – 50kg, bài tốn đặt ra đĩ là đáp ứng của 2 động cơ trong trường hợp tải trọng thay đổi. Để giải quyết bài tốn trên, giải thuật PID được sử dụng kết hợp với cảm biến Loadcell để xác định khối lượng của kệ hàng, ứng với mỗi chế độ tải trọng nhất định, sẽ cĩ một bộ số PID thích hợp được lựa chọn từ thực nghiệm, nhằm luơn đảm bảo đáp ứng của động cơ nằm trong khoảng yêu cầu.
Khối lượng tải xác định từ
Loadcell
y(t)
Hình 5. 3: Mơ hình tổng quát hệ thống điều khiển Yêu cầu thiết kế bộ điều khiển PID: Yêu cầu thiết kế bộ điều khiển PID:
- Thời gian đáp ứng 0.1s.
- Độ vọt lố 20%
5.2.1. Bộ điều khiển PID chế độ khơng tải
Lựa chọn hệ số PID
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Tiến hành khảo sát tốc độ động cơ và điện áp cấp vơ ở chế độ khơng tải, sử dụng Driver DCS3T cho kết quả như sau:
Hình 5. 4: Đáp ứng 2 động cơ ở chế độ khơng tải Từ kết quả khảo sát cho thấy: Từ kết quả khảo sát cho thấy:
- Quan hệ giữa tốc độ động cơ và xung PWM là tuyến tính với cả 2 động cơ
- Hệ số gĩc của đường đáp ứng 2 động cơ được xấp xỉ là 3.18
Tiến hành khảo sát đáp ứng của động cơ trái ở chế độ khơng tải, PWM =55%
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Hệ thống cĩ dạng bậc nhất:
Hình 5. 6: Độ thị đường cong đáp ứng của hệ bậc nhất Từ đồ thị, hàm truyền động cơ trái: Từ đồ thị, hàm truyền động cơ trái:
𝐺(𝑠) =
3.18
0,23𝑠 + 1 Sử dụng PID Tuner của Matlab để tìm bộ PID:
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.29, KI = 5.36, KD = 0.00153
Đáp ứng của động cơ trái:
- Settling time: Ts= 0.071s
- Overshoot: M = 12.7%
Tiến hành tương tự cho động cơ phải, hàm truyền tìm được:
𝐺(𝑠) = 0,15𝑠+ 1 3.18
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.14, KI = 6.21, KD = 0.00235
Đáp ứng của động cơ trái:
- Settling time: Ts= 0,098s
- Overshoot: M = 15,7%
5.2.2. Bộ điều khiển PID ở chế độ cĩ tải.
Tiến hành khảo sát đáp ứng của 2 động cơ ở chế độ cĩ tải 10kg:
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN Từ kết quả khảo sát cho thấy:
- Quan hệ giữa tốc độ động cơ và xung PWM là tuyến tính với cả 2 động cơ
- Hệ số gĩc của đường đáp ứng 2 động cơ được xấp xỉ là 2.01
Tiến hành tương tự các bước thiết kế ở chế độ khơng tải, xác định được hàm truyền của 2 động cơ và đáp ứng bộ điều khiển như sau:
❖ Động cơ trái:
𝐺(𝑠) = 0,32𝑠+ 1 2,01
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.23, KI = 9.16, KD = 0.000156
Hình 5. 9: Đáp ứng hệ động cơ trái ở PWM 55% Đáp ứng của động cơ trái:
- Settling time: Ts= 0,052s
- Overshoot: M = 13.8%
❖ Động cơ phải:
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Tiến hành hiệu chỉnh các hệ số qua thực nghiệm, ta tìm được các hệ số: KP = 0.13, KI = 7.68, KD = 0.00125
Hình 5. 10: Đáp ứng hệ động cơ phải PWM 55% Đáp ứng của động cơ phải:
- Settling time: Ts= 0,0018s
- Overshoot: M = 10.6%
Quá trình được tiếp tục ở các chế độ tải từ 1 – 50 kg, các bộ hệ số PID được trình bày ở Phụ lục A.
5.3. Giải thuật xử lý tín hiệu, xác định vị trí đường line sử dụng cảm biến Phototransistor Phototransistor
Với phương pháp dẫn hướng bằng đường băng kẻ sử dụng cảm biến Photo- Transistor, hiện tại cĩ 2 phương pháp được sử dụng phổ biến để xác định vị trí tâm đường line đĩ là: so sánh và xấp xỉ.
Phương pháp thứ nhất dùng bộ so sánh để xác định trạng thái đĩng/ngắt của các sensor, sau đĩ suy ra vị trí xe theo một bảng trạng thái đã định sẵn. Với phương pháp này, sai số dị line sẽ phụ thuộc vào thơng số của sensor được sử dụng, hay khoảng cách giữa các sensor. Phương pháp này cĩ đặc điểm phụ thuộc chủ yếu vào mức ngưỡng so sánh của các sensor, do đĩ tốc độ xử lý rất nhanh.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Phương pháp thứ hai xấp xỉ ra vị trí của xe so với tâm đường line từ các tín hiệu tương tự từ cảm biến. Cĩ 3 giải thuật xấp xỉ được giới thiệu đĩ là xấp xỉ theo bậc 2, tuyến tính và theo trọng số. Đặc điểm của phương pháp này là phụ thuộc chủ yếu vào thời gian đọc ADC tất cả các sensor của vi điều khiển, do đĩ thời gian xử lý sẽ lâu hơn phương pháp 1. Tuy nhiên độ phân giải cao hơn đáng kể so với phương án đầu.
5.3.1. Ca – líp cảm biến
Tín hiệu analog cho mỗi cảm biến là khác nhau, khi làm việc trong cùng điều kiện như nhau sẽ cho ra giá trị khác nhau. Do đĩ ta phải tiến hành calip cảm biến Cơng thức calip:
+ 𝑥𝑚𝑎𝑥,𝑖, 𝑥𝑚𝑖𝑛,𝑖 là giá trị nhỏ nhất và lớn nhất từ cảm biến thứ i
+ 𝑦𝑚𝑎𝑥, 𝑦𝑚𝑖𝑛 là giá trị lớn nhất và nhỏ nhất mà ta mong muốn cho tất cả
cảm biến
+ 𝑥𝑗,𝑖 là giá trị đọc được lần thứ j của cảm biến thứ i
+ 𝑦𝑗0 giá trị sau khi điều chỉnh 𝑥𝑗,𝑖
Từ giá trị thực nghiệm ta thu được cơng thức calip cho 7 cảm biến Bảng 5. 1:Calip cảm biến
5.3.2. Xác định vị trí tâm đường line
Sử dụng giải thuật xấp xỉ bậc 2 Cảm biến 𝑥𝑚𝑖 𝑛 𝑥𝑚 𝑎𝑥 𝑦𝑗0 𝑥0 205 3526 𝑦𝑗0 = 200 + 0.906(𝑥𝑗,0 − 205) 𝑥1 200 3420 𝑦𝑗1 = 200 + 0.879(𝑥𝑗,1 − 200) 𝑥2 253 2950 𝑦𝑗2 = 200 + 0.736(𝑥𝑗2 − 253) 𝑥3 203 3658 𝑦𝑗3 = 200 + 0.943(𝑥𝑗2 − 203) 𝑥4 261 3365 𝑦𝑗4 = 200 + 0.847(𝑥𝑗2 − 261) 𝑥5 249 3863 𝑦𝑗5 = 200 + 0.986(𝑥𝑗2 − 249) 𝑥 265 3447 𝑦 = 200 + 0.869(𝑥 −
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
Hình 5. 11: Giải thuật xấp xỉ bậc 2
Lấy 3 cảm biến cho giá trị cao nhất. Ta xấp xỉ tín hiệu đầu ra:
Vị trí tâm đƣờng line sẽ là vị trí mà hàm xấp xỉ đạt giá trị cực đại:
Kết quả thí nghiệm:
Kết quả mối quan hệ giữa giá trị tính tốn và giá trị thực của vị trí tâm đường line so với tâm cảm biến được thể hiện ở hình bên dưới. Với giá trị sai số e2max là 1,7 mm, sai số e2min là 0,8mm, sai số trung bình tính tốn so với sai số lý tưởng là 1,2 mm. Từ kết quả trả về cũng như sai số từ Hình 5.5, ta thu được đường tuyến tính xấp xỉ như sau: y = 0,51x - 0,31. Với đường xấp xỉ này, thì sai số e2 thỏa mãn yêu cầu đề bài đặt ra.
CHƯƠNG 5: THIẾT KẾ HỆ BỘ ĐIỀU KHIỂN
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG
Với các kết quả đã đạt được từ các chương trước, để robot cĩ thể hoạt động được trong mơi trường nhà kho, cần xây dựng một giải thuật điều khiển hợp lý. Trong chương này sẽ tập trung xây dựng lưu đồ giải thuật, giới thiệu giải thuật tìm đường ngắn nhất A*, mơ hình động học và tiến hành mơ phỏng trên Matlab.
6.1. Xây dựng lưu đồ giải thuật 6.1.1. Giải thuật chương trình chính 6.1.1. Giải thuật chương trình chính
S
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG Đ Process= Stop Process = Auto Process = Manual Đ Trở về chương trình trước
Điều khiển theo chế độ bằng tay Điều khiển theo chế độ tự động Đọc tín hiệu trả về từ ESP32 qua
truyền thơng UART Process = UART_Char_Get();
Làm việc theo tín hiệu điều
khiển
Hình 6.1 thể hiện lưu đồ giải thuật chương trình chính, tại thời điểm bắt đầu, sau khi cơng tắc nguồn robot được bật, người sử dụng nhấn nút Start trên robot, robot tiến hành gửi yêu cầu kết nối truyền thơng qua WIFI với máy chủ, khi quá trình kết nối hồn tất, đèn báo hiệu được bật, lúc này robot hoạt động hồn tồn theo sự điều khiển từ máy chủ.
6.1.2. Lưu đồ giải thuật làm việc theo tín hiệu điều khiển từ máy chủ
Hình 6. 2: Chương trình con: Làm việc theo tín hiệu điều khiển Sau khi hồn tất kết nối giao tiếp với máy chủ thơng qua mạng WIFI, mọi hoạt động của robot sẽ được giám sát qua máy tính, lúc này trên ứng dụng điều khiển người
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG
Đọc tín hiệu trả về từ ESP32 qua truyền thơng UART M_Process = UART_Char_Get();
M_Process= Stop
S
M_Process= Move Tín hiệu
RFID ROBOT dừng. Gửi tín hiệu thẻ RFID M_Process= Cylinder_Up M_Process= Cylinder_ Down M_Process= Cylinder_ Stop Trở về chương trình trước
Dừng cơ cấu nâng Hạ cơ cơ cấu nâng Nâng cơ cấu nâng
ROBOT dừng
Di chuyển bám quỹ đạo Điều khiển theo
chế độ bằng tay
sử dụng lựa chọn các chế độ hoạt động của robot gồm: Tự động và bằng tay. Ứng với mỗi chế độ được lựa chọn, chương trình con tương ứng sẽ được kích hoạt.
6.1.3. Chương trình con: Điều khiển theo chế độ bằng tay
Hình 6. 3: Chương trình con: Điều khiển theo chế độ bằng tay Ở chế độ điều khiển bằng tay, người sử dụng cĩ các lựa chọn như sau:
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG
Robot hoạt động tự động, di chuyển theo đường đi được gửi, nâng
hạ kệ hàng tự động. Trở về chương
trình trước
Lưu các giá trị Start_Node, Cargo_Node, End_Node, tập hợp
các điểm đi qua Đọc tín hiệu trả về từ ESP32
qua truyền thơng UART Điều khiển theo
chế độ tự động
- Nhấn nút Move: Robot di chuyển bám line theo đường thẳng, đến vị trí cĩ thẻ RFID sẽ dừng, gửi tín hiệu vị trí thẻ lên máy chủ, chờ tín hiệu điều khiển tiếp tục. Chức năng này giúp người điều khiển cĩ thể di chuyển Robot đến vị trí mong muốn.
- Trong mục Lifting, nhấn nút Up hoặc Down: Cơ cấu nâng tiến hành nâng lên
hoặc hạ xuống, cho đến khi chạm cơng tắc hành trình hoặc khi nhấn nút Stop. Chức năng này giúp người điều khiển cĩ thể nâng hạ kệ hàng bằng tay.
- Khi nhấn nút Stop: Robot dừng hoạt động.
6.1.4. Chương trình con: Điều khiển ở chế độ tự động
Hình 6. 4: Chương trình con: Điều khiển chế độ tự động
Trong chế độ tự động, người sử dụng nhập 3 giá trị bao gồm: Start Node (điểm bắt đầu), Cargo Node (vị trí kệ hàng được đặt) và End Node (Vị trí trả kệ hàng). Sau khi 3 giá trị được nhập, chương trình trên máy tính sẽ tiến hành xử lý thơng qua giải
CHƯƠNG 6: PHẦN LẬP TRÌNH – MƠ PHỎNG
đĩ tiến hành gửi xuống. Khi nhận được danh sách các điểm, robot di chuyển theo các điểm được gửi xuống, nâng hàng ở điểm Cargo Node, hạ kệ hàng ở vị trí End Node.
6.1.5. Chương trình con: Di chuyển bám đường