(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt

(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt(Luận văn thạc sĩ) Điều khiển Robot đa hướng bám theo quỹ đạo dùng kỹ thuật điều khiển trượt

LỜI CAM ĐOAN Tơi cam đoan cơng trình nghiên cứu Các số liệu, kết nêu luận văn trung thực chưa cơng bố cơng trình khác Tp Hồ Chí Minh, ngày 12 tháng năm 2011 (Ký tên ghi rõ họ tên) LÊ TRƯỜNG AN iii CẢM TẠ Trước hết, tơi xin bày tỏ lịng biết ơn sâu sắc đến Thầy giáo - TS Nguyễn Thanh Phương, người tận tình hướng dẫn, giúp đỡ tơi suốt q trình nghiên cứu hồn thành luận văn Tôi xin chân thành cảm ơn thầy, cô giáo thuộc Khoa Điện - Điện tử, Trường Đại học Sư Phạm Kỹ Thuật Thành phố Hồ Chí Minh, nhiệt tình truyền thụ kiến thức tạo điều kiện thuận lợi giúp đỡ suốt trình học tập nghiên cứu trường Tôi xin trân trọng cám ơn thầy giáo đồng nghiệp Khoa Tăng Thiết giáp, Trường Sỹ quan KTQS động viên, giúp đỡ tạo điều kiện để học tập nghiên cứu trường Đại học Sư Phạm Kỹ Thuật Thành phố Hồ Chí Minh Xin trân trọng ghi nhớ chân tình, giúp đỡ bạn học, người cung cấp chia sẻ tài liệu, thơng tin q báu suốt q trình học tập, nghiên cứu, hoàn thành luận văn Lê Trƣờng An iv TÓM TẮT Khác với loại robot di động sử dụng bánh truyền thống, robot di động sử dụng bánh đa hướng (gọi tắt robot di động đa hướng) có ưu điểm vượt trội như: khả thay đổi vị trí định hướng linh hoạt, độ xác cao , chúng có khả dịch chuyển quay đồng thời độc lập, robot di động đa hướng thu hút nhiều ý Luận văn trình bày thuật toán điều khiển robot di động đa hướng bám theo quỹ đạo mong muốn phương pháp điều khiển trượt sau: Đầu tiên, mơ hình động học robot di động đa hướng bánh trình bày mơ hình động lực học với tác động lực ma sát trượt suy từ định luật thứ hai Newton Thứ hai, điều khiển bám kết hợp điều khiển động học với điều khiển động lực học kiểu trượt tích phân thực để bám theo quỹ đạo mong muốn Một véc tơ sai số bám định nghĩa điều khiển động học thiết kế để đưa véc tơ sai số bám tiệm cận không Một véc tơ mặt trượt định nghĩa dựa véc tơ sai số vận tốc bám tích phân Bộ điều khiển động lực học kiểu trượt tích phân thiết kế để đưa véc tơ mặt trượt tích phân véc tơ sai số vận tốc bám tiệm cận không Độ ổn định hệ thống đảm bảo tiêu chuẩn ổn định Lyapunov Kết mô phần mềm Matlab kèm theo để hiển thị chứng minh tính khả thi thuật tốn đề xuất Từ khóa: Robot di động, robot di động đa hướng, điều khiển trượt tích phân v ABSTRACT Different from many mobile robot with conventional regular wheels, Mobile Robot with driving omnidirection wheels (Omnidirection Mobile Robot) has some advantages such as: changing position ability and flexible navigation, high accuracy , because they have the ability to move simultaneously and independently in translation and rotation So the omnidirectional mobile robot have attracted much more attention This thesis presented about controling algorithm of omnidirection mobile robot to track a desired trajectory by means of sliding control as follows: First, a kinematic modeling of the omnidirection mobile robot with three wheels is presented, and a dynamic modeling of the omnidirection mobile robot with disturbance and friction is derived based on the Newton’s second law of motion Second, a tracking controller that integrates a kinematic controller with an integral sliding mode dynamic controller of the omnidirection mobile robot with disturbance and friction is designed to track a desired trajectory A tracking error vector is defined, and a kinematic controller is designed to make the tracking error vector go to zero asymptotically An integral sliding surface vector is defined based on the velocity tracking error vector and its integral term An integral sliding mode dynamic controller is designed to make the integral siding surface vector and the tracking velocity error vector go to zero asymptotically Stability of the system is guaranteed by Lyapunov stability theory Simulation results using Matlab software is included to display and prove the feasibility of the proposed algorithm Keywords: Mobile robot, omnidirection mobile robot, integral sliding mode controller vi MỤC LỤC TRANG Trang tựa Quyết định giao đề tài Lý lịch cá nhân ii Lời cam đoan iii Cảm tạ iv Tóm tắt v Mục lục vii Danh sách chữ viết tắt ix Danh sách hình x Danh sách bảng xiii Chƣơng Tổng quan 1.1 Tổng quan robot 1.1.1 Lịch sử đời phát triển 1.1.2 Các hệ robot 1.1.3 Robot di động 1.1.4 Những xu hướng phát triển robot đại 1.1.5 Robot đa hướng 1.1.6 Các kết nghiên cứu robot đa hướng công bố 1.2 Mục tiêu khách thể đối tượng nghiên cứu 10 1.2.1 Mục tiêu 10 1.2.2 Đối tượng nghiên cứu 12 1.3 Nhiệm vụ đề tài phạm vi nghiên cứu 12 1.3.1 Nhiệm vụ đề tài 12 1.3.2 Phạm vi nghiên cứu 12 1.4 Phương pháp nghiên cứu 12 vii 1.5 Ý nghĩa khoa học thực tiễn đề tài 12 1.5.1 Ý nghĩa khoa học 12 1.5.2 Ý nghĩa thực tiễn 13 1.5 Kế hoạch thực 13 Chƣơng Cơ sở lý thuyết 14 2.1 Cơ sở lý thuyết ổn định Lyapunov 14 2.1.1 Hệ phi tuyến điểm cân 14 2.1.2 Khái niệm ổn định 16 2.1.3 Phương pháp trực tiếp Lyapunov 20 2.1.4 Tiêu chuẩn Lyapunov phục vụ thiết kế điều khiển 24 2.2 Các hệ thống Điều khiển Robot 25 2.2.1 Điều khiển theo quỹ đạo đặt 26 2.2.2 Các hệ thống điều khiển hệ tuyến tính 27 2.2.3 Các hệ thống điều khiển hệ phi tuyến 28 2.2.4 Các phương pháp điều khiển Robot 32 2.3 Kết luận 45 Chƣơng Mơ hình tốn học robot di động đa hƣớng 3.1 Cấu trúc hình học giả thiết 47 47 3.1.1 Cấu trúc hình học robot di động đa hướng 47 3.1.2 Các giả thiết cho mơ hình nghiên cứu 48 3.2 Mơ hình tốn học robot di động đa hướng 48 3.2.1 Mơ hình động học 48 3.2.2 Mơ hình động lực học 50 Chƣơng Điều khiển chuyển động OMR sử dụng phƣơng pháp điều khiển trƣợt 53 4.1 Giới thiệu 53 4.2 Bộ điều khiển trượt tích phân (ISMC) thiết kế cho OMR 53 4.3 Kết luận 58 viii Chƣơng Mô 59 5.1 Đặt vấn đề 59 5.2 Kết mô 59 5.2.1 Kết mơ với điều khiển có thông số thứ (I) 59 5.2.2 Kết mô với điều khiển có thơng số thứ hai (II) 68 5.3 Nhận xét 75 Chƣơng Kết luận hƣớng phát triển 76 6.1 Kết luận 76 6.2 Hướng phát triển đề tài 76 Tài liệu tham khảo 77 Phụ lục A Chứng minh phương trình (3.8) 79 Phụ lục B Chứng minh phương trình (3.13) 82 Phụ lục C Chứng minh phương trình (3.14) 84 Phụ lục D Chứng minh phương trình (4.3) (4.16) 88 Phụ lục E Chương trình mơ m.file 89 ix DANH SÁCH CÁC CHỮ VIẾT TẮT IR: Industrial Robot – robot công nghiệp PLC: Programmable Logic Controller – điều khiển logic lập trình PI: Tỷ lệ - tích phân PD: Tỷ lệ - vi phân PID: Tỷ lệ - vi phân - tích phân CCĐK: Cơ cấu điều khiển ĐTĐK: Đối tượng điều khiển OMR: Omnidirectional Mobile Robot – robot di động đa hướng KC: Kinematic Controller – điều khiển động học ISMC: Integral Sliding Mode Controller – điều khiển trượt tích phân x DANH SÁCH CÁC HÌNH HÌNH TRANG Hình 1.1 Một số hình ảnh robot di động Hình 1.2 Một số dạng robot đa hướng Hình 2.1 Điểm gốc O điểm ổn định (1) điểm không ổn định (2) 17 Hình 2.2 Trạng thái hội tụ khơng ổn định 17 Hình 2.3 Điểm gốc O điểm ổn định tiệm cận 18 Hình 2.4 Trạng thái hội tụ khơng ổn định 19 Hình 2.5 Sự phân kỳ trạng thái chuyển động dọc theo đường lượng thấp 22 Hình 2.6 Sự hội tụ đến tập bất biến lớn M 23 Hình 2.7 Ứng dụng tiêu chuẩn Lyapunov để thiết kế điều khiển 24 Hình 2.8 Sơ đồ khối điều khiển vị trí Robot 25 Hình 2.9 Ổn định hệ phi tuyến 29 Hình 1.10 Điều khiển tuyến tính hình thức điều khiển phản hồi trạng thái 30 Hình 2.11 Thiết kế điều khiển 31 Hình 2.12 Bù phi tuyến 32 Hình 2.13 Sơ đồ cấu trúc hệ điều khiển động lực học ngược 34 Hình 2.14 Hệ thống điều khiển thích nghi theo sai lệch 36 Hình 2.15 Hệ thống điều khiển thích nghi theo mơ hình chuẩn 37 Hình 2.16 Sơ đồ khối tổng qt hệ thích nghi 38 Hình 2.17 Đối tượng điều khiển rơle vị trí 39 Hình 2.18 Quỹ đạo pha với đường chuyển đổi e = 40 xi Hình 2.19 Hệ trượt với luật chuyển đổi mạch phản hồi 41 Hình 2.20 Mạch điều khiển với phản hồi nội 41 Hình 2.21 Quỹ đạo pha với đường chuyển đổi S = – (y1+y2) = 42 Hình 2.22 Sơ đồ nguyên lý điều khiển kiểu trượt 44 Hình 3.1 Cấu trúc hình học OMR 47 Hình 4.1 Định nghĩa véc tơ sai số bám 54 Hình 4.2 Sơ đồ khối cho ISMC 58 Hình 5.1 Quỹ đạo bám mơ 61 Hình 5.2 Véc tơ điều khiển mơ men ngõ vào  (I) 62 Hình 5.3 Véc tơ sai số bám ep thời điểm ban đầu (I) 62 Hình 5.4 Véc tơ sai số bám ep tồn thời gian (I) 63 Hình 5.5 Véc tơ mặt trượt Sv (I) 63 Hình 5.6 Véc tơ sai số vận tốc bám ev thời điểm ban đầu (I) 64 Hình 5.7 Véc tơ sai số vận tốc ev bám tồn thời gian (I) 64 Hình 5.8 Vận tốc góc 1, 2, 3 bánh xe (I) 65 Hình 5.9 Vận tốc tuyến tính vC OMR (I) 65 Hình 5.10 Vận tốc góc C OMR (I) 66 Hình 5.11 Chuyển động OMR thời gian bắt đầu (I) 66 Hình 5.12 Chuyển động OMR toàn thời gian (I) 67 Hình 5.13 Quỹ đạo đường elip có r1 = 0,3 m; r2 = 0,8 m (I) 67 Hình 5.14 Quỹ đạo đường cong có dạng chữ Ω (I) 68 Hình 5.15 Véc tơ điều khiển mơ men ngõ vào  (II) 69 Hình 5.16 Véc tơ sai số bám ep thời điểm ban đầu (II) 69 Hình 5.17 Véc tơ sai số bám ep toàn thời gian (II) 70 Hình 5.18 Véc tơ mặt trượt Sv (II) 70 xii axis equal; xlabel('X coordinate [m]');ylabel('Y coordinate [m]'); xr=[xr1,xr2,xr3,xr4]; yr=[yr1,yr2,yr3,yr4]; phir=[phr1,phr2,phr3,phr4]; wr=[wr1,wr2,wr3,wr4]; % Tinh dao ham cua xr, yr, phir,wr n=n1+n2+n3+n4; dxr(1)=0; dyr(1)=0; dphir(1)=0; dwr(1)=0; d2xr(1)=0; d2yr(1)=0; for i=2:n % dao ham bac mot dxr(i)=(xr(i)-xr(i-1))/dt; dyr(i)=(yr(i)-yr(i-1))/dt; dphir(i)=(phir(i)-phir(i-1))/dt; dwr(i)=(wr(i)-wr(i-1))/dt; % dao ham bac hai d2xr(i)=(dxr(i)-dxr(i-1))/dt; d2yr(i)=(dyr(i)-dyr(i-1))/dt; end; vr=Vr; save ellipRef xr yr phir dxr dyr wr vr n E.3 M.file tạo quỹ đạo chuẩn đƣờng cong có dạng chữ Ω %********************************************* % m.file tao quy dao chuan hinh omega %********************************************* close all; clear all; dt=0.01; Vr=0.012; Rr=0.3; Ri=0.15; %===================================================== % cac diem chuan x1=-2*Rr; y1=0; x2=x1+Rr; y2=Rr; x3=0; y3=y2+Rr; x4=x3+Rr; y4=y3-Rr; x5=x4+Rr; y5=0; % Doan xr1(1)=x1; yr1(1)=y1; phr1(1)=0; 93 wr1(1)=Vr/Rr; n1=round(pi*Rr/(2*Vr*dt))+1; for i=2:n1 if i==2 i12=0; end; xr1(i)=x1+Rr*sin(Vr*i12*dt/Rr); yr1(i)=y1+Rr*(1-cos(Vr*i12*dt/Rr)); phr1(i)=0+Vr*i12*dt/Rr; wr1(i)=Vr/Rr; i12=i12+1; end; % Doan xr2(1)=x2; yr2(1)=y2; phr2(1)=pi/2; wr2(1)=Vr/Rr; n2=n1; for i=2:n2 if i==2 i23=0; end; xr2(i)=x2+Rr*(1-cos(Vr*i23*dt/Rr)); yr2(i)=y2+Rr*sin(Vr*i23*dt/Rr); phr2(i)=pi/2-Vr*i23*dt/Rr; wr2(i)=Vr/Rr; i23=i23+1; end; % Doan xr3(1)=x3; yr3(1)=y3; phr3(1)=2*pi; wr3(1)=Vr/Rr; n3=n1; for i=2:n3 if i==2 i34=0; end; xr3(i)=x3+Rr*(sin(Vr*i34*dt/Rr)); yr3(i)=y3-Rr*(1-cos(Vr*i34*dt/Rr)); phr3(i)=2*pi-Vr*i34*dt/Rr; wr3(i)=Vr/Rr; i34=i34+1; end; % Doan xr4(1)=x4; yr4(1)=y4; phr4(1)=0; wr4(1)=Vr/Rr; n4=n1; for i=2:n4 if i==2 i45=0; end; 94 xr4(i)=x4+Rr*(1-cos(Vr*i45*dt/Rr)); yr4(i)=y4-Rr*(sin(Vr*i45*dt/Rr)); phr4(i)=-pi+Vr*i45*dt/Rr; wr4(i)=Vr/Rr; i45=i45+1; end; %======================================================= % Ve quy dao %======================================================= figure; plot([xr1,xr2,xr3,xr4],[yr1,yr2,yr3,yr4]); hold on; plot(x1,y1,'k*',x2,y2,'g*',x3,y3,'b*',x4,y4,'r*',x5,y5,'y*'); axis equal; xlabel('X coordinate [m]');ylabel('Y coordinate [m]'); xr=[xr1,xr2,xr3,xr4]; yr=[yr1,yr2,yr3,yr4]; phir=[phr1,phr2,phr3,phr4]; wr=[wr1,wr2,wr3,wr4]; %================================================== % Tinh dao ham cua xr, yr, phir,wr %================================================== n=n1+n2+n3+n4; dxr(1)=0; dyr(1)=0; dphir(1)=0; dwr(1)=0; d2xr(1)=0; d2yr(1)=0; for i=2:n % dao ham bac mot dxr(i)=(xr(i)-xr(i-1))/dt; dyr(i)=(yr(i)-yr(i-1))/dt; dphir(i)=(phir(i)-phir(i-1))/dt; dwr(i)=(wr(i)-wr(i-1))/dt; % dao ham bac hai d2xr(i)=(dxr(i)-dxr(i-1))/dt; d2yr(i)=(dyr(i)-dyr(i-1))/dt; end; vr=Vr; save omega_Ref xr yr phir dxr dyr wr vr n E.4 M.file mô điều khiển trƣợt tích phân bám đƣờng trịn với thông số thứ %************************************* % DIEU KHIEN TRUOT TICH PHAN %************************************* close all clear all 95 %************************************* % thong so bo dieu khien Qtemp=100; Ptemp=5; ktemp=30; Delta_temp=0.3; ctemp=1.5; fMtemp=2; fAtemp=1.5; dt=0.01; Q1=Qtemp; Q2=Qtemp; Q3=Qtemp; Q=[Q1 0;0 Q2 0;0 Q3]; P1=Ptemp; P2=Ptemp; P3=Ptemp; P=[P1 0;0 P2 0;0 P3]; Delta1=Delta_temp; Delta2=Delta_temp; Delta3=Delta_temp; k1=ktemp; k2=ktemp; k3=ktemp; K=diag([k1,k2,k3]); c1=ctemp; c2=ctemp; c3=ctemp; C=diag([c1,c2,c3]); load CircleRef xr yr phir dxr dyr wr vr n %**************************************** % Main Program %**************************************** % thong so cua OMR L=0.18; r=0.04; m=4.5; J=0.12; % cac ma tran M=[m 0;0 m 0;0 J]; % vi tri OMR luc dau x(1)=0.32; y(1)=0; phi(1)=101*pi/180; % van toc OMR luc dau dx(1)=0; dy(1)=0; dphi(1)=0; v(1)=0; w(1)=0; dq=[dx(1);dy(1);w(1)]; dqr=[dxr(1);dyr(1);wr(1)]; % bien cua nhieu fM1=fMtemp;fM2=fMtemp;fM3=fMtemp; fA1=fAtemp;fA2=fAtemp;fA3=fAtemp; 96 % gia toc OMR luc dau d2x(1)=0; d2y(1)=0; dw(1)=0; % matran H^-1 luc dau H_inv = [-sin(phi(1)) cos(phi(1)) L -sin((pi/3)-phi(1)) -cos((pi/3)-phi(1)) L sin((pi/3)+phi(1)) -cos((pi/3)+phi(1)) L]; H_inv_dot= [-w(1)*cos(phi(1)) -w(1)*sin(phi(1)) w(1)*cos((pi/3)-phi(1)) -w(1)*sin((pi/3)-phi(1)) w(1)*cos((pi/3)+phi(1)) w(1)*sin((pi/3)+phi(1)) H=inv(H_inv); % tinh z z=(1/r)*H_inv*dq; % cac sai so luc dau e1(1)=x(1)-xr(1); e2(1)=y(1)-yr(1); e3(1)=phi(1)-phir(1); e=[e1(1);e2(1);e3(1)]; % dao ham cac sai so luc dau de1(1)=dx(1)-dxr(1); de2(1)=dy(1)-dyr(1); de3(1)=w(1)-wr(1); % tin hieu van toc zd=(1/r)*H_inv*(-K*e+dqr); wd1(1)=zd(1); wd2(1)=zd(2); wd3(1)=zd(3); dwd1(1)=0; dwd2(1)=0; dwd3(1)=0; dzd=[dwd1(1);dwd2(1);dwd3(1)]; % sai so van toc ev=z-zd; ev1(1)=ev(1); ev2(1)=ev(2); ev3(1)=ev(3); % mat phang truot luc dau iev1(1)=0; iev2(1)=0; iev3(1)=0; iev=[iev1(1);iev2(1);iev3(1)]; S=ev+C*iev; 97 0 0]; S1(1)=S(1); S2(1)=S(2); S3(1)=S(3); %signS=[sign(S1(1));sign(S2(1));sign(S3(1))]; satS=[sign(S1(1)/Delta1);sign(S2(1)/Delta2);sign(S3(1)/Delta3)]; % nhieu ngau nhien luc dau f1d(1)=-fM1*sin(phi(1))-fM2*sin(pi/3phi(1))+fM3*sin(pi/3+phi(1))+fA1*cos(phi(1))+fA2*cos(2*pi/3+phi(1))+fA3*c os(4*pi/3+phi(1)); f2d(1)=fM1*cos(phi(1))-fM2*cos(pi/3-phi(1))fM3*cos(pi/3+phi(1))+fA1*sin(phi(1))+fA2*sin(2*pi/3+phi(1))+fA3*sin(4*pi/ 3+phi(1)); f3d(1)=L*(fM1+fM2-fM3); fd=[f1d(1);f2d(1);f3d(1)]; % tin hieu dieu khien luc dau tt=-r*H'; tt=tt*(r*M*H*(Q*S+P*satS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; %tt=tt*(r*M*H*(Q*S+P*signS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; tt1(1)=tt(1); tt2(1)=tt(2); tt3(1)=tt(3); % van toc banh xe luc dau w1(1)=z(1);w2(1)=z(2); w3(1)=z(3); % Main Loop for i=2:n t(i)=(i-1)*dt; % tinh gia toc cua xe d2q=M^-1*H_inv'*tt; d2x(i)=d2q(1); d2y(i)=d2q(2); dw(i)=d2q(3); % tinh van toc cua xe dx(i)=dx(i-1)+ d2x(i)*dt; dy(i)=dy(i-1)+ d2y(i)*dt; w(i)=w(i-1)+ dw(i)*dt; dq=[dx(i);dy(i);w(i)]; dqr=[dxr(i);dyr(i);wr(i)]; % tinh vi tri cua xe x(i)=x(i-1)+ dx(i)*dt; y(i)=y(i-1)+ dy(i)*dt; phi(i)=phi(i-1)+ w(i)*dt; 98 % cac sai so e1(i)=x(i)-xr(i); e2(i)=y(i)-yr(i); e3(i)=phi(i)-phir(i); e=[e1(i);e2(i);e3(i)]; % % % % dao ham cac sai so de1(i)=(e1(i)-e1(i-1))/dt; de2(i)=(e2(i)-e2(i-1))/dt; de3(i)=(e3(i)-e3(i-1))/dt; de1(i)=dx(i)-dxr(i); de2(i)=dy(i)-dyr(i); de3(i)=w(i)-wr(i); % matran H H_inv= [-sin(phi(i)) -sin((pi/3)-phi(i)) sin((pi/3)+phi(i)) cos(phi(i)) -cos((pi/3)-phi(i)) -cos((pi/3)+phi(i)) H_inv_dot= [-w(i)*cos(phi(i)) -w(i)*sin(phi(i)) w(i)*cos((pi/3)-phi(i)) -w(i)*sin((pi/3)-phi(i)) w(i)*cos((pi/3)+phi(i)) w(i)*sin((pi/3)+phi(i)) H=inv(H_inv); % tinh z z=(1/r)*H_inv*dq; % zd zd=(1/r)*H_inv*(-K*e+dqr); wd1(i)=zd(1); wd2(i)=zd(2); wd3(i)=zd(3); dwd1(i)=(wd1(i)-wd1(i-1))/dt; dwd2(i)=(wd2(i)-wd2(i-1))/dt; dwd3(i)=(wd3(i)-wd3(i-1))/dt; dzd=[dwd1(i);dwd2(i);dwd3(i)]; % sai so van toc ev=z-zd; ev1(i)=ev(1); ev2(i)=ev(2); ev3(i)=ev(3); % mat phang truot iev1(i)=iev1(i-1)+ev1(i)*dt; iev2(i)=iev2(i-1)+ev2(i)*dt; iev3(i)=iev3(i-1)+ev3(i)*dt; iev=[iev1(i);iev2(i);iev3(i)]; S=ev+C*iev; S1(i)=S(1); S2(i)=S(2); S3(i)=S(3); 99 L L L]; 0 0]; %signS=[sign(S1(1));sign(S2(1));sign(S3(1))]; satS=[sign(S1(i)/Delta1);sign(S2(i)/Delta2);sign(S3(i)/Delta3)]; % nhieu ngau nhien tac dong f1d(i)=-fM1*sin(phi(i))-fM2*sin(pi/3phi(i))+fM3*sin(pi/3+phi(i))+fA1*cos(phi(i))+fA2*cos(2*pi/3+phi(i))+fA3*c os(4*pi/3+phi(i)); f2d(i)=fM1*cos(phi(i))-fM2*cos(pi/3-phi(i))fM3*cos(pi/3+phi(i))+fA1*sin(phi(i))+fA2*sin(2*pi/3+phi(i))+fA3*sin(4*pi/ 3+phi(i)); f3d(i)=L*(fM1+fM2+fM3); fd=[f1d(i);f2d(i);f3d(i)]; % tin hieu dieu khien tt=-r*H'; tt=tt*(r*M*H*(Q*S+P*satS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; %tt=tt*(r*M*H*(Q*S+P*signS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; tt1(i)=tt(1); tt2(i)=tt(2); tt3(i)=tt(3); % z w1(i)=z(1); w2(i)=z(2); w3(i)=z(3); % V v(i)=sqrt(dx(i)*dx(i)+dy(i)*dy(i)); end; %*************************************** % KET QUA MO PHONG %*************************************** figure; plot(t,e1*1000,'r-',t,e2*1000,'k-',t,e3*180/pi,'b-'); xlabel('Time (s)');ylabel('Tracking error vector ep'); legend('e1','e2','e3',1); figure; plot(t,ev1*180/pi,'r-',t,ev2*180/pi,'k-',t,ev3*180/pi,'b-'); xlabel('Time (s)');ylabel('Velocity tracking error vector ev'); legend('ev1','ev2','ev3',1); figure; plot(t,w1,'r-',t,w2,'k-',t,w3,'b-'); xlabel('Time (s)');ylabel('Angular velocities of three wheels [rad/s]'); legend('w1','w2','w3',1); figure; plot(xr,yr,'g.',x,y,'r-');xlabel('X coordinate [m]');ylabel('Y coordinate [m]'); title('Movement of omnidirectional mobile robot'); axis equal; figure; plot(t,v,'r-'); 100 xlabel('Time (s)');ylabel('Linear velocity of OMR [m/s]'); figure; plot(t,w,'b-'); xlabel('Time (s)');ylabel('Angular velocity of OMR [rad/s]'); figure; plot(t,S1,'r-',t,S2,'k-',t,S3,'b-'); xlabel('Time(s)');ylabel('Sliding surface vector S [rad/s]'); legend('S1','S2','S3',1); figure; plot(t,tt1,'r-',t,tt2,'k-',t,tt3,'b-'); xlabel('Time(s)');ylabel('Torque input control vector [N.m]'); legend('tt1','tt2','tt3',1); E.5 M.file mô điều khiển trƣợt tích phân bám đƣờng trịn với thơng số thứ hai %************************************* % DIEU KHIEN TRUOT TICH PHAN %************************************* close all clear all %************************************* % thong so bo dieu khien Qtemp=100; Ptemp=5; ktemp=30; Delta_temp=0.3; ctemp=1.5; fMtemp=2; fAtemp=1.5; dt=0.01; Q1=Qtemp; Q2=Qtemp; Q3=Qtemp; Q=[Q1 0;0 Q2 0;0 Q3]; P1=Ptemp; P2=Ptemp; P3=Ptemp; P=[P1 0;0 P2 0;0 P3]; Delta1=Delta_temp; Delta2=Delta_temp; Delta3=Delta_temp; k1=ktemp; k2=ktemp; k3=ktemp; K=diag([k1,k2,k3]); c1=ctemp; c2=ctemp; c3=ctemp; C=diag([c1,c2,c3]); load CircleRef xr yr phir dxr dyr wr vr n %**************************************** % Main Program %**************************************** % thong so cua OMR L=0.18; r=0.04; m=4.5; 101 J=0.12; % cac ma tran M=[m 0;0 m 0;0 J]; % vi tri OMR luc dau x(1)=0.32; y(1)=0; phi(1)=101*pi/180; % van toc OMR luc dau dx(1)=0; dy(1)=0; dphi(1)=0; v(1)=0; w(1)=0; dq=[dx(1);dy(1);w(1)]; dqr=[dxr(1);dyr(1);wr(1)]; % bien cua nhieu fM1=fMtemp;fM2=fMtemp;fM3=fMtemp; fA1=fAtemp;fA2=fAtemp;fA3=fAtemp; % gia toc OMR luc dau d2x(1)=0; d2y(1)=0; dw(1)=0; % matran H^-1 luc dau H_inv = [-sin(phi(1)) cos(phi(1)) L -sin((pi/3)-phi(1)) -cos((pi/3)-phi(1)) L sin((pi/3)+phi(1)) -cos((pi/3)+phi(1)) L]; H_inv_dot= [-w(1)*cos(phi(1)) -w(1)*sin(phi(1)) w(1)*cos((pi/3)-phi(1)) -w(1)*sin((pi/3)-phi(1)) w(1)*cos((pi/3)+phi(1)) w(1)*sin((pi/3)+phi(1)) H=inv(H_inv); % tinh z z=(1/r)*H_inv*dq; % cac sai so luc dau e1(1)=x(1)-xr(1); e2(1)=y(1)-yr(1); e3(1)=phi(1)-phir(1); e=[e1(1);e2(1);e3(1)]; % dao ham cac sai so luc dau % de1(1)=0; %de2(1)=0; %de3(1)=0; de1(1)=dx(1)-dxr(1); de2(1)=dy(1)-dyr(1); de3(1)=w(1)-wr(1); 102 0 0]; % tin hieu van toc zd=(1/r)*H_inv*(-K*e+dqr); wd1(1)=zd(1); wd2(1)=zd(2); wd3(1)=zd(3); dwd1(1)=0; dwd2(1)=0; dwd3(1)=0; dzd=[dwd1(1);dwd2(1);dwd3(1)]; % sai so van toc ev=z-zd; ev1(1)=ev(1); ev2(1)=ev(2); ev3(1)=ev(3); % mat phang truot luc dau iev1(1)=0; iev2(1)=0; iev3(1)=0; iev=[iev1(1);iev2(1);iev3(1)]; S=ev+C*iev; S1(1)=S(1); S2(1)=S(2); S3(1)=S(3); %signS=[sign(S1(1));sign(S2(1));sign(S3(1))]; satS=[sign(S1(1)/Delta1);sign(S2(1)/Delta2);sign(S3(1)/Delta3)]; % nhieu ngau nhien luc dau f1d(1)=-fM1*sin(phi(1))-fM2*sin(pi/3phi(1))+fM3*sin(pi/3+phi(1))+fA1*cos(phi(1))+fA2*cos(2*pi/3+phi(1))+fA3*c os(4*pi/3+phi(1)); f2d(1)=fM1*cos(phi(1))-fM2*cos(pi/3-phi(1))fM3*cos(pi/3+phi(1))+fA1*sin(phi(1))+fA2*sin(2*pi/3+phi(1))+fA3*sin(4*pi/ 3+phi(1)); f3d(1)=L*(fM1+fM2-fM3); fd=[f1d(1);f2d(1);f3d(1)]; % tin hieu dieu khien luc dau tt=-r*H'; tt=tt*(r*M*H*(Q*S+P*satS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; %tt=tt*(r*M*H*(Q*S+P*signS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; tt1(1)=tt(1); tt2(1)=tt(2); tt3(1)=tt(3); % van toc banh xe luc dau w1(1)=z(1);w2(1)=z(2); w3(1)=z(3); 103 % Main Loop for i=2:n t(i)=(i-1)*dt; % tinh gia toc cua xe d2q=M^-1*H_inv'*tt; d2x(i)=d2q(1); d2y(i)=d2q(2); dw(i)=d2q(3); % tinh van toc cua xe dx(i)=dx(i-1)+ d2x(i)*dt; dy(i)=dy(i-1)+ d2y(i)*dt; w(i)=w(i-1)+ dw(i)*dt; dq=[dx(i);dy(i);w(i)]; dqr=[dxr(i);dyr(i);wr(i)]; % tinh vi tri cua xe x(i)=x(i-1)+ dx(i)*dt; y(i)=y(i-1)+ dy(i)*dt; phi(i)=phi(i-1)+ w(i)*dt; % cac sai so e1(i)=x(i)-xr(i); e2(i)=y(i)-yr(i); e3(i)=phi(i)-phir(i); e=[e1(i);e2(i);e3(i)]; % dao ham cac sai so % de1(i)=(e1(i)-e1(i-1))/dt; % de2(i)=(e2(i)-e2(i-1))/dt; % de3(i)=(e3(i)-e3(i-1))/dt; de1(i)=dx(i)-dxr(i); de2(i)=dy(i)-dyr(i); de3(i)=w(i)-wr(i); % matran H H_inv= [-sin(phi(i)) -sin((pi/3)-phi(i)) sin((pi/3)+phi(i)) cos(phi(i)) -cos((pi/3)-phi(i)) -cos((pi/3)+phi(i)) H_inv_dot= [-w(i)*cos(phi(i)) -w(i)*sin(phi(i)) w(i)*cos((pi/3)-phi(i)) -w(i)*sin((pi/3)-phi(i)) w(i)*cos((pi/3)+phi(i)) w(i)*sin((pi/3)+phi(i)) H=inv(H_inv); % tinh z z=(1/r)*H_inv*dq; % zd zd=(1/r)*H_inv*(-K*e+dqr); wd1(i)=zd(1); wd2(i)=zd(2); wd3(i)=zd(3); dwd1(i)=(wd1(i)-wd1(i-1))/dt; 104 L L L]; 0 0]; dwd2(i)=(wd2(i)-wd2(i-1))/dt; dwd3(i)=(wd3(i)-wd3(i-1))/dt; dzd=[dwd1(i);dwd2(i);dwd3(i)]; % sai so van toc ev=z-zd; ev1(i)=ev(1); ev2(i)=ev(2); ev3(i)=ev(3); % mat phang truot iev1(i)=iev1(i-1)+ev1(i)*dt; iev2(i)=iev2(i-1)+ev2(i)*dt; iev3(i)=iev3(i-1)+ev3(i)*dt; iev=[iev1(i);iev2(i);iev3(i)]; S=ev+C*iev; S1(i)=S(1); S2(i)=S(2); S3(i)=S(3); %signS=[sign(S1(1));sign(S2(1));sign(S3(1))]; satS=[sign(S1(i)/Delta1);sign(S2(i)/Delta2);sign(S3(i)/Delta3)]; % nhieu ngau nhien tac dong f1d(i)=-fM1*sin(phi(i))-fM2*sin(pi/3phi(i))+fM3*sin(pi/3+phi(i))+fA1*cos(phi(i))+fA2*cos(2*pi/3+phi(i))+fA3*c os(4*pi/3+phi(i)); f2d(i)=fM1*cos(phi(i))-fM2*cos(pi/3-phi(i))fM3*cos(pi/3+phi(i))+fA1*sin(phi(i))+fA2*sin(2*pi/3+phi(i))+fA3*sin(4*pi/ 3+phi(i)); f3d(i)=L*(fM1+fM2+fM3); fd=[f1d(i);f2d(i);f3d(i)]; % tin hieu dieu khien tt=-r*H'; tt=tt*(r*M*H*(Q*S+P*satS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; %tt=tt*(r*M*H*(Q*S+P*signS+(H_inv_dot+C*H_inv)*H*z-dzd-C*zd))+r*H'*fd; tt1(i)=tt(1); tt2(i)=tt(2); tt3(i)=tt(3); % z w1(i)=z(1); w2(i)=z(2); w3(i)=z(3); % V v(i)=sqrt(dx(i)*dx(i)+dy(i)*dy(i)); end; 105 %*************************************** % KET QUA MO PHONG %*************************************** figure; plot(t,e1*1000,'r-',t,e2*1000,'k-',t,e3*180/pi,'b-'); xlabel('Time (s)');ylabel('Tracking error vector ep'); legend('e1','e2','e3',1); figure; plot(t,ev1*180/pi,'r-',t,ev2*180/pi,'k-',t,ev3*180/pi,'b-'); xlabel('Time (s)');ylabel('Velocity tracking error vector ev'); legend('ev1','ev2','ev3',1); figure; plot(t,w1,'r-',t,w2,'k-',t,w3,'b-'); xlabel('Time (s)');ylabel('Angular velocities of three wheels [rad/s]'); legend('w1','w2','w3',1); figure; plot(xr,yr,'g.',x,y,'r-');xlabel('X coordinate [m]');ylabel('Y coordinate [m]'); title('Movement of omnidirectional mobile robot'); axis equal; figure; plot(t,v,'r-'); xlabel('Time (s)');ylabel('Linear velocity of OMR [m/s]'); figure; plot(t,w,'b-'); xlabel('Time (s)');ylabel('Angular velocity of OMR [rad/s]'); figure; plot(t,S1,'r-',t,S2,'k-',t,S3,'b-'); xlabel('Time(s)');ylabel('Sliding surface vector S [rad/s]'); legend('S1','S2','S3',1); figure; plot(t,tt1,'r-',t,tt2,'k-',t,tt3,'b-'); xlabel('Time(s)');ylabel('Torque input control vector [N.m]'); legend('tt1','tt2','tt3',1); 106 S K L 0 ... kỹ thuật điều khiển điều khiển quỹ đạo robot đa hướng, luận văn chọn kỹ thuật điều khiển trượt để thực khảo sát Đề tài nghiên cứu có tên: ? ?Điều khiển robot đa hướng bám theo quỹ đạo dùng kỹ thuật. .. Tìm hiểu kỹ thuật điều khiển trượt - Xây dựng mơ hình tốn học cho robot đa hướng - Thiết kế điều khiển bám quỹ đạo cho robot đa hướng sử dụng kỹ thuật điều khiển trượt - Mơ thuật tốn điều khiển. .. trung nghiên cứu kỹ thuật điều khiển trượt ứng dụng kỹ thuật điều khiển trượt xây dựng điều khiển để điều khiển robot đa hướng bám theo quỹ đạo cho trước với giới hạn sau: - Robot chuyển động