Nghiên cứu nâng cao hiệu quả định vị và dẫn đường robot di động trong môi trường không biết trước

-I HC QUẩC GIA H NậI TRìNG -I HC CặNG NGH Nguyạn Th Thanh VƠn NGHIN CU NNG CAO HIU QU„ -ÀNH VÀ V„ D„N -×Í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 LU„N „N TIN S NGNH CặNG NGH Kò THUT -IN T, TRUYN THặNG NGìI HìẻNG DN KHOA HC: 1.PGS.TS TrƯn Quang Vinh 2.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 cựu cừa tổi, chữa ữủ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ỷ dửng luên Ăn ãu ữủc trẵch dăn Ưy TĂc giÊ Nguyạn Th Thanh VƠn i LI CM èN Luên Ăn n y ữủc ho n th nh vợi sỹ giúp ù cừa nhiãu ngữới Lới Ưu tiản, tổi xin gỷi 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 nhỳng 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 cựu Tổi xin gỷi lới cÊm ỡn chƠn th nh tợi Tián sắ Phũng MÔnh Dữỡng, ỗng nghiằp nhõm nghiản cựu,  cõ nhỳng 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 nhỳng ThƯy, Cổ, ỗng nghiằp cừa Khoa -iằn tỷ - Viạn thổng, -Ôi hồc Cổng nghằ, -Ôi hồc 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 vứa giÊng dÔy vứa nghiản cựu tÔi Khoa Cuối cũng, xin gỷi tợi nhỳng ngữới thƠn yảu cừa 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 1.3.1 -ành t÷ìng èi 1.3.2 -ành 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 trúc 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 khiºn 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 mửc 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Ë LÅC KALMAN NÌRON MÍ 35 2.1 Giỵi thi»u 35 2.2 Bë låc Kalman mð rëng 35 2.3 Bë låc Kalman Nìron Mí 37 2.3.1 Cì sð i·u ch¿nh ma trªn R 37 2.3.2 MÔng nỡron mớ iãu chnh Rk(j; j) 38 2.4 H» thèng ành robot di ëng sû döng FNN-EKF 42 2.4.1 Mỉ h¼nh h» thèng 42 2.4.2 Mæ phäng 43 iv 2.4.3 Thüc nghi»m 55 2.5 Kát luên chữỡng 57 Ch÷ìng C„U TRĨC D„N -×ÍNG H„NH VI SÛ DƯNG -I„U KHI„N MÍ V„ TÈI ×U -A MƯC TI„U 59 3.1 Giỵi thi»u 59 3.2 C§u trúc dăn ữớng h nh vi 59 3.3 CĐu trúc dăn ÷íng h nh vi BBFM 61 3.4 H» thèng dăn ữớng sỷ dửng 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ỷ dửng cĐu trúc 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ỷ dửng cĐu trúc 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 K„T LU„N V„ KI„N NGHÀ 99 DANH MƯC CỈNG TR„NH KHOA HÅC CÕA T„C GI„ LI„N QUAN -„N LU„N „N 101 v [94] Sandeep Kumar Malu and Jharna Majumdar, Kinematics, Localization and Control of Differential Drive Mobile Robot , Global Journal of Researches in Engineering: H Robotics and Nano-Tech, V.14, Issue 1, 2014 Effect of different defuzzification methods in a fuzzy based load balancing application , International Journal of Computer Science Issues, V.8, N.1, 261-267 [96] Sourav Dutta (2010), Obstacle Avoidance of mobile robot using PSO based Neuro Fuzzy Technique, (IJCSE) International Journal on Computer Science and Engineering Vol 02, No 02, pp 301-304 [97] Spyros G Tezafestas (2014), Introduction Mobile Robot Control , Elsevier, pp 1-2 [98] Siegwart Roland and Nourbakhsh Illah R (2004), Introduction to Autonomous Mobile Robots , The MIT Press Cambridge, Massachusetts London, England, pp 10, 256 [99] Spyros G Tzafestas (2013), Introduction to Mobile Robot control , National Technical University of Athens Athens, Greece, pp 27-30 [100] S N Sivanandam, S Sumathi, S N Deepa (2007), Introduction to fuzzy logic using Matlab, Springer A Gener- alized Neural Network Approach to Mobile Robot Navigation and Obstacle Avoidance, Volume 193 of the series Advances in Intelligent Systems and Computing pp 25-42 114 [102] Sandeep Kumar Malu and Jharna Majumdar, Kinematics, Localization and Control of Differential Drive Mobile Robot , Global Journal of Researches in Engineering: H Robotics and Nano-Tech, Volume 14 Issue Version 1.0 Year 2014 [103] Sheng Jin, Byung-Jae Choi (2011), Fuzzy logic system based obstacle avoid- ance for a mobile robot , Control and Automation, and Energy System En- gineering Volume 256 of the series Communications in Computer and Infor-mation Science pp 1-6 [104] Siti Hajar Ashikin Mohammad, Muhammad Akmal Jeffril, Nohaidda Sar- iff, Mobile robot obstacle avoidance by using Fuzzy Logic technique , 2013 IEEE 3rd International Conference on System Engineering and Technology (IC-SET), DOI: 10.1109/ICSEngT.2013.6650194 [105] S J Julier and J K Uhlmann (1997), A New Extension of the Kalman Filter to Nonlinear Systems, In Proc of AeroSense: The 11th Int Symp on Aerospace/Defence Sensing, Simulation and Controls [106] S Julier (1997), Process Models for the Navigation of High-Speed Land Vehicles, Wadham College, Oxford, pp 184 [107] S Engelson and D McDermott (1992), Error correction in mobile robot map learning, In ICRA-92 [108] Sebastian Thrun (2002), Particle Filters in Robotics , In Proceedings of Uncertainty in AI (UAI), pp 511-518 [109] T D Tan, L.M.Ha, N T Long, N D Duc, N P Thuy, Land-vehicle mems INS/GPS positioning during GPS signal blockage periods , VNU Journal of Science, Mathematics - Physics 23 (2007) pp 243-251 [110] Ting-Kai Wang Quan Dang Pei-Yuan Pan (2010), Path Planning Approach in Unknown Environment, International Journal of Automation and Com-puting, 7(3), pp 310-316 [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 Xiaoning Shen, Yu Guo, Weili Hu (2006), Mobile Robot Path Planning Based on Multi-objective Evolutionary Programming , The Sixth [115] 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 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, [117] App.Rev.,Vol 29, No 3, pp 451-459, Aug W Li and H Leung (2003), Constrained Unscented Kalman Filter Based Fusion of GPS/INS/Digital Map for Vehicle Localization , IEEE International [118] 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ë låc 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 khin robot end function onTimerStop(timerObj, event, string-arg) delete(timerObj); robot.DcMotorVelocityNonTimeCtrAll(0,0,-32768, -32768, -32768, -32768); end 128 ... Coupled Device Context Depedent Blending Distributed Architecture for Mobile Navigation DDMR The differential drive mobiler Robot di ëng hai b¡nh vi sai robot DIF Distributed Information Fil- Bë... vĐn ã liản quan tợi robot di ởng cõ kát cĐu hai bĂnh vi sai mỉ h¼nh kiºm chùng mỉ phäng 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 chuyºn, c£m nhên... cục Cảm nhận Mơ hình mơi trường Bản đồ cục Nhận thức Lập kế hoạch đường Đường Thực thi kế hoạch Trích thơng tin Dữ liệu thơ Hành động Cảm biến Điều khiển chuyển động Định vị Xây dựng Mụi trng thc