目录
算法区别
作为路径规划的算法有很多，Dijkstra算法，遗传算法，A_star,蚁群算法，粒子群算法等，各算法有各自的优势。
1.A_star算法
A*算法在Dijkstra算法基础上进行了“启发式”改进，利用启发函数减少路径搜索的节点。
A*算法是一个“搜索算法”，实质上是广度优先搜索算法（BFS）的优化。从起点开始，首先遍历起点周围邻近的点，然后再遍历已经遍历过的点邻近的点，逐步的向外扩散，直到找到终点。 A*算法的思路类似图的Dijkstra算法，采用贪心的策略，即“若A到C的最短路径经过B，则A到B的那一段必须取最短”，找出起点到每个可能到达的点的最短路径并记录。
A*算法与Dijkstra算法的不同之处在于，A*算法是一个“启发式”算法，它已经有了一些我们告诉它的先验知识，如“朝着终点的方向走更可能走到”。它不仅关注已走过的路径，还会对未走过的点或状态进行预测。因此A*算法相交与Dijkstra而言调整了进行BFS的顺序，少搜索了哪些“不太可能经过的点”，更快地找到目标点的最短路径。另外一点，由于H选取的不同，A*算法找到的路径可能并不是最短的，但是牺牲准确率带来的是效率的提升。 ，如果可以用一种聪明的办法来判断哪些节点是有“价值”的，跳过一些不那么有意义的节点，只把那些我们认为有“价值”的节点加入 openlist ，这样显然就会提高搜索的效率。
如果可以用一种聪明的办法来判断哪些节点是有“价值”的，跳过一些不那么有意义的节点，只把那些我们认为有“价值”的节点加入 openlist ，这样显然就会提高搜索的效率。
2.JPS算法
JPS与A*算法主要区别在后继节点拓展策略上，不同于A*算法中直接获取当前节点所有非关闭的可达邻居节点来进行拓展的策略，JPS根据当前结点current的方向、并基于跳点的策略来扩展后继节点，遵循“两个定义、三个规则”。
3.搜索过程和结果对比动图
A*算法
JPS
两个定义、三个规则（重点）
两个定义
定义一，强迫邻居（forced neighbour）：
如果点n是x的邻居，并且点n的邻居有阻挡（不可行走的格子），并且从parent(x)、x、n的路径长度比其他任何从parent(x)到n且不经过x的路径短，其中parent(x)为路径中x的前一个点，则n为x的强迫邻居，x为n的跳点），例如图1.a、b中，p(x)代表parent(x)，分别展示了x的强迫邻居。
（人话：节点 x 的8个邻居中有障碍，且 x 的父节点 p 经过x 到达 n 的距离代价比不经过 x 到达的 n 的任意路径的距离代价小，则称 n 是 x 的强迫邻居。）
图1.a
图1.b
定义二，跳点（jump point）：
（1）如果点x是起点或目标点，则x是跳点，例如图2中，S是起点也是跳点，E是目标点也是跳点；
（2）如果x有邻居且是强迫邻居则x是跳点。
（3）如果parent(x)到x是对角线移动，并且x经过水平或垂直方向移动可以到达跳点，则x是跳点，例如图2中G是跳点，因为parent(G)为S，S到G为对角线移动，从G到跳点I为直线方向移动，I是跳点（因为从直线方向G到I，F是I的强迫邻居，所以I是跳点），所以G也是跳点。
（人话：
- 如果点 x是起点或目标点，则 x 是跳点。
- 如果x 有强迫邻居则 x 是跳点。
- 如果 parent(x)到 x 是对角线移动，并且 x 经过水平或垂直方向移动可以到达跳点，则 x 是跳点。）
三个规则
规则一
JPS搜索跳点的过程中，如果直线方向（为了和对角线区分，直线方向代表水平方向、垂直方向，下文所说的直线均为水平方向和垂直方向）、对角线方向都可以移动，则首先在直线方向搜索跳点，再在对角线方向搜索跳点。
规则二
（1）如果从parent(x)到x是直线移动，n是x的邻居，若有从parent(x)到n的路径不经过x且路径长度小于或等于从parent(x)经过x到n的路径，则走到x后下一个点不会走到n；
（2）如果从parent(x)到x是对角线移动，n是x的邻居，若有从parent(x)到n的路径不经过x且路径长度小于从parent(x)经过x到n的路径，则走到x后下一个点不会走到n（相关证明见论文中关于邻居修剪的规则）。
规则三
只有跳点才会加入openlist，因为跳点会改变行走方向，而非跳点不会改变行走方向，最后寻找出来的路径点也只会是跳点集合的子集。
算法流程
1.A*算法
Step 1. 将起始点start加入开启集合openlist
Step 2. 重复以下工作：
一、当openlist 为空，则结束程序，此时没有路径。
二、寻找openlist 中F值最小的节点，设为当前点current
三、从openlist 中移出当前点current
四、关闭集合closelist中加入当前点current
五、若current为目标点goal，则结束程序，此时有路径生成，此时由goal节点开始逐级追溯路径上每一个节点x的上一级父节点parent(x)，直至回溯到开始节点start，此时回溯的各节点即为路径。
六、对current的八个方向的每一个相邻点neighbor
1．如果neighbor不可通过或者已经在closelist中，略过。
2．如果neighbor不在closelist中，加入openlist 中
3．如果neighbor在openlist 中，G值判定，若此路径G值比之前路径 小，则neighbor的父节点为current，同时更新G与F值。反之，则保持原来的节点关系与G、F值。G值表示从起点到当前点路径耗费；H值表示不考虑不可通过区域，当前点到终点的理论路径耗费；F=G+H。
2.JPS算法
问题示例如图所示，5*5的网格，灰色代表阻挡区，S为起点，E为终点。 JPS要寻找从S到E的最短路径
首先初始化将S加入openlist中(绿色填充)。
从openlist取出F值最小的点S，并从openlist中删除，加入closelist（蓝色填充）
S的当前方向为空，则沿八个方向寻找跳点，在该图中只有下、右、右下三个方向可走(其他方向为边界)但向下遇到边界，向右遇到阻挡，因此都没有找到跳点，如下图
然后沿右下方向寻找跳点， 在G点，根据上文定义二的第（3）条，parent(G)为S，praent(G)到S为对角线移动，并且G经过垂直方向移动（向下移动）可以到达跳点H（黄色），因此G为跳点 ，将G加入openlist（绿色）。
从openlist取出F值最小的点G，并从openlist删除，加入closelist，因为G当前方向为对角线方向（从S到G的方向），因此在右、下、右下三个方向寻找跳点(沿该方向的水平和竖直分量的方向先寻找跳点，再沿该方向寻找)，在该图中只有向下可走，因此向下寻找跳点，根据上文定义二的第（2）条找到跳点H，将H加入openlist。
从openlist取出F值最小的点H，并从openlist删除，加入closelist，因为H的当前方向为直线方向（从G到H的方向），沿该方向向前寻找会到J然后到边界，而由于H有强迫邻居K，所以将K加入到openlist中。
从openlist取出F值最小的点K，并从openlist删除，加入closelist，因为K的当前方向为斜向方向，所以沿右，下，右下方向寻找跳点，向下到边界，向右找到点M，因为M存在强迫邻居，所以M加入到openlist中，由于为达到斜线终止条件，所以仍向右下方向寻找，先直线方向（水平和竖直方向）遇到边界，再向右下遇到障碍，达到斜向搜索终止条件，斜向搜索结束。
从openlist取出F值最小的点M，并从openlist删除，加入closelist，因为M的当前方向为直线方向，所以向右寻找跳点，P为M的强迫邻居，所以将P加入到openlist中，再向右遇到障碍物，直线搜索结束。
从openlist取出F值最小的点P，并从openlist删除，加入closelist，因为P的当前方向为斜向方向，所以沿右，上，右上方向寻找跳点，向右到边界，向上搜索到终点E，将E加入到openlist中，再向斜向方向搜索，遇到边界，斜向搜索结束。
从openlist取出F值最小的点E，因为E是目标点，因此寻路结束，路径是S、G、H、K、M、P、E。
如果到这里还很懵也没关系，下边再完整讲解一个 JPS 搜路的流程，如下图1~图10所示：
PS：下边过程非本人原创，旨在介绍清楚，如有侵权请联系删除。
- 图 1：绿色为起点，红色为终点，黑色为障碍物。
- 图 2：由前面的讲解，分别向水平和竖直方向搜索，因为一直未发现我们感兴趣的节点，所以沿着对角线方向移动。
- 图3 ：从黄色节点向水平方向搜索时，发现一个携带 forced neighbor 的节点(淡紫色)，因此结束这次搜索，同时将图 3 中的黄色节点加入 openlist ，绿色节点加入 closelist ，此时 openlist 中只有黄色节点。
- 图4 ：从 openlist 中弹出黄色节点，沿着之前的方向(绿色 -> 黄色)开始新一轮的搜索，由于依旧是对角线方向，所以先竖直检查，什么都没发现，再水平搜索，发现一个我们感兴趣的节点(淡紫色)，因为它携带 forced neighbor ，将这个淡紫色节点加入 openlist 。因为还没达到对角搜索终止的两个条件，所以继续向对角方向移动，什么都没发现，直到到达地图边界，结束此次对角线搜索。将图 4 中的黄色节点加入 closelist ，此时 openlist 中只有淡紫色节点。
- 图 5：从 openlist 中弹出黄色节点(图 4 的淡紫色节点)，沿着之前的方向(灰色-> 黄色)开始搜索，水平向右直到到达地图边界。正常来说，一次标准的直线运动到这应该结束了，但是这个黄色节点有一个 forced neighbor ，所以还必须向这个 forced neighbor 的方向进行搜索。
- 图 6：按照对角线运动的规则，沿着当前节点(黄色) -> 其 forced neighbor 的方向搜索。
- 图 7：竖直方向、水平方向均没有发现，继续对角移动。
- 图 8：按照之前的方向继续对角运动，向水平方向搜索时碰到边界，竖直方向搜索时发现目标节点，发现目标节点等同于发现了一个携带 forced neighbor 的节点，因此将当前节点(紫色)加入 openlist ，并结束这次对角搜索。
- 图 9：弹出当前 openlist 唯一的节点(黄色)，按照之前的方向对角运动，水平方向搜索碰到边界，竖直方向搜索时发现目标节点，将目标节点加入 openlist ，继续对角搜索时到达地图边界，结束此次对角搜索。
- 图 10：弹出当前 openlist 唯一的节点(红色)，发现是目标节点，因此完成整条路径的搜索，通过回溯找到最短路径。
图1 图2 图3 图4 图5
图6 图7 图8 图9
图10
其他地图算法对比
1.对比一：
A* JPS
左图为A*算法的路径图，图中红色填充点为加入到openlist中的点集，右图为JPS算法的路径图，途中黄色点为曾加入到openlist中之后又进入closelist中的点，绿色为openlist最终剩余点，可见JPS算法搜索点数小于A*算法的，因此其具有时间优势。
2.对比二
A* JPS
JPS 代码
以下为JPS算法的matlab代码，由于代码脚本较多，分开贴在这里，如果需要可以自行粘贴并建立脚本。代码经上述地图测试OK，未进行全面测试，主要用于学习，请勿作他途！
如果嫌麻烦，代码打包在这里：
如果嫌麻烦，代码打包在这里， JumpPointSearch--跳点搜索法matlab代码-交通文档类资源-CSDN文库
原论文部分译文在这里：
JPS2011算法译文部分内容学习笔记-讲义文档类资源-CSDN文库
1.main.m
clc;clear;close all
tic%计时开始
disp('JPS start!');
%% 绘基础图
map.X=20;%地图大小
map.Y=20;
map.start=[1 1];%起点
map.goal=[map.X-1 map.Y-1];%终点
obstacle=GetBoundary(map);%边界加入障碍物
obstacle=GetObstacles(obstacle,map);%生成障碍物
Fill_Plot(obstacle,'k',1)%填充障碍物
Plot_Grid(map)%绘制网格和起点、终点
%% JPS
%将起点加入openlist
% openlist的格式：jump_point_x|junmp_point_y|g_cost|direction_x|direction_y|f_cost|father_node_x|father_node_x
openlist=[map.start(1),map.start(2),0,0,0,Manhattan_cost(map.start,map.goal),0,0];
% closelist的格式：jump_point_x|junmp_point_y|g_cost|father_node_x|father_node_x
%初始化closelist
closelist=[];
%调用JPS核心代码
[openlist,closelist] = jps_core(openlist,closelist,map.start,map.goal,obstacle);
%% 绘制路径 openlist等
Fill_Plot(closelist,'y',0.5); %黄色填充closelist
Fill_Plot(openlist,[0.3,0.9,0.8],1); %淡蓝填充openlist
%将closelist中的节点x y坐标分别拿出 准备绘制路线
for pp=1:length(closelist(:,1))
x = closelist(pp,1);
y = closelist(pp,2);
A(pp,1)=x;
A(pp,2)=y;%这部分是将路径坐标拿出来另外存放
end
plot( A(:,1), A(:,2),'b','linewidth',4) %绘制路线
k=0;%用于存放路径长度的变量
for i=1:length(closelist(:,1))-1
b=10*sqrt(((closelist(i+1,1)-closelist(i,1))^2)+((closelist(i+1,2)-closelist(i,2))^2));%简单的两点间距离公式
k=k+b;%路径长度值的逐个累加
end
disp(['路径长度为',num2str(k)]);
2.GetBoundary.m
function obstacle = GetBoundary(map)
% 获取地图边界，将地图边界加入障碍物集 v2022.3.1 by jubobolv
% 输入参数：1.map 地图信息 该函数主要调用地图的X、Y最大值
% 输出参数：1.obstacle 障碍物集
obstacle=[];
for i1=0:(map.X+1)
obstacle=[obstacle;[i1,0]]; %地图下底边
end
for i2=1:(map.Y+1)
obstacle=[obstacle;[map.X+1,i2]]; %地图右侧边
end
for i3=0:(map.X)
obstacle=[obstacle;[i3,map.Y+1]]; %地图上底边
end
for i4=1:(map.Y)
obstacle=[obstacle;[0,i4]]; %地图左侧边
end
end
3.GetObstacles.m
function obstacle = GetObstacles(obstacle,map)
%设置地图中的障碍物 v2022.3.1 by jubobolv
% 输入参数：1.obstacle 未加障碍物前的障碍物集 2.map 留用
% 输出参数：1.obstacle 加入障碍物后障碍物集
new_ob=[1 9;
2,9;];
for i=2:9
new_ob=[new_ob;[5,i]];
end
for i=8:12
new_ob=[new_ob;[i,7]];
end
for i=1:4
new_ob=[new_ob;[16,i]];
end
for i=14:20
new_ob=[new_ob;[i,11]];
end
for i=5:8
new_ob=[new_ob;[13,i]];
new_ob=[new_ob;[i,13]];
end
for i=15:18
new_ob=[new_ob;[11,i]];
end
for i=1:3
new_ob=[new_ob;[11,i]];
end
new_ob=[new_ob;[10,3]];
new_ob=[new_ob;[6,18]];
new_ob=[new_ob;[6,19]];
new_ob=[new_ob;[6,20]];
new_ob=[new_ob;[13,13]];
new_ob=[new_ob;[14,15]];
obstacle=[obstacle;new_ob];
end
4. Fill_Plot.m
function Fill_Plot(coord,color,tp)
% 填充坐标栅格 v2022.3.1 by jubobolv
% 输入参数：1.coord 坐标点集 2.color 填充颜色 3.tp 填充透明度
% 输出参数：无
for i=1:length(coord(:,1))
fill([coord(i,1)-0.5,coord(i,1)+0.5,coord(i,1)+0.5,coord(i,1)-0.5],...
[coord(i,2)-0.5,coord(i,2)-0.5,coord(i,2)+0.5,coord(i,2)+0.5],color,'EdgeColor','none','FaceAlpha',tp);
hold on
end
set(gca,'FontSize',40,'Fontname', 'Times New Roman');%设置字体以及字号
set(gca,'XTick',0:5:20);
set(gca,'YTick',0:5:20);%设置坐标轴刻度
axis tight
end5.Plot_Grid.m
function Plot_Grid(map)
% 绘制栅格和起始点 v2022.3.1 by jubobolv
% 输入参数：1.map 地图信息 该函数主要调用地图的X、Y最大值
for i = 1:map.Y+3 %在地图周围有一圈围墙，所以是地图大小+3
line([-0.5,map.X+1.5],[i-1.5,i-1.5],'color',[0.5,0.5,0.5]);%划线，重点是line函数的参数用法 水平线
end
for j = 1:map.X+3
line([j-1.5,j-1.5],[-0.5,map.Y+1.5],'color',[0.5,0.5,0.5]); %竖线
end
plot(map.start(1),map.start(2),'og','MarkerSize',15,'LineWidth',3);%画出起点圈圈
hold on;
plot(map.goal(1),map.goal(2),'or','MarkerSize',15,'LineWidth',3);%画出目标点圈圈
axis([-0.5,map.X+1.5,-0.5,map.Y+1.5]);%设置坐标轴
axis equal;
end
6.jps_core.m
function [openlist,closelist] = jps_core(openlist,closelist,start,goal,obstacle)
% jps计算 v2022.3.1 by jubobolv
% 输入参数 1.openlist 加入了起点的openlist; 2.closelist 为空; 3.start 起始节点坐标； 4.goal 目标节点坐标； 5.obstacle 障碍物点集合；
% 输出参数 1.openlist 通过计算最终得到的openlist集合 2.closelist 从起点到目标节点最优路径节点集合
%任意节点周围8个邻居节点 依次为上 下 左 右 左上 右上 左下 右下
next=ToNext();
%起点的8个方向找跳点 因为是起点 所以在每个方向都寻找跳点
for i=1:8
%起点作为父节点
father_node=openlist(1,1:3);
%从父节点到当前邻居节点的方向
dir=next(i,1:2);
%判断从父节点到按d方向到当前邻居节点是否有跳点
[jump_point,~]=article_jump(father_node,dir,start,goal,obstacle);
%若有跳点 将跳点加入到openlist中
if ~isequal(jump_point,[]) %当时忘了可以这样判断--->~isempty(jump_point) 懒得改了 ^_^
% 将jump_point加入到openlist中 先将其格式化
% 格式为：jump_point_x|junmp_point_y|g_cost|direction|f_cost|father_node_x|father_node_y
successor=[jump_point(1),jump_point(2),jump_point(3),dir,jump_point(3)+...
Manhattan_cost(jump_point(1,1:2),goal),father_node(1),father_node(2)];
%将跳点插入openlist中
openlist = insert_successor(successor,openlist);
end
end
%起点周围跳点寻找完成 将起点（即openlist中此时的第一行）加入 closelist
closelist=[openlist(1,1:3),openlist(1,7:8);closelist];
% 从openlist中删除起点
openlist(1,:)=[];
%% while循环寻找到终点的路径
findFlag=false; %用于判断while循环是否结束的标签
while ~findFlag
%如果openlist为空 则没有路径 结束
if isempty(openlist(:,1))
disp('No path to goal!!');
return;
end
%判断目标点是否出现在openlist列表中，如果在返回在openlist中的行索引
[isopenFlag,Id]=isopen(goal,openlist);
if isopenFlag
disp('Find Goal!!');
toc %计时结束
%将目标点加入closelist
closelist = insert_closelist(openlist(Id,:),closelist);
openlist(Id,:)=[];
findFlag=true;
%结束循环
break;
end
[~,I]=sort(openlist(:,6)); %对OpenList按第6列（f_cost值）排序,该步骤获取每行按升序排序后的名次矩阵
openlist=(openlist(I,:));%将每行重新按升序排序
%---------------------以下：如果openlist里有f_cost相等的点,进行取舍----------ps.合理性待测试----------%
%如果openlist有多行数据
if length(openlist(:,1))>1
%如果排序后的openlist中前两行的f_cost值相等 根据距离目标值的横纵距离排序 距离大的换到openlist第一行 作为选取的节点
if isequal(openlist(1,6),openlist(2,6))
dist1=abs(openlist(1,1)-goal(1))+abs(openlist(1,2)-goal(2));
dist2=abs(openlist(2,1)-goal(1))+abs(openlist(2,2)-goal(2));
dist_big=dist1;
if dist1 < dist2 %将与目标点横纵距离大的点换到openlist第一行 作为选取的节点
temp=openlist(2,:);
openlist(2,:)=[];
openlist=[temp;openlist];
dist_big=dist2;
end
%比较openlist中其他行是否还有f_cost与第一行相同的点 如果有继续调整
for j = 3:length(openlist(:,1))
if isequal(openlist(1,6),openlist(j,6))
dist=abs(openlist(j,1)-goal(1))+abs(openlist(j,2)-goal(2));
if dist_big < dist
temp=openlist(j,:);
openlist(j,:)=[];
openlist=[temp;openlist];
dist_big=dist;
end
end
end
end
end
%----------------------------以上：如果openlist里有f_cost相等的点,进行取舍---------------------------%
%从排序后的openlist中弹出第一行（即f_cost最小的点） 获取最后将其加入时的方向和父节点
dir=openlist(1,4:5);
father_node=openlist(1,1:3);
%如果从opnelist中弹出的方向是斜向 说明上一次斜向找到了跳点但未完成斜向所有节点 需要继续该方向的跳点寻找
%由于上一次找到该跳点是斜向 说明在其直线方向的分方向存在着有强迫邻居的跳点 故先将其找出来并加入到openlist
if abs(dir(1))+abs(dir(2)) == 2
for i=1:2
if i==1 %竖直方向
new_dir=[0,dir(2)];
else %水平方向
new_dir =[dir(1),0];
end
[new_node,~]=article_jump(father_node,new_dir,start,goal,obstacle);
if ~isequal(new_node ,[])
successor=[new_node,new_dir,new_node(3)+Manhattan_cost(new_node(1,1:2),goal),...
father_node(1),father_node(2)];
openlist = insert_successor(successor,openlist);
end
end
else
%从opnelist中弹出的方向是直线，先直线寻找跳点，搜索到了就加入openlist;
[jump_point,~]=article_jump(father_node,dir,start,goal,obstacle);
if ~isequal(jump_point,[])%~isempty(jump_point)
successor=[jump_point,dir,jump_point(3)+Manhattan_cost(jump_point(1,1:2),goal),...
father_node(1),father_node(2)];
openlist = insert_successor(successor,openlist);
end
end
%判断从opnelist中弹出节点，沿其最后被加入的方向是否有强迫邻居 如果有 将其加入到openlist
[flag,forcedNeigh]=hasForcedNeigh(father_node(1,1:2),father_node(1,1:2)-dir,start,goal,obstacle);
if flag
% 找到的所有的forcedNeigh加入到openlist
for i=1:length(forcedNeigh(:,1))
new_dir=forcedNeigh(i,:)-father_node(1,1:2);
successor=[forcedNeigh(i,:),father_node(3)+14,new_dir,father_node(3)+14+Manhattan_cost(forcedNeigh(i,:),goal),...
father_node(1),father_node(2)];
openlist = insert_successor(successor,openlist);
end
end
%继续沿从opnelist中的方向向前寻找跳点
[jump_point,~]=article_jump(father_node,dir,start,goal,obstacle);
%如果存在则更新从opnelist中弹出的点到closelist中 并将其从openlist中删除 将本次找到的跳点插入到openlist
if ~isequal(jump_point,[]) %当时忘了可以这样判断--->~isempty(jump_point) 懒得改了 ^_^
successor=[jump_point,dir,jump_point(3)+Manhattan_cost(jump_point(1,1:2),goal),...
father_node(1),father_node(2)];
closelist = insert_closelist(openlist(1,:),closelist);
openlist(1,:)=[];
openlist = insert_successor(successor,openlist);
else %如果不存在则更新从opnelist中弹出的点到closelist中 并将其从openlist中删除
closelist = insert_closelist(openlist(1,:),closelist);
openlist(1,:)=[];
end
end
end
7.ToNext.m
function next_coor = ToNext()
%节点周围8个节点的坐标变化量和曼哈顿距离 v2022.3.1 by jubobolv
% 输入参数：null
next_coor=[0 1 10; %上
0 -1 10; %下
-1 0 10; %左
1 0 10; %右
-1 1 14; %左上
1 1 14; %右上
-1 -1 14; %左下
1 -1 14;]; %右下
end
8.article_jump.m
function [node,forcedNeigh]=article_jump(pre_node,dir,start,goal,obstacle)
% 判断pre_node沿dir方向是否有跳点 v.2022.3.1 by jubobolv
% 输入参数 1.pre_node 上一个节点 2.dir 从上个节点寻找跳点的方向 3.start 起点 4.goal 目标点 5.obstacle 障碍物集
% 输出参数 1.node 跳点坐标 2.forcedNeigh 强迫邻居
% 强迫邻居
forcedNeigh=[];
% 如果当前方向为直线方向 dir方向下一节点的g_cost加10 否则为斜线方向 dir方向下一节点的g_cost加14
if abs(dir(1))+abs(dir(2)) == 1
node=pre_node+[dir(1),dir(2),10];
else
node=pre_node+[dir(1),dir(2),14];
end
% 如果当前方向的下一节点是障碍物 则当前方向不存在跳点 结束寻找
if isObstacle(node(1,1:2),obstacle)
node=[];
return
end
% 如果当前方向的下一节点是目标点 结束寻找
if isequal(node(1,1:2) , goal)
return
end
% 寻找当前方向的下一节点的强迫邻居 判断是否有
[flag,forcedNeigh]=hasForcedNeigh(node(:,1:2),pre_node(:,1:2),start,goal,obstacle);
% 如果有强迫邻居则返回该节点和其强迫邻居 该强迫邻居为当前节点的强迫邻居 结束寻找
if flag
return
end
%如果当前方向为斜向 则在斜向的两个分直线方向分别寻找跳点 如果寻找到了则返回该节点 结束寻找
if abs(dir(1))+abs(dir(2)) == 2
for i=1:2
if i==1 %竖直方向
new_dir=[0,dir(2)];
else %水平方向
new_dir =[dir(1),0];
end
%在直线方向寻找跳点 如果有跳点说明当前节点跳点 达到终止条件 停止寻找
[new_node,~]=article_jump(node,new_dir,start,goal,obstacle);
if ~isequal( new_node , [])
return
end
end
end
% 以上为找到 则递归寻找
[node,~]=article_jump(node,dir,start,goal,obstacle);
end9.Manhattan_cost.m
function cost =Manhattan_cost(node,goal)
%计算启发函数代价值 ，这里采用曼哈顿算法 v2022.3.1 by jubobolv
% 输入参数：1.node 当前节点 2.目标节点
% 输出参数：1.当前节点到目标节点的曼哈顿距离
cost=10*abs(node(1)-goal(1))+10*abs(node(2)-goal(2));
end
10.insert_successor.m
function openlist = insert_successor(successor,openlist)
%插入successor到openlist 检测是否已经存在 然后进行插入 v.2022.3.1 by jubobolv
% 输入参数 1.successor待插入的跳点 2.openlist
% 思路： 在openlist中查找successor，如果可以找到，比较其f_cost，
% 若successor的f_cost大于相等 则不插入，否则将successor更新至openlist
% 如果找不到 直接插入
flag=0;
% 在openlist中查找successor
for i=1:length(openlist(:,1))
% 如果可以找到
if isequal(successor(1,1:2),openlist(i,1:2))
% 比较其f_cost 若successor的f_cost小 将successor更新至openlist
if successor(1,6) < openlist(i,6)
openlist(i,:)=successor;
return
else %若successor的f_cost大于相等已存在的 则不插入
flag=1;
end
else
continue
end
end
if flag %若successor的f_cost大于相等已存在的 则不插入
return
else %不存在则插入
openlist=[openlist;successor];
end
end11.isopen.m
function [isopenFlag,Id] = isopen(goal,openlist)
%判断目标点是否在openlist列表中，在openlist中，isopenFlag = 1,不在open中，isopenFlag = 0 .并反回索引号 v2022.3.1 by jubobolv
% 输入参数：1.goal 目标节点 2.openlist
% 输出函数：1.isopenFlag 是否在openlist列表中标签 2.Id 在openlist列表中的索引
isopenFlag = 0;
Id = 0;%初始化
if isempty(openlist)%如果open列表为空，则不在open列表中
isopenFlag = 0;
else %open列表不为空时
for i = 1:length(openlist(:,1))%列表中有多少个坐标就循环多少次
if isequal(goal(1:2),openlist(i,1:2))%在Openlist中
isopenFlag = 1;
Id = i;
return;
end
end
end
end
12.insert_closelist.m
function closelist = insert_closelist(point,closelist)
% 将跳点插入到closelist
% 此处显示详细说明
flag=0;
point_temp=[point(1,1:3),point(1,7:8)];
for i = 1:length(closelist(:,1))
if isequal(point(1,7:8),closelist(i,4:5))
closelist(i,:)=point_temp;
flag=1;
break
end
end
if flag
return
else
closelist=[point_temp;closelist];
end
end
13.isObstacle.m
function flag = isObstacle(coor,obstacle)
% 判断给定节点是否是障碍物 v2022.3.1 by jubobolv
% 输入参数：1.coor 给定节点 2.obstacle 障碍物集
flag=0;
for j = 1:length(obstacle(:,1))
if isequal(coor,obstacle(j,:))
flag=1;
break
end
end
end
14.hasForcedNeigh.m
function [flag,forcedNeigh] = hasForcedNeigh(node,pre_node,start,goal,obstacle)
%判断是否有强迫邻居 有flag=1 无flag=0 v.2022.3.1 by jubobolv
% 输入参数 1.node 当前节点 2.pre_node 上一个节点 3.start 起点 4.goal 目标点 5.obstacle 障碍物集
% 输出参数 1.flag 是否有标签 2.forcedNeigh 强迫邻居
%当前节点邻居中是否有障碍物，如果没有则返回False 如果有判断是否有强迫邻居
flag=0;
forcedNeigh=[];
%若为起点或终点 不考虑强迫邻居 否则按照从 横竖-->对角 方向 开始寻找
if isequal(node,start)
return
elseif isequal(node,goal)
return
else
if (abs(node(1)-pre_node(1))+abs(node(2)-pre_node(2))) == 1 %直线移动
if(node(2)-pre_node(2))==1 %竖直向上移动
if isObstacle([node(1)-1,node(2)],obstacle) && ~isObstacle([node(1)-1,node(2)+1],obstacle)%左侧是障碍 左前不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)+1];
end
if isObstacle([node(1)+1,node(2)],obstacle) && ~isObstacle([node(1)+1,node(2)+1],obstacle)%右侧是障碍 右前不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)+1;forcedNeigh];
end
elseif (node(2)-pre_node(2)) == -1 %竖直向下移动
if isObstacle([node(1)+1,node(2)],obstacle) && ~isObstacle([node(1)+1,node(2)-1],obstacle)%左侧是障碍 左前不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)-1];
end
if isObstacle([node(1)-1,node(2)],obstacle) && ~isObstacle([node(1)-1,node(2)-1],obstacle)%右侧是障碍 右前不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)-1;forcedNeigh];
end
elseif (node(1)-pre_node(1)) == 1 %水平向右移动
if isObstacle([node(1),node(2)+1],obstacle) && ~isObstacle([node(1)+1,node(2)+1],obstacle)%左侧是障碍 左前不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)+1];
end
if isObstacle([node(1),node(2)-1],obstacle) && ~isObstacle([node(1)+1,node(2)-1],obstacle)%右侧是障碍 右前不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)-1;forcedNeigh];
end
elseif (node(1)-pre_node(1)) == -1 %水平向左移动
if isObstacle([node(1),node(2)-1],obstacle) && ~isObstacle([node(1)-1,node(2)-1],obstacle)%左侧是障碍 左前不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)-1];
end
if isObstacle([node(1),node(2)+1],obstacle) && ~isObstacle([node(1)-1,node(2)+1],obstacle)%右侧是障碍 右前不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)+1;forcedNeigh];
end
end
else %% 对角运动
if node(1)-pre_node(1) == 1 && node(2) - pre_node(2) ==1 %斜向右上
if isObstacle([node(1)-1,node(2)],obstacle) && ~isObstacle([node(1)-1,node(2)+1],obstacle)%左侧是障碍 左上不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)+1];
end
if isObstacle([node(1),node(2)-1],obstacle) && ~isObstacle([node(1)+1,node(2)-1],obstacle)%右侧是障碍 右下不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)-1;forcedNeigh];
end
elseif node(1)-pre_node(1) == -1 && node(2) - pre_node(2) ==1 %斜向左上
if isObstacle([node(1),node(2)-1],obstacle) && ~isObstacle([node(1)-1,node(2)-1],obstacle)%左侧是障碍 左下不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)-1];
end
if isObstacle([node(1)+1,node(2)],obstacle) && ~isObstacle([node(1)+1,node(2)+1],obstacle)%右侧是障碍 右上不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)+1;forcedNeigh];
end
elseif node(1)-pre_node(1) == -1 && node(2) - pre_node(2) == -1 %斜向左下
if isObstacle([node(1)+1,node(2)],obstacle) && ~isObstacle([node(1)+1,node(2)-1],obstacle)%左侧是障碍 右下不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)-1];
end
if isObstacle([node(1),node(2)+1],obstacle) && ~isObstacle([node(1)-1,node(2)+1],obstacle)%右侧是障碍 右前不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)+1;forcedNeigh];
end
elseif node(1)-pre_node(1) == 1 && node(2) - pre_node(2) == -1 %斜向右下
if isObstacle([node(1),node(2)+1],obstacle) && ~isObstacle([node(1)+1,node(2)+1],obstacle)%左侧是障碍 右上不是障碍物
flag=1;
forcedNeigh=[node(1)+1,node(2)+1];
end
if isObstacle([node(1)-1,node(2)],obstacle) && ~isObstacle([node(1)-1,node(2)-1],obstacle)%右侧是障碍 左下不是障碍物
flag=1;
forcedNeigh=[node(1)-1,node(2)-1;forcedNeigh];
end
end
end
end
end
1084
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
