模拟退火法处理人工势场中的局部极值点问题源代码

clear all %初始化参数 area = [-2 12 -2 12];% 模拟区域范围 [xmin xmax ymin ymax] tic;%计算程序运行时间 startPoint=[0 0];%起点位置 finalPoint=[10,10];%目标位置 Katt=5;%计算引力需要的增益系数 Krep=15;%计算斥力的增益系数 obstacleR=0.5;%障碍物半径 influenceDistance=2;%障碍影响距离，当障碍和车的距离大于这个距离时，斥力为0，即不受该障碍的影响 stepSize=0.1;%步长 purposeDistance=stepSize;%当小车与目标点距离小于改值时，即认为到达目标 counter=1000;%循环迭代次数 %如果不能实现预期目标，可能也与初始的增益系数，influenceDistance设置的不合适有关 obstaclePoint1=[1 1;3 3;4 4;2 6;6 2;5 5;8 8];%这个向量是n*2维，即n个障碍物 obstaclePoint2=[0 2;2 4;2 5;4 2;5 4;5 6;5 9;8 8;7 9;5 5];%这个向量是n*2维，即n个障碍物 obstaclePoint3=[1 1;3 3;4 4;5 5;8 8;2 6;6 2;3 5;5 3;7 4;4 7;8 9;9 8;5 7];%这个向量是n*2维，即n个障碍物 obstaclePoint4=[2 1;2 2;2 3;2 4;5 5;6 5;5 6;6 6;8 5;9 5;8 6;9 6;5 2;6 2;7 2;6 8;7 8;4 7; 7 4;8 7];%这个向量是n*2维，即n个障碍物 obstaclePoint=obstaclePoint1; obstacleNum=size(obstaclePoint,1);%障碍个数 currentPoint=startPoint;%循环初始，将车的起始坐标赋给currentPoint goalPoint(1,:)=currentPoint;%goalPoint是保存车走过的每个点的坐标，刚开始先将起点放进该向量 %*****************以下是画出障碍物**************** r = obstacleR; theta = 0:pi/20:2*pi; for id=1:length(obstaclePoint(:,1)) x = r * cos(theta) + obstaclePoint(id,1); y = r *sin(theta) + obstaclePoint(id,2); axis(area); fill(x,y,'-k'); set(gca,'xtick',-2:1:12); set(gca,'ytick',-2:1:12); grid on; hold on; end %*****************以下是模拟退火法参数初始化********************** constT=10;%T值 a=0.99; IfTrappedcounter=0; IfTrappedcounterWhile=0; IfTrappedcounterWhile2=0; IfTrappedcounterWhile3=0; %***************初始化结束，开始主体循环****************** j=1; while(j<counter) %循环开始 %调用计算角度模块 angle=compute_angle(currentPoint,finalPoint,obstaclePoint,obstacleNum);%angle是计算出来的车和障碍，和目标之间的与X轴之间的夹角，统一规定角度为逆时针方向，用这个模块可以计算出来。 %调用计算引力模块 angleGoal=angle(1);%angle（1）是车和目标之间的角度，目标对车是引力 [Fattx,Fatty]=compute_Attract(currentPoint,finalPoint,Katt,angleGoal,influenceDistance); %计算出目标对车的引力在x,y方向的两个分量值。 [Frepx,Frepy]=compute_repulsion(currentPoint,obstaclePoint,Krep,angle,obstacleNum,influenceDistance);%计算出斥力在x,y方向的分量数组。 Fsumx=Fattx+Frepx;%x方向的合力 Fsumy=Fatty+Frepy;%y方向的合力 angleNextStep=atan2(Fsumy,Fsumx);%合力与x轴方向的夹角向量 %以下是模拟退火循环 if(j>2)%判断是否进入局部最优 ifTrapped(j)=norm(goalPoint(j,:)-goalPoint(j-2,:)); end if(((j>2)&&(ifTrapped(j)<0.01))||((Fsumx==0)&&(Fsumy==0)))%第j步与第j-2步距离过近或者合力为0，则认为进入了局部最优值 T=constT;%每次都重置T IfTrappedcounter=IfTrappedcounter+1; jTrapped=j;%用于记录进入局部最优点的位置，便于后面判断是否跳出了局部最优值 % UattTrapped=compute_attField(goalPoint(jTrapped,:),finalPoint,Katt,influenceDistance);%计算前一个点的引力势场 % UrepTrapped=compute_repField(goalPoint(jTrapped,:),obstaclePoint,Krep,obstacleNum,influenceDistance);%计算前一个点的斥力势场 % UTrapped=UattTrapped+UrepTrapped;%前一个点总势场 while(1)%进入模拟退火循环 ifObtained=0;%用于判断是否获得符合条件的随机点 ifCollided=0;%用于判断是否与障碍物相撞 IfTrappedcounterWhile=IfTrappedcounterWhile+1; randomAngle=2*pi*rand(1);%产生一个随机角度 randomPoint(1)=currentPoint(1)+stepSize*cos(randomAngle); randomPoint(2)=currentPoint(2)+stepSize*sin(randomAngle);%由随机角度计算出随机点 %以下为判断随机点是否碰到障碍物 for i=1:obstacleNum randomObstacle=norm(randomPoint-obstaclePoint(i,:)); if (randomObstacle<=obstacleR+stepSize) ifCollided=1; break; end end if (ifCollided==1)%此时若撞到障碍物，则重新产生一个随机点 IfTrappedcounterWhile=IfTrappedcounterWhile-1; continue; end Uattbefore=compute_attField(currentPoint,finalPoint,Katt,influenceDistance);%计算前一个点的引力势场 Urepbefore=compute_repField(currentPoint,obstaclePoint,Krep,obstacleNum,influenceDistance);%计算前一个点的斥力势场 Uattrandom=compute_attField(randomPoint,finalPoint,Katt,influenceDistance);%计算随机点的引力势场 Ureprandom=compute_repField(randomPoint,obstaclePoint,Krep,obstacleNum,influenceDistance);%计算随机点的斥力势场 Ubefore=Urepbefore;%前一个点总势场 Urandom=Ureprandom;%随机点总势场 Udeerta(IfTrappedcounterWhile)=Urandom-Ubefore; if (Udeerta(IfTrappedcounterWhile)<=0) %如果势场减小，则将随机点做为下一个目标点 IfTrappedcounterWhile2=IfTrappedcounterWhile2+1; nextPoint=randomPoint; ifObtained=1; else IfTrappedcounterWhile3=IfTrappedcounterWhile3+1; T=T*a; p(IfTrappedcounter)=exp(-Udeerta(IfTrappedcounterWhile)/T); randomP(IfTrappedcounter)=rand(1);%产生随机数 if(p(IfTrappedcounter)>randomP(IfTrappedcounter)) nextPoint=randomPoint; ifObtained=1; end end if(ifObtained==1)%判断是否获得满足条件的随机点 j=j+1; currentPoint=nextPoint; goalPoint(j,:)=currentPoint;%把最后一个点赋值为目标 %**************画出目标点、起点、路径点******************* plot(finalPoint(1),finalPoint(1,2),'hg',startPoint(1),startPoint(2),'hg',goalPoint(j-1:j,1),goalPoint(j-1:j,2),'.-g'); drawnow; end % if(Urandom<UTrapped) % break;% % end runDistance=norm(goalPoint(j,:)-goalPoint(jTrapped,:));%计算与进入局部最优点的距离 if(runDistance>(stepSize*5)) break;%若距离大于5个步长，则认为跳出了局部最优 end end else%若没有进入局部最优 nextPoint(1)=currentPoint(1)+stepSize*cos(angleNextStep); nextPoint(2)=currentPoint(2)+stepSize*sin(angleNextStep); currentPoint=nextPoint; j=j+1; goalPoint(j,:)=currentPoint;%把产生的点赋值为目标 %**************画出目标点、起点、路径点******************* plot(finalPoint(1),finalPoint(1,2),'hg',startPoint(1),startPoint(2),'hg',goalPoint(j-1:j,1),goalPoint(j-1:j,2),'.-b'); drawnow; end nextGoal(j)=norm(currentPoint-finalPoint);%计算当前点与目标点距离 if (nextGoal(j)<purposeDistance) %距离小于给定值，则认为到达目标点 break; end%如果不符合if的条件，重新返回循环，继续执行。 % pause(0.05) end%大循环结束 toc;%计算总的时间 % %*********************使用方法A所得路径点进行优化***************************** % tic;%计算缩短路径所用时间 % i=1; % i2=1; % iTest=2;%从第三个点开始索引 % goalPoint2(i2,:)=goalPoint(i,:);%将目标点的第一个点赋值给goalPoint2 % DisToObstacle=2*obstacleR;%缩短路径时判断与障碍物距离，若大于该值，则认为完全符合要求 % while(i<j)%遍历goalPoint里面点 % i=iTest-1; %从第iTest-1个点开始索引 % iTest=i+2;%从第i+2个点开始记起 % line(1,:)=goalPoint(i,:);%直线起点 % while(1)%不断索引i后面的点，直到不符合要求 % if(iTest>=j) % if(iTest>j)%超过goalPoint里面点数，直接跳出循环 % break; % else % goalPoint2(i2+1,:)=goalPoint(j,:);%将goalPoint里面最后一个点赋值给goalPoint2 % end % end % line(2,:)=goalPoint(iTest,:);%直线端点 % check