initial.m

粒子群(PSO)路径规划。这个是一个局部路径规划

%------给定初始化条件----------------------------------------------

%----粒子群的----
c1=1.4962;             %学习因子1
c2=1.4962;             %学习因子2
w=0.7298;              %惯性权重
MaxDT=10;            %最大迭代次数
m=10;                  %搜索空间维数（未知数个数）
N=30;                  %初始化群体个体数目
eps=10^(-6);           %设置精度(在已知最小值时候用)
Kmax = 10;          %初始化x时用的最大迭代次数
Qmax = 10;          %初始化x时粒子全部重新初始化用的最大迭代次数
fitw1 = 1;         %适应值函数中的两个权重
fitw2 = 1;
pathta = 2*pi/6;       %移动的角度为60度,pathta*2

%----路径的----
initialmap;
robotv=2;          %机器人半径

             
Nsteps = 500;          %机器人最多移动的步数
ploR = 50;            %极坐标半径
segR = ploR/m;            %极坐标的分段半径
mapmin=[0; 0];         % 地图大小
mapmax=[300;300];
curpoint = s;        %当前点的位置
hadsteps(1,1) =curpoint(1);        %走过的点记录
hadsteps(1,2) =curpoint(1);        %走过的点记录
curstep = 1;         %当前的步数
%rangOb;
obIndex = 0;         %记录在粒子范围内的障碍物点的索引
goalta= straightLine(curpoint(1),curpoint(2),...
                        g(1),g(2));
              %当前点到目标点的角度
movelen = ploR;         %机器人移动长度

pointIndex=1;      %记录点的索引
pathpoint(pointIndex,1) = curpoint(1);       %路径规划得出的点
pathpoint(pointIndex,2) = curpoint(2);

V(1) = ( (g(1)-curpoint(1))/abs(g(1)-curpoint(1)) )*...
    movelen*cos(goalta);
V(2) = ( (g(2)-curpoint(2))/abs(g(2)-curpoint(2)))*...
    movelen*sin(goalta);

numta = 2*pi/pathta;           %把圈分为几个角
for i=1:numta
    pamoveta(1,i)= 1;        %粒子的是否，维数由numta定
end

searchsued = false;      %是否搜索成功