Đồ án Nghiên cứu xây dựng phương pháp điều khiển Robot tự hành dựa trên Logic mờ

làm việc của chúng. Trong hình 3.2 đoạn [x2, x3] là miền xác định của tập mờ. c.Hàm liên thuộc Cho một tập hợp A, ánh xạ là μA : A→R được định nghĩa như sau: được gọi là hàm liên thuộc của A. Một tập luôn có μX(x) = 1 với mọi x được gọi là không gian nền (tập µ Miền tin cậy Miền xác định 1 Hình 3.3. Các đặc tính của hàm phụ thuộc d. Độ cao, miền xác định và miền tin cậy của tập mờ - Độ cao của tập mờ F (định nghĩa trên cơ sở M) là giá trị H=sup μF(x), xÎM Một tập mờ với ít nhất một phần tử có độ phụ thuộc bằng 1 được gọi là tập mờ chính tắc, tức là H=1. Ngược lại một tập mờ F với H<1 được gọi là tập mờ không chính tắc. Miền xác định của tập mờ F (định nghĩa trên cơ sở M ), được kí hiệu bởi S là tập con của M thỏa mãn: S= {x Î M | μF(x)>0 }. - Miền tin cậy của tập mờ F(định nghĩa trên cơ sở M ), được kí hiệu T, là tập con của M thỏa mãn: T={x Î M | μF(x)=1 }. e. Biến ngôn ngữ Biến ngôn ngữ là phần chủ đạo trong các hệ thống dùng logic Mờ. Về cơ bản biến ngôn ngữ là loại biến mà giá trị của nó là những từ, những câu nói theo ngôn ngữ tự nhiên hoặc không tự nhiên. Ví dụ trong trường hợp mô tả tốc độ ta có thể có các biến ngôn ngữ đặc trưng cho các tập mờ sau: rất chậm, chậm, trung bình, nhanh, rất nhanh. f. Hàm đặc trưng Mức độ thỏa mãn của một giá trị vật lý vào khái niệm ngôn ngữ được gọi là độ phụ thuộc. Đối với biến liên tục, mức độ này được biểu diễn bởi một hàn gọi là hàm đặc trưng. Hàm đặc trưng là ánh xạ tập hợp các giá trị vật lý thành tập giá trị phụ thuộc với các giá trị ngôn ngữ. Dưới đây là một số dạng hàm đặc trưng: x 0 x 1 0 1 y y Dạng tuyến tính Là dạng đơn giản nhất, thường dùng mô tả các khái niệm chưa biết hay chưa rõ ràng. 0 x 1 0.5 y a b g Dạng dường cong Công thức xác định độ phụ thuộc x: 0 1 0.5 y x b g Dạng hình chuông Độ phụ thuộc x được xác định : y 0 1 x y α β Dạng hình thang Độ phụ thuộc x được xác định: y 0 1 x Dạng tam giác Độ phụ thuộc x được xác định: y 0 1 x Dạng L Độ phụ thuộc x được xác định : y 0 1 x Dạng Γ Độ phụ thuộc x được xác định : g.Mệnh đề hợp thành, luật hợp thành Mệnh đề hợp thành Khái niệm: Mệnh đề hợp thành tương ứng với một luật điều khiển thường có dạng : IF THEN . Các quy tắc hợp thành mờ thường dùng: + Quy tắc Mandani: Nếu hệ thống có nhiều đầu vào và nhiều đầu ra thì mệnh đề tổng quát có dạng như sau: IF N= ni and M = mi and … Then R = ri and K=ki and… + Quy tắc hợp thành MIN: giá trị của mệnh đề hợp thành mờ là một tập mờ B’ định nghĩa trên nền Y (không gian nền của B) và có hàm liên thuộc: mB’ = min{mA, mB(y)}. + Quy tắc hợp thành PROD: Giá trị của mệnh đề hợp thành mờ B’ định nghĩa trên nền Y (không gian nền của B ) và có hàm liên thuộc : mB’ = mAmB(y). Luật hợp thành Luật hợp thành mờ là tên chung gọi mô hình R biểu diễn một hay nhiều hàm liên thuộc cho một hay nhiều mệnh đề hợp thành. Nói cách khác, đó chính là tập hợp của nhiều mệnh đề hợp thành. Các luật hợp thành được xây dựng theo các quy tắc hợp thành. Như thế sẽ có các luật hợp thành khác nhau như: + Luật hợp thành MAX-MIN + Luật hợp thành MAX- PROD + Luật hợp thành SUM – MIN + Luật hợp thành MAX – PROD + Luật hợp thành SUM-PROD h.Giải mờ Giải mờ là quá trình xác định rõ giá trị đầu ra từ hàm phụ thuộc của tập mờ. Có 3 phương pháp chính thường được dùng trong bước này: Phương pháp cực đại Phương pháp điểm trọng tâm Phương pháp độ cao 3.2.2.Bộ điều khiển Mờ Cấu trúc bên trong một bộ điều khiển mờ có dạng như hình 3.7 nó gồm 3 khâu : + Mờ hóa +Thực hiện luật hợp thành + Giải mờ R1: NẾU…THÌ… H2 H1 Rq:NẾU…THÌ… … … … x1 x2 MỜ HÓA GIẢI MỜ HỢP THÀNH Hình 3.7. Mô hình cấu trúc bên trong một bộ điều khiển mờ Một bộ điều khiển mờ hoạt động theo nguyên lý như sau: Luật điều khiển Thiết bị thực hiện hợp thành Giao diện đầu ra Giao diện đầu vào BỘ ĐIỀU KHIỂN MỜ Đối tượng ĐK Thiết bị đo Hình 3.8. Sơ đồ nguyên lý một bộ điều khiển mờ Các nguyên lý thiết kế hệ thống điều khiển mờ : - Giao diện đầu vào gồm các khâu : Mờ hóa, các khâu hiệu chỉnh tỷ lệ, tích phân, vi phân… - Thiết bị hợp thành: sự triển khai luật hợp thành Mờ. - Giao diện đầu ra gồm : khâu giải mờ và các khâu trực tiếp với đối tượng. Các bước xây dựng bộ điều khiển mờ: - Bước 1 : định nghĩa tất cả các biến ngôn ngữ vào/ra. - Bước 2 : xác định các tập mờ cho từng biến ngôn ngữ vào/ra (mờ hóa). - Bước 3: xây dựng luật hợp thành. - Bước 4 : chọn thiết bị hợp thành. - Bước 5 : giải mờ và tối ưu hóa CHƯƠNG IV XÂY DỰNG THUẬT TOÁN VỀ ỨNG DỤNG LOGIC MỜ TRONG KỸ THUẬT DẪN HƯỚNG CHO ROBOT TỰ HÀNH VÀ MÔ PHỎNG TRÊN MATLAB – SIMULATINK Như ở Chương II và Chương III đã đề cập tới vấn đề xây dựng mô hình động học cho robot tự hành kiểu 3 bánh và lý thuyết về điều khiển mờ. Trong Chương IV này sẽ đi vào xây dựng mô hình robot trên phần mềm mô phỏng Matlab- Simulink, và cũng trên đó một bộ điều khiển mờ sẽ được xây dựng và sử dụng để điều khiển mô hình robot đó. Nội dung chương này bao gồm: + Giới thiệu về Matlab Simulink. + Giới thiệu về Fuzzy Logic ToolBox. + Xây dựng bộ điều khiển Fuzzy control trong Fuzzy Logic Toolbox. + Xây dựng và mô phỏng robot được điều khiển bằng bộ điều khiển Fuzzy control. 4.1. Giới thiệu về Matlab Simulink và Fuzzy Logic Toolbox Matlab (Matrix Laboratory) là phần mếm tính toán, mô phỏng được ứng dụng rộng rãi trong nhiều lĩnh vực khoa học và trở thành công cụ tính toán kỹ thuật hàng đầu, bao gồm phân tích dữ liệu, thiết kế mô phỏng…rất phù hợp cho những bài toán về điều khiển tự động và xừ lý tín hiệu. Matlab có nhiều tính năng về đồ họa, có thể vẽ các đặc tuyến bất kỳ trên mặt phẳng hai chiều hoặc không gian 3 chiều. Matlab còn có khả năng lập trình như một ngôn ngữ thực sự, có cấu trúc lệnh giống Pascal hoặc C. 4.1.1.Matlab Simulink Được viết tắt từ Simulation and Link, là một chương trình đi chung với Matlab, là một chương trình mô phỏng rất tốt cho các hệ thống tự động. Đây là một chương trình đồ họa cho phép mô phỏng các hệ thống bằng cách sử dụng các khối trong thư viện Simulink và kết nối chúng lại với nhau một cách linh hoạt theo ý muốn bằng cách nhấp nhả chuột. Simulink có thể mô phỏng hầu hết các hệ thống phi tuyến, tuyến tính… Simulink cho phép chúng ta dễ dàng xây dựng các mô hình từ đầu, hoặc lấy các mô hình đã tạo sẵn và thêm những khối cho chúng. Simulink cho phép chúng ta thay đổi các thông số của các khối trong hệ thống. Ta có thể sử dụng tất cả các công cụ phân tích trên Matlab, do đó sẽ rất dễ dàng để lấy kết quả tính toán được để tiếp tục mô phỏng, phân tích và làm trực quan hóa chúng. Simulink biến máy tính trở thành một phòng thí nghiệm thu nhỏ cho việc mô phỏng, mô hình hóa và phân tích hệ thống mà điều này rất khó làm trong thực tế. Hình 4.1. Giao diện phần mềm Matlab version 7.0.4 Tổng quát, Simulink là một phần mềm sử dụng cho việc mô hình hóa, mô phỏng và phân tích hệ thống động. Nó hỗ trợ cả hai hệ thống phi tuyến và tuyến tính, mô phỏng trong khoảng thời gian liên tục, các khoảng thời gian lấy mẫu. Mô hình có thể được mô phỏng bằng các phương pháp phân tích mô hình từ các Menu của Simulink hoặc từ cửa sổ lệnh của Matlab. Có thể sử dụng đồng hồ đo, dao động kí hoặc các khối hiển thị kết quả để lấy kết quả mô phỏng. Từ đó có thể thay đổi các thông số của hệ thống để được kết quả như ý muốn. Sau đó các kết quả mô phỏng có thể được đưa vào vùng làm việc của Matlab để xử lý tiếp. Ngoài ra để phục vụ cho việc mô phỏng các hệ thống khác nhau trên máy tính, Matlab được tích một bộ công cụ toolbox với nhiều mô hình dành cho việc mô phỏng các hệ thống khác nhau như: xử lý tín hiệu số, hệ thống tự động điều khiển, hệ thống điện, mạng nơ ron, hệ thống suy luận mờ… Hình 4.2. Cửa sổ thư viện công cụ mô phỏng Simulink Chương trình mô phỏng Simulink được gọi ra từ chương trình chính Matlab bằng lệnh >>simulink hoặc nút trên thanh công cụ của Matlab. Cửa sổ xây dựng mô hình mô phỏng như hình 4.3 dưới đây. Hình 4.3. Cửa sổ xây dựng mô hình 4.1.2.Fuzzy Logic Toolbox Fuzzy Logic Toolbox là tập hợp những hàm xây dựng trong môi trường tính toán số của Matlab. Nó cung cấp nhiều công cụ hữu ích cho người sử dụng để tạo và soạn thảo hệ thống suy luận mờ ( FIS ) trong Command Windown của Matlab hoặc chúng có thể xây dựng riêng bằng ngôn ngữ lập trình mà gọi các hệ thống mờ trong Fuzzy Logic Toolbox. Người sử dụng có thể làm việc bằng các dòng lệnh đó, những Toolbox này thiên về những công cụ giao diện đồ họa (GUI) để giúp người sử dụng hoàn tất công việc một cách dễ dàng hơn. Hình 4.4. Fuzzy Logic Toolbox 4.2.Xây dựng bộ điều khiển Fuzzy Logic Controller (FLC) trên Fuzzy Logic Toolbox 4.2.1.Thuật toán điều khiển Mobile robot, định nghĩa các biến vào ra Như mô hình Mobile robot đã xây dựng ở Chương II, robot gồm 3 bánh, 2 bánh chủ động ở phía sau được gắn với 2 động cơ, 1 bánh lái phía trước có khả năng quay tự do. Nhiệm vụ của Mobile robot là di chuyển dọc theo tường và duy trì một khoảng cách xác định với tường. Nó sẽ được gắn hai cảm biến đo khoảng cách và đo góc của trục robot so với hướng di cần chuyển. Trong mô hình điều khiển này thì các yếu tố như nhiễu, ma sát trượt, lực quán tính của xe sẽ được bỏ qua. Ở đây ta chỉ xét tới các yếu tố liên quan tới robot là góc θ, khoảng cách Xđ từ tâm robot tới tường. Bộ phận được tác động để điều khiển robot là 2 động cơ gắn với 2 bánh phải và trái. Giả sử ta cho robot chạy theo một đường có khoảng cách so với tường là Xđ. Khi robot di chuyển, nó bị lệch ra khỏi đường đó, lúc đó sẽ có sự sai lệch cả về vị trí và tư thế của nó so với yêu cầu. Bộ điều khiển sẽ có nhiệm vụ xử lý các tín hiệu báo về từ các cảm biến để xác định các sai lệch, từ đó đưa ra các tín hiệu điều khiển 2 động cơ như thế nào đó để robot về quỹ đạo ban đầu. Bộ điều khiển mờ được thiết kế trong Fuzzy Logic Toolbox sẽ có đầu vào là các sai lệch về vị trí ∆ex (bao gồm sai lệch Âm và sai lệch Dương), sai lệch về góc ∆et (bao gồm sai lệch Âm và sai lệch Dương). y x X sai lệch Âm sai lệch Dương ∆ex ∆et Hình 4.5. Robot trong tọa độ Decade Bộ điều khiển sẽ có hai tín hiệu đầu vào là sai lệch góc ∆et và sai lệch khoảng cách ∆ex, hai đầu ra là hai tín hiệu điều khiển 2 động cơ chủ động, thực tế thì đó là các xung điện điều khiển tốc độ hai động cơ. Sơ đồ khối toàn bộ hệ thống được thể hiện ở hình 4.5. Giá trị của các sai lệch về góc định hướng θ là ∆et sẽ nằm trong khoảng từ -0,3 đến 0,3 radian. Các giá trị sai lệch về khoảng cách ∆ex sẽ nằm trong khoảng từ -4 inch đến 4 inch (khoảng -100mm đến 100mm). Giá trị đầu ra của bộ điều khiển mờ ( FLC ) sẽ là các giá trị vận tốc góc bánh phải (ωr) , bánh trái ( ωl ). Các giá trị này còn phụ thuộc sự ràng buộc về kết cấu cơ khí của robot. Trong trường hợp này ta chọn dải từ -3 đến 3 rad/s Kết quả tính toán ∆ex và ∆et Fuzzy Logic Controller ROBOT sensor θ , x ∆ex ∆ex wl wr Hình 4.6. Sơ đồ khối toàn bộ Mobile robot 4.2.2.Xác định tập mờ Miền giá trị vật lý cơ sở của các biến ngôn ngữ -Tín hiệu sai lệch giữa góc đặt như trên chọn trong khoảng : -0,3≤ ∆et ≤0,3 (rad) - Sai lệch khoảng cách nằm trong khoảng : -4 ≤ ∆ex ≤ 4 (inch) - Tín hiệu đầu ra, vận tốc góc bánh trái và bánh phải : -3 ≤ ωl ≤ 3 rad/s -3 ≤ ωr ≤ 3 rad/s Số lượng tập mờ ( giá trị ngôn ngữ ) Về nguyên tắc, số lượng tập mờ cho mỗi biến ngôn ngữ thường nằm trong khoảng từ 3 đến 10 giá trị. Nếu số lượng tập mờ nhỏ hơn 3 thì ít có ý nghĩa, vì không thể thực hiện được việc chi tiết hóa các phương án xử lý. Nếu lớn hơn 10 thì đòi hỏi người lập trình phải xử lý một khối lượng lớn các suy luận, phải nghiên cứu đầy đủ để đồng thời phân biệt khoảng từ 5 đến 9 phương án khác nhau và phần cứng phải có khả năng lưu trữ lớn, cũng như khả năng xử lý nhanh đặc biệt trong bài toán điều hướng robot hoạt động trong môi trường thực (là bài toán yêu cầu cần có tốc độ xử lý và khả năng lưu trữ rất lớn). Đối với quá trình điều khiển robot có thể chọn giá trị ngôn ngữ như sau : + ∆ex Î {ÂM, KHÔNG, DƯƠNG} + ∆et Î {ÂM, KHÔNG, DƯƠNG} + ωr Î {CHẬM, TRUNG BÌNH, NHANH} + ωl Î {CHẬM, TRUNG BÌNH, NHANH} Các hàm liên thuộc Đây là điểm cực kì quan trọng vì quá trình làm việc của bộ điều khiển mờ phụ thuộc rất nhiều vào dạng và kiểu hàm liên thuộc. Do vậy cần chọn kiểu hàm liên thuộc có phần chồng lên nhau và phủ kín miền giá trị vật lý, để trong quá trình điều khiển không xuất hiện “lỗ hổng” và tránh trường hợp với một giá trị vật lý rõ đầu vào x0 mà tập mờ đầu ra có độ cao bằng không (miền xác định là tập rỗng ) và bộ điều khiển không thể đưa ra một quyết định điều khiển nào được gọi là hiện tượng “cháy nguyên tắc”. Mặt khác, hàm liên thuộc được chọn sao cho miền tin cậy của nó chỉ có một phần tử, hay nói cách khác chỉ tồn tại một điểm vật lý có độ phụ thuộc bằng độ cao của tập mờ. Đối với quá trình điều khiển Mobile robot có 4 tập mờ. Mỗi tập mờ có 3 biến ngôn ngữ, tương ứng với nó là các giá trị ngôn ngữ. + Sai lệch khoảng cách Δex và sai lệch góc định hướng Δet có có các biến giá trị AM ( Âm ), KHONG ( Không ), DUONG ( Dương ). + Giá trị đầu ra là vận tốc góc ωl và ωl có các biến giá trị CHAM ( Chậm ), TRUNGBINH ( trung bình ), NHANH ( nhanh ). Mỗi giá trị ngôn ngữ sẽ được gắn với một hàm phụ thuộc dạng tam giác. 1 0.5 0 0 4 2 -2 Hàm phụ thuộc Sai lệch khoảng cách ( inch ) AM KHONG DUONG Hình 4.7. Hàm phụ thuộc của sai lệch khoảng cách Δex 1 0.5 0 0 0.3 0.15 -0.15 Hàm phụ thuộc Sai lệch góc định hướng Δet (radian) DUONG KHONG AM Hình 4.8. Hàm phụ thuộc của sai lệch góc định hướng Δet 1 0.5 0 0 3 1.5 -1.5 Hàm phụ thuộc Dải giá trị đầu ra vận tốc góc bánh phải ωr (rad/s) TRUNGBINH CHAM NHANH Hình 4.9. Hàm phụ thuộc đầu ra bánh phải ωr 1 0.5 0 0 3 1.5 -1.5 Hàm phụ thuộc Dải giá trị đầu ra vận tốc góc bánh trái ωl (rad/s) CHAM TRUNGBINH NHANH Hình 4.10. Hàm phụ thuộc đầu ra bánh trái ωl Rời rạc hóa các tập mờ - Hàm liên thuộc của sai lệch góc Δet là μ t(t) và được rời rạc tại 5 điểm : t Î {-0.3, -0.15, 0, 0.15, 0.3 } - Hàm liên thuộc của sai lệch khoảng cách ∆ex là μx(x) và được rời rạc hóa tại 5 điểm : X Î { -4, -2, 0, 2, 4 } - Hàm liên thuộc đầu ra ωr, ωl là μω(ω ) được rời rạc hóa tại 5 điểm: ω Î { -3, -1.5, 0, 1.5, 3 } Thứ tự quá trình xây dựng bộ Fuzzy Logic Cotroller được thực hiện trong Fuzzy Logic Toolbox theo thứ tự các hình dưới đây: Biến đầu vào Bộ hợp thành Lựa chọn luật hợp thành Phương pháp giải mờ Tên biến và giá trị Biến đầu ra Hình 4.11. Sơ đồ các biến vào/ra của FLC Hình 4.11.Xây dựng các biến vào ra cho FLC hình 4.12. Đặt giá trị cho các hàm phụ thuộc vào/ ra của FLC Xây dựng các luật hợp thành Từ kinh nghiệm thực tế mà ta xây dựng được 9 luật điều khiển : Bảng 4.1. Luật điều khiển cơ bản cho vận tốc góc bánh phải ωr Δex/Δet AM KHONG DUONG AM CHAM CHAM CHAM KHONG CHAM NHANH TRUNGBINH DUONG CHAM TRUNGBINH NHANH Bảng 4.2. Luật điều khiển cơ bản cho vận tốc góc bánh trái ωl Δex /Δet AM KHONG DUONG AM NHANH TRUNGBINH CHAM KHONG TRUNGBINH NHANH CHAM DUONG CHAM CHAM CHAM - Luật R1: NẾU Δex là AM VÀ Δet AM là THÌ ωr là CHAM VÀ ωl là CHAM - Luật R2: NẾU Δex là AM VÀ Δet là KHONG THÌ ωr là CHAM VÀ ωl là TRUNGBINH - Luật R3: NẾU Δex là AM VÀ Δet là DUONG THÌ ωr là CHAM VÀ ωl là CHAM - Luật R4: NẾU Δex là KHONG VÀ Δet là AM THÌ ωr là CHAM VÀ ωl là TRUNGBINH - Luật R5: NẾU Δex là KHONG VÀ Δet là KHONG THÌ ωr là NHANH VÀ ωl là NHANH - Luật R6: NẾU Δex là KHONG VÀ Δet là DUONG THÌ ωr là TRUNGBINH VÀ ωl là CHAM - Luật R7: NẾU Δex là DUONG VÀ Δet là AM THÌ ωr là CHAM VÀ ωl là CHAM - Luật R8: NẾU Δex là DUONG VÀ Δet là KHONG THÌ ωr là TRUNGBINH VÀ ωl là CHAM - Luật R9: NẾU Δex là DUONG VÀ Δet là DUONG THÌ ωr là NHANH VÀ ωl là CHAM Ví dụ một trường hợp luật R1 như hình 4.13, Mobile robot đi ra ngoài quỹ đạo, sai số khoảng cách một khoảng là Δex = AM (âm) và sai số góc định hướng Δet = AM. ICC r xm x ym y P 0 X Y q nhanh Chậm X Bên phải Bên trái Hình 4.13. Trường hợp robot đi sai khỏi quỹ đạo Lúc này, để về quỹ đạo ban đầu thì đòi hỏi bánh trái phải quay nhanh (NHANH ) và bánh phải quay chậm lại ( CHAM ) ứng với luật R1. Chọn thiết bị hợp thành Đối với trường hợp điều khiển Mobile robot, chúng ta sẽ chọn thiết bị thực hiện luật hợp thành theo phương pháp MAX- MIN. Chọn nguyên lý giải Mờ Giải mờ là quá trình xác định rõ giá trị đầu ra của bộ điều khiển. Việc chọn phương pháp giải mờ cũng gây ảnh hưởng đến độ phức tạp và trạng thái làm việc của hệ thống. Đối vơi bộ điều khiển Mờ FLC sử dụng cho Mobile robot ở đây ta dùng phương pháp điểm trọng tâm (Centroid ), bởi vì phương pháp này có sự tham gia của tất cả các kết luận điều khiển R1, R2, R3, R4, R5, R6, R7, R8, R9. Quá trình xây dựng luật điều khiển và hợp thành được tiến hành trên Fuzzy toolbox như trong hình 4.14 và 4.15. Hình 4.14. Xây dựng các luật điều khiển Hình 4.15. Các luật điều khiển Bộ điều khiển được mô hình hóa dưới dàng không gian 3D : Hình 4.16.Mô tả bộ điều khiển trên không gian 3D cho bánh phải Hình 4.17. Mô tả bộ điều khiển trên không gian 3D cho bánh trái Sau khi xây dựng xong, bộ điều khiển Mờ FLC sẽ được lưu dưới dạng một file FLC.fis. 4.3.Xây dựng và mô phỏng Mobile robot trên Matlab Simulink 4.3.1.Xây dựng mô hình Mobile robot Như ở chương II, phương trình mô tả động học của Mobile robot có dạng: (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. Toà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). Hình 4.19. mô hình robot 4.3.2.Xây dựng sơ đồ khối toà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 quá trình Để mô phỏng toà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). Đáp ứng khoảng cách tới tường X và góc hướng θ có dạng như trong hình 4.22. Kết quả giá trị góc định hướng θ trong TH1 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 robot 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 hoàn toàn như giá trị đặt nhưng robot luôn 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ề quá 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 toá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 toá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 toá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ư yê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. 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đ q = Tđ sai Hình 5.1. Sơ đồ thuật toán hoạt động của Mobile robot 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. Nguồn VI ĐIỀU KHIỂN PIC 18F4331 MẠCH NẠP CẢM BIẾN CỔNG I/O A, B, C, D, E THIẾT BỊ HIỂN THỊ KHỐI CÔNG SUẤT 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 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 ngoài linh động, có khả năng hoạt động ở tần số từ 32 KHz tới 40MHz. Bảng 5.1. Bảng thông số cơ bản của PIC18F4331 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 Peripherals 2 ccp Số kênh điều xung PWM 10 bit 8 Timer 8/16 bit 1/3 Điện áp cấp vào 2 ÷ 5 V Số PIN 40 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 quá 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. Vin LM7805 Vout 470 uF +5V Hình 5.6.Sơ đồ nguyên lý mạch ổn áp sử dụng IC LM7805 Mạch ổn áp sử dụng IC LM7805 giúp cho đầu vào chíp luôn ở 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. a/ Led 7 thanh loại 7x4 b/ Màn hình LCD 2x16 Hình 5.7.Các thiết bị hiển thị 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. MOBILE ROBOT Động cơ Bánh chủ động Bánh trước Thân robot 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 W. 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. M 0 w w M Rf = 0 Rf3 Rf2 Rf1 U = Udm U3 U2 U1 0 Mdm Mdm a b 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. Nguyên lý của phương pháp điều chỉnh độ rộng xung PWM Uv Ur V Ur T tm 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 Nguyên lý băm xung có đảo chiều T1 T2 T3 T4 D4 D1 D2 D3 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ở hoàn toà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. Khi động cơ làm việc theo chiều ngược thì T4 luôn 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ở hoàn toàn ở điện áp từ 10 ÷ 12 V. Hình 5.15. Sơ đô nguyên lý 1 mạch cầu H S D MOSFET kênh N UGS=12V UGS=9V UGS=7.5V UGS=6V ID G VS 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 hòa. Khi đầu vào 1 nối xuống 0v thì OPTOI khóa làm cực G và S đều treo lên là 12 V, do đó Q1 bị khóa. Q2 đóng mở tương tự Q1. Điều khiển van Q3: khi đầu vào làm OPTOI 3 mở . Điện trở R12 làm chênh lệch điện áp giữa G và S là 12V, Q3 mở. Khi đầu vào 3 nối xuống 0V (mức logic ‘0’) thì OPTOI 3 đóng, cả cực G và S đều được nối xuống 0V nên V3 khoá. Cách mở Q4 tương tự Q3. Các linh kiện sử dụng cho mạch cầu H : + MOSFET kênh N là loại IRF630N có cường độ dòng điện định mức là 9.3A, điện áp định mức là 200V. + MOSFET kênh P là loại IRF 9630 có khả năng chịu dòng 6,5A, điện áp định mức là 200V. + Diode, tụ gốm, tụ hóa, opto, điện trở, led đỏ, led xanh. Lựa chọn động cơ Hình 5.17.Động cơ - Động cơ sử dụng là động cơ một chiều có tốc độ 500RPM, sử dụng điện áp 12V. Có hộp giảm tốc để tăng mô men kéo và giảm tốc. 5.2.3.Gia công các khối mạch Quá trình gia công các mạch nguyên lý để tạo ra các bảng mạch điện tử được thực hiện theo quy trình sau: MẠCH NGUYÊN LÝ PHẦN MỀM THIẾT KẾ MẠCH: + ORCAD +PROTEUS +PROTEL +ALTIUM MẠCH IN GIA CÔNG BO MẠCH THỦ CÔNG CÔNG NGHIỆP Hình 5.18. Sơ đồ gia công các khối mạch Có rất nhiều phần mềm thiết kế mạch khác nhau như Orcad, Proteus, Protel, Circust maker…Trong đồ án này sẽ sử dụng phần mềm Altium Designer V6.8 để thiết kế mạch nguyên lý và mạch in PCB. Mạch in sau khi được thiết kế xong sẽ được sử dụng để gia công bo mạch trên phíp đồng. Trong giới hạn đồ án chỉ là những mạch không quá lớn, phức tạp, chỉ cần mạch in một lớp do vậy phương pháp làm mạch thủ công là đơn giản và hiệu quả hơn cả. Quá trình gia công theo sơ đồ sau: MẠCH IN IN LÊN GIẤY LÀ TRÊN PHÍP ĐỒNG ĂN MÒN HÓA CHẤT FeCl3 GẮN LINH KIỆN Altium Designer là phần mềm hỗ trợ thiết kế mạch khá mạnh. Nó cho phép thiết kế mạch nguyên lý, mạch in nhiều lớp. Hỗ trợ mạch in tối đa 12 lớp, mạch in có thể được số hóa để gia công trên các máyCNC. Hình 5.19.Thiết kế mạch nguyên lý trên phần mềm Altium Designer 6.8 Hình 5.20.Thiết kế mạch in PCB trên phần mềm Altium Designer 6.8 Mạch in sau khi thiết kế sẽ được sử dụng để gia công trên phíp đồng. Hình 5.21.Mạch cầu H Hình 5.22.Mạch điều khiển 5.2.3. Cảm biến Mobile robot bám tường sử dụng 2 cảm biến siêu âm SRF 05 (hình 5.20). Nó có các đặc điểm sau: + Điện áp sử dụng: 5V + Dòng thấp: 4mA + Tần số: 40Khz + Phạm vi sử dụng: 1cm-4m + Đầu vào kích bởi xung: min 10us + Kích thước 43x20x17mm a/Phía trước với 2 đầu thu/phát. b/Các chân nối Hình 5.23.Cảm biến siêu âm SRF 05 Hình.5.24.Sơ đồ hoạt động của SRF05 SRF 05 có 2 chân Trigger và Echo là 2 chân điều khiển, Trigger là chân phát xung còn Echo là chân nhận xung về, cho tín hiệu ra vi điều khiển. Ban đầu cấp xung 10us cho chân trigger, chân ra Echo sẽ có tín hiệu ra. Giới hạn của tín hiệu ra từ 100us đến 25ms có nghĩa SRF05 chỉ cho phép đo được khoảng cách từ 1mm đến 4m. Để phát hiện vật thể, trước hết phải cấp xung 10us cho Trigger để cảm biến hoạt động. SRF05 sẽ xuất ra 8 chu kỳ siêu âm tại tần số 40khz và Echo có tín hiệu. Khi xung phát ra thì đồng thời bộ đếm thời gian (timer 16bit) hoạt động, khi có xung phản hồi về chân Echo thì bộ vi điều khiển sẽ tính được thời gian đi của sóng âm. Giới hạn thời gian sóng âm xuất ra tới khi phản hồi về từ 100us – 25ms, sau 30ms mà không có tín hiệu về chân Echo thì không xác định được vật thể. SRF05 S dgg Hình 5.25.Sơ đồ tình khoảng cách của SRF05 Thời gian mà bộ đếm thời gian tính được sẽ sử dụng để tính ra khoảng cách. Sóng âm có vận tốc 340m/s, cứ đi hết 1inch=25,4 mm thì hết 74us. Khi cách vật cản 1 khoảng S thì sóng âm đi hết 1 quãng đường là 2S. Thời gian mà vi điều khiển là T thì quãng đường S= T/(74.2) = T/148 (Inch) =T/58(mm) 5.4.Thiết kế cơ khí Trong quá trình thực hiện đề tài này, phần thiết kế cơ khí được mô hình hóa trên phần mềm thiết kế 3D là Audesk Inventor version 9.0. Đây là phần mềm thiết kế 3D hàng đầu giúp cho quá trình thiết kế chi tiết một cách nhanh chóng, dễ dàng. Hình 5.26.Thiết kế mô hình trên AutoDesk Inventor Khi lắp ráp tất cả các phần lại ta được mô hình thực tế : Hình 5.27.Mô hình thực tế của Mobile robot 5.5.Lập trình cho robot MOBILE ROBOT TƯỜNG X Hình 5.28.Đường đi của robot - Sau khi đã hoàn thành phần cứng về cơ khí và điện tử, chương trình điều khiển robot được sử dụng bộ điều khiển mờ dưới dạng ngôn ngữ C. - Bộ điều khiển mờ viết cho robot có các biến đầu vào xác định từ 2 cảm biến siêu âm, và biến đầu ra là các giá trị xung điều khiển điện áp đặt lên 2 động cơ. - Các thiết bị hiển thị đi kèm hiển thị các thông số về khoảng cách từ robot tới tường, khoảng cách đặt. 5.6. Đánh giá quá trình thực nghiệm trên mô hình thực Sau nhiều lần thử nghiệm, mô hình robot đã đạt được những kết quả : Đo khoảng cách từ robot tới tường. Xác định góc hợp giữa robot và tường. Thời gian để robot đi về đúng quỹ đạolà khoảng từ 3÷5 giây, tùy thuộc sai lệch ban đầu mình đặt là lớn hay nhỏ. Tuy nhiên, cảm biến siêu âm khi robot di chuyển xuất hiện các sai số. Đặc biệt khi robot đi nghiêng 1 góc so với tường thì khoảng cách đo được là không còn chính xác nữa và xảy ra các hiệu ứng nhiễu do xung của 2 siêu âm lẫn vào nhau làm robot mật phương hướng và đi lệch ra khỏi quỹ đạo. KẾT LUẬN VÀ HƯỚNG PHÁT TRIỂN Kết luận Đề tài đã nghiên cứu phương pháp điều khiển robot tự hành dựa trên cơ sở ligic Mờ. Mô hình động học của robot tự hành đã được xây dựng và mô hình hóa trên phần mềm mô phỏng Matlab-Simulink. Lý thuyết về logic Mờ cũng được nghiên cứu để xây dựng lên bộ điều khiển Mờ. Kết hợp lại ta được một hệ thống điều khiển robot tự hành trên phần mềm mô phỏng Matlab-Simulink. Khảo sát hệ thống khi mô phỏng cho thấy: Bộ điều khiển Mờ đã đạt yêu cầu đề ra là đưa robot về quỹ đạo đặt. Khảo sát và đánh giá khả năng khả thi của bộ điều khiển mờ, từ đó xây dựng mô hình thật Mô hình được thực hiện hoàn thiện bao gồm cả phần thiết kế cơ khí và mạch điện tử. Nghiên cứu và ứng dụng vi điều khiển, cảm biến siêu âm. Hoàn thành việc thiết kế các mạch điều khiển và mạch công suất cho robot. - Sử dụng cảm biến siêu âm để xác định góc và khoảng cách cho robot. - Đã xây dựng bộ điều khiển mờ sử dụng Vi điều khiển PIC18F4331. Kết quả : hệ thống có khả năng điều hướng cho robot trở về quỹ đạo đặt trong khoảng 3÷5s. Tuy nhiên, thực tế khi siêu âm được gắn lên robot thì trong quá trình di chuyển xuất hiện các nhiễu làm robot mất phương hướng và lệch ra khỏi quỹ đạo. Hướng phát triển của đề tài - Từ mô hình robot với 2 cảm biến siêu âm , có khả năng di chuyển bám tường, ta có thể mở rộng ra với 6 cảm biến siêu âm. Tức là mỗi bên sẽ có 2 cảm biến, trước và sau có một cảm biến. Với mô hình như vậy cho phép robot có khả năng phát hiện vật cản cả ở phía trước và sau, di chuyển linh hoạt, bám tường trái hoặc phải đều được. - Từ những nghiên cứu về logic mờ trong đề tài sẽ là cơ sở ban đầu để tiếp tục nghiên cứu và ứng dụng nó. Đặc biệt trong công việc chẩn đoán kỹ thuật phương tiện giao thông. Nó cho phép chẩn đoán các hỏng hóc của các phương tiện mà không cần các thông tin chính xác và độ tin cậy cao. Nó có thể tổng hợp kinh nghiệm của các chuyên gia, kỹ thuật viên giỏi. tÀI LIỆU THAM KHẢO 1.Phan Xuân Minh và Nguyễn Doãn Phước, Lý thuyết điều khiển Mờ, nhà xuất bản khoa học kỹ thuật 2. Hồ Thuần và Đặng Thanh Hà , Logic Mờ và ứng dụng, Nhà xuất bản Đại học Quốc gia. 3. Nguyễn Hoàng Hải- Nguyễn Việt Anh, Lập trình Matlab và ứng dụng, Nhà xuất bản khoa học và kỹ thuật. 4. J.Borenstein, Where am I?Sensors and Methods for Moble Robot Positioning, The University of Michigan. 5. Vamsi Mohan Pery, Fuzzy logic controler for an autonomous mobilerobot, Jawaharlal Nehru Techbological University India 6. John Holland, Designing Autonomous Mobile robots 7. Kristof Goris, Autonomous Mobile Robot Mechanical Design, University Brussel. Các website : Http:// www.microchip.com. //Trang web về cơ điện tử Việt Nam PHỤ LỤC A.Sơ đồ nguyên lý mạch động lực Các linh kiện sử dụng và số lượng : -Opto P521 : 8 - Điện trở 47 Ω: 8 - Điện trở 1K : 2 - Điện trở 10K : 12 - Tụ gốm 1000uF: 3 -Tụ hóa 1000uF 25V : 2 -Diode 2A: 8 - Tụ gốm 104 : 2 - Mosfet IRF 9630: 4 - Mosfet IRF 630N: 4 - Điện áp nguồn : 12V - LM7812: 1 B.Sơ đồ nguyên lý mạch điều khiển Các linh kiện sử dụng và số lượng: VĐK PIC18F4331 : 1 Công tắc 6 chân : 2 LM7805 : 1 Diode 2A : 1 Led đỏ : 1 Điện trở 1K: 1 Điện trở 10K: 1 Biến trở 10K :1 Tụ 33uF : 3 Tụ gốm 1000uF: 1 C. Chương trình áp dụng logic mờ cho Mobile robot 1.Chương trình chính //--------------DE TAI TOT NHGIEP------------ //--SU DUNG FUZZY LOGIC DIEU KHIEN MOBILE ROBOT--- //-----SVTH: NGUYEN XUAN HOANG----- //-----GVHD: AN TRI TAN----- #include "E:\ProgramHoang\fuzzy chicken\main.h" #include "E:\ProgramHoang\lcd_lib_4bit.c" #include "E:\ProgramHoang\Thiet ke mo hinh\program\led7.c" #include "E:\ProgramHoang\Thiet ke mo hinh\sieu am.c" #include //---KHAI BAO CAC CHUONG TRINH CON--- float min(float, float); float max(float, float); float dokhoangcach(float, float); float dogoc(float, float); void sailechduong(float, float); void sailecham(float,float); //---chuong trinh chinh---- void main() { setup_adc_ports(NO_ANALOGS|VSS_VDD); setup_adc(ADC_OFF|ADC_TAD_MUL_0|ADC_WHEN_INT0|ADC_INT_EVERY_OTHER); setup_spi(FALSE); setup_wdt(WDT_OFF); setup_timer_0(RTCC_INTERNAL); setup_timer_1(T1_DISABLED); setup_timer_2(T2_DIV_BY_16,255,1); setup_ccp1(CCP_PWM); setup_ccp2(CCP_PWM); set_pwm1_duty(0); set_pwm2_duty(0); setup_oscillator(False); set_tris_a(0x00); set_tris_b(0x00); set_tris_c(0x00); set_tris_d(0x00); set_tris_e(0x00); // TODO: USER CODE!! LCD_init(); while(1) { int8 r1,r2; //khoang cach cac sieu am do duoc milimet float ex,et; //sailech khoang cach va sai lech goc r1=kt_sieuam1(); //sieu am truoc r2=kt_sieuam2(); //sieu am sau ex=dokhoangcach(r1,r2); et=dogoc(r1,r2); LCD_putcmd(0x80); LCD_putchar("Et="); LCD_putcmd(0x83); LCD3(et);//hien thi Ex LCD_putcmd(0x88); LCD_putchar("Ex="); LCD_putcmd(0x8c); LCD3(ex);//hien thi Ex LCD_PUTCMD(0XC0); LCD_putchar("S1:"); LCD_putcmd(0xc3); LCD3(r1); LCD_PUTCMD(0XC8); LCD_putchar("S2: "); LCD_putcmd(0xcb); LCD3(r2); //---DOAN DIEU KHIEN DONG CO--- //---banh trai=pwm1/banhphai=pwm2--- if(ex==0&&et==0) { set_pwm1_duty(1000); set_pwm2_duty(1000);//di thang } else { if(ex>=0) sailechduong(ex,et); else sailecham(ex,et); } } } //Ket thuc chuong trinh chinh //chuong trinh tinh gia tri MAX,MIN float max(float r,float g) { if(r>g) return(r); else return(g); } float min(float r,float g) { if(r>g) return(g); else return(r); } //chuong trinh do khoang cach float dokhoangcach(float r1,float r2) { float distance,edistance; distance=max(r1,r2)+ abs(r1-r2)/2; edistance=3-distance;//khoang cach dat la 300mm edistance=floor(edistance);//lam tron so if(edistance>3) edistance==3; if(edistance<-3) edistance==-3; return(edistance); } float dogoc(float r1,float r2) { float etheta; etheta=0 -(r1-r2); //0 la goc dat ban dau if(etheta>2) etheta==2; if(etheta<-2) etheta==-2; return(etheta); } //chuong trinh con dieu khien dong co //---sai lech duong--- void sailechduong(float ex, float et) { //Ex=3 if(ex==3&&et==2) { set_pwm2_duty(300); set_pwm1_duty(1000); } if(ex==3&&et==1) { set_pwm2_duty(300); set_pwm1_duty(850); } if(ex==3&&et==0) { set_pwm2_duty(300); set_pwm1_duty(700); } if(ex==3&&et==-1) { set_pwm2_duty(300); set_pwm1_duty(600); } if(ex==3&&et==-2) { set_pwm2_duty(300); set_pwm1_duty(500); } //Ex=2 if(ex==2&&et==2) { set_pwm2_duty(300); set_pwm1_duty(900); } if(ex==2&&et==2) { set_pwm2_duty(300); set_pwm1_duty(1000); } if(ex==2&&et==1) { set_pwm2_duty(300); set_pwm1_duty(800); } if(ex==2&&et==0) { set_pwm2_duty(300); set_pwm1_duty(700); } if(ex==2&&et==-1) { set_pwm2_duty(300); set_pwm1_duty(600); } if(ex==2&&et==-2) { set_pwm2_duty(300); set_pwm1_duty(500); } //Ex=1 if(ex==1&&et==2) { set_pwm2_duty(500); set_pwm1_duty(800); } if(ex==1&&et==1) { set_pwm2_duty(450); set_pwm1_duty(700); } if(ex==1&&et==0) { set_pwm2_duty(400); set_pwm1_duty(600); } if(ex==1&&et==-1) { set_pwm2_duty(350); set_pwm1_duty(500); } if(ex==1&&et==-2) { set_pwm2_duty(300); set_pwm1_duty(450); } //Ex=0 if(ex==0&&et==2) { set_pwm2_duty(500); set_pwm1_duty(800); } if(ex==0&&et==1) { set_pwm2_duty(500); set_pwm1_duty(700); } if(ex==0&&et==-1) { set_pwm2_duty(700); set_pwm1_duty(500); } if(ex==0&&et==2) { set_pwm2_duty(800); set_pwm1_duty(500); } }//end sailechduong void sailecham(float ex, float et) { //Ex=-1 if(ex==-1&&et==2) { set_pwm2_duty(500); set_pwm1_duty(700); } if(ex==-1&&et==1) { set_pwm2_duty(500); set_pwm1_duty(650); } if(ex==-1&&et==0) { set_pwm2_duty(500); set_pwm1_duty(600); } if(ex==-1&&et==-1) { set_pwm2_duty(600); set_pwm1_duty(500); } if(ex==-1&&et==-2) { set_pwm2_duty(700); set_pwm1_duty(500); } //Ex=-2 if(ex==-2&&et==2) { set_pwm2_duty(500); set_pwm1_duty(800); } if(ex==-2&&et==1) { set_pwm2_duty(700); set_pwm1_duty(500); } if(ex==-2&&et==0) { set_pwm2_duty(600); set_pwm1_duty(500); } if(ex==-1&&et==-1) { set_pwm2_duty(500); set_pwm1_duty(650); } if(ex==-1&&et==-2) { set_pwm2_duty(400); set_pwm1_duty(700); } //Ex=-2 if(ex==-2&&et==2) { set_pwm2_duty(800); set_pwm1_duty(500); } if(ex==-2&&et==1) { set_pwm2_duty(900); set_pwm1_duty(500); } if(ex==-2&&et==0) { set_pwm2_duty(400); set_pwm1_duty(700); } if(ex==-2&&et==-1) { set_pwm2_duty(500); set_pwm1_duty(800); } if(ex==-2&&et==-2) { set_pwm2_duty(500); set_pwm1_duty(900); } //Ex=-3 if(ex==-3&&et==2) { set_pwm2_duty(700); set_pwm1_duty(500); } if(ex==-3&&et==1) { set_pwm2_duty(600); set_pwm1_duty(500); } if(ex==-3&&et==0) { set_pwm2_duty(500); set_pwm1_duty(700); } if(ex==-3&&et==-1) { set_pwm2_duty(400); set_pwm1_duty(900); } if(ex==-3&&et==-2) { set_pwm2_duty(400); set_pwm1_duty(1000); } }//End sailecham 2.Thư viện LCD //----Thu vien 4bit LCD 2x16--- #include #define LCD_RS PIN_D2 //#define LCD_RW PIN_A1 #define LCD_EN PIN_D3 #define LCD_D4 PIN_D4 #define LCD_D5 PIN_D5 #define LCD_D6 PIN_D6 #define LCD_D7 PIN_D7 // misc display defines- #define Line_1 0x80 #define Line_2 0xC0 #define Clear_Scr 0x01 // prototype statements #separate void LCD_Init ( void );// ham khoi tao LCD #separate void LCD_SetPosition ( unsigned int cX );//Thiet lap vi tri con tro #separate void LCD_PutChar ( unsigned int cX );// Ham viet1kitu/1chuoi len LCD #separate void LCD_PutCmd ( unsigned int cX) ;// Ham gui lenh len LCD #separate void LCD_PulseEnable ( void );// Xung kich hoat #separate void LCD_SetData ( unsigned int cX );// Dat du lieu len chan Data // D/n Cong #use standard_io ( B ) #use standard_io (A) //khoi tao LCD********************************************** #separate void LCD_Init ( void ) { LCD_SetData ( 0x00 ); delay_ms(200); /* wait enough time after Vdd rise >> 15ms */ output_low ( LCD_RS );// che do gui lenh LCD_SetData ( 0x03 ); /* init with specific nibbles to start 4-bit mode */ LCD_PulseEnable(); LCD_PulseEnable(); LCD_PulseEnable(); LCD_SetData ( 0x02 ); /* set 4-bit interface */ LCD_PulseEnable(); /* send dual nibbles hereafter, MSN first */ LCD_PutCmd ( 0x2C ); /* function set (all lines, 5x7 characters) */ LCD_PutCmd ( 0b00001100); /* display ON, cursor off, no blink */ LCD_PutCmd ( 0x06 ); /* entry mode set, increment & scroll left */ LCD_PutCmd ( 0x01 ); /* clear display */ } #separate void LCD_SetPosition ( unsigned int cX ) { /* this subroutine works specifically for 4-bit Port A */ LCD_SetData ( swap ( cX ) | 0x08 ); LCD_PulseEnable(); LCD_SetData ( swap ( cX ) ); LCD_PulseEnable(); } #separate void LCD_PutChar ( unsigned int cX ) { /* this subroutine works specifically for 4-bit Port A */ output_high ( LCD_RS ); LCD_PutCmd( cX ); output_low ( LCD_RS ); } #separate void LCD_PutCmd ( unsigned int cX ) { /* this subroutine works specifically for 4-bit Port A */ LCD_SetData ( swap ( cX ) ); /* send high nibble */ LCD_PulseEnable(); LCD_SetData ( swap ( cX ) ); /* send low nibble */ LCD_PulseEnable(); } #separate void LCD_PulseEnable ( void ) { output_high ( LCD_EN ); delay_us ( 3 ); // was 10 output_low ( LCD_EN ); delay_ms ( 3 ); // was 5 } #separate void LCD_SetData ( unsigned int cX ) { output_bit ( LCD_D4, cX & 0x01 ); output_bit ( LCD_D5, cX & 0x02 ); output_bit ( LCD_D6, cX & 0x04 ); output_bit ( LCD_D7, cX & 0x08 ); } void LCD3(int16 m) { int a,b,c,d; a = m/1000 + 48; LCD_PutChar(a); b = (m/100)%10 + 48; LCD_PutChar(b); c = (m/10)%10 + 48; LCD_PutChar(c); d = m%10 + 48; LCD_Putchar(d); } void LCD1(int m) { int a; a = m%10 + 48; LCD_Putchar(a); } 3.Chương trình hiển thị trên LED 7x4 // Chuong trinh Qet led 7x4 int16 n; //Chuong trinh cho led 7x4 char ma[]={0x00, 0x10, 0x20, 0x30, 0x40, 0x50, 0x60, 0x70, 0x80, 0x90}; //khai bao mang chua cac so tu 0-9 long dv,tram,chuc,ngan; void call(void)//ham tra ve cac gia tri can hien thi { ngan= n/1000; tram=(n/100)%10; chuc=(n/10)%10; dv=n%10; } void quet(void)//hien thi cac so { call(); output_b(ma[dv]+1); delay_ms(3); output_b(0x00); delay_ms(3); if(n>9) { output_b(ma[chuc]+2); delay_ms(3); output_b(0x00); delay_ms(3); } if(n>99) { output_b(ma[tram]+4); delay_ms(3); output_b(0x00); delay_ms(3); } if(n>999) { output_b(ma[ngan]+8); delay_ms(3); output_b(0x00); delay_ms(3); } } //end 4.Chương trình cho 2 cảm biến siêu âm //Chuong trinh do khoang cach bang 2 cam bien sieu am SRF05 //su dung 2 chân c6 và c7 làm dau vào và dau ra cho siêu am 1 và a1,a2 cho siêu âm 2 #define IN1_05 pin_c6 #define OUT1_05 pin_c7 #define IN2_05 pin_a1 #define OUT2_05 pin_a2 //clockrate/(4 clock per intruction*T1_div_by_4)*1000000 #define CTM 1.25 //Convert To Microsesor //KIEM TRA TINH TOAN CHO SIEU AM 1 int16 kt_sieuam1() { int16 time; output_high(IN1_05); delay_us(10); output_low(IN1_05);//phat xung 10us setup_timer_0(RTCC_INTERNAL|RTCC_DIV_4); while(!input(OUT1_05)){}//cho cho khi co tin hieu tai dau ra sieu am set_timer0(0);//bat dau hoat dong timer1 while(input(OUT1_05)){} time=get_timer0();//dem thoi gian nhan dc xung time=time/CTM; if(time>=42000) { //ko lam ji; return 0; } else { return time/148; //chuyen ra ink } } //KIEM TRA TINH TOAN CHO SIEU AM 2 int16 kt_sieuam2() { int16 time; output_high(IN2_05); delay_us(10); output_low(IN2_05);//phat xung 10us setup_timer_1(T1_INTERNAL | T1_DIV_BY_4); while(!input(OUT2_05)){}//cho cho khi co tin hieu tai dau ra sieu am set_timer1(0);//bat dau hoat dong timer1 while(input(OUT2_05)){} time=get_timer1();//dem thoi gian nhan dc xung time=time/CTM; if(time>=42000) { //ko lam ji; return 0; } else { return time/148; //chuyen ra inh } }