向量場直方圖演算法（VFH）

環境用網格表示（二維資訊）
每個單元賦值表示該單元有障礙的可能性
在每一步環境資訊轉化為1維直方圖
引入代價函式值G，為所有可以通過的方向賦值
選擇具有最小代價函式值G的方向

G=a·目標方向+b ·輪轉動角度+c ·原運動方向

VFH+
用簡單形式考慮機器人可行軌跡（動力學約束下）
機器人運動軌跡為弧形
擋住某方向的障礙物，阻止所有通過該方向上的軌跡弧線

VFH+程式碼

記得這裡有個問題就是要先有地圖，我忘了傳了，不然執行會報錯的！！

%%% VFH+演算法 考慮機器人寬度
%function h=vfh4(obstacle,startpoint,endpoint)
clc;
clear;
%load obstacle 'obstacle'
 
;
%load startpoint 'startpoint';

ob=imread('map3.bmp');
% ob=edge(ob,'sobel');
%imshow(ob);
z=ones(20,20);
ob=imdilate(ob,z);
ob=edge(ob,'sobel');
imshow(ob);
s=ob;
s=double(s);
[row,flow]=size(ob);
n=1;
obstacle=zeros(2000,2);
startpoint=[0,0];
endpoint=[500,500];
for i=1:row
    for j=1:flow
        if s(i,j)==1
 

            obstacle(n,1:2)=[j row-i+1];
            n=n+1;
        end
    end
end
subplot(2,2,1);
plot(obstacle(:,1),obstacle(:,2),'.k');
% axis([0 500 0 500]);
% set(gca,'XTick',0:10:500);
% set(gca,'YTick',0:10:500);
grid on
hold on
plot(startpoint(1),startpoint(2),'.b');
hold on
plot(endpoint(1),endpoint(2),'.r'
 
)
hold on
title('VFH+路徑規劃演算法');
%step=0.1;
step=10;
f=5;                                      %角解析度,單位：度
%dmax = 2 ;                                %視野
dmax=200;
smax = 18;                                %大於18為寬波谷
b=2.5;                                    %常量
a=1+b.*(dmax.^2);                         %常量,與VFH不同
C=15;                                      %cv值，原始值15
alpha = deg2rad(f);                       %單位：弧度
n=360/f;                                  %分為72個扇區
threshold=1300;                            %閾值為150
thresholdlow=400;
%rsafe=0.6;                                  %擴大半徑和安全距離0.6
rsafe=20;
robot=startpoint;                         %機器人位於起始點位置
kb=90/f;                                     %最優的方向

kt=round(caculatebeta(robot,endpoint)/alpha);    %定義目標方向
if(kt==0)
    kt=n;
end
ffff=zeros(1,72);
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%          演算法：H值>>>安全形>>>機器人下一座標

while   norm(robot-endpoint)~=0            % 機器人不到終點
    if(norm(robot-endpoint))>step          % 機器人位置和終點位置差距大於0.1時

        i=1;mag = zeros(n,1);his=zeros(n,1);       %初值
        while (i<=length(obstacle))

            %%%%%%%%%%% 下面一段程式得到機器人360度範圍視野內的障礙物分佈值  計算每個扇區極座標障礙物密度

            d = norm(obstacle(i,:) - robot); % 障礙物柵格與機器人之間距離
            if (d<dmax)
                beta = caculatebeta(robot,obstacle(i,:));  % 障礙物柵格向量的方向
                rangle=asin(rsafe/d);        % 擴大的角度
                k = round(beta/alpha);       % 逆時針數，第k個扇區區域
                if(k == 0)
                    k = n;
                end
                % 更新後的極座標直方圖的h值
                if((5*k>rad2deg(beta)-rad2deg(rangle))&&(5*k<rad2deg(beta)+rad2deg(rangle)))
                    h(k)=1;
                else
                    h(k)=0;
                end
                % i=i+1;

                m = C^2*(a-b*(d.^2));        % 障礙物柵格的幅度值，與VFH演算法不同
                mag(k)=max(mag(k),m.*h(k));      % 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)                  % 寬波谷
                    c(j)   = round(kl - smax/2); % 朝向左側
                    c(j+1) = round(kr + smax/2); % 朝向右側
                    j=j+2;
                    if(kt >=kr && kt <= kl)
                        c(j) = kt;               % straight at look ahead
                        j=j+1;
                    end
                elseif(kl-kr > smax/5)           % 窄波谷
                    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)=5*howmany(g(i),kt)+2*howmany(g(i),kb)+2*howmany(g(i),kb);              % how的第i元素為g(i)與kt間的向量數，g中與目標最近的
        end                                       % how為差距向量

        ft=find(how==min(how));
        fk=ft(1);
        kb=g(fk);                                 %前進向量
        %%%%   跟蹤位置避障
        %         if(norm(robot-endpoint))>100
        %             target=robot+[100*cos(kb*alpha),100*sin(kb*alpha)];
        %             scatter(target(1),target(2),'*r');
        %             drawnow;
        %         else
        %             target=endpoint;
        %         end
        %
        %         while(norm(robot-target))>step
        %             robot=robot+[step*cos(kb*alpha),step*sin(kb*alpha)];
        %             scatter(robot(1),robot(2),'.b');
        %             drawnow;
        %         end

        %%%%  跟蹤速度避障
        robot=robot+[step*cos(kb*alpha),step*sin(kb*alpha)];
        scatter(robot(1),robot(2),'.b');
        drawnow;


        kt=round(caculatebeta(robot,endpoint)/alpha);
        if(kt==0)
            kt=n;
        end
        %%%%%%%%% 畫極座標直方圖
        plotHistogram(his,kt,kb,threshold,thresholdlow);

        nn=plotBinHistogram(his,kt,kb,threshold,thresholdlow,ffff);
        ffff=nn;
    else
        break
    end
    pause(0.1)
