pathplanning.m

来自「一个用粒子群做的路径规划。功能比较简单」· M 代码 · 共 25 行

%-----路径规划函数--------------------------------------
function x = pathplanning(void)
%---声明全局变量-------
global segR;
global V;
global curpoint;
%----
%求出机器人的预计下一个点
nextpoint(1) = curpoint(1)+V(1);
nextpoint(2) = curpoint(2)+V(2);
%判断当前点和下一个点之间的连线是否有障碍物点
%如果有，用PSO，没有直接返回x
if  Conn(curpoint(1),curpoint(2),nextpoint(1),nextpoint(2))
    x(1) = nextpoint(1);
    x(2) = nextpoint(2);
    return;
else
    pg = PSO();%坐标变换
    for i = 1:length(pg)
        [x(i,1),x(i,2)]=plorTozhijiao(i*segR,pg(i));
    end
end