环境用网格表示（二维信息） 每个单元赋值表示该单元有障碍的可能性 在每一步环境信息转化为1维直方图 引入代价函数值G，为所有可以通过的方向赋值 选择具有最小代价函数值G的方向
G=a·目标方向+b ·轮转动角度+c ·原运动方向
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)]);