main.m

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

%------main主函数--------------------------------------------------

%------初始格式化--------------------------------------------------

clear all;
clc;
format bank;

%------定义全局变量----------------------------------------------

%-粒子群的-
global c1;             %学习因子1
global c2;             %学习因子2
global w;              %惯性权重
global MaxDT;            %最大迭代次数
global m;                  %搜索空间维数（未知数个数）
global N;                  %初始化群体个体数目
global eps;           %设置精度(在已知最小值时候用)
global Kmax;          %初始化x时用的最大迭代次数
global Qmax;          %初始化x时粒子全部重新初始化用的最大迭代次数
global fitw1;         %适应值函数中的两个权重
global fitw2;
%-路径规划的-
global robotv;          %机器人体积
global s;               %起始点
global g;               %目标点
global obstacles;       %障碍物点
global Nsteps;          %机器人最多能移动的步数
global ploR;            %极坐标半径
global segR;            %极坐标的分段半径
global mapmin;         % 地图大小
global mapmax;
global curpoint;        %当前点的位置
global hadsteps;        %只记录curpoint的集合
global curstep;         %当前的步数
global rangOb;          %在粒子范围内的障碍物点
global obIndex;         %记录在粒子范围内的障碍物点的索引
global goalLine;        %记录当前点和目标点之间连线的方程
global movelen;         %机器人移动长度
global V;               %机器人的速度(矢量)

global pointIndex;      %记录点的索引
global pathpoint;       %记录所有的点

%-------初始化矩阵----------------------------------------------

initial;

%-------开始寻找路径--------------------------------------------

%for循环，寻找路径
for k=1:Nsteps
    
    %如果当前点到目标距离小于极坐标半径，那么用dis1代替ploR
    if distance(curpoint(1),curpoint(2),g(1),g(2))<ploR
        movelen = distance(curpoint(1),curpoint(2),g(1),g(2));
    end
    %机器人的速度(矢量)
    V(1) = ( (g(1)-curpoint(1))>=0 )*...
                movelen*cos(atan(goalLine(1))); 
    V(2) = ( (g(2)-curpoint(2))>=0 )*...
                movelen*sin(atan(goalLine(1)));

    %判断是否有障碍物点在移动的范围内
    %如果有就进行路径规划，没有就直接往目标点移动
    for i = 1:size(obstacles)
        %有障碍物在移动范围内
        %将在范围内的障碍物点存入rangOb中
        if movelen+robotv>distance(curpoint(1),curpoint(2),...
                         obstacles(i,1),obstacles(i,2))
            obIndex = obIndex+1;
            rangOb(obIndex,1)=obstacles(i,1);
            rangOb(obIndex,2)=obstacles(i,2);
        end
    end
    
    %如果有障碍物在移动范围内，进行路径规划，否则往目标点移动
    if obIndex>0
        %进行路径规划，得出走过的点
        pPoint = pathplanning();
        pPoint
        %将点记录到pathpoint
        for i= 1:size(pPoint)
            %索引加加
            pointIndex = pointIndex + 1;
            pathpoint(pointIndex,1) = pPoint(i,1);
            pathpoint(pointIndex,2) = pPoint(i,2);
        end

        %更新当前的值
        curpoint(1) = pathpoint(pointIndex,1);
        curpoint(2) = pathpoint(pointIndex,2);
        obIndex = 0;
    else
        %对当前点进行赋值 
        curpoint(1) = curpoint(1)+V(1);
        curpoint(2) = curpoint(2)+V(2);
        pointIndex = pointIndex + 1; 
        pathpoint(pointIndex,1) = curpoint(1);
        pathpoint(pointIndex,2) = curpoint(2);
    end
    curpoint

    %将走过的点记录下来
    hadsteps(k,1) = curpoint(1);
    hadsteps(k,2) = curpoint(2);
    
    %判断机器人是否到达目标点
    %如果到达就退出，没有到达就继续寻找
    if distance(curpoint(1),curpoint(2),g(1),g(2))<=robotv
        break;
    end
    
    %更新goalLine
    movelen = ploR;
    [goalLine(1),goalLine(2)] = straightLine(curpoint(1),curpoint(2),...
                        g(1),g(2));
end
%-最后处理-----------------------------
pointIndex = pointIndex+1;
pathpoint(pointIndex,1) = g(1);
pathpoint(pointIndex,2) = g(2);

pathpoint

%-------画---------------------------------------
plot(5,5,'s',25,25,'x')
axis([0 30 0 30])
hold on
xlabel('x');
ylabel('y');
title('Obstacles (o), initial vehicle (square) and goal (x) positions');
hold on
% Plot obstacle positions (sets obstaclefunction)
plot(20,15,'o',8,10,'o',10,10,'o',12,10,'o',24,20,'o',18,20,'o')
%plot(pointIndex,pathpoint(:,1),'k-',pointIndex,pathpoint(:,2),'k--')
plot(pathpoint(:,1),pathpoint(:,2),'r-')
% plot(hadsteps(:,1),hadsteps(:,2),'b-')
plot(5,5,'s',25,25,'x')

hold off
%plot(pathpoint);