网上关于JPS算法原理介绍的文章有很多，本文就不过多的介绍了，直接从JPS算法源码开始分析
github上面有一个c++写的JPS算法源码仓库：https://github.com/KumarRobotics/jps3d
还有一个ros的JPS算法插件源码仓库，它依赖于上面的jps3d库：https://github.com/eborghi10/jps_global_planner
jps3d库中jps算法的主要执行部分在src目录下的：jps_planner.cpp和graph_search.cpp文中
首先分析jps_planner.cpp和jps_planner.h
在JPSPlanner类里面的构造函数会对地图进行初始化
JPSPlanner<Dim>::JPSPlanner(bool verbose): planner_verbose_(verbose) {
planner_verbose_ = verbose;
if(planner_verbose_)
printf(ANSI_COLOR_CYAN "JPS PLANNER VERBOSE ON\n" ANSI_COLOR_RESET);
}
template <int Dim>
void JPSPlanner<Dim>::setMapUtil(const std::shared_ptr<JPS::MapUtil<Dim>> &map_util) {
map_util_ = map_util;
}
//栅格地图更新，将栅格地图存储在cmap_中，1表示阳，0表示非占据
template <int Dim>
void JPSPlanner<Dim>::updateMap() {
Veci<Dim> dim = map_util_->getDim();
if(Dim == 3) {
cmap_.resize(dim(0)*dim(1)*dim(2));
for( int z = 0; z < dim(2); ++z) {
for( int y = 0; y < dim(1); ++y) {
for( int x = 0; x < dim(0); ++x) {
Veci<Dim> pn;
pn << x, y, z;
cmap_[x+y*dim(0)+z*dim(0)*dim(1)] = map_util_->isOccupied(pn) ? 1:0;
}
}
}
}
else {
cmap_.resize(dim(0)*dim(1));
for( int y = 0; y < dim(1); ++y)
for( int x = 0; x < dim(0); ++x)
cmap_[x+y*dim(0)] = map_util_->isOccupied(Veci<Dim>(x,y)) ? 1:0;
}
}
jps_planner.cpp中最关键的函数是JPSPlanner::plan，主要步骤为：
1、首先对将起点和终点转换为栅格地图的坐标点，并进行合法性检查
2、然后根据地图维度实例化一个2D或在3D的JPS::GraphSearch对象，并执行graph_search_->plan();函数开始进行搜索
if(Dim == 3) {
graph_search_ = std::make_shared<JPS::GraphSearch>(cmap_.data(), dim(0), dim(1), dim(2), eps, planner_verbose_);
graph_search_->plan(start_int(0), start_int(1), start_int(2), goal_int(0), goal_int(1), goal_int(2), use_jps);
}
else {
graph_search_ = std::make_shared<JPS::GraphSearch>(cmap_.data(), dim(0), dim(1), eps, planner_verbose_);
graph_search_->plan(start_int(0), start_int(1), goal_int(0), goal_int(1), use_jps);
}3、得到初始路径，最终存在raw_path_
//**** raw path, s --> g
vec_Vecf<Dim> ps;
for (const auto &it : path) {
if(Dim == 3) {
Veci<Dim> pn;
pn << it->x, it->y, it->z;
ps.push_back(map_util_->intToFloat(pn));
}
else
ps.push_back(map_util_->intToFloat(Veci<Dim>(it->x, it->y)));
}
raw_path_ = ps;
std::reverse(std::begin(raw_path_), std::end(raw_path_));4、最后对得到的初始路径进行一些优化，这些其实跟JPS算法已经关系不大了
path_ = removeCornerPts(raw_path_);
std::reverse(std::begin(path_), std::end(path_));
path_ = removeCornerPts(path_);
std::reverse(std::begin(path_), std::end(path_));
path_ = removeLinePts(path_);
然后进入到graph_search.cpp文件里面，分析下GraphSearch::plan函数，GraphSearch::plan函数有两个重载，一个用于2D地图，一个用于3D地图，下面分析2D地图的代码。GraphSearch::plan函数设置起点、终点，并计算法起点状态。然后调用plan函数
bool GraphSearch::plan(int xStart, int yStart, int xGoal, int yGoal, bool useJps, int maxExpand)
{
use_2d_ = true;
pq_.clear();
path_.clear();
hm_.resize(xDim_ * yDim_);
seen_.resize(xDim_ * yDim_, false);
// Set jps
use_jps_ = useJps;
// Set goal
int goal_id = coordToId(xGoal, yGoal);
xGoal_ = xGoal; yGoal_ = yGoal;
// Set start node
int start_id = coordToId(xStart, yStart);
StatePtr currNode_ptr = std::make_shared<State>(State(start_id, xStart, yStart, 0, 0));
currNode_ptr->g = 0;
currNode_ptr->h = getHeur(xStart, yStart);
return plan(currNode_ptr, maxExpand, start_id, goal_id);
}
plan的函数执行的流程就是我们所熟悉的A*算法流程：
1、维护一个优先队列openlist，每次从openlist里面取出优先级最高的节点
2、对该节点的邻居进行检查，查看是否能加入openlist
3、若可以加入openlist则计算节点的g值和h值，相加作为该节点的优先级，并设置该节点的夫节点为当前节点
4、将当前节点加入closelist。循环执行1-4直到找到目标节点
bool GraphSearch::plan(StatePtr& currNode_ptr, int maxExpand, int start_id, int goal_id) {
// Insert start node
currNode_ptr->heapkey = pq_.push(currNode_ptr);
currNode_ptr->opened = true;
hm_[currNode_ptr->id] = currNode_ptr;
seen_[currNode_ptr->id] = true;
int expand_iteration = 0;
while(true)
{
expand_iteration++;
// get element with smallest cost //从优先队列里面拿出代价最小的节点并弹出这个节点
currNode_ptr = pq_.top(); pq_.pop();
currNode_ptr->closed = true; // Add to closed list //将弹出的节点加入clodelist
if(currNode_ptr->id == goal_id) {
if(verbose_)
printf("Goal Reached!!!!!!\n\n");
break;
}
//printf("expand: %d, %d\n", currNode_ptr->x, currNode_ptr->y);
std::vector<int> succ_ids;
std::vector<double> succ_costs;
// Get successors //successors:继任者，这里意思是子节点
//得到能够顺利到达的子节点ID，并且计算从当前节点到达子节点的cost
//如果是A*则将子节点是周围节点，如果是JPS则子节点是跳点
if(!use_jps_)
getSucc(currNode_ptr, succ_ids, succ_costs);
else
getJpsSucc(currNode_ptr, succ_ids, succ_costs);
// if(verbose_)
// printf("size of succs: %zu\n", succ_ids.size());
// Process successors
//判断所有找出的子节点能否被加入openlist 和 优先队列pq_
for( int s = 0; s < (int) succ_ids.size(); s++ )
{
//see if we can improve the value of succstate
StatePtr& child_ptr = hm_[succ_ids[s]];
double tentative_gval = currNode_ptr->g + succ_costs[s];
if( tentative_gval < child_ptr->g )
{
child_ptr->parentId = currNode_ptr->id; // Assign new parent
child_ptr->g = tentative_gval; // Update gval
//double fval = child_ptr->g + child_ptr->h;
// if currently in OPEN, update
if( child_ptr->opened && !child_ptr->closed) {
pq_.increase( child_ptr->heapkey ); // update heap
child_ptr->dx = (child_ptr->x - currNode_ptr->x);
child_ptr->dy = (child_ptr->y - currNode_ptr->y);
child_ptr->dz = (child_ptr->z - currNode_ptr->z);
if(child_ptr->dx != 0)
child_ptr->dx /= std::abs(child_ptr->dx);
if(child_ptr->dy != 0)
child_ptr->dy /= std::abs(child_ptr->dy);
if(child_ptr->dz != 0)
child_ptr->dz /= std::abs(child_ptr->dz);
}
// if currently in CLOSED
else if( child_ptr->opened && child_ptr->closed)
{
printf("ASTAR ERROR!\n");
}
else // new node, add to heap
{
//printf("add to open set: %d, %d\n", child_ptr->x, child_ptr->y);
child_ptr->heapkey = pq_.push(child_ptr);
child_ptr->opened = true;
}
} //
} // Process successors
//如果扩展节点次数大于最大阈值，则结束搜索，没有找到终点
if(maxExpand > 0 && expand_iteration >= maxExpand) {
if(verbose_)
printf("MaxExpandStep [%d] Reached!!!!!!\n\n", maxExpand);
return false;
}
//如果优先队列里面没有节点，整个地图已经被搜索完毕，没有找到终点
if( pq_.empty()) {
if(verbose_)
printf("Priority queue is empty!!!!!!\n\n");
return false;
}
}
if(verbose_) {
printf("goal g: %f, h: %f!\n", currNode_ptr->g, currNode_ptr->h);
printf("Expand [%d] nodes!\n", expand_iteration);
}
path_ = recoverPath(currNode_ptr, start_id);
return true;
}jps和A*算法的区别是，jps会向一个方向一直搜索，直到找到一个跳点。jps会将这个跳点加入到openlist。
跳点定义：当前点 x 满足以下三个条件之一：
- 节点 x 是起点/终点。
- 节点 x 至少有一个强迫邻居。
- 如果父节点在斜方向（意味着这是斜向搜索），节点x的水平或垂直方向上有满足条件a，b的点。
强迫邻居定义：节点 x 的8个邻居中有障碍，且 x 的父节点 p 经过x 到达 n 的距离代价比不经过 x 到达的 n 的任意路径的距离代价小，则称 n 是 x 的强迫邻居。
跳点想理解得更详细可看这篇博客：JPS/JPS+ 寻路算法_GJQI12的博客-CSDN博客
plan函数中GraphSearch::getJpsSucc函数用来寻找跳点，GraphSearch::getJpsSucc函数中调用jump函数寻找跳点。我对2D地图搜索部分进行了注释，3D地图其实也是差不多的过程。
//寻找当前节点的子节点，即寻找跳点
void GraphSearch::getJpsSucc(const StatePtr& curr, std::vector<int>& succ_ids, std::vector<double>& succ_costs) {
if(use_2d_) {
const int norm1 = std::abs(curr->dx)+std::abs(curr->dy);
int num_neib = jn2d_->nsz[norm1][0];
int num_fneib = jn2d_->nsz[norm1][1];
int id = (curr->dx+1)+3*(curr->dy+1);
//对当前所有的邻居方向和强迫节点方向进行搜索
// no move (norm 0): 8 neighbors always added
// 0 forced neighbors to check (never happens)
// 0 neighbors to add if forced (never happens)
// straight (norm 1): 1 neighbor always added
// 2 forced neighbors to check
// 2 neighbors to add if forced
// diagonal (norm sqrt(2)): 3 neighbors always added
// 2 forced neighbors to check
// 2 neighbors to add if forced
for( int dev = 0; dev < num_neib+num_fneib; ++dev) {
int new_x, new_y;
int dx, dy;
//对必定存在的邻居进行搜索
if(dev < num_neib) {
dx = jn2d_->ns[id][0][dev];
dy = jn2d_->ns[id][1][dev];
if(!jump(curr->x, curr->y, dx, dy, new_x, new_y)) continue;
}
//对可能存在的强迫邻居进行搜索
else {
int nx = curr->x + jn2d_->f1[id][0][dev-num_neib];
int ny = curr->y + jn2d_->f1[id][1][dev-num_neib];
//如果当前节点旁边有障碍物则可能存在强迫节点，需要搜索
if(isOccupied(nx,ny)) {
dx = jn2d_->f2[id][0][dev-num_neib];
dy = jn2d_->f2[id][1][dev-num_neib];
if(!jump(curr->x, curr->y, dx, dy, new_x, new_y)) continue;
}
else
continue;
}
//jump返回false则该搜索方向碰到了障碍物、边界，continue结束本次循环
//jump返回true则找到了跳点，进行跳点的存储
int new_id = coordToId(new_x, new_y);
if(!seen_[new_id]) {
seen_[new_id] = true;
hm_[new_id] = std::make_shared<State>(new_id, new_x, new_y, dx, dy);
hm_[new_id]->h = getHeur(new_x, new_y);
}
//将跳点ID以及当前点到跳点的cost存储起来
succ_ids.push_back(new_id);
succ_costs.push_back(std::sqrt((new_x - curr->x) * (new_x - curr->x) +
(new_y - curr->y) * (new_y - curr->y)));
}
}
else {
const int norm1 = std::abs(curr->dx)+std::abs(curr->dy)+std::abs(curr->dz);
int num_neib = jn3d_->nsz[norm1][0];
int num_fneib = jn3d_->nsz[norm1][1];
int id = (curr->dx+1)+3*(curr->dy+1)+9*(curr->dz+1);
for( int dev = 0; dev < num_neib+num_fneib; ++dev) {
int new_x, new_y, new_z;
int dx, dy, dz;
if(dev < num_neib) {
dx = jn3d_->ns[id][0][dev];
dy = jn3d_->ns[id][1][dev];
dz = jn3d_->ns[id][2][dev];
if(!jump(curr->x, curr->y, curr->z,
dx, dy, dz, new_x, new_y, new_z)) continue;
}
else {
int nx = curr->x + jn3d_->f1[id][0][dev-num_neib];
int ny = curr->y + jn3d_->f1[id][1][dev-num_neib];
int nz = curr->z + jn3d_->f1[id][2][dev-num_neib];
if(isOccupied(nx,ny,nz)) {
dx = jn3d_->f2[id][0][dev-num_neib];
dy = jn3d_->f2[id][1][dev-num_neib];
dz = jn3d_->f2[id][2][dev-num_neib];
if(!jump(curr->x, curr->y, curr->z,
dx, dy, dz, new_x, new_y, new_z)) continue;
}
else
continue;
}
int new_id = coordToId(new_x, new_y, new_z);
if(!seen_[new_id]) {
seen_[new_id] = true;
hm_[new_id] = std::make_shared<State>(new_id, new_x, new_y, new_z, dx, dy, dz);
hm_[new_id]->h = getHeur(new_x, new_y, new_z);
}
succ_ids.push_back(new_id);
succ_costs.push_back(std::sqrt((new_x - curr->x) * (new_x - curr->x) +
(new_y - curr->y) * (new_y - curr->y) +
(new_z - curr->z) * (new_z - curr->z)));
}
}
}jump是一个递归函数，会在一个方向上一直搜索直到碰到障碍物、边界、跳点。
bool GraphSearch::jump(int x, int y, int dx, int dy, int& new_x, int& new_y ) {
new_x = x + dx;
new_y = y + dy;
if (!isFree(new_x, new_y))
return false;
if (new_x == xGoal_ && new_y == yGoal_)
return true;
//判断当前点是否有强迫节点
if (hasForced(new_x, new_y, dx, dy))
return true;
//继续在当前方向上深度搜索
//norm1不可能等于0，因为只有起始点的年norm1等于0
//norm1 == 1直接执行return jump，因为直线方向只有一个方向要搜索
//norm1 == 2先执行for里面的jump 2次再执行return jump
//因为斜线方向需要先搜索两个直线方向再在斜线方向搜索
const int id = (dx+1)+3*(dy+1);
const int norm1 = std::abs(dx) + std::abs(dy);
int num_neib = jn2d_->nsz[norm1][0];
for( int k = 0; k < num_neib-1; ++k )
{
int new_new_x, new_new_y;
if(jump(new_x, new_y, jn2d_->ns[id][0][k], jn2d_->ns[id][1][k],
new_new_x, new_new_y)) return true;
}
return jump(new_x, new_y, dx, dy, new_x, new_y);
}在graph_search.h文件中有个JPS2DNeib结构体
有个常量数组static constexpr int nsz[3][2] = {{8, 0}, {1, 2}, {3, 2}};
///Search and prune neighbors for JPS 2D
struct JPS2DNeib {
// for each (dx,dy) these contain:
// ns: neighbors that are always added
// f1: forced neighbors to check //需要检查是否是障碍物的邻居
// f2: neighbors to add if f1 is forced //检查是否是强迫邻居
int ns[9][2][8];
int f1[9][2][2];
int f2[9][2][2];
// nsz contains the number of neighbors for the four different types of moves:
// no move (norm 0): 8 neighbors always added
// 0 forced neighbors to check (never happens)
// 0 neighbors to add if forced (never happens)
// straight (norm 1): 1 neighbor always added
// 2 forced neighbors to check
// 2 neighbors to add if forced
// diagonal (norm sqrt(2)): 3 neighbors always added
// 2 forced neighbors to check
// 2 neighbors to add if forced
static constexpr int nsz[3][2] = {{8, 0}, {1, 2}, {3, 2}};
void print();
JPS2DNeib();
private:
void Neib(int dx, int dy, int norm1, int dev, int& tx, int& ty);
void FNeib(int dx, int dy, int norm1, int dev,
int& fx, int& fy, int& nx, int& ny);
};
nsz[0] = {8,0}表示运动方向的曼哈顿距离 abs(dx)+abs(dy) == 0时，即起点处初始没有运动方向，他会有8个必定要被检查邻居，0个可能的强迫邻居
nsz[0] = {1,2}表示运动方向的曼哈顿距离 abs(dx)+abs(dy) == 1时，即直线运动方向，他会有1个必定要被检查邻居，即下图中的栅格5，有2个可能的强迫邻居，即下图中的栅格3和8.
nsz[0] = {3,2}表示运动方向的曼哈顿距离 abs(dx)+abs(dy) == 2时，即斜线运动方向，他会有3个必定要被检查邻居即栅格4、7、x，2个可能的强迫邻居即栅格1、8
JPS2DNeib结构体中
// for each (dx,dy) these contain:
// ns: neighbors that are always added
// f1: forced neighbors to check //需要检查是否是障碍物的邻居
// f2: neighbors to add if f1 is forced //检查是否是强迫邻居
int ns[9][2][8];
int f1[9][2][2];
int f2[9][2][2];ns表示根据当前移动方向必须要检查的邻居，如：dx = 0,dy = 0.表示起点，有8个必被检查的邻居；dx = 1，dy = 0，表示移动方向向右，当前节点右边的节点必须被检查；dx = 1，dy = 1，表示移动方向为右上方，当前节点上方、右方、右上方3个节点必须被检查。
f1表示需要检查是否有障碍物的位置，f2表示如果存在障碍物这需要检查的强迫邻居节点的位置。以上所说的位置都用相对位置表示。如向右上方运动，则f1里面存储的是栅格4和7的坐标，算法将检查栅格4和7有没有被障碍物占据，如果有障碍物，f2里面存储的是栅格1和8的坐标，算法将栅格1和8是否为x的强迫邻居。
本文参考：
495
被折叠的 条评论
为什么被折叠?
到【灌水乐园】发言
