initial.m

一个用粒子群做的路径规划。功能比较简单

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

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

%----路径的----
s = [5;5];              %起始点
g = [25;25];            %目标点
robotv=0.5;          %机器人体积

obstacles = [20,15;     %障碍物点
            8,10;
            10,10;
            12,10;
            24,20;
            18,20];                
Nsteps = 500;          %机器人最多移动的步数
ploR = 5;            %极坐标半径
segR = ploR/m;            %极坐标的分段半径
mapmin=[0; 0];         % 地图大小
mapmax=[30;30];
curpoint = s;        %当前点的位置
%hadsteps;        %走过的点记录
curstep = 0;         %当前的步数
%rangOb;
obIndex = 0;         %记录在粒子范围内的障碍物点的索引
[goalLine(1),goalLine(2)] = 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))>=0 )*...
    movelen*cos(atan(goalLine(1)));
V(2) = ( (g(2)-curpoint(2))>=0 )*...
    movelen*sin(atan(goalLine(1)));