mô phỏng áp dụng thuật toán fuzzy vào mô hình thực tế để điều khiển định hướng cho robot rắn

mô phỏng áp dụng thuật toán fuzzy vào mô hình thực tế để điều khiển định hướng cho robot rắn

Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân LỜI CẢM ƠN Đất nƣớc ta đà phát triển khoa học kĩ thuật đóng vai trị quan trọng đời sống ngƣời.Việc áp dụng khoa học kỹ thuật làm tăng suất lao động đồng thời góp phần khơng nhỏ việc thay sức lao động ngƣời lao động cách có hiệu nhất, đảm bảo an tồn cho họ q trình làm việc Các robot thay tuyệt vời cho sức ngƣời công việc nguy hiểm công việc mô trƣờng làm việc khắc nghiệt Luận văn tốt nghiệp môn học giúp cho sinh viên ngành Cơ Điện Tử ứng dụng kiến thức đƣợc học nhà trƣờng ứng dụng vào thực tế kiểm tra lý thuyết học so với thực tế để có nhìn cụ thể với vấn đề học.Từ rút kinh nghiệm để làm đề tài công việc nhà máy, xí nghiệp, cơng ty sinh viên trƣờng Trong phạm vi đề, kiến thức từ môn sở nhƣ Nguyên Lý Máy, Chi Tiết Máy, Thiết Kế Vẽ Bằng Máy Tính,Vi Xử Lý, Giao Tiếp Máy Tính, Kỹ Thuật Ngƣời Máy…đƣợc áp dụng Đây môn học mà môn Cơ Điện Tử trang bị cho sinh viên trƣớc làm Luận Văn Tốt Nghiệp.Đó thực quan trọng cần thiết sinh viên trình thực đề tài Em xin chân thành cảm ơn TS Võ Tƣờng Quân tận tâm hƣớng dẫn em hoàn thành Luận Văn Tốt Nghiệp Em xin chân thành cảm ơn thầy hƣớng dẫn học kì buổi phản biện giúp em có thêm kiến thức, kinh nghiệm thực vẽ Đồng thời em xin cám ơn tập thể lớp CK07CD, bạn nhiều giúp đỡ em hồn thiện đồ án Đây Đề Tài Luận Văn Tốt Nghiệp nên không tránh đƣợc thiếu sót thiếu kinh nghiệm việc tính tốn, chọn lựa chi tiết,trong mơ hình điều khiển.Em kính mong đƣợc dẫn thêm quý thầy cô để em đƣợc củng cố kiến thức đúc kết thêm kinh nghiệm quý báu phục vụ cho công việc sau HCM, Ngày ……Tháng……Năm 2011 Bùi Thanh Vinh I Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân TÓM TẮT LUẬN VĂN Robot rắn đề tài đƣợc nghiên cứu từ lâu thu hút nhà khoa học,các sinh viên Trong năm gần ,những nghiên cứu lĩnh vực có nhiều kết xuất sắc Luận văn đƣợc thực với mục tiêu nghiên cứu, mô chuyển động robot rắn môi trƣờng mặt phẳng ngang các phƣơng pháp điều khiển liên quan Đề tài xem tổng hợp kiến thức chƣơng trình Cơ điện tử bao gồm mơ hình hóa, mơ phỏng, thiết kế khí, thiết kế mạch điện, thiết kế thuật tốn điều khiển Lý thuyết tính tốn độnglực học đƣợc ứng dụng từ nghiên cứu giới cho mơ hình robot rắn chuyển động mặt phẳng Q trình tính tốn, mơ áp dụng luật điều khiển đƣợc thực Matlab để kiểm chứng chuyển động robot rắn Trong giới hạn thời gian luận văn, luận văn mơ áp dụng thuật tốn Fuzzy vào mơ hình thực tế để điều khiển định hƣớng cho robot rắn II Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân MỤC LỤC LỜI CẢM ƠN I TÓM TẮT LUẬN VĂN II MỤC LỤC III DANH SÁCH HÌNH VẼ VI CHƢƠNG I - TỔNG QUAN 1 1.1 GIỚI THIỆU 1.2 TÌNH HÌNH NGHIÊN CỨU Ở TRONG VÀ NGOÀI NƢỚC 1.2.1 Nƣớc Ngoài 1.2.2 Trong nƣớc 1.3 ĐẶC ĐIỂM SINH HỌC CỦA RẮN 1.3.1 Cấu tạo rắn 1.3.2 Các dạng chuyển động rắn : 1.4 CÁC PHƢƠNG PHÁP TÍNH ĐỘNG HỌC CỦA ROBOT RẮN 1.4.1 Phƣơng pháp quy ƣớc Denavit- Hartenberg 1.4.2 Các liên kết nonholonomic 1.4.3 Các đƣờng xƣơng sống robot môi trƣờng liên tục 1.5 CÁC PHƢƠNG PHÁP ĐỘNG LỰC HỌC 1.6 MỤC TIÊU CỦA ĐỀ TÀI 1.7 TÓM TẮT NỘI DUNG CỦA ĐỀ TÀI VÀ QUÁ TRÌNH THỰC HIỆN 1.7.1 Nội dung thực 1.7.2 Các bƣớc thực CHƢƠNG II – TÍNH TỐN ĐỘNG HỌC - ĐỘNG LỰC HỌC Robot 10 2.1 LỰA CHỌN MƠ HÌNH 10 2.2 ĐỊNH NGHĨA VÀ KÝ HIỆU 10 III Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân 2.3 PHƢƠNG PHÁP SVD 12 2.4 DÙNG PHƢƠNG PHÁP LAGRANGE ĐỂ TÍNH ĐỘNG LỰC HỌC robot 12 2.4.1 Tính moment quán tính 13 2.4.2 Tính lực liên kết khâu 13 2.4.3 Tính vận tốc góc đốt robot 15 2.4.4 Tính vận tốc robot rắn 23 CHƢƠNG III – TÍNH TỐN THIẾT KẾ CƠ KHÍ 26 3.1 GIỚI THIỆU VÀ PHÂN TÍCH MỘT SỐ HỆ THỐNG CƠ KHÍ 26 3.1.1 Hình thức di chuyển nhờ ma sát 26 3.1.2 Một hình thức chuyển động ma sát theo hai trục 26 3.1.3 Mơ hình chuyển động nhờ bánh xe 28 3.2 QUÁ TRÌNH THIẾT KẾ CƠ KHÍ 29 3.2.1 Đặt vấn đề : 29 3.2.2 Giới thiệu động RC servo 29 3.2.3 Cấu tạo động RC servo chuẩn 29 3.2.4 Phân loại động RC servo 30 3.2.5 Nguyên tắc hoạt động 31 3.2.6 Các thông số kỹ thuật RC 31 3.2.7 Tính tốn moment xoắn động 32 3.2.8 Lựa chọn động 32 3.2.9 Thiết kế hệ thống khí : 33 CHƢƠNG IV - ĐIỀU KHIỂN VÀ THIẾT KẾ MẠCH ĐIỀU KHIỂN 37 4.1 ĐIỀU KHIỂN 37 4.1.1 Phƣơng án điều khiển 37 4.1.2 Điều khiển động RC servo 39 IV Luận Văn Tốt Nghiệp 4.2 ĐIỀU KHIỂN ĐỊNH HƢỚNG 41 4.2.1 4.3 GVHD : TS.Võ Tƣờng Quân Bộ điều khiển Fuzzy 41 THIẾT KẾ MẠCH ĐIỆN CHO ROBOT RẮN 48 4.3.1 Mạch Nguồn cho vi điều khiển robot 48 4.3.2 Mạch cảm biến xác định hƣớng 49 4.3.3 Mạch vi điều khiển 50 4.3.4 Moduel động Led báo tín hiệu động 51 4.4 LƢU ĐỒ ĐIỀU KHIỂN ROBOT RẮN 52 CHƢƠNG V : MÔ PHỎNG VÀ THỰC NGHIỆM 54 5.1 CÁC THÔNG SỐ MÔ PHỎNG 54 5.2 MÔ PHỎNG VỚI SIM MECHANICS 54 5.2.1 Xây dựng mơ hình mơ 54 5.2.2 Xây dựng mơ hình mục tiêu 55 5.2.3 Bộ điều khiển robot 56 5.2.4 Kết mô 58 5.3 THỰC NGHIỆM 60 5.3.1 Xây dựng mơ hình 60 5.3.2 Kết Quả 63 CHƢƠNG VI : KẾT LUẬN 64 6.1 KẾT QUẢ ĐẠT ĐƢỢC 64 6.2 KẾT QUẢ CHƢA ĐẠT ĐƢỢC 64 6.3 HƢỚNG PHÁT TRIỂN 64 TÀI LIỆU THAM KHẢO 65 PHỤ LỤC A .A1 PHỤ LỤC B : B1 V Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Qn DANH SÁCH HÌNH VẼ Hình 1.1 : Robot rắn OmniTread(OT-8) Hình 1.2 robot rắn ACM-R5 Hình 1.3 : Quá trình chuyển động rắn Hình 1.4 : Chuyển động sóng ngang Hình 1.5 : Chuyển động lƣợn bên Hình 1.6 bậc tự robot rắn Hình 1.7: khâu liên kết ACM III Hình 2.1 : n - đốt robot rắn Hình 2.2 : Mơ hình hóa động Hình 2.3 : Mơ hình lực moment robot rắn Hình 2.4 : Đồ Thị góc vận tốc góc khâu – Hình 2.5 : Đồ Thị góc vận tốc góc khâu – Hình 2.6 : Đồ Thị góc vận tốc góc khâu – Hình 2.7 : Đồ Thị góc vận tốc góc khâu – Hình 2.8 : Khoảng cách di chuyển robot theo thời gian Hình 2.9 : Khoảng cách di chuyển robot theo thời gian Hình 2.10 : Vận tốc di chuyển robot theo thời gian Hình 2.11 : Vận tốc di chuyển robot theo thời gian Hình 3.1 : kết cấu robot rắn chuyển động nhở ma sát Hình 3.2 : Cấu tạo khớp nối bậc tự Hình 3.3 : mơ hình chuyển động ma sát theo trục Hình 3.4 : ACM III Tiến sĩ Shigeo Hirose Nhật Bản Hình 3.5 : Robot ACM chuyển động nhờ bánh xe VI Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân Hình 3.6: Cấu tạo động RC servo Hình 3.7 Ảnh động thực tế dùng mơ hình Hình 3.8 : Mơ hình 3D RC servo Hình 3.9 : Mơ hình 3D giá động Hình 3.10 : Mơ hình 3D ống làm đốt robot Hình 3.11 : Mơ hình 3D đốt robot Hình 3.12 : Mơ hình 3D ống nối đốt Hình 3.13 : Một đốt thực tế robot rắn Hình 3.14: Robot rắn đốt Hình 4.1 : P1 chuyển động theo khâu đầu , P2 chuyển động hình sin Hình 4.2 : Giản đồ xung điều khiển động RC servo Hình 4.3 : Lƣu đồ thuật tốn tạo xung điều khiển động Hình 4.4 : Mơ hình xác định hƣớng robot Hình 4.5 : Mạch quang điện trở Hình 4.6 : Sự biến thiên quang điện trở Hình 4.7 : Bố trí cảm biến định hƣớng Hình 4.8 : Sơ đồ điều khiển Fuzzy Hình 4.9 : Sơ đồ Membership Function input Hình 4.10 : Sơ đồ Membership Function output Hình 4.11: Phƣơng pháp giải mờ theo tọa độ trọng tâm Hình 4.12 : Sơ đồ hệ luật Fuzzy Hình 4.13 : Pin sài cho mơ hình robot Hình 4.14: Mạch nguồn cho vi điều khiển robot Hình 4.15 : Sơ đồ mạch định hƣớng LDR VII Luận Văn Tốt Nghiệp GVHD : TS.Võ Tƣờng Quân Hình 4.16 : Mạch vi điều khiển robot Hình 4.17: Module nối tới động Hình 4.18 : Các đèn Led báo tín hiệu điều khiển động Hình 4.19 : Lƣu đồ giải thuật điều khiển robot rắn Hình 5.1 : Mơ hình mơ robot Hình 5.2 : Mơ hình lực ma sát Hình 5.3 : Mơ hình động Servo Hình 5.4 : Mơ hình mục tiêu Hình 5.5 : Bộ điều khiển Fuzzy bám mục tiêu Hình 5.6 : Membership Function input Hình 5.7 : Membership Function output Hình 5.8 : Bộ Điều khiển Fuzzy trung tâm tín hiệu điều khiển Hình 5.9 : Đồ thị tọa độ tâm robot Hình 5.10 : Quá trình di chuyển qua mục tiêu robot Hình 5.11 : Mơ hình 3D di chuyển đến mục tiêu Hình 5.12 : Mạch điều khiển robot Hình 5.13 : Một đốt thực tế robot rắn Hình 5.14 : Mơ hình thật tế robot rắn Hình 5.15 : Mơ hình thực tế robot rắn Hình 5.16 : Mơ hình thực tế robot rắn Hình 5.17 : Mơ hình thực tế robot rắn VIII Chƣơng I : Tổng Quan CHƢƠNG I - TỔNG QUAN 1.1 GIỚI THIỆU Trƣớc hầu hết thiết bị có chuyển động dùng đến loại bánh xe,những thiết bị chuyển với tốc độ cao bề mặt phẳng nhiêu di chuyển bền mặt gồ ghề gặp nhiều khó khăn.Và vấn đề đƣợc đặc tìm dạng di chuyển khác hoạt động dễ dàng bề mặt gồ ghề,một số loại di chuyển đƣợc tìm di chuyển lồi rắn.Chúng di chuyển nhờ ma sát thân bề mặt.Chính nhờ điều mà rắn di chuyển nhiều địa hình khác nhau,ngồi rắn cịn di chuyển dƣới nƣớc , leo … Vì ứng dụng chuyển động rắn lên robot đƣợc nhiều ngƣời quan tâm nghiên cứu.Và Nhiều robot rắn đời với nhiều ứng dụng khác phục vụ cho công tác nghiên cứu sống Chuyển động robot rắn có độ ổn định , linh hoạt , hƣ hỏng cấu Nhƣng lại mang tải trọng Hứa hẹn ứng dụng nhiều ngành nhƣ : giải phẩu , cứu hỏa , cứu hộ ngƣời mắc kẹt , thám hiểm … 1.2 TÌNH HÌNH NGHIÊN CỨU Ở TRONG VÀ NGỒI NƢỚC 1.2.1 Nƣớc Ngồi Những nƣớc đầu công nghệ nghiên cứu chế tạo nhiều loại robot rắn khác cho đời nhiều hệ robot rắn có khả linh hoạt cao nhƣ:  Tại Đại học Michigan có robot rắn OmniTread(OT-8) có khả di chuyển nhiều địa hình phức tạp, có khả di chuyển đƣờng ống di chuyển qua hang nhỏ Hình 1.1 : Robot rắn OmniTread(OT-8) [3] Trang Chƣơng I : Tổng Quan  Tại Nhật có robot rắn ACM-R5 tiến sĩ Shigeo Hirose có khả di chuyển dƣới nƣớc cạn cách linh hoạt Hình 1.2 robot rắn ACM-R5 [9]  Và nhiều trƣờng đại học lớn khác giới có sản phẩm robot rắn nhƣ CMU đại học Carnegie Mellon,JL-I đại học Hamburg 1.2.2 Trong nƣớc Tại Việt Nam bắt đầu nghiên cứu chế tạo số robot rắn nhƣng ít.Chủ yếu dừng lại trình tính tốn chính.Và chƣa có sản phẩm robot rắn có khả di chuyển linh hoạt đƣợc  Tại trƣờng đại học Bách Khoa TP HCM có đề tài luận văn tốt nghiệp robot rắn nhƣng mơ hình làm có kết cấu khí khơng đáp ứng đủ để di chuyển linh hoạt đƣợc 1.3 ĐẶC ĐIỂM SINH HỌC CỦA RẮN 1.3.1 Cấu tạo rắn  Cấu tạo xƣơng sống Một xƣơng rắn thƣờng có từ 100 ÷ 400 đốt Khoảng chuyển động khớp thƣờng từ 10 -200 để di chuyển qua lại, khoảng vài độ để di chuyển theo phƣơng lên xuống Ngồi khớp xƣơng cịn quay góc nhỏ dọc theo trục xƣơng sống để thuận lợi cho dạng chuyển động phức tạp  Cấu tạo da : Trang Phụ Lục A : Chƣơng trình tính động lực học if [(i1~=m(1))&(i1~=m(2))]&(i1~=m(3)) invm(i1,i1) = 1/D(i1,i1); end end invM = V*invm*UU'; elseif(size_m(1) == 4) invm(m(1),m(1)) = 0; invm(m(2),m(2)) = 0; invm(m(3),m(3)) = 0; invm(m(4),m(4)) = 0; for i1=1:6 if [[(i1~=m(1))&(i1~=m(2))]&(i1~=m(3))]&(i1~=m(4)) invm(i1,i1) = 1/D(i1,i1); end end invM = V*invm*UU'; elseif(size_m(1) == 5) invm(m(1),m(1)) = 0; invm(m(2),m(2)) = 0; invm(m(3),m(3)) = 0; invm(m(4),m(4)) = 0; invm(m(5),m(5)) = 0; for i1=1:6 if [[[(i1~=m(1))&(i1~=m(2))]&(i1~=m(3))]&(i1~=m(4))]&(i1~=m(5)) invm(i1,i1) = 1/D(i1,i1); end end invM = V*invm*UU'; elseif(size_m(1) == 6) invm(m(1),m(1)) = 0; invm(m(2),m(2)) = 0; invm(m(3),m(3)) = 0; invm(m(4),m(4)) = 0; invm(m(5),m(5)) = 0; invm(m(5),m(5)) = 0; invM = V*invm*UU'; else invM = V*inv(D)*UU'; end % Su dung phuong trinh Ode45 de giai theta1, theta2, theta3, theta4, theta5, theta6,theta1_dot, theta2_dot, theta3_dot, theta4_dot, theta5_dot, theta6_dot % Ham 'fishrobotfunc_Input_Torque' khai bao giong nhu bai vi du mau minh copy cho em hom truoc [t,y]= ode45('snakerobot',tspan,x); % ham fishrobot file snakerobot.m x la gia tri ban dau y0 Trang A7 Phụ Lục A : Chƣơng trình tính động lực học y_tmp = size(y,1); x0(x_tmp+1,:) = y(y_tmp,:); %Tinh luc ma sat Ft1=m1*g*mit*sign(x(2)); Ft2=m2*g*mit*sign(x(4)); Ft3=m3*g*mit*sign(x(6)); Ft4=m4*g*mit*sign(x(8)); Ft5=m5*g*mit*sign(x(10)); Ft6=m6*g*mit*sign(x(12)); Fn1=m1*g*min*sign(x(2)); Fn2=m2*g*min*sign(x(4)); Fn3=m3*g*min*sign(x(6)); Fn4=m4*g*min*sign(x(8)); Fn5=m5*g*min*sign(x(10)); Fn6=m6*g*min*sign(x(12)); %luc theo phuong x y Fx1= sin(x(1))*Ft1-cos(x(1))*Fn1; Fx2= sin(x(3))*Ft2-cos(x(3))*Fn2; Fx3= sin(x(5))*Ft3-cos(x(5))*Fn3; Fx4= sin(x(7))*Ft4-cos(x(7))*Fn4; Fx5= sin(x(9))*Ft5-cos(x(9))*Fn5; Fx6= sin(x(11))*Ft6-cos(x(11))*Fn6; Fy1= cos(x(1))*Ft1+sin(x(1))*Fn1; Fy2= cos(x(3))*Ft2+sin(x(3))*Fn2; Fy3= cos(x(5))*Ft3+sin(x(5))*Fn3; Fy4= cos(x(7))*Ft4+sin(x(7))*Fn4; Fy5= cos(x(9))*Ft5+sin(x(9))*Fn5; Fy6= cos(x(11))*Ft6+sin(x(11))*Fn6; % Tinh tong cac luc Fm1=[c1*x(2)-c1*x(4)+k1*(x(1)-x(3))]; Fm2=[c1*x(2)+(c1+c2)*x(4)-c2*x(6)+(k1+k2)*x(3)-k1*x(1)-k2*x(5)]; Fm3=[c2*x(4)+(c2+c3)*x(6)-c2*x(8)+(k2+k3)*x(5)-k1*x(3)-k2*x(7)]; Fm4=[c3*x(6)+(c3+c4)*x(8)-c2*x(10)+(k3+k4)*x(7)-k1*x(5)-k2*x(9)]; Fm5=[c4*x(8)+(c4+c5)*x(10)-c2*x(12)+(k4+k5)*x(9)-k1*x(7)-k2*x(11)]; Fm6=[c5*x(10)+c5*x(12)+k5*x(12)-k5*x(10)]; FT1=[Fy2*cos(x(1))-Fx2*sin(x(1))]+[cos(x(1))*(Fy2+Fy3+Fy4+Fy5+Fy6)sin(x(1))*(Fx2+Fx3+Fx4+Fx5+Fx6)]+Fm1; FT2=[Fy3*cos(x(3))-Fx3*sin(x(3))]+[cos(x(3))*(Fy3+Fy4+Fy5+Fy6)sin(x(3))*(Fx3+Fx4+Fx5+Fx6)]+Fm2; FT3=[Fy4*cos(x(5))-Fx4*sin(x(5))]+[cos(x(5))*(Fy4+Fy5+Fy6)sin(x(5))*(Fx4+Fx5+Fx6)]+Fm3; FT4=[Fy5*cos(x(7))-Fx5*sin(x(7))]+[cos(x(7))*(Fy5+Fy6)-sin(x(7))*(Fx5+Fx6)]+Fm4; FT5=[Fy6*cos(x(9))-Fx6*sin(x(9))]+[cos(x(9))*(Fy6)-sin(x(9))*(Fx6)]+Fm5; Trang A8 Phụ Lục A : Chƣơng trình tính động lực học FT6=Fm6; Ff=FT1+FT2+FT3+FT4+FT5+FT6; FF= [FF Ff]; end theta1 = x0(:,1); ans1 = (180*x0(:,1)/pi); % theta1 ans2 = (18000*x0(:,2)/pi); % theta1_dot theta2 = x0(:,3); ans3 = 180*x0(:,3)/pi; % theta2 ans4 = (18000*x0(:,4)/pi); % theta2_dot theta3 = x0(:,5); ans5 = 180*x0(:,5)/pi; % theta3 ans6 = (18000*x0(:,6)/pi); % theta3_dot theta4 = x0(:,7); ans7 = (180*x0(:,7)/pi); % theta4 ans8 = (18000*x0(:,8)/pi); % theta4_dot theta5 = x0(:,9); ans9 = 180*x0(:,9)/pi; % theta5 ans10 = (18000*x0(:,10)/pi); % theta5_dot theta6 = x0(:,11); ans11 = 180*x0(:,11)/pi; % theta6 ans12 = (18000*x0(:,12)/pi); % theta6_dot tt = 0:0.01:i_end_2/100; time_theta2_zero = 0:0.01:(theta2_begin_time/100)-0.01; time_theta2 = (theta2_begin_time/100):0.01:(i_end_2/100 + theta2_begin_time/100); for i = 1:theta2_begin_time theta2(i,:) = 0; end for i = theta2_begin_time+1:i_end_2 theta2(i,:) = ans3(i,:); end xG_initial = [0 0]; for i = 1:i_end_2 i s_t = (i-1)*delta_t; e_t = i*delta_t; tspan_AV = [s_t e_t]; Trang A9 Phụ Lục A : Chƣơng trình tính động lực học xG_tmp = size(xG_initial,1); xG = xG_initial(xG_tmp,:); FF_AV = FF(:,i); [t,yG] = ode45('Velocity_Function',tspan_AV,xG); yG_tmp = size(yG,1); xG_initial(xG_tmp+1,:) = yG(yG_tmp,:); end Moving_Distance = (xG_initial(:,1)); Real_Velocity = (xG_initial(:,2)); figure(1); subplot(321); plot(tt,ans1,'b','LineWidth',2); title('Link 1'); ylabel('Displacement(Degree)'); xlabel('Time (s)'); grid on; subplot(322); plot(tt,ans2,'r','LineWidth',2); title('Link 1'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); xlim([0 i_end_2/100]); grid on; subplot(323); plot(tt,ans3,'b','LineWidth',2); title('Link 2'); ylabel('Displacement(Degree)'); xlabel('Time (s)'); grid on; subplot(324); plot(tt,ans4,'r','LineWidth',2); title('Link 2'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); grid on; subplot(325); plot(tt,ans5,'b','LineWidth',2); title('Link 3'); ylabel('Displacement (Degree)'); xlabel('Time (s)'); grid on; Trang A10 Phụ Lục A : Chƣơng trình tính động lực học subplot(326); plot(tt,ans6,'r','LineWidth',2); title('Link 3'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); grid on; figure(2); subplot(321); plot(tt,ans7,'b','LineWidth',2); title('Link 4'); ylabel('Displacement (Degree)'); xlabel('Time (s)'); grid on; subplot(322); plot(tt,ans8,'r','LineWidth',2); title('Link 4'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); grid on; subplot(323); plot(tt,ans9,'b','LineWidth',2); title('Link 5'); ylabel('Displacement(Degree)'); xlabel('Time (s)'); grid on; subplot(324); plot(tt,ans10,'r','LineWidth',2); title('Link 5'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); grid on; subplot(325); plot(tt,ans11,'b','LineWidth',2); title('Link 6'); ylabel('Displacement(Degree)'); xlabel('Time (s)'); grid on; subplot(326); plot(tt,ans12,'r','LineWidth',2); Trang A11 Phụ Lục A : Chƣơng trình tính động lực học title('Link 6'); ylabel('Angular velocity(Degree/s)'); xlabel('Time (s)'); grid on; figure(3); time = 0:0.01:i_end_2/100; subplot(211); plot(time,Moving_Distance,'-k','LineWidth',2); title('The relation between moving distance and time'); xlabel('Time (s)'); ylabel('Moving distance (mm)'); grid on; time1 = 0:0.01:i_end_2/100; subplot(212); plot(time1,Real_Velocity,'-k','LineWidth',2); title('The relationship between real velocity and time'); xlabel('Time (s)'); ylabel('Velocity of snake robot (mm/s)'); grid on; hàm Snakerobot function xp = snakerobot(t,x) global invM; global N1; global N2; global N3; global N4; global N5; global N6; % Ha bac cua phuong trinh: M x Theta_2dot = N % > Theta_2dot = inv(M)*N xp = zeros(12,1); xp(1)=x(2); xp(2) = invM(1,1)*N1 + invM(1,2)*N2 + invM(1,3)*N3 + invM(1,4)*N4 + invM(1,5)*N5 + invM(1,6)*N6; xp(3)=x(4); xp(4) = invM(2,1)*N1 + invM(2,2)*N2 + invM(2,3)*N3 + invM(2,4)*N4 + invM(2,5)*N5 + invM(2,6)*N6; xp(5)=x(6); xp(6) = invM(3,1)*N1 + invM(3,2)*N2 + invM(3,3)*N3 + invM(3,4)*N4 + invM(3,5)*N5 + invM(3,6)*N6; xp(7)=x(8); xp(8) = invM(4,1)*N1 + invM(4,2)*N2 + invM(4,3)*N3 + invM(4,4)*N4 + invM(4,5)*N5 + invM(4,6)*N6; Trang A12 Phụ Lục A : Chƣơng trình tính động lực học xp(9)=x(10); xp(10) = invM(5,1)*N1 + invM(5,2)*N2 + invM(5,3)*N3 + invM(5,4)*N4 + invM(5,5)*N5 + invM(5,6)*N6; xp(11)=x(12); xp(12) = invM(6,1)*N1 + invM(6,2)*N2 + invM(6,3)*N3 + invM(6,4)*N4 + invM(6,5)*N5 + invM(6,6)*N6; Hàm vận tốc function xG_dot =Velocity_Function(t,xG) global FF_AV; m = 2.4; xG_dot = zeros(2,1); xG_dot(1) = xG(2); xG_dot(2) = (1/m)*FF_AV; Trang A13 Phụ Lục B : Chƣơng trình cho vi điều khiển PHỤ LỤC B : CHƢƠNG TRÌNH CHO VI ĐIỀU KHIỂN #include #include #fuses HS,NOPROTECT,NOLVP,NOWDT #use delay(clock=20000000) int gamma; unsigned int16 value[5]; unsigned int16 counter0=0; unsigned int16 counter1=0; unsigned int16 counter2=0; unsigned int16 counter3=0; unsigned int16 counter4=0; // Ham tim gia tri minimum float min(float x,float y){ if (xy) return(x); else return(y); } Trang B1 Phụ Lục B : Chƣơng trình cho vi điều khiển // Ham khoi tao ADC void ADC_Init(){ setup_adc_ports(AN0_TO_AN1); setup_adc(ADC_CLOCK_INTERNAL); } // Ham khoi tao Timer0 void Timer0_Init(){ setup_timer_0(RTCC_INTERNAL|RTCC_DIV_2); set_timer0(231); enable_interrupts(int_timer0); enable_interrupts(global); } // Ngat Timer0 xuat xung PWM #int_timer0 void isr_timer0(void) { set_timer0(231); if (counter0 == 0) { output_high(PIN_B0); } else if (counter0 == value[1]) { output_low(PIN_B0); } else if (counter0 == 2000) { counter0=0; Trang B2 Phụ Lục B : Chƣơng trình cho vi điều khiển } Counter0++; if (counter1 == 0) { output_high(PIN_B1); } else if (counter1 == value[2]) { output_low(PIN_B1); } else if (counter1 == 2000) { counter1=0; } Counter1++; if (counter2 == 0) { output_high(PIN_B2); } else if (counter2 == value[3]) { output_low(PIN_B2); } else if (counter2 == 2000) { counter2=0; } Counter2++; if (counter3 == 0) { output_high(PIN_B3); Trang B3 Phụ Lục B : Chƣơng trình cho vi điều khiển } else if (counter3 == value[4]) { output_low(PIN_B3); } else if (counter3 == 2000) { counter3=0; } Counter3++; if (counter4 == 0) { output_high(PIN_B4); } else if (counter4 == value[5]) { output_low(PIN_B4); } else if (counter4 == 2000) { counter4=0; } Counter4++; } // Ham de tinh Fuzzy void Fuzzy(int16 del_xi){ float sum_moment=0; float sum_mass=0; int step= 1; Trang B4 Phụ Lục B : Chƣơng trình cho vi điều khiển int z[90]; float fz[90]; float Ne,Ze,Po; for(i=0;i