end

function lic = actxlicense(progid)

if strcmpi(progid, 'air.airctrl.1')
lic = 'Copyright (c) 1996 ';
return;
end


%計算角度

function beta = caculatebeta(s,e)
dy = e(2) - s(2);
dx = e(1) - s(1);
if dx==0
    beta=pi/2;
else
    beta = atan(dy/dx); 
    if(dx < 0)
        if(dy > 0)
            beta = pi - abs(beta);
        else
            beta = pi + abs(beta);
        end
    else
        if(dy < 0)
            beta = 2*pi- abs(beta);
        end
    end
end

%計算角度

function rangle = caculaterangle(s,e)
dy = e(2) - s(2);
dx = e(1) - s(1);
if dx==0
    rangle=pi/2;
else
    rangle = asin(dy/dx); 
    if(dx < 0)
        if(dy > 0)
            rangle = pi - abs(rangle);
        else
            rangle = pi + abs(rangle);
        end
    else
        if(dy < 0)
           rangle = 2*pi- abs(rangle);
        end
    end
end

function drawpoint
global obstacle;
load obstacle;
axis([0 10 0 10]);
grid on;

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

function nn=plotBinHistogram(his,kt,kb,threshold,thresholdlow,nn)

mm=zeros(1,72);
n=length(his);
x1 = 1:n;
k1 = kt;   k2=kb;
hh=his;
for i=1:n
if (hh(i)>=threshold)
    mm(i)=1;
elseif (hh(n)<=thresholdlow)
    mm(i)=0;
    else 
        mm(i)=nn(i);

end
end

subplot(2,2,3)
hold off
bar(x1,mm);            %%%%%%%%沒有加實際線
axis([0 80 0 3]); 
hold on
ylabel('H值');
xlabel('扇區');
title('二值極座標直方圖');

line([k1,k1],[0,2],'LineStyle','-.', 'color','r');%line([X1 X2],[Y1 Y2],S);
line([k2,k2],[0,2],'LineStyle','--', 'color','g');

legend('危險程度','目標方向','避障方向','閾值')
nn=mm;
subplot(2,2,1);

%%畫極座標直方圖

function plotHistogram(his,kt,kb,threshold,thresholdlow)
%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
subplot(2,2,2);
hold off
bar(x1,his);            %%%%%%%%沒有加實際線
hold on
ylabel('H值');
xlabel('扇區');
title('主極座標直方圖');

line([k1,k1],[0,max(his)],'LineStyle','-.', 'color','r');%line([X1 X2],[Y1 Y2],S);
line([k2,k2],[0,max(his)],'LineStyle','--', 'color','g');

line([0,n],[threshold,threshold],'LineStyle','-', 'color','k');
line([0,n],[thresholdlow,thresholdlow],'LineStyle','-', 'color','y');
legend('危險程度','目標方向','避障方向','高閾值','低閾值')

 %%直方圖和避障圖交替進行
subplot(2,2,3);

function dif=howmany(c1,c2)
n = 72; % 扇區數目
dif = min([abs(c1-c2), abs(c1-c2-n), abs(c1-c2+n)]);