[1] 判斷矢量
function angle = caculateangle(s,e)
dy = e(2) - s(2);
dx = e(1) - s(1);
if dx==0
    angle=pi/2;
else
    angle = atan(dy/dx); 
    if(dx < 0)
        if(dy > 0)
            angle = pi - abs(angle);
        else
            angle = pi + abs(angle);
        end
    else
        if(dy < 0)
            angle = 2*pi- abs(angle);
        end
    end
end

[2] 障礙點
function drawobstacle
global obstacle
axis([0 10 0 10])
grid on

for i=1:length(obstacle)
    plot(obstacle(i,1),obstacle(i,2),'.k');
    hold on
end

[3] 終點
function getendpoint
global endpoint
[endpoint_x,endpoint_y] = ginput(1)
plot(endpoint_x,endpoint_y,'.r')
endpoint=[endpoint_x endpoint_y]
save endpoint endpoint

[4] 隨機輸入障礙
function getobstacle
clear
axis([0 10 0 10])
grid on
hold on
n=0; bn=1
global obstacle
obstacle=[];
while bn==1
    [xn,yn,bn]=ginput(1)
    plot(xn,yn,'.k');
    n=n+1;
    obstacle=[obstacle ;[xn,yn]];
end
save obstacle obstacle

[5] function getstartpoint
global startpoint
[startpoint_x,startpoint_y] = ginput(1)
plot(startpoint_x,startpoint_y,'.b')
startpoint=[startpoint_x startpoint_y]
save startpoint startpoint

[6] 矢量相鄰程度
function dif=howmany(c1,c2)
n = 72; % number of sektors 
dif = min([abs(c1-c2), abs(c1-c2-n), abs(c1-c2+n)]);

[7] 矢量柱狀圖
function plotHistogram(his,kt,kb)
%global his kt kb
n=length(his);
x1 = [1:n];
k1 = kt;   k2=kb;

y = [0:max(his)];
if(max(his) <=1)
    y=[0:0.01:1]; %to get a smoother line
end
figure(2)
hold off
%subplot(211)
bar(x1,his);            %%%%%%%%沒有加實際線
hold on
ylabel('H值');
xlabel('矢量');

%subplot(212)
%bar(x2,his);           %%%%%%%%%%沒有加實際線
%plot(k2,y,'.r');
%plot(a2,y,'.b');
%ylabel('H值');
%xlabel('角度');
line([k1,k1],[0,max(his)],'LineStyle','-', 'color','r');
line([k2,k2],[0,max(his)],'LineStyle','--', 'color','g');

legend('危險程度','目標(biāo)方向','避障方向')
figure(1)

[8] 避障
%function h=vfh2(obstacle,startpoint,endpoint)
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%可用！
drawobstacle;
plot(startpoint(1),startpoint(2),'.b');
plot(endpoint(1),endpoint(2),'.r');
step=0.1;
f=5;                                      %角分辨率,度!
dmax = 2.5;                                 %視野
smax = 18;                                %寬波谷
b=2;                                      %常量
a=b*dmax;                                 %常量
C=15;                                     %cv
alpha = deg2rad(f);                       %弧度!
n=360/f;                                  %分區(qū)



%global kt his angle

robot=startpoint;                         %機器人起始點

kt=round(caculateangle(robot,endpoint)/alpha);    %初始目標(biāo)方向向量,個!
if(kt==0)
    kt=n;
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   算法[H值>>>安全角>>>機器人下一坐標(biāo)]
while   norm(robot-endpoint)~=0
    if(norm(robot-endpoint))>step
        i=1;mag = zeros(n,1);his=zeros(n,1);            %初值
        while (i<=length(obstacle))  %%%%%%%%%%%%%%%%%%%%%%%%%%%   下面一段程序得到機器人360度范圍視野內(nèi)的障礙物分布值

            d = norm(obstacle(i,:) - robot);%這個障礙與機器人之間距離
            if (d<dmax)
                beta = caculateangle(robot,obstacle(i,:));
                k = round(beta/alpha); %這個障礙是第k個向量
                if(k == 0)
                    k = n;
                end
                m = C^2*(a-b*d); %這個障礙的值
                mag(k)=max(mag(k),m);  %mag為zeros(n,1)，mag的第k個元素為m
                i=i+1;
            else
                i=i+1;
            end
        end
        his=mag;   %現(xiàn)his是一個含72個元素的向量

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   下面一段程序計算出目標(biāo)向量kt，最佳前進方向angle，機器人下一坐標(biāo)robot
        j=1;q=1;
        while (q<=n)       %所有合適的方向全部找出來
            %%%%%%%%%%%%%%%%%%%%
            if(his(q)==0)
                kr=q;                  %找到了波谷的左端
                while(q<=n & his(q)==0)  %這一小段找到了波谷的右端
                    kl=q;
                    q=q+1;
                end
                if(kl-kr > smax) % wide valley
                    c(j)   = round(kl - smax/2); % towards the left side
                    c(j+1) = round(kr + smax/2); % towards the right side
                    j=j+2;
                    if(kt >= kr & kt <= kl)
                        c(j) = kt; % straight at look ahead
                        j=j+1;
                    end
                elseif(kl-kr > smax/5) % narrow valley
                    c(j) = round((kr+kl)/2);
                    j=j+1;
                end

            else
                q=q+1;            %his(q)不為0，直接下一個

            end                  %退出if選擇，再次進入while條件循環(huán)
        end                         %退出while循環(huán)
        %%%%%%%%%%%%%%%合適的方向都存到c里面了
        g=zeros(j-1,1);how=zeros(j-1,1);
        for (i=1:j-1)
            g(i)=c(i);              %g中不含目標(biāo)向量
            how(i)=howmany(g(i),kt);%how的第i元素為g(i)與kt間的向量數(shù)，g中與目標(biāo)最近的
        end                         %how為差距向量
        ft=find(how==min(how));
        fk=ft(1);
        kb=g(fk);  %前進向量
        robot=robot+[step*cos(kb*alpha),step*sin(kb*alpha)];
        scatter(robot(1),robot(2),'.b');
        drawnow;
        kt=round(caculateangle(robot,endpoint)/alpha);
        if(kt==0)
            kt=n;
        end
        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
        plotHistogram(his,kt,kb);
    else
        break
    end
    pause(0.5)
