基于搜索的路径规划算法
基于搜索的路径规划算法
——针对A* LPA*算法的理解
1.引言
对于研究系统优化和生产调度相关的课题，掌握对应的路径规划算法是必不可少的能力。移动一个简单的物体看起来是容易的，而路径搜索是复杂的。
根据是否考虑经验法则算法，可以将路径规划算法分为盲目式算法和启发式搜索算法。
解决有向正权值图类规划的Dijkstra算法，可以计算含有负权的Ford算法，系统检查图中所有节点的广度优先算法等等，都属于盲目式算法。这类算法的特点是，可以找到最优路径或最短路径，尽管搜索过程过于盲目即耗费的时间长，搜索的范围广。
启发式搜索利用启发性信息进行制导的搜索，利用启发函数来导航，有效避免了传统方法盲目搜索带来的弊端。典型的算法比如最佳优先算法和贪婪算法。贪婪算法总是做出当前看来最好的选择，也就是说，不从整体最优上加以考虑，只做出在某种意义上的局部最优解。
这种贪心策略的选择，使得大部分启发式路径规划算法只能找到解决问题的近似解而非最优解。
我们需要算法有方向地进行扩散，即启发式搜索，另一方面我们需要得到尽可能最短的路径，于是A就诞生了， 它结合了传统方法Dijkstra和启发式算法的优点，以从起点到该点的距离加上该点到终点的估计距离之和作为该点在图中的优先级，避免了盲目搜索带来大量不必要的浪费，也能够得到问题的最优解。 但在实际应用中，路径常常是动态变化的，一些移动障碍物的存在使得路径规划变得复杂，用上述的算法解决这种问题就变得复杂。2004年，Koenig和Likhachev提出了LPA***
算法（Lifelong Planning A），被用来处理动态环境下从给定起始点到给定目标点的最短路径问题，但在行走过程中，环境中的节点发生变化，想获得新的路径，就得以机器人当前节点为起始点，重新用LPA算法解算，降低了算法的计算效率，增加了时间复杂度。针对该情况，Koenig和Likhachev随后又提出了D Lite算法。与 LPA 采用的正向搜索算法不同，D* Lite 采用反向搜索方式，效果与D* 算法相当，能够很好的适用于未知环境做路经规划，由于其增量规划的思想，它可以做到较少重规划次数以及较少的重规划影响节点数。
在以上各种适用范围不同，复杂度不同的算法中，我着重分析理解了两种算法：其一是A算法，集启发式搜素和Dijkstra算法优点于一身，不仅效率高还可以找出最佳路径。另外一种是较为复杂的LPA算法，其灵活性更高，可以解决移动障碍的问题，在实际应用中更为广泛。
2.A*算法
2.1算法详解
一般的搜索策略都可以归纳为初始化队列，而后重复执行取队列头值并检测，拓展队列，增加候选点的算法执行过程。A算法略微不同的地方就在于添加了启发式函数：F = G + H，
这里，G 为从起点 A 移动到指定方格的移动代价，沿着到达该方格而生成的路径，H 为从指定的方格移动到终点 B 的估算成本。
我们假设存在一张方格地图，上面有白色和黑色的方格，白色表示该路径可以行走，黑色表示障碍，不能通过，行走一步的范围是当前所在节点的八领域（排除障碍和close list中存在的节点）于是设定G：行走代价的值；对于H，有很多方法可以估算 H 值，这里我们使用 Manhattan 方法，计算从当前方格横向或纵向移动到达目标所经过的方格数，忽略对角移动，然后把总数乘以 10 。我们给定左下角为起点，右上角为终点，要求找到一条从起点出发到达终点的路径，且要求路径最短。
首先我们需要建立两个列表，open list和close list。Open list用来存放当前待检查的节点，close list中存放已形成的路径节点集合。从起点出发，我们把起点放入open list。此时open list中只有一个节点，我们别无选择，将它放入close list中，与此同时，起点周围能够行走的点加入到open list中，通过启发式函数计算每个节点F值，选取其中最小的一个放入close list当中。然后检查所有与它相邻的方格，忽略其中在 close list 中或是不可走的方格 ，如果方格不在open list 中，则把它们加入到 open list 中。把我们选定的方格设置为这些新加入的方格的父亲。如果某个相邻的方格已经在 open list 中，则检查这条路径是否更优，也就是说经由当前方格 ( 我们选中的方格 ) 到达那个方格是否具有更小的 G 值。如果没有，不做任何操作。相反，如果 G 值更小，则把那个方格的父亲设为当前方格 ( 我们选中的方格 ) ，然后重新计算那个方格的 F 值和 G 值。不断重复这个过程，直至终点被加入open list中，此时close list当中的节点就是我们找到的最短路径。
2.2算法实现
下图是A寻路算法的一个简单实现。黑色方格代表障碍，要求找到一条从左下角出发，到达右上角的最短路径。行走区域为当前节点的四领域范围。红色方格为寻找到的路径。代码详见附录。
3.LPA*算法
3.1两类算法比较
A，LPA都可以用于静态环境下移动机器人的路径规划，此时二者计算效率相差不大，都利用了启发式搜索来提高效率，LPA的增量式搜索在这时没有任何帮助。但对于动态环境的路径规划，A算法却有心无力，对于动态环境下进行二次搜索，LPA效率明显高于A。
3.2 LPA算法详解
LPA算法是一种增量启发式搜索版本的A算法，它常被用来处理动态环境下从给定起始点到给定目标点的最短路径问题。启发式搜索和增量式搜索的区别在于，启发式搜索是利用启发函数来对搜索进行指导，从而实现高效的搜索，启发式搜索是一种“智能”搜索。增量搜索是对以前的搜索结果信息进行再利用来实现高效搜索，大大减少搜索范围和时间。
和A算法一样，LPA算法采用非负、一致性的启发函数H，表示当前位置网格点到目标点距离的估计，沿用了open list来表示被搜索网格点的集合。引入了g(s),g(s)和rhs(s)（s为当前节点，s‘为当前节点的子节点）:g*(s)表示起点到当前s点的最短路径距离，g(s)是对g*(s)的值的估计，当前s点的g*(s)值为：s点与某子节点s’的移动代价与该子节点g*(s’)之和序列中的最小值。rhs(s)被定义为s点与某子节点s’的移动代价与该子节点g(s’)之和序列中的最小值。当环境没有发生变化时，g(s)=rhs(s)=A+B，此时s点为局部一致收敛。如果全局的每个节点都满足g(s)=rhs(s)，此时LPA算法退化为A算法，不是我们的重点研究范围。当环境发生变化时，边缘代价函数变为无穷大，rhs(s)也相应发生了变化，与g(s)的值不一致，此时s点为局部不一致。局部不一致又分为过一致和欠一致。当 g(s)>rhs(s)时，被称为局部过一致，即 s’ 点到 s 点的代价变低，代表网格上障碍物被清除或搜索到一条更短的“捷径”。当 g(s)
4.启发式搜索
启发式策略可以通过指导搜索，确定一个最佳的启发式函数，使得搜索过程向最有希望一面发展，降低了时间复杂性。并得到令人能接受的解（虽然大多数情况下很难得到最优解）。
然而，启发式策略是极易出错的。在解决问题的过程中，启发仅仅是下一步将要采取措施的一个猜想，常常根据经验和直觉来判断。由于启发式搜索只有有限的信息，要想预测进一步搜索过程中状态空间的具体行为则很难。一个启发式搜索可能得到一个很好的近似解，也可能一无所获。这是启发式搜索固有的局限性。这种局限性不可能由所谓更好的启发式策略或更有效的搜索算法来消除。一般说来，启发信息越强，扩展的无用节点就越少。引入强的启发信息，有可能大大降低搜索工作量，但不能保证找到最小耗散值的解路径(最佳路径)。因此，在实际应用中，最好能引入降低搜索工作量的启发信息而不牺牲找到最佳路径的保证。
5.附录
from tkinter import *
import enum
import time
import _thread
class PointState(enum.Enum):
BARRIER = 'black'
UNUSED = 'white'
TRAVERSED = 'yellow'
PATH = 'red'
class MiniMap:
class Point:
def __init__(self, x, y, state, rectangle):
self.x = x
self.y = y
self.state = state
self.rectangle = rectangle
class Vertex:
def __init__(self, x, y, f, g, father):
self.x = x
self.y = y
self.f = f
self.g = g
self.father = father
def __init__(self, *args, **kwargs):
self.row = args[0]
self.column = args[1]
self.size = args[2]
self.start = args[3]
self.end = args[4]
self.delay = args[5]
self.root = Tk()
self.root.title("A*")
self.canva = Canvas(self.root, width=self.column * self.size + 3, height=self.row * self.size + 3)
self.points = self.generatePoints() # 生成点集
self.generateMesh() # 生成网格
self.canva.bind("", self.createBarrier)
self.canva.bind("", self.cleanMap)
self.canva.bind("", self.navigation)
self.canva.pack(side=TOP, expand=YES, fill=BOTH)
self.root.resizable(0, 0)
self.root.mainloop()
def createBarrier(self, event):
x = int((event.x + 3) / self.size)
y = int((event.y + 3) / self.size)
if x < self.column and y < self.row:
if (self.points[x][y].state == PointState.BARRIER.value):
self.points[x][y].state = PointState.UNUSED.value
self.canva.itemconfig(self.points[x][y].rectangle, fill=self.points[x][y].state)
else:
self.points[x][y].state = PointState.BARRIER.value
self.canva.itemconfig(self.points[x][y].rectangle, fill=self.points[x][y].state)
def navigation(self, event):
_thread.start_new_thread(self.generateMap, (self.start, self.end))
def cleanMap(self, event):
# 清空画布
for i in range(self.column):
for j in range(self.row):
if (self.points[i][j].state != PointState.BARRIER.value):
self.points[i][j].state = PointState.UNUSED.value
self.canva.itemconfig(self.points[i][j].rectangle, fill=self.points[i][j].state)
def generateMap(self, start, end):
x1 = start[0]
y1 = start[1]
x2 = end[0]
y2 = end[1]
dictopen = {}
dictclose = {}
dictopen[(x1, y1)] = self.Vertex(x1, y1, 10 * (abs(x2 - x1) + abs(y2 - y1)), 0, None)
# 寻路循环
while 1:
fMin = -1
pMin = None
for v in dictopen.keys():
if fMin < 0 or dictopen[v].f <= fMin:
fMin = dictopen[v].f
pMin = v
dictclose[pMin] = dictopen[pMin]
del dictopen[pMin]
# 检测上方点
if (pMin[1] > 0):
pNew = (pMin[0], pMin[1] - 1)
if (pNew not in dictclose.keys() and self.points[pNew[0]][pNew[1]].state != PointState.BARRIER.value):
if (pNew in dictopen.keys()):
if ((10 + dictclose[pMin].g) < dictopen[pNew].g):
dictopen[pNew].g = 10 + dictclose[pMin].g
dictopen[pNew].f = dictopen[pNew].g + 10 * (abs(x2 - pNew[0]) + abs(y2 - pNew[1]))
dictopen[pNew].father = pMin
else:
dictopen[pNew] = self.Vertex(pNew[0], pNew[1], dictclose[pMin].g + 10 + 10 * (
abs(x2 - pNew[0]) + abs(y2 - pNew[1])), dictclose[pMin].g + 10, pMin)
self.points[pNew[0]][pNew[1]].state = PointState.TRAVERSED.value
self.canva.itemconfig(self.points[pNew[0]][pNew[1]].rectangle,
fill=self.points[pNew[0]][pNew[1]].state)
# 检测右侧点
if (pMin[0] < self.column - 1):
pNew = (pMin[0] + 1, pMin[1])
if (pNew not in dictclose.keys() and self.points[pNew[0]][pNew[1]].state != PointState.BARRIER.value):
if (pNew in dictopen.keys()):
if ((10 + dictclose[pMin].g) < dictopen[pNew].g):
dictopen[pNew].g = 10 + dictclose[pMin].g
dictopen[pNew].f = dictopen[pNew].g + 10 * (abs(x2 - pNew[0]) + abs(y2 - pNew[1]))
dictopen[pNew].father = pMin
else:
dictopen[pNew] = self.Vertex(pNew[0], pNew[1], dictclose[pMin].g + 10 + 10 * (
abs(x2 - pNew[0]) + abs(y2 - pNew[1])), dictclose[pMin].g + 10, pMin)
self.points[pNew[0]][pNew[1]].state = PointState.TRAVERSED.value
self.canva.itemconfig(self.points[pNew[0]][pNew[1]].rectangle,
fill=self.points[pNew[0]][pNew[1]].state)
# 检测下方点
if (pMin[1] < self.row - 1):
pNew = (pMin[0], pMin[1] + 1)
if (pNew not in dictclose.keys() and self.points[pNew[0]][pNew[1]].state != PointState.BARRIER.value):
if (pNew in dictopen.keys()):
if ((10 + dictclose[pMin].g) < dictopen[pNew].g):
dictopen[pNew].g = 10 + dictclose[pMin].g
dictopen[pNew].f = dictopen[pNew].g + 10 * (abs(x2 - pNew[0]) + abs(y2 - pNew[1]))
dictopen[pNew].father = pMin
else:
dictopen[pNew] = self.Vertex(pNew[0], pNew[1], dictclose[pMin].g + 10 + 10 * (
abs(x2 - pNew[0]) + abs(y2 - pNew[1])), dictclose[pMin].g + 10, pMin)
self.points[pNew[0]][pNew[1]].state = PointState.TRAVERSED.value
self.canva.itemconfig(self.points[pNew[0]][pNew[1]].rectangle,
fill=self.points[pNew[0]][pNew[1]].state)
# 检测左侧点
if (pMin[0] > 0):
pNew = (pMin[0] - 1, pMin[1])
if (pNew not in dictclose.keys() and self.points[pNew[0]][pNew[1]].state != PointState.BARRIER.value):
if (pNew in dictopen.keys()):
if ((10 + dictclose[pMin].g) < dictopen[pNew].g):
dictopen[pNew].g = 10 + dictclose[pMin].g
dictopen[pNew].f = dictopen[pNew].g + 10 * (abs(x2 - pNew[0]) + abs(y2 - pNew[1]))
dictopen[pNew].father = pMin
else:
dictopen[pNew] = self.Vertex(pNew[0], pNew[1], dictclose[pMin].g + 10 + 10 * (
abs(x2 - pNew[0]) + abs(y2 - pNew[1])), dictclose[pMin].g + 10, pMin)
self.points[pNew[0]][pNew[1]].state = PointState.TRAVERSED.value
self.canva.itemconfig(self.points[pNew[0]][pNew[1]].rectangle,
fill=self.points[pNew[0]][pNew[1]].state)
# 检测是否终止
if ((x2, y2) in dictopen.keys()):
self.points[x2][y2].state = PointState.PATH.value
self.canva.itemconfig(self.points[x2][y2].rectangle, fill=self.points[x2][y2].state)
pNext = dictopen[(x2, y2)].father
while dictclose[pNext].father in dictclose.keys():
self.points[pNext[0]][pNext[1]].state = PointState.PATH.value
self.canva.itemconfig(self.points[pNext[0]][pNext[1]].rectangle,
fill=self.points[pNext[0]][pNext[1]].state)
pNext = dictclose[pNext].father
self.points[pNext[0]][pNext[1]].state = PointState.PATH.value
self.canva.itemconfig(self.points[pNext[0]][pNext[1]].rectangle,
fill=self.points[pNext[0]][pNext[1]].state)
break
if (len(dictopen) == 0):
print('Unreachable!')
break
# 等待绘制
time.sleep(self.delay)
def generatePoints(self):
points = [[self.Point(i, j, PointState.UNUSED.value,
self.canva.create_rectangle((i * self.size + 3, j * self.size + 3),
((i + 1) * self.size + 3, (j + 1) * self.size + 3),
fill=PointState.UNUSED.value)) for j in range(self.row)] for i
in range(self.column)]
return points
def generateMesh(self):
for i in range(self.row + 1):
self.canva.create_line((3, i * self.size + 3), (self.column * self.size + 3, i * self.size + 3))
for i in range(self.column + 1):
self.canva.create_line((i * self.size + 3, 3), (i * self.size + 3, self.row * self.size + 3))
#参数为行数，列数，方格尺寸,起点坐标，终点坐标，延迟时间
demo = MiniMap(6, 6, 80, (5, 0), (0, 5), 0.02)
6.参考
https://blog.csdn.net/Nomuo/article/details/108519341?utm_medium=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-5.channel_param&depth_1-utm_source=distribute.pc_relevant.none-task-blog-BlogCommendFromMachineLearnPai2-5.channel_param