straightline.m

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

%----求直线方程，知道两点得出直线的k和b值
function theta = straightLine(x1,y1,x2,y2)

a=x2-x1+j*(y2-y1);
angleA=angle(a);
if angleA >=0
    theta = angleA;
else
    theta = 2*pi+angleA;
end