Python 如何改变';成本';这条路怎么走?
我正在阅读关于星型算法的python代码。对我来说,我理解这个算法是如何工作的,但是当我谈到代码时,我得到了一些令人困惑的东西,直到我理解为止。我希望能够在这里更改路径的成本。我的意思是,在计算最佳路径时,我需要能够设置每个像素的成本,但我不知道在哪里做。我有2条路径,我希望算法选择底部路径,因为顶部路径的代价。我该怎么做 路径的图像。我希望算法选择底部的一个 算法的完整代码:Python 如何改变';成本';这条路怎么走?,python,path-finding,a-star,motion-planning,Python,Path Finding,A Star,Motion Planning,我正在阅读关于星型算法的python代码。对我来说,我理解这个算法是如何工作的,但是当我谈到代码时,我得到了一些令人困惑的东西,直到我理解为止。我希望能够在这里更改路径的成本。我的意思是,在计算最佳路径时,我需要能够设置每个像素的成本,但我不知道在哪里做。我有2条路径,我希望算法选择底部路径,因为顶部路径的代价。我该怎么做 路径的图像。我希望算法选择底部的一个 算法的完整代码: """ A* grid planning author: Atsushi Sakai(@Atsushi_twi)
"""
A* grid planning
author: Atsushi Sakai(@Atsushi_twi)
Nikos Kanargias (nkana@tee.gr)
See Wikipedia article (https://en.wikipedia.org/wiki/A*_search_algorithm)
"""
import math
import matplotlib.pyplot as plt
show_animation = True
class AStarPlanner:
def __init__(self, ox, oy, reso, rr):
"""
Initialize grid map for a star planning
ox: x position list of Obstacles [m]
oy: y position list of Obstacles [m]
reso: grid resolution [m]
rr: robot radius[m]
"""
self.reso = reso
self.rr = rr
self.calc_obstacle_map(ox, oy)
self.motion = self.get_motion_model()
class Node:
def __init__(self, x, y, cost, pind):
self.x = x # index of grid
self.y = y # index of grid
self.cost = cost
self.pind = pind
def __str__(self):
return str(self.x) + "," + str(self.y) + "," + str(
self.cost) + "," + str(self.pind)
def planning(self, sx, sy, gx, gy):
"""
A star path search
input:
sx: start x position [m]
sy: start y position [m]
gx: goal x position [m]
gy: goal y position [m]
output:
rx: x position list of the final path
ry: y position list of the final path
"""
nstart = self.Node(self.calc_xyindex(sx, self.minx),
self.calc_xyindex(sy, self.miny), 0.0, -1)
ngoal = self.Node(self.calc_xyindex(gx, self.minx),
self.calc_xyindex(gy, self.miny), 0.0, -1)
open_set, closed_set = dict(), dict()
open_set[self.calc_grid_index(nstart)] = nstart
while 1:
if len(open_set) == 0:
print("Open set is empty..")
break
c_id = min(
open_set,
key=lambda o: open_set[o].cost + self.calc_heuristic(ngoal,
open_set[
o]))
current = open_set[c_id]
# show graph
if show_animation: # pragma: no cover
plt.plot(self.calc_grid_position(current.x, self.minx),
self.calc_grid_position(current.y, self.miny), "xc")
# for stopping simulation with the esc key.
plt.gcf().canvas.mpl_connect('key_release_event',
lambda event: [exit(
0) if event.key == 'escape' else None])
if len(closed_set.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 open_set[c_id]
# Add it to the closed set
closed_set[c_id] = current
# expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2], c_id)
n_id = self.calc_grid_index(node)
# If the node is not safe, do nothing
if not self.verify_node(node):
continue
if n_id in closed_set:
continue
if n_id not in open_set:
open_set[n_id] = node # discovered a new node
else:
if open_set[n_id].cost > node.cost:
# This path is the best until now. record it
open_set[n_id] = node
rx, ry = self.calc_final_path(ngoal, closed_set)
return rx, ry
def calc_final_path(self, ngoal, closedset):
# generate final course
rx, ry = [self.calc_grid_position(ngoal.x, self.minx)], [
self.calc_grid_position(ngoal.y, self.miny)]
pind = ngoal.pind
while pind != -1:
n = closedset[pind]
rx.append(self.calc_grid_position(n.x, self.minx))
ry.append(self.calc_grid_position(n.y, self.miny))
pind = n.pind
return rx, ry
@staticmethod
def calc_heuristic(n1, n2):
w = 1.0 # weight of heuristic
d = w * math.hypot(n1.x - n2.x, n1.y - n2.y)
return d
def calc_grid_position(self, index, minp):
"""
calc grid position
:param index:
:param minp:
:return:
"""
pos = index * self.reso + minp
return pos
def calc_xyindex(self, position, min_pos):
return round((position - min_pos) / self.reso)
def calc_grid_index(self, node):
return (node.y - self.miny) * self.xwidth + (node.x - self.minx)
def verify_node(self, node):
px = self.calc_grid_position(node.x, self.minx)
py = self.calc_grid_position(node.y, self.miny)
if px < self.minx:
return False
elif py < self.miny:
return False
elif px >= self.maxx:
return False
elif py >= self.maxy:
return False
# collision check
if self.obmap[node.x][node.y]:
return False
return True
def calc_obstacle_map(self, ox, oy):
self.minx = round(min(ox))
self.miny = round(min(oy))
self.maxx = round(max(ox))
self.maxy = round(max(oy))
print("minx:", self.minx)
print("miny:", self.miny)
print("maxx:", self.maxx)
print("maxy:", self.maxy)
self.xwidth = round((self.maxx - self.minx) / self.reso)
self.ywidth = round((self.maxy - self.miny) / self.reso)
print("xwidth:", self.xwidth)
print("ywidth:", self.ywidth)
# obstacle map generation
self.obmap = [[False for i in range(self.ywidth)]
for i in range(self.xwidth)]
for ix in range(self.xwidth):
x = self.calc_grid_position(ix, self.minx)
for iy in range(self.ywidth):
y = self.calc_grid_position(iy, self.miny)
for iox, ioy in zip(ox, oy):
d = math.hypot(iox - x, ioy - y)
if d <= self.rr:
self.obmap[ix][iy] = True
break
@staticmethod
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 = 1.0 # [m]
sy = 1.0 # [m]
gx = 50.0 # [m]
gy = 50.0 # [m]
grid_size = 10.0 # [m]
robot_radius = 1.0 # [m]
# set obstacle positions
ox, oy = [], []
for i in range(-10, 60):
ox.append(i)
oy.append(-10.0)
for i in range(-10, 60):
ox.append(60.0)
oy.append(i)
for i in range(-10, 61):
ox.append(i)
oy.append(60.0)
for i in range(-10, 61):
ox.append(-10.0)
oy.append(i)
for i in range(-10, 40):
ox.append(20.0)
oy.append(i)
for i in range(0, 10):
ox.append(40.0)
oy.append(i)
for i in range(0, 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, "og")
plt.plot(gx, gy, "xb")
plt.grid(True)
plt.axis("equal")
a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
rx, ry = a_star.planning(sx, sy, gx, gy)
if show_animation: # pragma: no cover
plt.plot(rx, ry, "-r")
plt.show()
plt.pause(0.001)
if __name__ == '__main__':
main()
“”“
A*电网规划
作者:酒井Atsushi(@Atsushi_twi)
尼科斯·卡纳里亚斯(nkana@tee.gr)
参见维基百科文章(https://en.wikipedia.org/wiki/A*_搜索(U算法)
"""
输入数学
将matplotlib.pyplot作为plt导入
显示动画=真
阿斯塔普兰纳级:
定义初始值(自身、牛、oy、reso、rr):
"""
初始化星图规划的栅格地图
ox:x障碍物位置列表[m]
oy:y位置障碍物列表[m]
分辨率:网格分辨率[m]
rr:机器人半径[m]
"""
self.reso=reso
self.rr=rr
自我计算障碍地图(ox,oy)
self.motion=self.get\u motion\u model()
类节点:
定义初始值(self、x、y、cost、pind):
self.x=x#网格索引
self.y=y#网格索引
自我成本=成本
self.pind=pind
定义(自我):
返回str(self.x)+“,”+str(self.y)+“,”+str(
self.cost)+“,”+str(self.pind)
def计划(自我、sx、sy、gx、gy):
"""
星径搜索
输入:
sx:开始x位置[m]
sy:开始y位置[m]
gx:目标x位置[m]
gy:目标y位置[m]
输出:
rx:x最终路径的位置列表
y:最终路径的y位置列表
"""
nstart=self.Node(self.calc_xyindex(sx,self.minx),
自计算指数(sy,self.miny),0.0,-1)
ngoal=self.Node(self.calc_xyindex(gx,self.minx),
自计算指数(gy,自计算指数),0.0,-1)
打开集合,关闭集合=dict(),dict()
开放集[自计算网格索引(nstart)]=nstart
而1:
如果len(open_set)==0:
打印(“打开集为空…”
打破
c_id=min(
开放集,
key=lambda o:open_set[o]。成本+自身。计算启发式(ngoal,
开放集[
o] ))
当前=打开\u集[c\u id]
#显示图形
如果显示动画:#杂注:无封面
plt.绘图(自计算网格位置(当前x、自最小x),
自计算网格位置(当前y、自最小),“xc”)
#用于使用esc键停止模拟。
plt.gcf().canvas.mpl\u connect('key\u release\u event',
lambda事件:[退出](
0)如果event.key=='escape'否则无])
如果len(closed_set.keys())%10==0:
plt.暂停(0.001)
如果当前.x==ngoal.x且当前.y==ngoal.y:
打印(“查找目标”)
ngoal.pind=当前的.pind
ngoal.cost=当前成本
打破
#从打开的集合中删除该项
del open_set[c_id]
#将其添加到闭合集
闭合集[c_id]=当前
#基于运动模型扩展网格搜索网格
对于i,在枚举(自运动)中:
node=self.node(当前.x+self.motion[i][0],
当前.y+自运动[i][1],
当前成本+自动运动[i][2],c_id)
n\u id=self.calc\u grid\u索引(节点)
#如果节点不安全,请不要执行任何操作
如果不是self.verify_节点(node):
持续
如果n_id位于闭合_集中:
持续
如果n_id不在open_集合中:
打开_集[n_id]=节点#发现一个新节点
其他:
如果打开\u集[n\u id]。成本>节点成本:
#这条路是迄今为止最好的。记录下来
打开\u集[n\u id]=节点
rx,ry=自计算最终路径(非高斯,闭合集)
返回rx,ry
def calc_最终路径(自、非通用、关闭集):
#生成最终课程
rx,ry=[self.calc\u grid\u位置(ngoal.x,self.minx)][
自计算网格位置(ngoal.y,self.miny)]
pind=ngoal.pind
而品脱!=-1:
n=关闭设置[pind]
附加(自计算网格位置(n.x,自最小))
y.append(自计算网格位置(n.y,自最小))
品脱
返回rx,ry
@静力学方法
def calc_启发式(n1,n2):
w=1.0#启发式权重
d=w*数学正压(n1.x-n2.x,n1.y-n2.y)
返回d
def calc_网格位置(自、索引、minp):
"""
计算网格位置
:参数索引:
:param minp:
:返回:
"""
pos=索引*self.reso+minp
返回位置
def校准指数(自身、位置、最小位置):
返回回合((位置-最小位置)/自我决议)
def计算网格索引(自身,节点):
返回(node.y-self.miny)*self.xwidth+(node.x-self.minx)
def验证_节点(自身,节点):
px=自计算网格位置(node.x,self.minx)
py=自计算网格位置(node.y,self.miny)
如果px # expand_grid search grid based on motion model
for i, _ in enumerate(self.motion):
cost = 0
if(current.x + self.motion[i][0] >= 45 and (current.x + self.motion[i][0]) <= 55 and current.y + self.motion[i][1] >= 20 and current.y + self.motion[i][1] <= 31):
cost = 15
node = self.Node(current.x + self.motion[i][0],
current.y + self.motion[i][1],
current.cost + self.motion[i][2] + cost, c_id)
n_id = self.calc_grid_index(node)