文章目录
- A*算法在扩展节点过程中,会出现许多对称路径,形状类似且cost相同。这些节点会被均匀的扩散访问,最终得到规划结果时,会扩展许多无用节点。因此需要在许多类似的路径中带有倾向性的选择其中一条,节省计算资源。
- JPS算法基本步骤与A*相同,在选择需要扩展的邻居节点中有些不同。
- A*扩展几何上的所有相邻点,适用于栅格地图或拓扑地图。
- JPS带有选择性的扩展邻居,以得到路径上影响运行方向的中继点,在中继点前后路段的方向不同(水平、垂直、对角)。带有倾向性的优先选择某个方向。最终生成的路径也是几何上间断的点集。但仅适用于栅格地图。
1.look ahead rule
寻找邻居节点:通过父节点和子节点的连接方向,及障碍物的相对位置,选择需要扩展的邻居节点。
邻居节点的分类
- inferior neighbors:劣性邻居,不需要扩展。判断条件:从父节点直接到达此节点的cost 比父节点经由子节点再到达此节点的cost更加cheaper, c o s t y → n cost_{y\rightarrow n} costy→n <= c o s t y → x → n cost_{y\rightarrow x\rightarrow n} costy→x→n。
- natural neighbors:天然邻居,需要扩展。判断条件:从父节点经由子节点再到达此节点的cost 更cheaper, c o s t y → n cost_{y\rightarrow n} costy→n > c o s t y → x → n cost_{y\rightarrow x\rightarrow n} costy→x→n
- forced neighbors: 强制邻居,需要扩展。判断条件:父节点直接到达此节点的cost更cheaper,但路径被障碍物挡住,只能经由子节点到达。
子节点选择扩展邻居的准则
- 无障碍物时:当前节点x,父节点y,邻居节点n。若 c o s t y → n cost_{y\rightarrow n} costy→n <= c o s t y → x → n cost_{y\rightarrow x\rightarrow n} costy→x→n,那么节点n是不需要x扩展的邻居。
- 有障碍物时:判断是否有forced neighbor
图例:
- 横向扩展时 (竖直向下、竖直向上、横向向左时等价)
- 蓝色格点为父节点y,
- 黄色格点为子节点x,
- 黑色格点为障碍物
- 灰色格点为inferior neighbors
- 白色格点为 natural neighbors
- 紫色格点为forced neighbors
- 对角扩展时 (对角左下、对角右下、对角右上等价)
2. jumping rule
JPS在扩展邻居节点时,并非直接把邻居节点放入open list中,而是通过邻居节点m不断递归跳跃找到中继点(jump point)(在中继点上,路径的方向发生改变),将中继点放入open list中待扩展。
规则
- 如果邻居节点为target node,邻居节点为jump point
- 如果邻居节点有forced neighbor,邻居节点为jump point (代表路径在经过邻居节点时方向会发生改变)
- 递归的横向/垂直(straight pruning rule)扩展邻居节点的同向邻居
- 同向邻居允许通行,递归扩展同向邻居的同向邻居
- 同向邻居为地图边界、或是障碍物,跳跃失败,没有找到跳跃点
- 同向邻居有forced neighbor, 此同向邻居是jump point,事实上最初的邻居也是jump point, 具体见伪代码。
- 递归的对角(diagonal pruning rule)扩展,对角向扩展一步,在此点先横向/垂直的搜索jump point, 若没找到,继续对角扩展一步
最终生成路径只有几个间隔的跳跃点。
3.代码
3.1整体伪代码
def Main
起点s加进OpenList中
While(OpenList.Count > 0):
从OpenList中取出f值最小的点并设置为当前点n
把当前点n加进CloseList
邻居点set = 获取邻居点(当前点)
for m : 邻居点set
跳跃点jump_point = 递归寻找跳跃点(m)
if (jump_point) && (jump_point not in CloseList):
计算jump_point 的g值new_g=n.g + edge_cost(n,jump_point)
计算jump_point 的h值
计算new_f = g+h
if (jump_point in openList) && (new_f < jump_point .f):
jump_point.parent_node = n
jump_point.g = new_g
jump_point.f = new_f
else
jump_point.g = new_g
jump_point.h = h
jump_point.f = new_f
jump_point.parent_node = n
OpenList.add(jump_point)
3.2获取邻居节点
def 获取邻居点
if 当前点是起点
返回当前点九宫格内的非障碍点
elseif 当前点与父节点是对角向
判断并添加相对位置右方的邻居点
判断并添加相对位置下方的邻居点
判断并添加相对位置对角的邻居点
判断并添加相对位置左下角的强迫邻居
判断并添加相对位置左上角的强迫邻居
elseif 当前点与其父节点是横向
判断并添加相对位置右方的邻居点
判断并添加相对位置上方的强迫邻居
判断并添加相对位置下方的强迫邻居
elseif 当前点与父节点是纵向
同横向逻辑,判断并处理下方,左右向强迫邻居
对角向图示举例:
代码
public List<Point> GetNeighbors(Point point)
{
var points = new List<Point>();
Point parent = point.ParentPoint;
//起始点,扩展9宫格中非障碍物点
if (parent == null)
{
//获取此点的邻居
//起点则parent点为null,遍历邻居非障碍点加入。
for (int x = -1; x <= 1; x++)
{
for (int y = -1; y <= 1; y++)
{
//点自身
if (x == 0 && y == 0)
continue;
if (IsWalkable(x + point.X, y + point.Y))
{
points.Add(new Point(x + point.X, y + point.Y));
}
}
}
return points;
}
//非起点邻居点判断(Mathf.Clamp是限制point.x - parent.x的取值范围)
int xDirection = Mathf.Clamp(point.X - parent.X, -1, 1);
int yDirection = Mathf.Clamp(point.Y - parent.Y, -1, 1);
if (xDirection != 0 && yDirection != 0)
{
//对角方向
bool neighbourForward =IsWalkable(point.X, point.Y + yDirection);
bool neighbourRight =IsWalkable(point.X + xDirection, point.Y);
bool neighbourLeft =IsWalkable(point.X - xDirection, point.Y);
bool neighbourBack =IsWalkable(point.X, point.Y - yDirection);
if (neighbourForward)
{
points.Add(new Point(point.X, point.Y + yDirection));
}
if (neighbourRight)
{
points.Add(new Point(point.X + xDirection, point.Y));
}
if ((neighbourForward || neighbourRight) && IsWalkable(point.X + xDirection, point.Y + yDirection))
{
points.Add(new Point(point.X + xDirection, point.Y + yDirection));
}
//强迫邻居的处理
if (!neighbourLeft && neighbourForward)
{
if (IsWalkable(point.X - xDirection, point.Y + yDirection))
{
points.Add(new Point(point.X - xDirection, point.Y + yDirection));
}
}
if (!neighbourBack && neighbourRight)
{
if (IsWalkable(point.X + xDirection, point.Y - yDirection))
{
points.Add(new Point(point.X + xDirection, point.Y - yDirection));
}
}
}
else
{
if (xDirection == 0)
{
//纵向
if (IsWalkable(point.X, point.Y + yDirection))
{
points.Add(new Point(point.X, point.Y + yDirection));
//强迫邻居
if (!IsWalkable(point.X + 1, point.Y) &&IsWalkable(point.X + 1, point.Y + yDirection))
{
points.Add(new Point(point.X + 1, point.Y + yDirection));
}
if (!IsWalkable(point.X - 1, point.Y) &&IsWalkable(point.X - 1, point.Y + yDirection))
{
points.Add(new Point(point.X - 1, point.Y + yDirection));
}
}
}
else
{
//横向
if (IsWalkable(point.X + xDirection, point.Y))
{
points.Add(new Point(point.X, point.Y + yDirection));
//强迫邻居
if (!IsWalkable(point.X, point.Y + 1) &&IsWalkable(point.X + xDirection, point.Y + 1))
{
points.Add(new Point(point.X + xDirection, point.Y + 1));
}
if (!IsWalkable(point.X, point.Y - 1) &&IsWalkable(point.X + xDirection, point.Y - 1))
{
points.Add(new Point(point.X + xDirection, point.Y - 1));
}
}
}
}
return points;
}
3.3 递归跳跃寻找jump point
def 递归寻找跳跃点(传入点,朝向:某一方向)
if 传入点是终点
返回终点
if 传入朝向是对角向
if 传入点存在强迫邻居
返回此传入点
if (递归寻找跳跃点 传入点:横向+1 朝向:横向)结果不为空
返回此传入点
if (递归寻找跳跃点 传入点:纵向+1 朝向:纵向)结果不为空
返回此传入点
elseif 横向
if 上下方有强迫邻居
返回此传入点
elseif 纵向
if 左右方有强迫邻居
返回此传入点
返回 递归寻找跳跃点(传入点按照同父节点传入方向的下一个点, 朝向:同父节点传入方向)
代码
private Point Jump(int curPosx, int curPosY, int xDirection, int yDirection, Point end)
{
//当前点不可行,返回空
if (!IsWalkable(curPosx, curPosY))
return null;
//递归最大深度 || 搜索到终点
if (end.X == curPosx && end.Y == curPosY)
return new Point(curPosx, curPosY);
//对角向
if (xDirection != 0 && yDirection != 0)
{
//如果当前点有force neighbor,当前点是一个跳点
if ((IsWalkable(curPosx + xDirection, curPosY - yDirection) && !IsWalkable(curPosx, curPosY - yDirection)) || (IsWalkable(curPosx - xDirection, curPosY + yDirection) && !IsWalkable(curPosx - xDirection, curPosY)))
{
return new Point(curPosx, curPosY);
}
//横向递归寻找强迫邻居,如果横向有点构成跳点,那么当前点也是跳点
if (Jump(curPosx + xDirection, curPosY, xDirection, 0, end) != null)
{
return new Point(curPosx, curPosY);
}
//纵向向递归寻找强迫邻居,如果纵向有点构成跳点,那么当前点也是跳点
if (Jump(curPosx, curPosY + yDirection, 0, yDirection, end) != null)
{
return new Point(curPosx, curPosY);
}
}
else if (xDirection != 0)
{
//横向,如果当前点有force neight,那么当前点是跳点
if ((IsWalkable(curPosx + xDirection, curPosY + 1) && !IsWalkable(curPosx, curPosY + 1)) || (IsWalkable(curPosx + xDirection, curPosY - 1) && !IsWalkable(curPosx, curPosY - 1)))
{
return new Point(curPosx, curPosY);
}
}
else if (yDirection != 0)
{
//纵向
if ((IsWalkable(curPosx + 1, curPosY + yDirection) && !IsWalkable(curPosx + 1, curPosY)) || (IsWalkable(curPosx - 1, curPosY + yDirection) && !IsWalkable(curPosx - 1, curPosY)))
{
return new Point(curPosx, curPosY);
}
}
//按照原方向继续扩展
return Jump(curPosx + xDirection, curPosY + yDirection, xDirection, yDirection, end);
}