I HÅC QUÈC GIA H NËI TR ÕNG I HÅC CặNG NGH Nguyạn Th Thanh VƠn NGHIN CU NNG CAO HIU QU ÀNH VÀ V DN ÕNG ROBOT DI ËNG TRONG MặI TR ếNG KHặNG BIT TR C Chuyản ng nh: K thuêt iằn t M sậ: 62 52 02 03 LUN N TIN S NGNH CỈNG NGH Kfl THUT IN TÔ, TRUYN THặNG NG ếI H NG D N KHOA HC: PGS.TS TrƯn Quang Vinh TS Lả VÙ H H NỴi - 2017 LÕI CAM OAN TÊi xin cam oan luên Ăn n y l cấng trẳnh nghiản cu ca tấi, cha ềc xuĐt bÊn tÔi bĐt k˝ nÏi n o MÂi nguÁn thÊng tin tham kh£o s dng luên Ăn Ãu ềc trẵch dăn Ưy TĂc giÊ Nguyạn Th Thanh VƠn i LếI CM ÌN Luªn ¡n n y ˜Ịc ho n th nh vểi sá gip ễ ca nhiÃu ngèi Lèi Ưu tiản, tấi xin gi lèi cÊm ẽn sƠu sc án Ph giĂo s, tián sắ TrƯn Quang Vinh v Tián sắ Lả V H , l nhng ngèi ThƯy  trác tiáp hểng dăn, hẩ trề v ẻng viản tấi quĂ trẳnh nghiản cu Tấi xin gi lèi cÊm ẽn chƠn th nh tểi Tián sắ Phng MÔnh Dẽng, ng nghiằp cng nhm nghiản cu,  c nhng hẩ trề quĂ trẳnh thác hiằn thác nghiằm v xuĐt bÊn c¡c cÊng tr¼nh cÊng bË TÊi cÙng xin g˚i lÌi cÊm ẽn chƠn th nh tểi nhng ThƯy, Cấ, ng nghiằp ca Khoa iằn t - Viạn thấng, Ôi hc Cấng nghằ, Ôi hc Quậc Gia H Nẻi  hẩ trề, tÔo iÃu kiằn v ẻng viản tấi rĐt nhiÃu thèi gian va giÊng dÔy va nghiản cu tÔi Khoa Cuậi cng, xin gi tểi nhng ngèi thƠn yảu ca gia ẳnh tấi vểi tĐm lÃng biát ẽn sƠu sc, luấn luấn hẩ trề ẻng viản tấi c thº ho n th nh ˜Ịc luªn ¡n n y ii MữC LữC Trang ph bẳa LÌi cam oan i LÌi c£m Ïn ii Mˆc lˆc iii Danh mˆc c¡c k˛ hi»u v ch˙ vi¸t tt vi Danh mˆc c¡c b£ng xiii Danh mˆc c¡c h¼nh v³, Á th‡ xiv M– U xvii Ch˜Ïng TÊNG QUAN ROBOT DI ËNG 1.1 GiÓi thi»u 1.2 H» thËng robot di Ỵng 1.3 ‡nh v‡ 1.3.1 ‡nh v‡ t˜Ïng Ëi 1.3.2 ‡nh v‡ tuy»t Ëi 1.3.3 TÍng hỊp d˙ li»u c£m bi¸n iii 1.3.4 Nhªn x²t 1.4 Dăn ˜Ìng 1.4.1 Lêp ká hoÔch èng i 1.4.2 Tr¡nh vªt c£n 10 1.4.3 C§u trc dăn èng 10 1.4.4 C§u trÛc h nh vi 11 1.4.5 Nhªn x²t 17 1.5 K thuêt iÃu khin robot di ẻng 18 1.5.1 i·u khiºn mÌ 18 1.5.2 MÔng nẽron 21 1.5.3 Quyát nh tậi u a mc tiảu 24 1.6 Robot di Ỵng hai b¡nh vi sai 26 1.6.1 HoÔt ẻng vi sai 26 1.6.2 Mấ hẳnh ẻng hÂc thuªn 28 1.6.3 Kh£ n«ng i·u khiºn v quan s¡t 30 1.7 Tẳnh hẳnh nghiản c˘u v ngo i n˜Óc 32 1.8 Kát luên chẽng 34 Ch˜Ïng ÀNH V SÔ DữNG Bậ LC KALMAN NèRON Mế 35 2.1 GiÓi thi»u 2.2 BỴ lÂc Kalman m rỴng 2.3 BỴ lÂc Kalman NÏron MÌ 2.3.1 CÏ s i·u ch¿nh ma trªn R 2.3.2 MÔng nẽron mè i·u ch¿nh Rk(j; j) 2.4 H» thËng ‡nh v‡ robot di Îng s˚ dˆng FNN-EKF 2.4.1 MÊ h¼nh h» thËng 2.4.2 MÊ ph‰ng iv 2.4.3 Th¸c nghi»m 55 2.5 Kát luên chẽng 57 Chẽng CU TRC DN ếNG HNH VI SÔ DữNG IU KHIN MÕ V TÈI U A M÷C TIU 59 3.1 GiÓi thi»u 59 3.2 CĐu trc dăn èng h nh vi 59 3.3 CĐu trc dăn èng h nh vi BBFM 61 3.4 Hằ thậng dăn èng s dng c§u trÛc BBFM 64 3.4.1 MÊ h¼nh h» thËng 64 3.4.2 Thi¸t k¸ BBFM 65 3.5 MÊ ph‰ng ¡nh gi¡ hằ thậng dăn èng s dng cĐu trc BBFM 75 3.5.1 Chu©n b‡ mÊ ph‰ng 75 3.5.2 K¸t qu£ mÊ ph‰ng 77 3.6 Th¸c nghi»m Ănh giĂ hằ thậng dăn èng s dng cĐu trc BBFM 83 3.6.1 Chuân b thác nghiằm 83 3.6.2 K¸t qu£ th¸c nghi»m 86 3.7 Hằ thậng dăn èng kát hềp BBFM v FNN-EKF 92 3.7.1 MÊ h¼nh h» thËng 92 3.7.2 MÊ ph‰ng 93 3.7.3 Th¸c nghi»m 96 3.8 Kát luên ch˜Ïng 99 KT LUN V KIN NGHÀ DANH MữC CặNG TRNH KHOA HC CA TC GI LIN QUAN N LUN N 101 v 99 TI LIU THAM KHO 103 PH÷ L÷C 117 vi [111] V Fox, J Hightower, L Liao, and D Shulz (2003), Bayesian Filtering for Localization Estimation , V.2, I.3, Pervasive Computing IEEE, pp 24-33 115 [112] Vasko Sazdovski, Tatjana Kolemishevska-Gugulovska, Mile Stankovski (2005), Fuzzy Adaptive Extended Kalman Filter , 6th International PhD Workshop on Systems and Control, October 4-8, Izola, Slovenia [113] V.J Lumelsky, T Skewis (2002), Incorporating range sensing in the robot navigation function, IEEE Transactions on Systems, Man, and Cybernetics, V.20, I.5, pp 1058 - 1069 [114] Xi Li and Byung-Jae Choi, Design of obstacle avoidance system for mobile robot using fuzzy logic systems, International Journal of Smart Home Vol 7, No 3, May, 2013, pp 321-328 [115] Xiaoning Shen, Yu Guo, Weili Hu (2006), Mobile Robot Path Plan- ning Based on Multi-objective Evolutionary Programming , The Sixth World Congress on Intelligent Control and Automation, WCICA 2006 [116] X Mao, M Wada, H Hashimoto, M Saito, and I.Suzuki (2003), EM/Unscented Smoothing Based Parameter Learning for Nonlinear Models for GPS Positioning , 10th World Congress on Intelligent Transport Systems, Madrid [117] W.L.Xu and S.K.Tso (2000), Sensor-based fuzzy reactive navigation of a mobile robot through local target switching , IEEE Trans.Syst.,Man Cybern,.C, App.Rev.,Vol 29, No 3, pp 451-459, Aug [118] W Li and H Leung (2003), Constrained Unscented Kalman Filter Based Fusion of GPS/INS/Digital Map for Vehicle Localization , IEEE International Conference on Intelligent transportation systems, Shangai 116 PHữ LữC Mấ hẳnh robot di ẻng hai b¡nh vi sai function [x1,y1,theta1]=robot-model(vL,vR,x,y,theta) R=0.05; b=0.6; dt=0.1; dSL=dt*R*vL; dSR=dt*R*vR; dS=(dSL+dSR)/2; dTheta=(dSR-dSL)/b; x1=x+ dS*cos(theta); y1=y+ dS*sin(theta); theta1=theta+dTheta; MÔng Nẽron mÌ b1=-17.1;a1=-0.163;b2=13.8;a2=0.1765;sig=0.16;c=-0.0126;z1=-0.1; z2=0.01;z3=0.5; eta=1; t=[-0.4:0.05:1]; s=size(t); for i=1:s(2) A1(i)=1/(1+exp(-b1*(t(i)-a1))); A2(i)=1/(1+exp(-b2*(t(i)-a2))); A3(i)=exp(-((t(i)-c)2)/(2*sig2)); end load dom3.txt; load deltar3.txt; x=dom3(:,1); y=deltar3(:,1); s1=size(x); for i=1:s1(1) 117 o(i)=(A1(i)*z1+A2(i)*z2+A3(i)*z3)/(A1(i)+A2(i)+A3(i)); E(i)=0.5*(o(i)-y(i))2; diffz1=((1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))*z2+ exp(-1/2*(x(i)-c)2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+ exp(-b2*(x(i)-a2)))+exp(-1/2*(x(i)-c) 2/sig2))-y(i))/(1+exp(-b1*(x(i)-a1))) /(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i)a2)))+exp(-1/2* (x(i)-c)2/sig2)); diffz2= % DÔng giậng diffz1 diffz3= % DÔng giậng diffz1 diffa1=((1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))*z2+ exp(-1/2*(x(i)-c)22/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2 (x(i)-a2)))+exp(-1/2*(x(i)-c) 2/sig2))-y(i))*(-1/(1+exp(-b1*(x(i)-a1))) 2*z1 b1*exp(-b1*(x(i)-a1))/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i)-a2))) +exp(-1/2*(x(i)-c)2/sig2))+(1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2 (x(i)-a2)))*z2+exp(-1/2*(x(i)-c) 2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1 /(1+exp(-b2*(x(i)-a2)))+exp(-1/2*(x(i)-c) 2/sig2))2/(1+exp(-b1*(x(i)-a1)))2 b1*exp(-b1*(x(i)-a1))); diffb1= %DÔng giậng diffa1 diffa2= %DÔng giậng diffa1 diffb2= %DÔng giậng diffa1 diffsig=((1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))*z2+ exp(1/2*(x(i)-c)2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i) -a2)))+exp(-1/2*(x(i)-c)2/sig2))-y(i))*((x(i)-c)2/sig2*exp(-1/2*(x(i)-c)2/sig2)* z3/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i)-a2)))+exp(-1/2*(x(i)-c) /sig2))-(1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))*z2+exp(-1/2 (x(i)-c)2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i)-a2))) +exp(-1/2*(x(i)-c)2/sig2))2*(x(i)-c)2/sig23*exp(-1/2*(x(i)-c)2/sig2)); diffc=((1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))*z2+ exp(-1/2*(x(i)-c)2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2 (x(i)-a2)))+exp(-1/2*(x(i)-c) 2/sig2))-y(i))*((x(i)-c)/sig2*exp(-1/2*(x(i)-c)2/ sig2)*z3/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp(-b2*(x(i)-a2)))+exp(-1/2* (x(i)-c)2/sig2))-(1/(1+exp(-b1*(x(i)-a1)))*z1+1/(1+exp(-b2*(x(i)-a2)))* z2+exp(-1/2*(x(i)-c)2/sig2)*z3)/(1/(1+exp(-b1*(x(i)-a1)))+1/(1+exp 118 (-b2*(x(i)-a2)))+exp(-1/2*(x(i)-c) 2/sig2))2*(x(i)-c)/sig2*exp(-1/2* (x(i)-c)2/sig2)); z1=z1-eta*diffz1; z2=z2-eta*diffz2; z3=z3-eta*diffz3; a1=a1-eta*diffa1; b1=b1-eta*diffb1; a2=a2-eta*diffa2; b2=b2-eta*diffb2; sig=sig-eta*diffsig; c=c-eta*diffc; end Mấ hẳnh bẻ lc FNN-EKF Tong-so-diem =i; N=Tong-so-diem; for index=1:solanchay pos-true-X=zeros(N,1); pos-true-Y=zeros(N,1); pos-true-T=zeros(N,1); pos-mes-X=zeros(N,1); pos-mes-Y=zeros(N,1); pos-mes-T=zeros(N,1); pos2-est-X=zeros(N,1); pos2-est-Y=zeros(N,1); pos2-est-T=zeros(N,1); devX2=zeros(N,1); devY2=zeros(N,1); devT2=zeros(N,1); i=2; x(1)=xa; y(1)=ya; theta(1)=thetaa; 119 vL=vL2(1);vR=vR2(1); pos-true-X(1)=xa;pos-true-Y(1)=ya;pos-true-T(1)=thetaa; pos-mes-X(1)=xa; pos-mes-Y(1)=ya; pos-mes-T(1)=thetaa; pos2-est-X(1)=pos-mes-X(1); pos2-est-Y(1)=pos-mes-Y(1); pos2-est-T(1)=pos-mes-T(1); dev2X(1)=pos2-est-X(1)-pos-true-X(1); dev2Y(1)=pos2-est-Y(1)-pos-true-Y(1); dev2T(1)=pos2-est-T(1)-pos-true-T(1); R=[0.01 0; 0.01 0; 0 0.018]; P=R; N=1000; Var-ONoise-x = R(1,1); Std-ONoisex = sqrt(Var-ONoise-x)'; ONoise-X = Std-ONoise-x* randn(1,N+1); VarONoise-y = [R(2,2)]; Std-ONoise-y = sqrt(Var-ONoise-y)'; ONoise-Y = Std-ONoise-y* randn(1,N+1); VarONoise-t = [R(3,3)]; Std-ONoise-t = sqrt(Var-ONoise-t)'; ONoise-T = (Std-ONoise-t* randn(1,N+1)); resi=zeros(1,3); fuzzy1=readfis('C:FNN1.fis'); fuzzy2=readfis('C:FNN2.fis'); fuzzy3=readfis('C:FNN3.fis'); %(C¡c h m FNN1.fis, FNN2.fis v FNN3.fis l cĂc mÔng Nẽron mè) while(i0.05) u-save=[]; w-save=[]; save-u=[]; save-w=[]; save-degree-u=[]; save-degree-w=[]; temp-x=x; temp-y=y; ro=sqrt((x-target-x)2+(y-target-y)2); if theta < (180*pi/180) alpha=(atan2((y-target-y),(x-target-x)))-theta; else alpha=(atan2((y-target-y),(x-target-x)))-theta+2*pi; end (output, IRR, ORR, ARR)=evalfis([left forward right],obstacle-avoidance); m=max(ARR(:,1)); for i=1:101 temp=ARR(i,1); 124 if temp==m a=range-u(i); u-save=[u-save;a]; end end s=size(u-save); if s(1)==1 u-theory=u-save; index=1; elseif s(1)==101 ([output, IRR, ORR, ARR])=evalfis([ro alpha],goal-reaching); u-theory=defuzz(rangeu,ARR(:,1),'centroid'); else ([output, IRR, ORR, ARR])=evalfis([ro alpha],goal-reaching); save-degree-u=[]; for j=1:s(1) for i=1:101 if u-save(j)==range-u(i) a=ARR(i,1); save-degree-u=[save-degree-u;a]; break; end end end m=max(save-degree-u); s=size(save-degree-u); u-theory1=[]; save-degree-u1=[]; for i=1:s(1) temp=save-degree-u(i); if temp==m u-theory1=[u-theory1;u-save(i)]; save-degree-u1=[save-degree-u1;save-degree-u(i)]; 125 end end s=size(u-theory1); if s(1)==1 u-theory=u-theory1; else u-theory=defuzz(u-theory1,save-degree-u1,'mom'); end end ([output, IRR, ORR, ARR])=evalfis([left forward right],obstacle-avoidance); m=max(ARR(:,2)); for i=1:101 temp=ARR(i,2); if temp==m a=range-w(i); w-save=[w-save;a]; end end s=size(w-save); if s(1)==1 w-theory=w-save; elseif s(1)==101 index=1; ([output, IRR, ORR, ARR])=evalfis([ro alpha],goal-reaching); w-theory=defuzz(rangew,ARR(:,2),'centroid'); else ([output, IRR, ORR, ARR])=evalfis([ro alpha],goal-reaching); s=size(w-save); save-degree-w=[]; for j=1:s(1) for i=1:101 if w-save(j)==range-w(i) a=ARR(i,2); 126 save-degree-w=[save-degree-w;a]; break; end end end m=max(save-degree-w); s=size(save-degree-w); w-theory=[]; w-theory1=[]; save-degree-w1=[]; for i=1:s(1) temp=save-degree-w(i); if temp==m w-theory1=[w-theory1;w-save(i)]; save-degree-w1=[save-degree-w1;save-degree-w(i)]; end end s=size(w-theory1); if s(1)==1 w-theory=w-theory1; else w-theory=defuzz(w-theory1,save-degree-w1,'mom'); end end u-theory-save=[u-theory-save;u-theory]; w-theory-save=[w-theory-save;w-theory]; vL=(u-theory-(0.6/2)*w-theory)/0.05; vR=(u-theory+(0.6/2)*w-theory)/0.05; ([x,y,theta])=robot-model(vL,vR,x,y,theta); temp-dist=sqrt((temp-x-x)2+(temp-y-y)2); dist=dist+temp-dist; index1=index1+1; end 127 Truy·n thÊng gi˙a robot Sputnik v m¡y t½nh i·u khiºn robot = actxcontrol('DrRobotSDKCONTROL.DrRobotSDKControlCtrl.1'); robot.connectRobot('DrRobotMatlab'); robot.ResumeDcMotor(0); robot.ResumeDcMotor(1); Left0=robot.GetEncoderPulse1(); Right0=robot.GetEncoderPulse2(); mTimer = timer; mTimer.Period = 0.3; mTimer.TasksToExecute =85; mTimer.ExecutionMode = 'fixedRate'; mTimer.TimerFcn = @onTimer; mTimer.StopFcn = @onTimerStop, 'Robot controller stopped'; start(mTimer); function onTimer(timerObj,event,tcp-obj) % Thu thªp, x˚ l˛ d˙ li»u v i·u khiºn robot end function onTimerStop(timerObj, event, string-arg) delete(timerObj); robot.DcMotorVelocityNonTimeCtrAll(0,0,-32768, -32768, -32768, -32768); end 128 ... vĐn à liản quan tểi robot di ẻng c kát cĐu hai bĂnh vi sai mấ hẳnh kim chng mấ phng v thác nghiằm cĂc à xuĐt 1.2 Hằ thậng robot di ẻng Robot di ẻng l mẻt robot c khÊ nông di chuyn, cÊm nhên v... mơihình trườngtrường Lập kếLậphoạchkếhoạch Hành Hàđộngh động (b) Lập kếLậphoạchkếhoạch mức caomức cao Thực Thựcthiđiềuthi điều khiển khiển Cảm biếnCảm biến Phản Phảnứng ứng Hành Hàđộngh động (c)... muËn [98] SÏ Á i·u khiºn chung cıa robot di Ỵng ˜Ịc thº hi»n nh˜ Hẳnh 1.1 VĐn à iÃu khin robot di ẻng ềc phƠn th nh mc thĐp v cao iÃu khin mc thĐp liản quan án iÃu khin chuyn ẻng trác tiáp ẻng