pathplanning.m

用深度优先算法做的一个路径规划。可以从开始点一直移动到目标点

%-----路径规划函数--------------------------------------
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;
%------
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表示来的路
            %将走的点赋值给当前值，继续搜索
            curpoint(1) = nextpoint(1);
            curpoint(2) = nextpoint(2);
            hadsteps(curstep,1) = curpoint(1);
            hadsteps(curstep,2) = curpoint(2);
            if pathplanning()
                result = true;
                return;
            end
        else
            pamoveta(curstep,dimtemp) = 0;%2表示已经走过了
        end
        %如果不能走通的话，继续循环（什么都不用做）
    end
end

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






% %----
% %求出机器人的预计下一个点
% nextpoint(1) = curpoint(1)+V(1);
% nextpoint(2) = curpoint(2)+V(2);
% %判断当前点和下一个点之间的连线是否有障碍物点
% %如果有，用PSO，没有直接返回x
% %如果区域连通则让pamoveta在相应的区域为2，
% %让下一个点的pamoveta对应域为3.
% if  Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
%     k = ceil(goalta/pathta); %求在第几个区域
%     pamoveta(curstep,k) = 2;%2表示已经走过了
%     pamoveta(curstep+1,:)= 1;
%     pamoveta(curstep+1,mod(numta/2+k,numta)) = 3;%3表示来的路
%     x(1) = nextpoint(1);
%     x(2) = nextpoint(2);
%     return;
% else
%     pg = PSO();
%     %如果PSO成功就进行PSO，否则将当前的值赋值给x
%     if pathused
%         for i = 1:length(pg)
%             [x(i,1),x(i,2)]=plorTozhijiao(i*segR,pg(i));
%         end
%     else
%         %不成功返回值%有问题
%         x(1) = curpoint(1);
%         x(2) = curpoint(2);
%     end
% end