end



[9] %function h=vfh4(obstacle,startpoint,endpoint)
drawobstacle;
plot(startpoint(1),startpoint(2),'.b');
plot(endpoint(1),endpoint(2),'.r');
step=0.1;
f=5;                                      %角分辨率,度!
dmax = 2 ;                                 %視野
smax = 18;                                %寬波谷
b=2.5;                                      %常量
a=b*dmax;                                 %常量
C=15;                                     %cv
alpha = deg2rad(f);                       %弧度!
n=360/f;                                  %分區(qū)
threshold=150;
%global kt his kb

robot=startpoint;                         %機器人起始點

kt=round(caculateangle(robot,endpoint)/alpha);    %初始目標(biāo)方向向量,個!
if(kt==0)
    kt=n;
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   算法[H值>>>安全角>>>機器人下一坐標(biāo)]



while   norm(robot-endpoint)~=0
    if(norm(robot-endpoint))>step


        i=1;mag = zeros(n,1);his=zeros(n,1);            %初值
        while (i<=length(obstacle))  %%%%%%%%%%%%%%%%%%%%%%%%%%%   下面一段程序得到機器人360度范圍視野內(nèi)的障礙物分布值

            d = norm(obstacle(i,:) - robot);%這個障礙與機器人之間距離
            if (d<dmax)
                beta = caculateangle(robot,obstacle(i,:));
                k = round(beta/alpha); %這個障礙是第k個向量
                if(k == 0)
                    k = n;
                end
                m = C^2*(a-b*d); %這個障礙的值
                mag(k)=max(mag(k),m);  %mag為zeros(n,1)，mag的第k個元素為m
                i=i+1;
            else
                i=i+1;
            end
        end
        his=mag;   %現(xiàn)his是一個含72個元素的向量
        %%%下面一段程序計算出目標(biāo)向量kt，最佳前進方向angle，機器人下一坐標(biāo)robot
        j=1;q=1;


        while (q<=n)       %所有合適的方向全部找出來
            %%%%%%%%%%%%%%%%%%%%
            if(his(q)< threshold)
                kr=q;                  %找到了波谷的左端
                while(q<=n & his(q)< threshold)  %這一小段找到了波谷的右端
                    kl=q;
                    q=q+1;
                end

               if(kl-kr > smax) % wide valley
                    c(j)   = round(kl - smax/2); % towards the left side
                    c(j+1) = round(kr + smax/2); % towards the right side
                    j=j+2;
                    if(kt >= kr & kt <= kl)
                        c(j) = kt; % straight at look ahead
                        j=j+1;
                    end
                elseif(kl-kr > smax/5) % narrow valley
                    c(j) = round((kr+kl)/2);
                    j=j+1;
                end

            else
                q=q+1;              %his(q)不為0，直接下一個

            end                   %退出if選擇，再次進入while條件循環(huán)
            %%%%%%%%%%%%%%%%%%%%
        end                         %退出while循環(huán)
        %%%%%%%%%%%%%%%%%合適的方向都存到c里面了
        g=zeros(j-1,1);how=zeros(j-1,1);
        for (i=1:j-1)
            g(i)=c(i);              %g中不含目標(biāo)向量
            how(i)=howmany(g(i),kt);%how的第i元素為g(i)與kt間的向量數(shù)，g中與目標(biāo)最近的
        end                         %how為差距向量

        ft=find(how==min(how));
        fk=ft(1);
        kb=g(fk);  %前進向量
        robot=robot+[step*cos(kb*alpha),step*sin(kb*alpha)];
        scatter(robot(1),robot(2),'.b');
        drawnow;
        kt=round(caculateangle(robot,endpoint)/alpha);
        if(kt==0)
            kt=n;
        end
        %%%%%%%%%
        plotHistogram(his,kt,kb);
    else
        break
    end
    pause(0.5)
end