Mô hình robot gồm bộ điều khiển trung tâm là vi điều khiển. Mỗi động cơ có gắn encoder hồi tiếp góc quay và vận tốc về vi điều khiển. Động cơ được điều khiển qua driver công suất bằng xung PWM cấp từ vi điều khiển. Cảm biến từ đo giá trị độ lệch của xe và hồi tiếp về vi điều khiển. Tín hiệu trả về của cảm biến từ là tín hiệu analog. Máy tính truyền lệnh xuống vi điều khiển qua cổng COM của mô đun bluetooth. Vi điều khiển nhận chỉ dẫn đường đi qua kết nối với mô đun bluetooth.
Hình 4.1: Sơ đồ khối hệ thống điều khiển. 4.2.Lựa chọn thông số các thiết bị
4.2.1.Vi điều khiển
Arduino là loại vi điều khiển phổ biến trên thị trường do có những ưu điểm như: dễ sử dụng, có nhiều nguồn tài liệu hỗ trợ. Phần mềm biên dịch Arduino IDE sử dụng ngôn ngữ C++. Do đó sử dụng Arduino sẽ thuận lợi khi lập trình.
Board mạch được sử dụng ở đây là Arduino Mega 2560 có các thông số sau: VI ĐIỀU KHIỂN
Nguồn DC 5V Cảm biến từ
Encoder động cơ phải Driver công suất
Nguồn 12V Động cơ trái Máy tính Động cơ phải Encoder động cơ trái
- Chip vi xử lý: ATmega2560. - Xung clock: 16 (Mhz). - Điện áp đầu vào: 6 ÷ 12 (V). - Điện áp làm việc: 5 (V).
- Số chân digital: 54 (chân), 15 chân có thể xuất xung PWM. - Số chân analog: 16 (chân).
- Các bộ giao tiếp: 4 bộ UART, 1 bộ SPI, 1 bộ I2C. - Có 6 chân ngắt ngoài là: 2, 3, 18, 19, 20, 21.
Hình 4.2: Vi điều khiển Arduino Mega 2560. 4.2.2.Mạch công suất động cơ
Các yêu cầu của mạch công suất:
- Dòng ngõ ra đủ đáp ứng cho động cơ. - Điều khiển được bằng xung PWM. - Hoạt động ổn định trong thời gian dài.
- Có thể lắp đặt trên robot thường xuyên di chuyển. Thông số động cơ 57BL03A DC servo như sau:
- Điện áp hoạt động 12 (V). - Dòng không tải: 1,5 (A). - Dòng định mức: 8.5 (A). - Công suất 135 (W).
- Encoder có hai kênh A, B.
Từ những thông số động cơ, có thể chọn mạch Hbr-M công suất 200 (W) có thông số như sau:
- Điện áp hoạt động: 10÷28 (VDC). - Dòng điện liên tục lên đến: 10 (A). - Điện áp tín hiệu điều khiển: 3,3÷5 (V). - Tần số PWM tối đa: 100 (KHz).
- Có bảo vệ quá nhiệt.
Hình 4.3: Mạch công suất động cơ DC Hbr-M công suất 200(W).
Các chân của mạch công suất là:
- P , GND: nguồn cấp cho phần công suất. - VCC, GND: nguồn điều khiển.
- PWM: cấp xung PWM cho mạch cầu H. - DIR: đảo chiều động cơ.
4.3.Sử dụng cảm biến từ
4.3.1.Các thông số của cảm biến:
Cảm biến từ trường dẫn đường được chọn là module MACOME GS-2744B. Điện áp hoạt động từ 10,8 ÷ 30 (VDC) và đường dẫn từ bề rộng 25 (mm). Cảm biến từ có các thông số như sau:
- Điện áp hoạt động: từ 10,8 ÷ 30 (VDC). - Dòng tiêu thụ: 30 (mA).
- Điện áp đầu ra: analog 2 ÷ 8 (V), trả về 5 (V) khi không lệch tâm line. - Khoảng nhận biết: 150 (mm).
- Chiều cao lắp đặt: từ 5 ÷ 40 (mm).
Hình 4.4: Cảm biến từ trường MACOME GS-2744B.
Sử dụng vi điều khiển đọc cảm biến từ
Sơ đồ nguyên lý hoạt động của cảm biến từ GS-2744B như sau:
Hình 4.5: Sơ đồ nguyên lý hoạt động cảm biến từ.
Bộ lọc ngưỡng Các phần tử cảm biến Đường dẫn từ Bộ phân tích Mạch ngõ ra lệch trái lệch phải Tín hiệu analog
Tín hiều ngõ ra của cảm biến từ 2 ÷ 8 (V), tuy nhiên vi điều khiển chỉ đọc được khoản giá trị từ 0 ÷ 5 (V). Do vậy cần sử dụng mạch chuyển đổi giá trị điện cho áp phù hợp với vi điều khiển. Theo datasheet của cảm biến từ GS-2744B, đầu ra analog có thể chịu tải với điện trở tải nhỏ nhất là 2 (kΩ). Như vậy, có thể sử dụng mạch phân áp để đọc được giá trị bằng vi điều khiển. Các thông số của điện trở là: = 1,2 (kΩ) và = 2 (kΩ).
Hình 4.6: Nguyên lý mạch phân áp.
Tín hiệu qua cầu chia áp cần xác định giá trị điện áp tương ứng với độ lệch thực của cảm biến. Có thể sử dụng công thức tính điện áp. Tuy nhiên, khi mắc điện trở tại đầu ra, tín hiệu có thể suy giảm và giá trị đọc được không chính xác. Do đó, tiến hành thí nghiệm để có kết quả chính xác hơn.
1.1.1.Mô hình thí nghiệm cảm biến từ
Sử dụng mô hình cho đường dẫn từ tịnh tiến từ cạnh trái đến cạnh phải của cảm biến. Vi điều khiển đọc giá trị cảm biến sau mỗi lần dịch chuyển 0,1 (mm). Trước khi đọc giá trị cảm biến, cho dừng dịch chuyển một giây để không còn rung lắc. Các giá trị điện áp của vị trí cần xác định là: lệch phải lớn nhất, lệch trái lớn nhất, không lệch.
3. Cảm biến từ trường. 4. Đường dẫn từ. 5. Đai răng truyền động. 6. Công tắc hành trình.
Hình 4.7: Mô hình thí nghiệm cảm biến.
Các bước tiến hành thí nghiệm là:
- Gá đường dẫn từ lên bề mặt cơ cấu tịnh tiến.
- Cho đường dẫn chạy đến chạm công tắc hành trình. Chọn vị trí này là vị trí ban đầu.
- Gá cảm biến từ song song với mặt phẳng tịnh tiến. Chiều cao so với mặt tịnh tiến là 20 (mm), trong khoảng hoạt động của cảm biến từ là 5÷40 (mm).
- Di chuyển cảm biến đến sát cạnh bên đường dẫn nhưng vẫn nằm ngoài đường dẫn. Dùng ê-ke canh cho cạnh cảm biến chạm cạnh đường dẫn và gá chặt cảm biến.
- Dùng vi điều khiển để đọc giá trị đồng thời điều khiển động cơ bước dịch chuyển. Khoảng cách di chuyển là 175 (mm) rồi dừng lại.
- Xử lý tín hiệu thu được: Đọc toàn bộ giá trị thu được. Các giá trị đọc được từ 0÷12,5(mm) và từ 162,5÷175 (mm) bị loại, thu kết quả thí nghiệm trong khoảng 12,5÷162,5(mm). 4 5 2 3 6 1 1 . Động cơ bước. 2 . Cơ cấu tịnh tiến.
Giải thích cách lấy giá trị cảm biến: Tâm đường dẫn là tâm ảo, nên chỉ có thể suy ra được từ một cạnh đường dẫn. Theo cách gá cảm biến, ban đầu tâm đường dẫn tại vị trí −12,5 (mm). Có thể nhìn thấy rõ theo sơ đồ sau:
Hình 4.8: Sơ đồ mô hình thí nghiệm cảm biến. 4.3.2.Kết quả thí nghiệm cảm biến
Kết quả thu được từ mô hình thực nghiệm là giá trị điện áp của cảm biến ứng với mỗi vị trí từ phải qua trái.
Hình 4.9: Đồ thị giá trị đọc được từ cảm biến.
Vị trí tâm line đo được nằm giữa cảm biến theo lý thuyết là 75 (mm). Theo đồ thị, khoảng nhận biết tâm line nằm giữa cảm biến là từ 70,6 ÷ 76,2 (mm). Như vậy, khi tín hiệu cảm biến báo về vị trí là 75 (mm), thì khoảng sai lệch là ±5,6 (mm).
150 mm 175 mm Cảm biến từ Vị trí tâm line kết thúc Vị trí tâm line ban đầu Hướng di chuyển 25 mm 80 100 120 140 160 180 200 220 240 260 0102030405060708090100110120130140150 Vị trí dịch chuyển cảm biến (mm)
Các giá trị thu được là: = 1,92 (V), = 4,72 (V). Giá trị điện áp khi không lệch line từ là 3,32 (V). Từ những giá trị trên, có thể thiết lập quan hệ giá trị giữa điện áp và độ lệch line từ của cảm biến. Các thông số trên được sử dụng trong lập trình và hiệu chỉnh robot khi vận hành.
4.4.Điều khiển động cơ DC encoder
Sử dụng bộ điều khiển PID cho từng động cơ để điều khiển vận tốc. Bộ điều khiển PID sử dụng ngõ vào là số xung từ encoder, cho ra giá trị độ rộng xung cấp cho mạch công suất của động cơ.
Hình 4.10: Sơ đồ bộ điều khiển PID cho động cơ.
Áp dụng sơ đồ và các công thức toán để xây dựng giải thuật điều khiển cho động cơ.
Hình 4.11: Chương trình PID điều khiển tốc độ động cơ.
Bắt đầu
Speed = Pulse – pre_Pulse pre_Pulse = Pulse Err = ctrl_Speed – Speed
P = kP*Err I = kI*Err*sam_Time D = kD*(Err – pre_Err)/sam_Time Output = P + I + D Output >= 0 abs(Output) > Period Quay thuận Output = Period xuất xung PWM pre_Err = Err Kết thúc Set kP, kI, kD Quay nghịch Đ S S Đ
Hình 4.12: Chương trình đọc giá trị encoder.
Sử dụng phương pháp Ziegler-Nichols để tìm các thông số cho bộ điều khiển PID. Ban đầu và được gán bằng không. Độ lợi được tăng từ 0 đến khi tín hiệu trả về bắt đầu dao động. Khi đó ta đạt được độ lợi tới hạn , tín hiệu dao động với chu kì . Từ đó suy ra được độ lợi cho các bộ điều khiển nhờ bảng sau:
Bảng 4.1: Công thức tính thông số các bộ điều khiển.
Dạng điều khiển P 0,5 ∞ 0 PI 0,45 1,2 / 0 Encoder Kênh A: A ngắt Kênh B: B input S Bắt đầu Kết thúc Ngắt A Pin B = High Đ Đ Pulse ++ Pulse -- S
PID 0,6 2 / /8
Tiến hành lập trình tăng độ lợi của từng động cơ quan sát qua Serial Plotter của Arduino IDE. Khi vận tốc bắt đầu dao động đều thì ghi nhận lại giá trị . Cho động cơ chạy với độ lợi vừa tìm được và lưu lại kết quả vận tốc. Từ kết quả, tính toán và đưa ra thông số của bộ điều khiển PID cho động cơ.
4.5.Thiết kế bộ điều khiển bám đường
Nhiệm vụ của bộ điều khiển là đưa sai lệch đường đi về không. Khi di chuyển, robot cần tính toán vận tốc phù hợp cho cả hai động cơ. Để đơn giản hóa bộ điều khiển, sử dụng chênh lệch vận tốc của hai động cơ là đầu ra của bộ điều khiển. Khi đó bộ điều khiển chỉ có một ngõ vào và một đầu ra. Như vậy, có thể áp dụng luật điều khiển PID cho bộ điều khiển robot bám đường.
Hình 4.13: Lưu đồ giải thuật chương trình bám line.
Chương này đã hoàn thành lựa chọn thông số cho các thiết bị điện. Cũng như sử dụng và thiết kế giải thuật điều khiển các thiết bị.
Bắt đầu
Đọc giá trị cảm biến
Tính giá trị chênh lệch vận tốc bằng công thức PID
PID điều khiển vận tốc từng động cơ
CHƯƠNG 5:XÂY DỰNG GIẢI THUẬT DI CHUYỂN
Giải thuật điều khiển cho một hệ thống robot tự hành rất quan trọng. Chương này phân tích và thiết kế giải thuật điều khiển cho robot vận hành trên hệ thống
5.1.Giải thuật trên máy tính
Sơ đồ nhà xưởng thử nghiệm gồm: khu vực xuất phát của robot, các vị trí đặt hàng và nơi giao hàng. Quá trình vận chuyển hàng của robot là: nhận tín hiệu yêu cầu món hàng (thông tin vị trí kệ hàng), di chuyển đến đúng kệ hàng, nhận hàng chuyển đến điểm giao hàng.
Hình 5.1: Sơ đồ bố trí hàng trong nhà xưởng.
Với một nhánh rẽ robot có thể đến được hai dãy hàng. Cách bố trí rẽ nhánh như vậy sẽ giảm được chiều đường dẫn, tiết kiệm chi phí. Các ký hiệu chữ là các cột, các số là thứ tự hàng. Ô vuông là vị trí kệ hàng, các đường nối và rẽ nhánh là đường dẫn từ trường.
Giao diện điều khiển được lập trình trên máy tính, khi có người chọn một món hàng thì đường đi đến vị trí hàng sẽ được gửi đến robot. Robot di chuyển theo đường đi đó để lấy hàng và đem đến điểm giao hàng.
Chương trình tính toán đường đi dựa vào thứ tự các cột và hàng. Cột A tương ứng với thứ tự cột là 1, tương tự cho các chữ tiếp sau. Từ đường đi chính (đường thẳng từ Bắt đầu đến Kết thúc) các nhánh rẽ có thứ tự từ trái qua phải, bắt đầu là số 1. Giải thuật yêu cầu phải chỉ rõ hướng đi khi gặp một giao điểm.
Gọi các biến thứ tự cột là x, hàng là y, nhánh rẽ là n thì mỗi kệ hàng có vị trí là (x,y). Ban đầu robot di chuyển từ vị trí xuất phát đến kệ hàng để lấy hàng. Sau đó robot di chuyển hàng đến vị trí kết thúc. Cuối cùng robot quay về lại vị trí bắt đầu chờ lệnh tiếp theo. Giữa ba giai đoạn di chuyển trên là quay xe 180°.
Quy định hướng đi bằng các kí hiệu như sau: s là đi thẳng, b là quay 180°, l là rẽ trái, r là rẽ phải, e là kết thúc.
Ban đầu, robot sẽ di chuyển từ vị trí xuất phát đến vị trí hàng. Giải thuật giúp robot có thể thực hiện đúng các bước di chuyển đó.
Hình 5.2: Lưu đồ giải thuật tạo đường đi đến vị trí một món hàng.
Khi nhận được hàng, robot sẽ di chuyển đến điểm giao hàng và quay về vị trí đợi lệnh tiếp theo. Trình tự di chuyển được điều khiển theo giải thuật sau:
n = y/2 Xuất phát
Đi thẳng n-1 giao điểm, rẽ phải, đi thẳng x-1 giao điểm Kệ hàng Vị trí (x,y) y chẵn ? Đ S n = y Rẽ trái y chẵn ? Đ S Rẽ phải
Hình 5.3: Lưu đồ giải thuật đi từ kệ hàng về lại vị trí xuất phát. 5.2.Giải thuật trên robot
Robot cần thực hiện đúng các lệnh mà máy tính gửi xuống. Các lệnh đó gồm: s là đi thẳng, b là quay 180°, l là rẽ trái, r là rẽ phải, e là kết thúc. Vi điều khiển lưu lệnh với biến chuỗi, thực hiện đến hết độ dài của chuỗi lệnh. Cần thiết lập sẵn các thông số cho chế độ rẽ phải, trái, quay 180° bằng tính toán và thực nghiệm.
Vi điều khiển nhận lệnh di chuyển qua UART. Trong Arduino, có thể sử dụng hàm serialEvent() trên arduino để đọc dữ liệu ngay khi nhận. Hàm này không gây ra độ trễ cho hệ thống.
Rẽ phải Kệ hàng
Đi thẳng x-1 giao điểm, rẽ phải, đi thẳng 2-n giao điểm End Vị trí (x,y) y chẵn ? Đ S Rẽ trái
Quay đầu lại, đi thẳng n giao điểm
Hình 5.4: Lưu đồ giải thuật hàm sự kiện nhận dữ liệu.
Robot cần phát hiện được giao điểm và thực hiện ngay lựa chọn đường đi. Do đó, sử dụng một chương trình ngắt cho cảm biến khi gặp giao điểm.
Hình 5.5: Lưu đồ giải thuật chương trình ngắt gặp giao điểm.
Kết thúc lệnh “start” Đ S
Sự kiện nhận dữ liệu UART
ready_flag = 1, i = 0, path = chuỗi đường đi
Kết thúc Gặp giao điểm
Khi đã phát hiện giao điểm, robot cần xử lý để đi đúng đường. Các bước xử lý gồm chuyển lệnh thành lựa chọn đường đi và thực hiện lựa chọn đó. Giải thuật xử lý giao điểm giúp robot thực hiện các công việc trên.
Hình 5.6: Lưu đồ giải thuật chương trình xử lý giao điểm.
Kết thúc S cài vL > vR path[i] = r S path[i] = l S path[i] = b Đ S cài vL < vR path[i] = s cài vL < vR Đ Đ Đ Bắt đầu Góc quay đạt turn = 0, i++ turn = 1 Đ S path[i] = e S Đ vL = vR = 0
Giải thuật chương trình chính trên robot sẽ tổng hợp các giải thuật con. Sử dụng các giải thuật con đúng thời điểm giúp robot thực hiện được nhiệm vụ. Chương trình chính kết hợp các nhiệm vụ nhỏ như: nhận và lưu vị trí điểm đến, ra lệnh di chuyển, nhận kết quả từ cảm biến để lựa chọn đường đi.
Hình 5.7: Lưu đồ giải thuật chương trình chính.
Chương này đã thiết kế được giải thuật điều khiển gồm: giải thuật chương trình điều khiển trên máy tính và giải thuật thực hiện lệnh của robot.
Khởi động Kết thúc ready_flag = 1 Đ S
Khởi động các mô đun, khởi tạo các biến
Đ
S
Xử lý giao điểm turn = 1
CHƯƠNG 6:THỰC NGHIỆM VÀ ĐÁNH GIÁ KẾT QUẢ
Chương này sẽ áp dụng các giải thuật đã thiết kế vào mô hình robot thử nghiệm. Từ đó có thể rút ra nhận xét về việc áp dụng hệ thống robot vào thực tế.