附件1基于slam单机器人导航、避障系统设计

一式2采用A4纸单面打印左侧装（包含附件，项目运行时间为1-3年，必须在项目毕业前完成，须院部组织专家组评审并签署意见后，将《检查报告》报送教务处，纸质版由项目和院部各留存一份。一、项目主要进展（附已或其他成果复印件（重点介绍目前已主要开展工作，中期目标完成情况，取得的主要成果。如具备国家级选拔资格，应重点体现项目价值、深入研究潜力、预期成果与项目成员的投入）度年第二季研究地图的表境中特征的生成仿真地度年第三季算法改进，基卡尔曼滤波器的方法基于扩展滤波进度年第四季基于机器人件的程仿真程序的实度年第一季购置硬件设备调试编编调试成度年第二季硬件调硬件搭建成STM32DreamerMaple二维调试成度年第三基于ROS操作系统SLAMROS操作系统及硬件搭ROS实现部分综航中的重要环节。基于SLAM的单机器人导航、避障系统是利用扩 现阶段本项目组已利用使仿真地图构建成功，并利用扩展要求，因此改用ARM板进行计算，并将硬件搭建成功。随后我们进行了二维激光的调试，在计算机上得到周围环境的二维扫描地图。后来改进，基于ROSARMROS光对周围环境进行360°扫描，得到与参照物之间的距离和角度，从而实 、 （或6轴惯性导航模块）计算角度、角速度、（2、机器人底座搭建3、选择ARM板作为机器人的控制中心。初次设计时，使用的是STC12C5A60S2ARMTX，TXARMRX（4、编测速调试4WD/3PA小车编采用非接触式方式把角位移转换成电信号。遮断并通过计算小车在一定时间内产生的脉冲个数可以换算出的转速。 原理5、二 连接及调试二 外将其各个管脚与arm板相连， 附近摆 物 试二 与arm板连接结果显示图（地图基本轮廓可以显示出来但由 实时扫描并显示图像会有闪动6、ROS操作系统及硬件搭arm2ROSROS提供一些标准操作系统服务，例如硬件抽象，底层设备控制，分布式控制，常用功能实现，进程间消息以及数据包管理。ROS是基于一种图状架构，从而不同节人的相关功能，不使用ROS能实ROS得机器人软件搭建的工作更方便，效率更高ROS由.cpp将整个工程（Project） 编写第N+1个节点，然后编译，然后运行，即有N+1个节点共同运行。 Topic：发布者与订阅者之间数据传输“管道”的名字，消息:Message:int,char，7、ROS实现部整体导航包的格局如下图所sensortransformstfsensor 这里是机器人导航传感器数据输odometryodometrysource机器人的导航需要输入里程计的数二、下步工作计1SLAM利用基于ROS系统对SLAMSLAM ，，的地图表达形式的是通过传感器获得的观测信息用来别或者环境中的物体来导航。但是由于感知信息的不确SLAM22016三、存在问题、建议及需要说明的情 代替项目的 硬件—— 求。而在之后使用一维，虽比超声波测距模块更加精360°全方位扫描物而达到预期效果。因此采用二维。后期需要不断调试修改。之前使用5节5号电池给机器人供电机器人行走用编测量。硬件连接更加方便。四、经费使用情况和经费安排计已报销经费：0下一步经费安排计即将报销项金额（元打印费、WIFI五、指导教师意 六、审核意 院部审核意 检查结果(请打专家检查意见(对项目进展情况的过程性评价及下一步工作的建议附件仿仿真程functiondata=ekfslam_sim(lm,%functiondata=ekfslam_sim(lm,%%%%%lm-setoflandmarks//标wp-setofwaypoints//% data-adatastructure data.path:thevehiclepathestimate(ie,whereSLAMestimatesthevehiclewent) SLAM data.state(k).x:theSLAMstatevectorattimek,k data.state(k).P:thediagonalsoftheSLAMcovariancematrixattime k% ThisprogramisaSLAM forthedesiredvehiclepath). Theprogram'frontend.m'maybeusedtocreatethissimulatedenvironment（仿真环境）type'helpfrontend'formoreinformation. Theconfigurationofthesimulatorismanagedbythescriptfile'configfile.m'.Toaltertheparametersofthevehicle,sensors,etc adjustthisfile.Therearealsoseveralswitchesthatcontrol filter%%TimBaileyandJuanNieto%Versionclearall;closeloadexample_densemap;%将数据导入到工作区间formatcompact%setupplotsholdon,axisequalplot(wp(1,:),wp(2,:),'g',wp(1,:),wp(2,:),'r.')xlabel('metres'),ylabel('metres')set(fig,'name','EKF-SLAMconfigfile;**USETHISFILETOCONFIGURE（配置）THEEKF-SLAM**h=setup_animations;veh=[0-WHEELBASE-WHEELBASE;0-22];%vehicle%plines=[];%forlaserlineanimation%initialisextrue=zeros(3,1);%初始化机器人的位姿[0,0,0]'x=zeros(3,1);%初始化系统状态向量P=zeros(3);%%initialiseothervariablesandconstantsdt=DT_CONTROLS;ftag=1:size(lm,2);%identifierforeachlandmarkda_table=zeros(1,size(lm,2));%dataassociationtableiwp=1;%indextofirstwaypointG=0;%initialsteerangledata=initialise_store(x,P,xtrue);%storeddataforoff-lineQE=Q;RE=(ie,addstabilisingnoise)%ifSWITCH_SEED_RANDOM,randn('state',SWITCH_SEED_RANDOM),%mainloopwhileiwp~=0%computetrue[G,iwp]=compute_steering(xtrue,wp,iwp,AT_WAYPOINT,G,RATEG,MAXG,dt);

%performloops:iffinalwaypointreached,gobacktoxtrue=vehicle_model(xtrue,V,G,WHEELBASE,dt);[Vn,Gn]=add_control_noise(V,G,Q,%EKFpredict[x,P]=predict(x,P,Vn,Gn,QE,WHEELBASE,dt);%%ifheadingknown,observe%[x,P]=observe_heading(x,P,xtrue(3),%EKFupdatedtsum=dtsumdt;%控制进行观测的时间间隔，DT_OBSERVE=8*DT_CONTROLS;观测的间隔时间为0.2sifdtsum>=DT_OBSERVEdtsum=0;[z,ftag_visible]=get_observations(xtrue,lm,ftag,z=add_observation_noise(z,R,ifSWITCH_ASSOCIATION_KNOWN==[zf,idf,zn,da_table]=data_associate_known(x,z,ftag_visible,da_table);[zf,idf,zn]=data_associate(x,P,z,RE,GATE_REJECT,[x,P]=augment(x,P,zn,RE);landmarkxifSWITCH_USE_IEKF==[x,P]=update_iekf(x,P,zf,RE,idf,1);[x,P]=update(x,P,zf,RE,idf,%offlinedatadata=store_data(data,x,P,%xt=xv=transformtoglobal(veh,x(1:3));%将局部变量转换成全局变量set(h.xt,'xdata',xt(1,:),'ydata',xt(2,:))set(h.xv,'xdata',xv(1,:),'ydata',set(h.xf,'xdata',x(4:2:end),'ydata',x(5:2:end))%在地图上标记landmarkpcov(:,1:size(ptmp,2))=ptmp;ifdtsum==0set(h.cov,'xdata',pcov(1,:),'ydata',pcov(2,:))pcount=pcount+1;ifpcount==15%1515set(h.pth,'xdata',data.path(1,1:data.i),'ydata',set(h.truepth,'xdata',data.truepath(1,1:data.i),'ydata',data.truepath(2,1:data.i))if~isempty(z)%zplines=make_laser_linesset(h.obs,'xdata',plines(1,:),'ydata',plines(2,:))%线pcov=make_covariance_ellipses(x,P);%

data=set(h.pth,'xdata',data.path(1,:),'ydata',set(h.truepth,'xdata',data.truepath(1,:),'ydata',%%functionh=%EraseMode normal background xor noneh.xt=patch(0,0,'b','erasemode','xor');%vehicletrueh.xv=patch(0,0,'r','erasemode','xor');vehicleestimateh.truepth=plot(0,0,'r.','markersize',1,'erasemode','background');vehiclepathestimateSLAM估计路径h.pth=plot(0,0,'k.','markersize',1,'erasemode','background');vehiclepathestimateSLAM估计路径h.obs=plot(0,0,'y','erasemode','xor');%observationslandmarkh.xf=plot(0,0,'r+','erasemode','xor');estimatedfeatures%估计的landmarkh.cov=plot(0,0,'k','erasemode','xor');covarianceellipse%%functionp=make_laser_lines%computesetoflinesegmentsforlaserrange-bearingmeasurementsifisempty(rb),p=[];return,endlen=lnes(1,:)=zeros(1,len)+lnes(2,:)=zeros(1,len)+lnes(3:4,:)=rb(1,:).*sin(rb(2,:))],p=line_plot_conversion%%functionp=computeellipsesforplottingstatecovariancesN=10;%可以控制椭圆的平滑度inc=2*pi/N;phi=lenx=length(x);lenf=(lenx-p=zerosp(:,ii)=make_ellipse(x(1:2),P(1:2,1:2),2,ctr=N+3;fori=1:lenfii=jj=2+2*i;jj=%%

p(:,ii)=make_ellipse(x(jj),P(jj,jj),2,phi);ctr=ctr+N+2;functionp=make_ellipse(x,P,s,%makeasingle2-Dellipseofs-sigmasoverphiangleintervalsr=sqrtm(P);%r*r=Pp(2,:)=[a(2,:)+x(2)NaN];%p(1,:)=[a(1,:)+x(1)%%functiondata=initialise_store(x,P,%offlinestorageinitialisationdata.path=x;data.true=xtrue;data.state(1).x=x;data.state(1).P=P;%data.state(1).P=%%functiondata=store_data(data,x,P,%addcurrentdatatoofflinestorageCHUNK=5000;ifdata.i==size(data.path,2)%growarrayinchunkstoamortisedata.path=[data.pathzeros(3,CHUNK)];data.truepath=[data.truepathzeros(3,CHUNK)];data.true=[data.truezeros(3,CHUNK)];i=data.i+1;data.i=i;data.path(:,i)=x(1:3);data.truepath(:,i)=xtrue(1:3);data.true(:,i)=xtrue;data.state(i).x=x;data.state(i).P=%data.state(i).P=%%functiondata=%offlinestoragefinalisationdata.path=data.path(:,1:data.i);data.truepath=data.truepath(:,1:data.i);data.true=data.true(:,1:data.i);#include"sys.h"#include"usart.h"#include"delay.h"#include"timer.h"#include"motor.h"int{Stm32_Clock_Init(9);delay_init(72);//延时初始化uart_init(72,9600);//串口初始化ˉTIM3__Init(1000,0,200,200);{}} 程////# :Laurenfrom//# ://#Productname:WheelEncodersforDFRobot3PAand4WD//#ProductSKU://#//#ThesketchforusingtheencoderontheDFRobotMobile//#// leftwheelencoder->Digitalpin// rightwheelencoder->Digitalpin//#defineLEFT#defineRIGHTlongcoder[2]={0,0};intlastSpeed[2]={0,0};void //inittheSerialporttoprintthedataattachInterrupt(LEFT,LwheelSpeed,CHANGE); //inittheinterruptmodeforthedigitalpin2attachInterrupt(RIGHT,RwheelSpeed, //initthemodeforthedigitalpin}voidstaticunsignedlongtimer=0; //printmanagertimerif(millis()-timer>100){Serial.print("Codervalue:");Serial.print("[LeftWheel]");Serial.println("[RightWheel]");lastSpeed[LEFT]=coder[LEFT]; //recordthelatestspeedvaluelastSpeed[RIGHT]=coder[RIGHT];coder[LEFT]=0; //clearthedatabuffercoder[RIGHT]=0;timer=}}void{coder[LEFT] //counttheleftwheelencoder}void{coder[RIGHT]++;//counttherightwheelencoder}二 程RoboPeakRPLIDARArduinoThisexampleshowstheeasyandcommonwaytofetchdatafromanYoumayfreelyaddyourapplicationcodebasedonthis1.DownloadthissketchcodetoyourArduino(Pin0andPin1)3.ConnecttheRPLIDAR'smotorctrlpintotheArduinoboardpinCopyright(c)2014,All Redistributionanduseinsourceandbinaryforms,withorwithoutarepermittedprovidedthatthefollowingconditionsare1.Redistributionsofsourcecodemustretaintheabovecopyrightthislistofconditionsandthefollowing 2.Redistributionsinbinaryformmustreproducetheabovecopyrightthislistofconditionsandthefollowingdi erintheand/orothermaterialsprovidedwiththeTHISSOFTWAREISPROVIDEDBYTHECOPYRIGHTHOLDERSANDCONTRIBUTORS"ASIS"ANDANYEXPRESSORIMPLIEDWARRANTIES,INCLUDING,BUTNOTLIMITEDTO,THEIMPLIEDWARRANTIESOFMERCHANTABILITYANDFITNESSFORAPARTICULARPURPOSEARE ED.INNOEVENTSHALLTHECOPYRIGHTHOLDERORCONTRIBUTORSBELIABLEFORANYDIRECT,INDIRECT,INCIDENTAL,SPECIAL,EXEMPLARY,ORCONSEQUENTIALDAMAGES(INCLUDING,BUTNOTLIMITEDTO,PROCUREMENTOFSUBSTITUTEGOODSORSERVICES;LOSSOFUSE,DATA,ORPROFITS;ORBUSINESSINTERRUPTION)HOWEVERCAUSEDANDONANYTHEORYOFLIABILITY,WHETHERINCONTRACT,STRICTLIABILITY,