JPS跳点搜索算法流程、注意事项、matlab代码实现

水瓶小小

已于 2024-10-10 23:20:52 修改

阅读量818

点赞数 7

文章标签：算法 matlab

于 2024-09-26 16:45:11 首次发布

本文链接：https://blog.csdn.net/m0_69611489/article/details/142559999

版权

一、JPS跳点搜索算法简介

JPS跳点搜索算法[1]是一种基于图搜索的路径规划算法，它保留了 A*算法的启发式搜索机制，仅在扩展节点的方式上有所不同。JPS 算法通过跳点搜索策略来识别并有选择性地扩展栅格地图中满足条件的节点，从而有效地减少搜索空间，提高搜索效率。JPS 算法与 A*算法的启发式评价函数一致。

[1]任祥瑞,王正刚,汤俊杨.改进JPS算法融合DWA的多机器人路径规划[J/OL].计算机应用研究,1-9[2024-09-26].https://doi.org/10.19734/j.issn.1001-3695.2024.03.0099.

二、算法流程

1）记估价函数为F，从初始节点到当前节点实际代价为G，从当前节点到目标节点的估计代价H，开放集合K，关闭集合B，根节点集合P；

2）将起点S放入开放集合K中，由于起点S到自身的距离为0，记G(S)=0，P(S)=0;

3）若K中无节点则结束，否则计算开放集合K中所有点的F（F=G+H），取F最小的点为当前点N（F值有多个时任意取一个即可）；

4）按照拓展规则将点N周围拓展点记为集合W，并将集合W中的点放入开放集合K中；

拓展规则：

根据当前点N的父节点P(N)是水平/竖直或者斜向移动至当前点N，确定删除的点，并加入强迫节点：

①若当前点N的父节点P(N)是水平/竖直移动至当前点N，删除满足以下条件的节点：

水平移动

以上图为例，节点4为节点x父节点，此时根据计算除节点5外，其他节点均满足上式被删除，例如：节点3，4→x→3为，4→2→3也为，因此相等满足上式，被剔除。

以此为例，可推出：

节点x的父节点为4时，只有节点5为拓展节点；父节点为7时，只有节点2为拓展节点；

节点x的父节点为5时，只有节点4为拓展节点；父节点为2时，只有节点7为拓展节点；

②若当前点N的父节点P(N)是斜向移动至当前点N，删除满足以下条件的节点：

符号表示与上式相同，但此时仅小于才会删除，等于不会删除，同样也会作为拓展节点。

斜向移动

以上图为例，节点6为节点x父节点，此时根据计算除节点2、3、5外，其他节点均满足上式被删除，例如：节点4，6→4为1，6→x→4为，因此小于满足上式，被剔除。

以此为例，可推出：

节点x的父节点为6时，节点2、3、5为拓展节点；

节点x的父节点为1时，节点5、7、8为拓展节点；

节点x的父节点为8时，节点1、2、4为拓展节点；

节点x的父节点为3时，节点4、6、7为拓展节点；

③强迫节点：

如果节点 x 的周围（水平或竖直，斜向不算！）存在障碍物（黑色节点），满足式(3.4)条件的劣性节点 n 将被定义为强迫扩展节点，重新加入扩展列表，并称当前节点 x 为关键节点。

5）若W中的点G(W)>G(N)+Distance(N,W)，则令H(W)=G(N)+Distance(N,W)，P(W)=N，并把N放入关闭集合B中；

6）若W中包含目标点E，则结束，否则重复步骤3）~5）。

三、实现结果

对比A*算法结果：

JPS算法结果：路径长度为7.8284

A*算法结果：路径长度为8.6569

四、代码实现

完整代码有偿，私聊！

clc
clear
close all
%% 设置地图
map=[0 0 0 0 0;0 0 1 1 0;1 0 0 1 0;0 1 0 1 0;0 0 0 1 0;0 0 0 0 0];%地图
S=[0.5 0.5];  %起点
E=[4.5 5.5];  %终点
G=inf*ones(size(map));%实际代价
P=cell(size(map));    %根节点
B=zeros(size(map));   %关闭
K=zeros(size(map));   %开放
Limit=cal(map);
%% 处理起点
Ju={[0 1 0;0 0 0;0 0 0];[0 1 1;0 0 1;0 0 0];[0 0 0;0 0 1;0 0 0];[0 0 0;0 0 1;0 1 1];
    [0 0 0;0 0 0;0 1 0];[0 0 0;1 0 0;1 1 0];[0 0 0;1 0 0;0 0 0];[1 1 0;1 0 0;0 0 0]};
DirE=[8 1 2;7 0 3;6 5 4];
%obj=Direction(N,P);
J_S=Change(map,S,1);
G(J_S(1),J_S(2))=0;
for obj=1:8
    PN=S;
    PP=S;
    [G,P,B,K]=cal_P(G,P,B,K,obj,Ju,map,Limit,PN,E);
end
%处理终点
N_E=Change(map,E,1);
%% 循环寻路
while 1
    [chx,chy]=find(K==1);
    if isempty(chx)
        disp('未找到路径')
        break
    end
    W=[];
    for i=1:size(chx,1)
        Sch=Change(map,[chx(i),chy(i)],-1);
        W=[W,G(chx(i),chy(i))+norm(E-Sch)];
    end
    [~,index]=min(W);
    index=index(1);
    chW=[chx(index),chy(index)];
    SJ_chW=Change(map,chW,-1);
    JP_chW=P{chW(1),chW(2)};
    SP_chW=Change(map,JP_chW,-1);
    obj=Direction(SJ_chW,SP_chW);
    [io,NP]=Forced_N(obj,Ju,map,Limit,SJ_chW,E);
    cu=DirE(NP==1);
    for i=1:size(cu,1)
        obj=cu(i);
        [G,P,B,K]=cal_P(G,P,B,K,obj,Ju,map,Limit,SJ_chW,E);    
    end
    if G(N_E(1),N_E(2))~=inf
        break
    end
end
%% 倒推路径
Pe=Change(map,E,1);
Ps=Change(map,S,1);
Pn=Pe;
Path=E;
while sum(abs(Pn-Ps))~=0
    CH_p=P{Pn(1),Pn(2)};
    Path=[Path;Change(map,CH_p,-1)];
    Pn=CH_p;
end
Path=flipud(Path);
%% 画图
[m,n]=size(map);
for i=0:m
    plot([0 n],[i i],'-k');
    hold on
end
for j=0:n
    plot([j j],[0 m],'-k');
    hold on
end
for i=1:m
    for j=1:n
        if map(i,j)==1
            fill([j-0.5-0.5 j-0.5+0.5 j-0.5+0.5 j-0.5-0.5],[m-i+0.5-0.5 m-i+0.5-0.5 m-i+0.5+0.5 m-i+0.5+0.5],'k')
            hold on
        end
    end
end
plot(S(1),S(2),'.g','MarkerSize',20)
hold on
text(S(1),S(2),'起点')
hold on
plot(E(1),E(2),'.m','MarkerSize',20)
hold on
text(E(1),E(2),'终点')
hold on
plot(Path(:,1),Path(:,2),'-r','LineWidth',2)

D=0;
for i=1:size(Path,1)-1
D=D+norm(Path(i,:)-Path(i+1,:));
end
disp(['路径长度为',num2str(D)])