Matlab航迹规划仿真—A*算法
1. 初始化参数
主要参数:
地图大小
起始点和目标点坐标
clcclear allm = 30;n = 30;spoint = [3 3]; %起始点坐标epoint = [29 22]; %目标点坐标
2. 构建地图
-inf表示不可到达的障碍物点
%%构建地图for i = 1:m+2 if i == 1 for j = 1:n+2 matrix(i,j) = -inf; end elseif i == m+2 for j = 1:n+2 matrix(i,j) = -inf; end else for j = 1:n+2 if ((j == 1)|(j == n+2)) matrix(i,j) = -inf; else matrix(i,j) = inf; end end endend%%障碍for j=2:10 matrix(5,j)=-inf;for j=2:15 matrix(24,j)=-inf;for j=9:24%for j=6:24 matrix(10,j)=-inf;for j=20:31 matrix(15,j)=-inf;for j=5:20 matrix(20,j)=-inf;for j=18:27 matrix(28,j)=-inf;for i=2:6 matrix(i,18)=-inf;for i=17:20 matrix(i,5)=-inf;for i=23:25 matrix(i,20)=-inf;for i=13:17 matrix(i,13)=-inf;endendendendendendendendendend%end% 显示地图%subplot(2,2,1);h1 = plot(spoint(1),spoint(2),'go');hold onh2 = plot(epoint(1),epoint(2),'ro');
3. a*算法搜索路径
%%寻路matrix(spoint(1),spoint(2))=0;matrix(epoint(1),epoint(2))=inf;g=matrix;f=matrix;openlist=matrix;closelist=matrix;parentx=matrix;parenty=matrix;openlist(spoint(1),spoint(2)) =0;%closelist(epoint(1),epoint(2))=inf;for i = 1:n+2 for j = 1:m+2 k = matrix(i,j); if(k == -inf) %subplot(2,2,1); h3 = plot(i,j,'k.');% elseif(k == inf) % show green feasible point% %subplot(2,2,1);% plot(i,j,'gh');% else% %subplot(2,2,1);% plot(i,j,'gh'); end hold on endendaxis([0 m+3 0 n+3]);%subplot(2,2,1);plot(epoint(1),epoint(2),'b+');%subplot(2,2,1);plot(spoint(1),spoint(2),'b+');while(1) num=inf; for p=1:m+2 for q=1:n+2 if(openlist(p,q)==0&&closelist(p,q)~=1) outpoint=[p,q]; if(f(p,q)>=0&&num>f(p,q)) num=f(p,q); nextpoint=[p,q]; end end end end closelist(nextpoint(1),nextpoint(2))=1; for i = 1:3 for j = 1:3 k = g(nextpoint(1)-2+i,nextpoint(2)-2+j); if(i==2&&j==2|closelist(nextpoint(1)-2+i,nextpoint(2)-2+j)==1) continue; elseif (k == -inf) g(nextpoint(1)-2+i,nextpoint(2)-2+j) = g(nextpoint(1)-2+i,nextpoint(2)-2+j); closelist(nextpoint(1)-2+i,nextpoint(2)-2+j)=1; elseif (k == inf) distance=((i-2)^2+(j-2)^2)^0.5; g(nextpoint(1)-2+i,nextpoint(2)-2+j)=g(nextpoint(1),nextpoint(2))+distance; openlist(nextpoint(1)-2+i,nextpoint(2)-2+j)=0; % h=((nextpoint(1)-2+i-epoint(1))^2+(nextpoint(2)-2+j-epoint(2))^2)^0.5;%欧几里德距离启发函数 h_diagonal=min(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较复杂的对角线启发函数 h_straight=abs(nextpoint(1)-2+i-epoint(1))+abs(nextpoint(2)-2+j-epoint(2)); h=sqrt(2)*h_diagonal+(h_straight-2*h_diagonal); % h=max(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较简单的对角线函数 f(nextpoint(1)-2+i,nextpoint(2)-2+j)=g(nextpoint(1)-2+i,nextpoint(2)-2+j)+h; parentx(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(1); parenty(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(2); else distance=((i-2)^2+(j-2)^2)^0.5; if(k>(distance+g(nextpoint(1),nextpoint(2)))) k=distance+g(nextpoint(1),nextpoint(2)); % h=((nextpoint(1)-2+i-epoint(1))^2+(nextpoint(2)-2+j-epoint(2))^2)^0.5; %欧几里德距离启发函数 h_diagonal=min(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较复杂的对角线启发函数 h_straight=abs(nextpoint(1)-2+i-epoint(1))+abs(nextpoint(2)-2+j-epoint(2)); h=sqrt(2)*10*h_diagonal+10*(h_straight-2*h_diagonal); % h=max(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较简单的对角线函数 f(nextpoint(1)-2+i,nextpoint(2)-2+j)=k+h; parentx(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(1); parenty(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(2); end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; endend p=[]; s=1;while(1) if(num==inf) break; end %subplot(2,2,1); h4 = plot(epoint(1),epoint(2),'b+'); p(s,:)=epoint; s=s+1;% pause(1); xx=epoint(1); epoint(1)=parentx(epoint(1),epoint(2)); epoint(2)=parenty(xx,epoint(2)); if(parentx(epoint(1),epoint(2))==spoint(1)&&parenty(epoint(1),epoint(2))==spoint(2)) %subplot(2,2,1); plot(epoint(1),epoint(2),'b+'); p(s,:)=epoint; break; endendp(s+1,:)=spoint;legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');count=0;for i=2:12 for j=2:12 if(g(i,j)~=inf&&g(i,j)~=-inf) count=count+1; end endendcount
4. 路径优化
%将得到的折现曲线拟合成光滑的曲线p=p';a=[];b=[];a=p(1,:);b=p(2,:);figure%subplot(2,2,3);plot(a,b);axis([0,n+3,0,n+3]);values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);figure%subplot(2,2,4);plot(values(1,:),values(2,:),'r');axis([0,m+3,0,m+3]);
5. 效果图
a*路径
优化后路径
6. 下载链接
直接复制到matlab即可使用，或者也可以点击下载。
量子霸权是什么，量子霸权是什么时候实现的
Artix-7 FPGA AC701 评估套件产品描述
小鱼易连云视频，按下智慧法院“快捷键”
Waymo训练自动驾驶汽车能够识别行人和警察，还能识别交警的手势
美国超级计算机性能超中国最强超算神威太湖之光的两倍？
Matlab航迹规划仿真—A*算法
介绍一种四开关BUCK-BOOST的无缝切换空载方法
华为Mate9、荣耀V9对比评测：华为自家“双9之争”谁更厉害？
改期通知 | 关于2022自动驾驶地图与定位大会、自动驾驶与人机共架论坛改期的通知
基于Bang-Bang控制的温湿度调节系统
电子语音录放模块
点亮乡村孩子科学梦 走进vivo实验室探访手机生产“前世今生”
MAX31785 6通道智能风扇控制器
富昌电子系统设计中心SiC版逆变器方案
基于RFID技术的生产流水线自动系统实现方案
看来华为是压力很大！华为mate9再降500元，阻击小米6的心很坚定
盼着盼着，无线充电真的来了
中山小榄LED企业创新升级拓展全球新兴市场
基于c/s架构的网络化款式设计系统
ESP32、GD32、STM32MCU的区别
主要参数:
地图大小
起始点和目标点坐标
clcclear allm = 30;n = 30;spoint = [3 3]; %起始点坐标epoint = [29 22]; %目标点坐标
2. 构建地图
-inf表示不可到达的障碍物点
%%构建地图for i = 1:m+2 if i == 1 for j = 1:n+2 matrix(i,j) = -inf; end elseif i == m+2 for j = 1:n+2 matrix(i,j) = -inf; end else for j = 1:n+2 if ((j == 1)|(j == n+2)) matrix(i,j) = -inf; else matrix(i,j) = inf; end end endend%%障碍for j=2:10 matrix(5,j)=-inf;for j=2:15 matrix(24,j)=-inf;for j=9:24%for j=6:24 matrix(10,j)=-inf;for j=20:31 matrix(15,j)=-inf;for j=5:20 matrix(20,j)=-inf;for j=18:27 matrix(28,j)=-inf;for i=2:6 matrix(i,18)=-inf;for i=17:20 matrix(i,5)=-inf;for i=23:25 matrix(i,20)=-inf;for i=13:17 matrix(i,13)=-inf;endendendendendendendendendend%end% 显示地图%subplot(2,2,1);h1 = plot(spoint(1),spoint(2),'go');hold onh2 = plot(epoint(1),epoint(2),'ro');
3. a*算法搜索路径
%%寻路matrix(spoint(1),spoint(2))=0;matrix(epoint(1),epoint(2))=inf;g=matrix;f=matrix;openlist=matrix;closelist=matrix;parentx=matrix;parenty=matrix;openlist(spoint(1),spoint(2)) =0;%closelist(epoint(1),epoint(2))=inf;for i = 1:n+2 for j = 1:m+2 k = matrix(i,j); if(k == -inf) %subplot(2,2,1); h3 = plot(i,j,'k.');% elseif(k == inf) % show green feasible point% %subplot(2,2,1);% plot(i,j,'gh');% else% %subplot(2,2,1);% plot(i,j,'gh'); end hold on endendaxis([0 m+3 0 n+3]);%subplot(2,2,1);plot(epoint(1),epoint(2),'b+');%subplot(2,2,1);plot(spoint(1),spoint(2),'b+');while(1) num=inf; for p=1:m+2 for q=1:n+2 if(openlist(p,q)==0&&closelist(p,q)~=1) outpoint=[p,q]; if(f(p,q)>=0&&num>f(p,q)) num=f(p,q); nextpoint=[p,q]; end end end end closelist(nextpoint(1),nextpoint(2))=1; for i = 1:3 for j = 1:3 k = g(nextpoint(1)-2+i,nextpoint(2)-2+j); if(i==2&&j==2|closelist(nextpoint(1)-2+i,nextpoint(2)-2+j)==1) continue; elseif (k == -inf) g(nextpoint(1)-2+i,nextpoint(2)-2+j) = g(nextpoint(1)-2+i,nextpoint(2)-2+j); closelist(nextpoint(1)-2+i,nextpoint(2)-2+j)=1; elseif (k == inf) distance=((i-2)^2+(j-2)^2)^0.5; g(nextpoint(1)-2+i,nextpoint(2)-2+j)=g(nextpoint(1),nextpoint(2))+distance; openlist(nextpoint(1)-2+i,nextpoint(2)-2+j)=0; % h=((nextpoint(1)-2+i-epoint(1))^2+(nextpoint(2)-2+j-epoint(2))^2)^0.5;%欧几里德距离启发函数 h_diagonal=min(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较复杂的对角线启发函数 h_straight=abs(nextpoint(1)-2+i-epoint(1))+abs(nextpoint(2)-2+j-epoint(2)); h=sqrt(2)*h_diagonal+(h_straight-2*h_diagonal); % h=max(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较简单的对角线函数 f(nextpoint(1)-2+i,nextpoint(2)-2+j)=g(nextpoint(1)-2+i,nextpoint(2)-2+j)+h; parentx(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(1); parenty(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(2); else distance=((i-2)^2+(j-2)^2)^0.5; if(k>(distance+g(nextpoint(1),nextpoint(2)))) k=distance+g(nextpoint(1),nextpoint(2)); % h=((nextpoint(1)-2+i-epoint(1))^2+(nextpoint(2)-2+j-epoint(2))^2)^0.5; %欧几里德距离启发函数 h_diagonal=min(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较复杂的对角线启发函数 h_straight=abs(nextpoint(1)-2+i-epoint(1))+abs(nextpoint(2)-2+j-epoint(2)); h=sqrt(2)*10*h_diagonal+10*(h_straight-2*h_diagonal); % h=max(abs(nextpoint(1)-2+i-epoint(1)),abs(nextpoint(2)-2+j-epoint(2)));%比较简单的对角线函数 f(nextpoint(1)-2+i,nextpoint(2)-2+j)=k+h; parentx(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(1); parenty(nextpoint(1)-2+i,nextpoint(2)-2+j)=nextpoint(2); end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; end end if(((nextpoint(1)-2+i)==epoint(1)&&(nextpoint(2)-2+j)==epoint(2))|num==inf) parentx(epoint(1),epoint(2))=nextpoint(1); parenty(epoint(1),epoint(2))=nextpoint(2); break; endend p=[]; s=1;while(1) if(num==inf) break; end %subplot(2,2,1); h4 = plot(epoint(1),epoint(2),'b+'); p(s,:)=epoint; s=s+1;% pause(1); xx=epoint(1); epoint(1)=parentx(epoint(1),epoint(2)); epoint(2)=parenty(xx,epoint(2)); if(parentx(epoint(1),epoint(2))==spoint(1)&&parenty(epoint(1),epoint(2))==spoint(2)) %subplot(2,2,1); plot(epoint(1),epoint(2),'b+'); p(s,:)=epoint; break; endendp(s+1,:)=spoint;legend([h1,h2,h3,h4],'起始点','目标点','障碍物','航迹点');count=0;for i=2:12 for j=2:12 if(g(i,j)~=inf&&g(i,j)~=-inf) count=count+1; end endendcount
4. 路径优化
%将得到的折现曲线拟合成光滑的曲线p=p';a=[];b=[];a=p(1,:);b=p(2,:);figure%subplot(2,2,3);plot(a,b);axis([0,n+3,0,n+3]);values = spcrv([[a(1) a a(end)];[b(1) b b(end)]],3);figure%subplot(2,2,4);plot(values(1,:),values(2,:),'r');axis([0,m+3,0,m+3]);
5. 效果图
a*路径
优化后路径
6. 下载链接
直接复制到matlab即可使用，或者也可以点击下载。
量子霸权是什么，量子霸权是什么时候实现的
Artix-7 FPGA AC701 评估套件产品描述
小鱼易连云视频，按下智慧法院“快捷键”
Waymo训练自动驾驶汽车能够识别行人和警察，还能识别交警的手势
美国超级计算机性能超中国最强超算神威太湖之光的两倍？
Matlab航迹规划仿真—A*算法
介绍一种四开关BUCK-BOOST的无缝切换空载方法
华为Mate9、荣耀V9对比评测：华为自家“双9之争”谁更厉害？
改期通知 | 关于2022自动驾驶地图与定位大会、自动驾驶与人机共架论坛改期的通知
基于Bang-Bang控制的温湿度调节系统
电子语音录放模块
点亮乡村孩子科学梦 走进vivo实验室探访手机生产“前世今生”
MAX31785 6通道智能风扇控制器
富昌电子系统设计中心SiC版逆变器方案
基于RFID技术的生产流水线自动系统实现方案
看来华为是压力很大！华为mate9再降500元，阻击小米6的心很坚定
盼着盼着，无线充电真的来了
中山小榄LED企业创新升级拓展全球新兴市场
基于c/s架构的网络化款式设计系统
ESP32、GD32、STM32MCU的区别