pathplanning.m

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

%-----路径规划函数--------------------------------------
function result = pathplanning(void)
%---声明全局变量-------
global segR;
global V;
global curpoint;
global pathused;
global curstep;
global pamoveta;
global goalta;
global pathta;
global numta;
global g;
global movelen
global searchsued;      %是否搜索成功
global robotv;
global ploR;
global hadsteps;
global pointIndex;
global pathpoint;
%------
result = false;

%判断是否找到目标点了
%%如果找到则searchsued为true
if distance(curpoint(1),curpoint(2),g(1),g(2))<=robotv
    searchsued = true;
    %判断当前点到目标点的距离是否小于传感器范围
    %%如果小于的话就重新设置机器人的移动距离
else if distance(curpoint(1),curpoint(2),g(1),g(2))<ploR
        movelen = distance(curpoint(1),curpoint(2),g(1),g(2));
    else
        movelen = ploR
    end
end

%判断搜索是否成功，成功的话就退出
if searchsued
    result = true;
    return;
end

%---接下来就是搜索主题了---
%求出当前点与目标点的方程
gaolta = straightLine(curpoint(1),curpoint(2),g(1),g(2));

%求出下一个点在第几维空间中
dim = ceil(gaolta/pathta)

%从维数中依此搜索路径
%一个for循环
for i=1:numta
    %求出在第几维中
    if mod(dim+i-1,numta) == 0
        dimtemp = numta;
    else
        dimtemp = mod(dim+i-1,numta);
    end

    %算出机器人的速度(矢量)
    V(1) = movelen*cos(gaolta+(i-1)*pathta);
    V(2) = movelen*sin(gaolta+(i-1)*pathta);
    %求出下一个
    nextpoint(1) = curpoint(1)+V(1);
    nextpoint(2) = curpoint(2)+V(2);
    %先判断这一维是否可以走通
    if pamoveta(curstep,dimtemp) == 1
        %判断在这一维中是否与目标点相连
        %%如果相连的话
        if Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
            %---------更新链表-------------------------------------
            %将这一维的字赋值维2,然后初始化下一个节点
            pamoveta(curstep,dimtemp) = 2;%2表示已经走过了
            curstep = curstep+1;
            pamoveta(curstep,:)= 1;
            %将下一段的维值
            tempta = mod(numta/2+dimtemp,numta);
            if  tempta == 0
                tempta = 6;
            end
            pamoveta(curstep,tempta) = 3;%3表示来的路
            %----------

            %--------------更新点-----------------


            pointIndex = pointIndex+1;
            %将走的点赋值给当前值，继续搜索
            curpoint(1) = nextpoint(1);
            curpoint(2) = nextpoint(2);
            hadsteps(curstep,1) = curpoint(1);
            hadsteps(curstep,2) = curpoint(2);
            %路径规划得出的点
            pathpoint(pointIndex,1) = curpoint(1);       %路径规划得出的点
            pathpoint(pointIndex,2) = curpoint(2);
            pathpoint
            %------

            if pathplanning()
                result = true;
                return;
            end
            %如果到最远点有障碍物的话
            %就进行PSO运算，
            %如果PSO可以走通的话，就让pathpoint记录PSO的点
            %如果PSO走不通的话就对pamoveta赋值，继续下一个
        else if PSO(dimtemp)
                if pathplanning()
                    result = true;
                    return;
                end

            else
                pamoveta(curstep,dimtemp) = 0;%2表示已经走过了
            end
        end
        %如果不能走通的话，继续循环（什么都不用做）
    end
end

if i==numta && pamoveta(curstep,i)~=1
    curstep = curstep-1;
end