A星算法中八个方向的这些值是怎么给出的呢？
How did we give these values of the eight directions in A star algorithm?
我正在尝试基于此代码实现我自己的代码。当我阅读这段代码时，我对这部分感到困惑。以下函数的一部分：
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
作者是如何给出这些值的？。我知道我们什么时候开始检查 8 个邻居，但这些数字是多少？我的意思是这些 [1, 0, 1] 等等。如果我想为 3D 生成它们会是什么？
下面是A星算法Python代码：
"""
A* grid based planning
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import matplotlib.pyplot as plt
import math
show_animation = True
class Node:
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
def calc_final_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
return rx, ry
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
motion = get_motion_model()
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
while 1:
c_id = min(
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
# show graph
if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
# expand search grid based on motion model
for i, _ in enumerate(motion):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if n_id in closedset:
continue
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
else:
if openset[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
rx, ry = calc_final_path(ngoal, closedset, reso)
return rx, ry
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x >= maxx:
return False
elif node.y >= maxy:
return False
if obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(ox, oy, reso, vr):
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
# print("minx:", minx)
# print("miny:", miny)
# print("maxx:", maxx)
# print("maxy:", maxy)
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# print("xwidth:", xwidth)
# print("ywidth:", ywidth)
# obstacle map generation
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':
main()
这些行上方的注释给出了线索：
# dx, dy, cost
这里的成本显然对应于欧氏距离，即 dx²+dy² 的平方根。
每个三元组代表一个方向：
- x坐标的变化：可以是-1（向左）、0（不沿X轴移动）、（向右）
- y坐标上的同步变化：可以是-1（向上），0（不沿y轴移动），1（向下）
- 此步代表的距离：如果以上两个数字之一为0，则方向为上、下、左、右，距离恰好为1。但如果两个数字都非零，然后我们进行对角线移动（左上、右上、左下、右下），在这种情况下，距离是 2 的平方根。
motion 也可以定义为：
motion = [[
[dx, dy, math.sqrt(dx*dx+dy*dy)]
for dx in ([-1,0,1] if dy else [-1, 1])
] for dy in [-1,0,1]]
注意这个欧几里得距离公式在代码的后面是如何出现的，例如用于计算路径剩余部分的启发式成本，作为当前单元格和目标单元格之间的直线距离。
如果您想使用 3D，那么每个方向将有 4 个值：dx、dy、dz，距离将是 dx²+dy²+dz² 的平方根：
- 1 如果这三个中只有一个是 1
- 如果这三个中的两个是 1，则为 2 的平方根
- 如果三个都非零，则为 3 的平方根
我正在尝试基于此代码实现我自己的代码。当我阅读这段代码时，我对这部分感到困惑。以下函数的一部分：
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
作者是如何给出这些值的？。我知道我们什么时候开始检查 8 个邻居，但这些数字是多少？我的意思是这些 [1, 0, 1] 等等。如果我想为 3D 生成它们会是什么？
下面是A星算法Python代码：
"""
A* grid based planning
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import matplotlib.pyplot as plt
import math
show_animation = True
class Node:
def __init__(self, x, y, cost, pind):
self.x = x
self.y = y
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.pind)
def calc_final_path(ngoal, closedset, reso):
# generate final course
rx, ry = [ngoal.x * reso], [ngoal.y * reso]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(n.x * reso)
ry.append(n.y * reso)
pind = n.pind
return rx, ry
def a_star_planning(sx, sy, gx, gy, ox, oy, reso, rr):
"""
gx: goal x position [m]
gx: goal x position [m]
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
nstart = Node(round(sx / reso), round(sy / reso), 0.0, -1)
ngoal = Node(round(gx / reso), round(gy / reso), 0.0, -1)
ox = [iox / reso for iox in ox]
oy = [ioy / reso for ioy in oy]
obmap, minx, miny, maxx, maxy, xw, yw = calc_obstacle_map(ox, oy, reso, rr)
motion = get_motion_model()
openset, closedset = dict(), dict()
openset[calc_index(nstart, xw, minx, miny)] = nstart
while 1:
c_id = min(
openset, key=lambda o: openset[o].cost + calc_heuristic(ngoal, openset[o]))
current = openset[c_id]
# show graph
if show_animation: # pragma: no cover
plt.plot(current.x * reso, current.y * reso, "xc")
if len(closedset.keys()) % 10 == 0:
plt.pause(0.001)
if current.x == ngoal.x and current.y == ngoal.y:
print("Find goal")
ngoal.pind = current.pind
ngoal.cost = current.cost
break
# Remove the item from the open set
del openset[c_id]
# Add it to the closed set
closedset[c_id] = current
# expand search grid based on motion model
for i, _ in enumerate(motion):
node = Node(current.x + motion[i][0],
current.y + motion[i][1],
current.cost + motion[i][2], c_id)
n_id = calc_index(node, xw, minx, miny)
if n_id in closedset:
continue
if not verify_node(node, obmap, minx, miny, maxx, maxy):
continue
if n_id not in openset:
openset[n_id] = node # Discover a new node
else:
if openset[n_id].cost >= node.cost:
# This path is the best until now. record it!
openset[n_id] = node
rx, ry = calc_final_path(ngoal, closedset, reso)
return rx, ry
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.sqrt((n1.x - n2.x)**2 + (n1.y - n2.y)**2)
return d
def verify_node(node, obmap, minx, miny, maxx, maxy):
if node.x < minx:
return False
elif node.y < miny:
return False
elif node.x >= maxx:
return False
elif node.y >= maxy:
return False
if obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(ox, oy, reso, vr):
minx = round(min(ox))
miny = round(min(oy))
maxx = round(max(ox))
maxy = round(max(oy))
# print("minx:", minx)
# print("miny:", miny)
# print("maxx:", maxx)
# print("maxy:", maxy)
xwidth = round(maxx - minx)
ywidth = round(maxy - miny)
# print("xwidth:", xwidth)
# print("ywidth:", ywidth)
# obstacle map generation
obmap = [[False for i in range(ywidth)] for i in range(xwidth)]
for ix in range(xwidth):
x = ix + minx
for iy in range(ywidth):
y = iy + miny
# print(x, y)
for iox, ioy in zip(ox, oy):
d = math.sqrt((iox - x)**2 + (ioy - y)**2)
if d <= vr / reso:
obmap[ix][iy] = True
break
return obmap, minx, miny, maxx, maxy, xwidth, ywidth
def calc_index(node, xwidth, xmin, ymin):
return (node.y - ymin) * xwidth + (node.x - xmin)
def get_motion_model():
# dx, dy, cost
motion = [[1, 0, 1],
[0, 1, 1],
[-1, 0, 1],
[0, -1, 1],
[-1, -1, math.sqrt(2)],
[-1, 1, math.sqrt(2)],
[1, -1, math.sqrt(2)],
[1, 1, math.sqrt(2)]]
return motion
def main():
print(__file__ + " start!!")
# start and goal position
sx = 10.0 # [m]
sy = 10.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 1.0 # [m]
robot_size = 1.0 # [m]
ox, oy = [], []
for i in range(60):
ox.append(i)
oy.append(0.0)
for i in range(60):
ox.append(60.0)
oy.append(i)
for i in range(61):
ox.append(i)
oy.append(60.0)
for i in range(61):
ox.append(0.0)
oy.append(i)
for i in range(40):
ox.append(20.0)
oy.append(i)
for i in range(40):
ox.append(40.0)
oy.append(60.0 - i)
if show_animation: # pragma: no cover
plt.plot(ox, oy, ".k")
plt.plot(sx, sy, "xr")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
rx, ry = a_star_planning(sx, sy, gx, gy, ox, oy, grid_size, robot_size)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
if __name__ == '__main__':
main()
这些行上方的注释给出了线索：
# dx, dy, cost
这里的成本显然对应于欧氏距离，即 dx²+dy² 的平方根。
每个三元组代表一个方向：
- x坐标的变化：可以是-1（向左）、0（不沿X轴移动）、（向右）
- y坐标上的同步变化：可以是-1（向上），0（不沿y轴移动），1（向下）
- 此步代表的距离：如果以上两个数字之一为0，则方向为上、下、左、右，距离恰好为1。但如果两个数字都非零，然后我们进行对角线移动（左上、右上、左下、右下），在这种情况下，距离是 2 的平方根。
motion 也可以定义为：
motion = [[
[dx, dy, math.sqrt(dx*dx+dy*dy)]
for dx in ([-1,0,1] if dy else [-1, 1])
] for dy in [-1,0,1]]
注意这个欧几里得距离公式在代码的后面是如何出现的，例如用于计算路径剩余部分的启发式成本，作为当前单元格和目标单元格之间的直线距离。
如果您想使用 3D，那么每个方向将有 4 个值：dx、dy、dz，距离将是 dx²+dy²+dz² 的平方根：
- 1 如果这三个中只有一个是 1
- 如果这三个中的两个是 1，则为 2 的平方根
- 如果三个都非零，则为 3 的平方根