vfh.m

这是一个关于机器人的路径算法（vfh算法）在matlab中的仿真。

[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('危险程度','目标方向','避障方向')
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;                                  %分区



%global kt his angle

robot=startpoint;                         %机器人起始点

kt=round(caculateangle(robot,endpoint)/alpha);    %初始目标方向向量,个!
if(kt==0)
    kt=n;
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   算法[H值>>>安全角>>>机器人下一坐标]
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度范围视野内的障碍物分布值

            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;   %现his是一个含72个元素的向量

        %%%%%%%%%%%%%%%%%%%%%%%%%%%%%
   下面一段程序计算出目标向量kt，最佳前进方向angle，机器人下一坐标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条件循环
        end                         %退出while循环
        %%%%%%%%%%%%%%%合适的方向都存到c里面了
        g=zeros(j-1,1);how=zeros(j-1,1);
        for (i=1:j-1)
            g(i)=c(i);              %g中不含目标向量
            how(i)=howmany(g(i),kt);%how的第i元素为g(i)与kt间的向量数，g中与目标最近的
        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;                                  %分区
threshold=150;
%global kt his kb

robot=startpoint;                         %机器人起始点

kt=round(caculateangle(robot,endpoint)/alpha);    %初始目标方向向量,个!
if(kt==0)
    kt=n;
end



%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%   算法[H值>>>安全角>>>机器人下一坐标]



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度范围视野内的障碍物分布值

            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;   %现his是一个含72个元素的向量
        %%%下面一段程序计算出目标向量kt，最佳前进方向angle，机器人下一坐标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条件循环
            %%%%%%%%%%%%%%%%%%%%
        end                         %退出while循环
        %%%%%%%%%%%%%%%%%合适的方向都存到c里面了
        g=zeros(j-1,1);how=zeros(j-1,1);
        for (i=1:j-1)
            g(i)=c(i);              %g中不含目标向量
            how(i)=howmany(g(i),kt);%how的第i元素为g(i)与kt间的向量数，g中与目标最近的
        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