Như ở chương II, phương trình mơ tả động học của Mobile robot có dạng:
x r y l 1 1 cosθ cosθ 2 2 v (t) v 1 1 v (t) = sinθ sinθ v 2 2 θ(t) 1/L -1/L & (4.1)
Trong đó, các thơng số cơ bản của Mobile robot : + khoảng cách 2 bánh chủ động L= 200mm=7,8” + bán kính bánh chủ động r = 30mm = 1,18”
Xây dựng trên Matlab Simulink ta có mơ hình mobile robot như sau:
Hình 4.18. Mơ hình Mobile robot trên Simulink
Khối gồm 2 đầu vào ωr (bánh phải) và ωl (bánh trái). Các khối Rr, Rl là các giá trị bán kính bánh xe robot. Integrator là các khối tính tích phân. Đầu ra là các giá trị về vị trí của robot trong tọa độ Decade P( X, Y, T), T là giá trị góc định
hướng. Tồn bộ khối robot trên sẽ được tích hợp lại thành một khối trong
Subsystem, đặt tên là robot ( hình 4.19).
4.3.2.Xây dựng sơ đồ khối tồn bộ của Mobile robot
Mơ hình hoạt động của robot xây dựng trên Matlab Simulink có thể được xây dựng dựa trên sơ đồ hình 4.6. Nó có dạng như sau :
Hình 4.20. Sơ đồ khối mô phỏng của Mobile robot sử dụng bộ điều khiển FLC
Sơ đồ trên gồm hai đầu vào Td và Xd là hai giá trị đặt ban đầu cho Mobile robot. Td là góc định hướng, ban đầu đặt Td = 0, tức là robot chạy song song với tường. Xev là khoảng cách từ robot tới tường , ban đầu đặt Xd=4 inch . Các thông số của hai giá trị đầu vào này được xác lập trong cửa sổ hình 4.21.
Hình 4.21. Xác lập các thông số đầu vào ban đầu cho Mobile robot
Bộ điều khiển mờ FLC chính là khối FUZZY CONTROL. Tín hiệu đầu vào và tín hiệu hồi được cộng lại bằng các bộ sum sẽ được đưa vào bộ điều khiển FUZZY CONTROL. Các tín hiệu đầu ra từ đây sẽ đi đến tác động lên khối
subsystem robot. Kết quả thu được sẽ là vị trí P(x,y) của robot, góc định hướng θ.
Hai tín hiệu hồi tiếp X và T được lấy ra, đưa về để tính sai lệch vị trí Δex và sai lệch góc định hướng Δet để đưa vào khối điều khiển FLC. Quá trình điều khiển cứ lặp liên tục như vậy theo một vịng kín.
4.3.4.Kết quả mơ phỏng q trình
Để mơ phỏng tồn bộ hoạt động của khối Mobile robot, ban đầu phải đặt một giá trị Xd và Td cho đầu vào. Các giá trị đặt phải thỏa mãn sao cho tín hiệu đầu vào của khối điều khiển FLC nằm trong dải mà ta xây dựng. Sau đây là một số trường hợp mà tơi đã khảo sát.
• Trường hợp 1 : Xd = 4 inch, Td = 0 (robot phải duy trì được khoảng cách với tường là 4 inch và hướng là song song với tường).
a. Kết quả giá trị góc định hướng θ trong TH1
b. Kết quả giá trị khoảng cách X Hình 4.22. Kết quả mơ phỏng cho TH1
Như biểu đồ ta thấy thời gian để Mobile robot đạt được vị trí đặt ban đầu mất khoảng 1.5s. Trong trường hợp này ta không xét tới những ảnh hưởng của nhiễu, tức là giá trị các nhiễu trắng (white noise) mà ta thay thế cho các nhiễu ảnh hưởng bởi cảm biến.
• Trường hợp 2 : Td = 0, Xd là hàm nấc thang có 2 bước, bước 1 Xd = 2inch duy trì trong 5s, bước 2 Xd = 4 inch duy trì trong vịng 5s. Ta thu được kết quả mơ phỏng:
a. Kết quả mơ phỏng góc định hướng θ khi có nhiễu
b. Kết quả mơ phỏng khoảng cách đặt khi có nhiễu Hình 4.23. Kết quả mơ phỏng TH2
Trong 2 trường hợp trên, ta coi các điều khiện mô phỏng là lý tưởng. Bây giờ, ta sẽ xét những ảnh hưởng của nhiễu do sai số của cảm biến gây ra. Các nhiễu đó trong sơ đồ mơ hình mơ phỏng sẽ là các White Noice .
• Trường hợp 3 : Khi có các nhiễu do sai số cảm biến siêu âm gây ra. Kết quả mô phỏng :
a/ Đồ thị đáp ứng góc của robot
b/ Đồ thị đáp ứng khoảng cách của robotHình 4.23. Kết quả mơ phỏng của TH3 Hình 4.23. Kết quả mơ phỏng của TH3
Rõ ràng khi xét tới nhiễu đường đáp ứng của hệ thống khơng cịn trơn mịn như trước nữa, tuy nhiên hệ thống vẫn bám theo quỹ đạo đặt với sai lệch là chấp nhận được.
Đánh giá chung quá trình khảo sát hệ thống điều khiển robot tự hành :
- Trong cả 3 trường hợp, robot đều có khả năng trở về quỹ đạo đặt.
- Thời gian quá độ là khơng dài có nghĩa hệ thống có khả năng tác động tương đối nhanh (khoảng 1,2 ÷ 1,8 s) ngay cả khi có nhiễu tuy rằng đường đi của robot khơng hồn tồn như giá trị đặt nhưng robot ln có xu hướng bám sát với quỹ đạo đặt. Điều đó cho thấy hệ thống làm việc tương đối ổn định và việc áp dụng bộ điều khiển mờ vào bài toán điều hướng cho robot tự hành là hoàn toàn khả thi.
- Phần giới thiệu về q trình xây dựng mơ hình thực cũng như áp dụng bộ điều khiển mờ cho bài tốn điều hướng tự hành hoạt động trong mơi trường khơng biết trước sẽ được trình bày chi tiết trong chương V.
CHƯƠNG V
XÂY DỰNG MƠ HÌNH ROBOT TỰ HÀNH
Nội dung của chương này bào gồm : - Sơ đồ khối của Mobile robot
- Nghiên cứu xây dựng khối điện tử: + Mạch động lực điều khiển động cơ + Thiết kế mạch Vi điều khiển
+ Cảm biến siêu âm - Xây dựng mơ hình cơ khí
5.1. Xây dựng sơ đồ khối hoạt động của robot
Ở đây, ta sử dụng mơ hình Mobile robot 3 bánh. Như trong phần mơ phỏng, bài tốn đặt ra là làm sao để nó di chuyển theo bề mặt tường và luôn cách tường một khoảng X đặt trước. Robot khơng có bản đồ hay bất cứ thơng tin nào về mơi trường làm việc, do đó robot sẽ được điều hướng theo phương pháp phản ứng. Sơ đồ thuật tốn của Mobile robot như hình 5.1. Thực tế, hai thơng số về sai số góc định hướng, khoảng cách so với tường sẽ được đo bằng hai cảm biến siêu âm SRF05. Khi di chuyển, ban đầu nếu Mobile robot có hướng đi và vị trí là như u cầu (góc θ = 0, X= Xd) thì nó sẽ được điều khiển tiếp tục đi thẳng và duy trì khoảng cách tới tường.
Khi có sai lệch thì nhiệm vụ của bộ điều khiển mờ là đưa ra tín hiệu điều khiển, điều khiển 2 động cơ theo luật nào đó để nó đưa robot về quỹ đạo yêu cầu. Không giống như ở mơ phỏng, các tín hiệu đầu ra của FLC là vận tốc góc của mỗi bánh trái và phải, ở thực tế thì nó lại là các xung điện, điều khiển điện áp đặt lên mỗi động cơ. Tuy nhiên ở đây, do những hạn chế trong thời gian làm đề tài, Mobile robot chỉ có các cảm biến ở 1 bên để bám tường mà lại khơng có thiết bị để phát hiện vật cản ở phía trước.
Hình 5.1. Sơ đồ thuật tốn hoạt động của Mobile robot
Bắt đầu
Bật hai cảm biến siêu âm
Tính ∆ex và ∆et ∆ex=0 ∆et =0 đúng FLC X=Xđ θ = Tđ sai
5.2. Xây dựng khối điều khiển cho Mobile robot
Khối điều khiển Mobile robot sẽ bao gồm khối xử lý tín hiệu và khối cơng suất, khối hiển thị. Sơ đồ như hình 5.2.
Hình 5.2. Sơ đồ khối của robot
5.2.1. Khối xử lý tín hiệu
a. Vi điều khiển Pic 18F4331
Khối Xử lý của Mobile robot sử dụng vi điều khiển Pic 18F4331. Có thể nói đây là trung tâm của một con robot. Nó có nhiệm vụ lưu trữ chương trình, xử lý tín hiệu vào/ ra. Pic18F4331 có sơ đồ chân như hình 5.3 .
Hình 5.3.Vi điều khiển Pic18F4331
Nguồn
Nguồn VI ĐIỀU KHIỂNVI ĐIỀU KHIỂNPIC 18F4331PIC 18F4331 MẠCH NẠP MẠCH NẠP CẢM BIẾN CẢM BIẾN CỔNG I/O A, B, C, D, E CỔNG I/O A, B, C, D, E THIẾT BỊ HIỂN THỊ THIẾT BỊ HIỂN THỊ KHỐI CÔNG SUẤT KHỐI CÔNG SUẤT
Hình 5.4. Sơ đồ cấu trúc các chân của PIC18F4331
PIC18F4331 là vi điều khiển họ 18Fxxxx có tập lệnh có độ dài là 16bit. Mỗi một chu kì lệnh có thời gian là 200ns. Bộ nhớ chương trình 8KB, bộ nhớ dữ liệu 256KB. Gồm 5 cổng A, B, C, D, E, với 36 chân xuất/ nhập dữ liệu . PIC18F4331 hỗ trợ khá tốt điểu khiển động cơ, nó có tới 8 kênh PWM. Nó có cấu trúc bộ dao động ngồi linh động, có khả năng hoạt động ở tần số từ 32 KHz tới 40MHz.
Loại bộ nhớ chương trình Flash
SRAM 768 bytes
EEPROM 256 bytes
Số chân vào/ra 36
Số kênh ADC 10 bit 9
Capture/Compare/PWM 2 ccp Số kênh điều xung PWM 10 8
Timer 8/16 bit 1/3
Điện áp cấp vào 2 ÷ 5 V
Một số tính năng khác của PIC18F4331
- Bộ nhớ Flash có khả năng ghi xóa được 100.000 lần.
- Bộ nhớ EEPROM có khả năng ghi dọc là 100000 lần, lưu trữ trên 40 năm. Ngơn ngữ lập trình thơng dụng cho vi điều khiển có thể là C, Assembly. Khi lập trình cho PIC bằng ngơn ngữ C, phần mềm CCS Complie giúp q trình lập trình trở nên thuận tiện hơn rất nhiều. CCS Complie với giao diện GUI giúp người sử dụng có thể khai báo các hàm ban đầu cho PIC một cách trực quan.
Mạch nạp là mạch giao tiếp giữa vi điều khiển với máy tính. Nó được sử dụng để nạp chương trình vào chip. Có hai loại mạch nạp thơng dụng là loại mạch nạp USB và mạch nạp giao tiếp theo cổng COM (chuẩn RS232 ).
a/ Mạch nạp PIC qua cổng COM b/ Mạch nạp PIC qua cổng USB Hình 5.5. Các loại mạch nạp cho Vi điều khiển
Tương ứng với mạch nạp cổng COM thì sử dụng phần mềm nạp là Winpic 800, nạp qua cổng USB sẽ dùng phần mềm PICKIT2. Các phần mềm này sẽ điều khiển quá trình truyền dữ liệu từ máy tính vào chip.
b.Mạch nguồn cho khối điều khiển
Để ổn định nguồn cấp cho vi điều khiển khi hoạt động, thì vai trị của nguồn là hết sức quan trọng. Mạch nguồn phải đảm bảo cung cấp điện áp và dòng điện ổn định cho chíp và các thiết bị mở rộng kèm theo như LCD, led, cảm biến. Mạch nguồn có sơ đồ như hình 5.6.
Hình 5.6.Sơ đồ nguyên lý mạch ổn áp sử dụng IC LM7805
Vin LM7805 Vout
470 uF
Mạch ổn áp sử dụng IC LM7805 giúp cho đầu vào chíp ln ở ngưỡng ~ 5V.
c. Các thiết bị hiển thị
Các thiết bị hiển thị bao gồm led 7 thanh 7x4, màn hình LCD, nó sẽ có vai trị hiển thị các thơng số của robot theo ý muốn, giúp cho dễ quan sát quá trình điều khiển.
Hình 5.7.Các thiết bị hiển thị c. Sơ đồ nguyên lý mạch vi điều khiển
Hình 5.8.Sơ đồ nguyên lý mạch điều khiển
5.2.2. Khối mạch cơng suất
a. Phân tích lựa chọn phương pháp điều khiển
Bản chất của việc điều khiển Mobile robot thực ra là điều khiển các động cơ của robot đó. Mobile robot được xây dựng bao gồm 2 động cơ dẫn động 2 bánh sau. Hướng đi của robot sẽ do tốc độ của 2 bánh này quyết định, do vậy nó yêu cầu phải thay đổi chiều quay liên tục, đảo chiều quay.
Hình 5.10. Mơ hình Mobile robot
Động cơ sử dụng ở đây là động cơ một chiều, kích từ độc lập. Có 3 cách để điều khiển tốc độ động cơ là :
+ Thay đổi từ thông động cơ
+ Thay đổi điện áp đặt lên phần ứng động cơ + Thay đổi trở phụ vào phần ứng động cơ
Động cơ sử dụng là loại nam châm vĩnh cửu nên từ thơng là khơng đổi. Do đó chỉ có thể điều chỉnh tốc độ động cơ bằng cách là thêm trở phụ vào phần ứng hoặc thay đổi điện áp đặt lên phần ứng động cơ. Họ đặc tính cơ của hai phương pháp điều chỉnh tốc độ trên là như sau:
Hình 5.2a là họ đặc tính cơ động cơ điện một chiều khi thay đổi điện trở phụ mắc vào phần ứng, trong đó: Rf1> Rf2>Rf3>Rf = 0 Ω. Hình 5.2b là họ đặc tính cơ động cơ điện một chiều khi thay đổi điện áp đặt vào phần ứng, trong đó : U1< U2
< U3 < U= Uđm. MOBILE ROBOT Động cơ Bánh chủ động Bánh trước Thân robot
Hình 5.11. Họ đặc tính cơ của động cơ điện một chiều khi. a. Thêm điện trở phụ; b. Thay đổi điện áp đặt vào phần ứng.
Trong hai phương pháp trên, rõ ràng phương pháp điều chỉnh điện áp là có nhiều ưu điểm hơn vì :
- Thứ nhất: khi thay đổi tốc độ động co bằng thay đổi điện áp thì độ cứng đặc tính cơ khơng đổi, cịn khi thay đổi tốc độ bằng thay đổi điện trở phụ thì đặc tính cơ là thay đổi (nhỏ hơn).
- Thứ hai: thay đổi điện áp đặt lên phần ứng động cơ không gây ra tổn hao năng lượng, còn khi sử dụng điện trở phụ sẽ gây ra tổn hao trên trở phụ.
- Thứ ba: thay đổi trở phụ mắc vào phần ứng động cơ làm cho hằng số thời gian động cơ thay đổi dẫn đến các đặc tính quá độ thay đổi, điều này là không tốt, cần tránh.
Từ những điểm trên, ta lựa chọn phương pháp điều chỉnh tốc độ bằng cách thay đổi điện áp đặt lên phần ứng động cơ. Một phương pháp thường sử dụng ở đây để thay đổi điện áp vào động cơ là phương pháp băm xung ( điều chế độ rộng xung PWM ).
b. Thiết kế mạch lực
Điều chỉnh độ rộng xung là sử dụng các xung điện để đóng ngắt nguồn với tải một cách có chu kỳ. Phần tử thực hiện nhiệm vụ đó là các van bán dẫn như Thyristor, Mosfet, IGBT, Tranzitor bipolar. Ở đây, 2 động cơ là cơng suất nhỏ nên ta sử dụng Mosfet vì điều khiển đơn giản hơn và thông dụng.
0 ω ω M Rf = 0 Rf3 Rf2 Rf1 U = Udm U 3 U2 U1 0 M d m M d m a b
Nguyên lý của phương pháp điều chỉnh độ rộng xung PWM
Hình 5.12. Nguyên lý băm xung điện một chiều
Một xung được phát ra sẽ có chu kì là T, trong đó có thời gian xung ở mức cao là tm và thời gian xung ở mức thấp. Khi xung ở mức cao chính là lúc nó mở các van, cấp điện áp cho động cơ. Điện áp cấp trong một chu kì xung được tính :
Ur = Uv. tm / T
Ngun lý băm xung có đảo chiều
Hình 5.14. Sơ đồ băm xung điện áp một chiều có đảo chiều
Theo sơ đồ trên, muốn đảo chiều động cơ chỉ cần đóng/ngắt các van theo một quy tắc như sau:
Khi động cơ làm việc theo chiều thuận thì T3 mở hồn tồn, T2 và T4 khóa, T1 được đóng ngắt theo chu kì xung vào. Chiều dịng điện từ dương nguồn → T1
→ động cơ → T3 → âm nguồn.
U v Ur V U r T tm T1 T2 T 3 T4 D4 D1 D2 D3
Khi động cơ làm việc theo chiều ngược thì T4 ln mở, T2 đóng ngắt theo chu kì xung, T1 và T3 khóa. Chiều dịng điện : dương nguồn → T2 → động cơ → T4 → âm nguồn. Trong cả hai chiều quay, trị số điện áp ra được tính theo cơng thức:
Ur = tm.Uv/ T
Các điode có nhiệm vụ bảo vệ các van khi ngắt dòng động cơ ra khỏi nguồn.
c. Sơ đồ mạch cầu H
Mạch cầu H là mạch sử dụng để băm xung điện áp một chiều và đảo chiều quay động cơ. Trong đó được sử dụng 2 MOSFET kênh P và 2 MOSFET kênh N. Đặc tính mở của Mosfet phụ thuộc vào sự chênh lệch điện áp giữa 2 chân G và S. Nó được mở hồn tồn ở điện áp từ 10 ÷ 12 V.
Hình 5.16. Đặc tuyến làm việc của Mosfet kênh N
Sơ đồ dùng MOSFET nên việc quan trọng là phải mở bão hòa được các van MOSFET. Theo sơ đồ mạch như hình 5.16
- Điều khiển Q1: khi đầu vào được nối lên +5V thì OPTOI 1 mở. Điện trở R1 tạo ra sự chênh lệch điện áp giữa 2 cực S và G của Q1 là 12V làm Q1 mở bão