matlab练习程序（Bug2算法）

Bug算法是一种避障算法，思路就是想象从起始点有一只小虫，不停延直线靠近目标，如果遇见障碍物，则沿着障碍物边界移动，绕过障碍物后继续沿直线靠近目标。

从上面说的思想就可以认为该算法由两种路径组成，一种直线路径，一种障碍物环绕路径。

直线路径可以认为是起点到终点的线段，不过该线段要去除掉和障碍物相交的部分。

环绕路径这里用了取巧的方法，由于这里用的是栅格图，因此先用腐蚀膨胀的图像处理方法得到所有障碍物边缘路径，然后再做边界跟踪，就得到环绕路径了。

最后合成直线路径和环绕路径，即可得到最终结果。

matlab程序如下：

clear all;
close all;
clc;

path=[];
se = strel('cube',3);
map = imread('bg.bmp');
edge = map - imerode(map,se);
flag=zeros(size(edge));

imshow(map);
hold on;

p=ginput();                 %选取起始与结束位置
plot(p(:,1),p(:,2),'ro')

startp = p(1,:);
endp = p(2,:);

curp = startp;
keyp =[];
d = norm(endp-startp);
direct = atan2(endp(1)-startp(1),endp(2)-startp(2));
sin_dir = sin(direct);
cos_dir = cos(direct);
for r=0:d                       %生成起始到终点的直线路径
    p = floor(startp + r*[sin_dir cos_dir]);
    if map(p(2),p(1))==255
        curp=[curp;p];
    end
    
    if edge(p(2),p(1))==255 && map(p(2),p(1))==255
        if isempty(keyp)==1
            keyp = [keyp;p];
        end
        if norm(p-keyp(end,:))<2
            keyp(end,:) = p;
        else
            keyp = [keyp;p];
        end
    end
end

near=[-1 -1;-1 0; -1 1;
    0 1;0 -1;
    1 1;1 0;1 -1];
if isempty(keyp)~=1             %生成环绕障碍物的路径
    pcpcell = cell(length(keyp)/2,1);
    for i=1:2:length(keyp)
        aroundp = keyp(i,:);
        endp = keyp(i+1,:);
        flag(aroundp(2),aroundp(1)) = 1;
        
        while norm(aroundp(end,:)-endp)>1
            tmp = aroundp(end,:)+near;
            for j=1:8
                if edge(tmp(j,2),tmp(j,1))==255 && flag(tmp(j,2),tmp(j,1))==0
                    aroundp = [aroundp;tmp(j,:)];
                    flag(tmp(j,2),tmp(j,1)) = 1;
                    break;
                end
            end
        end
        pcpcell{(i+1)/2} = aroundp;
    end
    
    j=1;                    %对两类路径进行合并
    for i=1:length(curp)
        if j<=length(pcpcell)
            if curp(i,:) == keyp(j*2-1,:)
                path=[path;pcpcell{j}];
                j = j+1;
            else
                path=[path;curp(i,:)];
            end
        else
            path=[path;curp(i,:)];
        end
    end
else
    path = curp;
end

plot(path(:,1),path(:,2),'g.');

结果如下：

计算路径：