[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 <=
vfh.rar_VFH的路径规划_vfh_vfh*_向量直方图_路径规划VFH
版权申诉
5星 · 超过95%的资源 121 浏览量
2022-09-20
18:42:02
上传
评论
收藏 3KB RAR 举报
刘良运
- 粉丝: 66
- 资源: 1万+
最新资源
- tiamo软件教程.doc
- nvm管理Nodejs多版本工具
- Unit1Howcanwebecomegoodlearners知识点整理(良心出品必属精品).doc
- 基于JSP在线维他茶饮销售网站平台源码.zip
- web学习笔记.doc
- 基于pytorch+Unet进行MRI肝脏图像分割源码+数据集+模型.zip
- 在Android Studio中开发一个Android App项目步骤
- 基于yolov8实现进行物体跟踪源码.zip
- Java多线程学习Java多线程学习Java多线程学习Java多线程学习.txt
- 算法数据结构-动态规划算法(Dynamic Programming)超详细总结加应用案例讲解.txt
资源上传下载、课程学习等过程中有任何疑问或建议,欢迎提出宝贵意见哦~我们会及时处理!
点击此处反馈
评论2