|—search()—|—添加一个列表变量Expend,存储每个小格扩展时为第几步，可打印出
| |—打印运动表
|—A*—|— heuristic()
|—Dynamic programming(动态规划)—|—Value编程
| |—应用于现实
| |—algorithm simulation
这是一种路径规划的算法，查找起点到终点路径的过程称为规划，这里讲的是规划的离散方法，用这些方法进行连续运动.
cost：每条路径花费的时间
每一个MOVE前、后移动，每一次Turn left、Turn right，都耗费一定成本单元
路径规划或搜索问题就是寻找最短的动作序列，将机器人从开始状态引导到结束状态
现在的问题是是否能找出起点到终点最短路径的程序。
解决这个问题：给小格编号，在容器open中从起始点向周围扩展，并标上步数g值，当扩展到终点坐标时，最小的g值就是起点到终点的步数（秘诀：每次只展开最小的g值节点）
程序化这个过程：
# 编写search() 返回列表[g, row, col].
# 需要最终返回结果[11, 4, 5].到达不了返回'fail' grid = [[0, 0, 1, 0, 0, 0],
[0, 0, 1, 0, 0, 0],
[0, 0, 0, 0, 1, 0],
[0, 0, 1, 1, 1, 0],
[0, 0, 0, 0, 1, 0]]
heuristic = [[9, 8, 7, 6, 5, 4],
[8, 7, 6, 5, 4, 3],
[7, 6, 5, 4, 3, 2],
[6, 5, 4, 3, 2, 1],
[5, 4, 3, 2, 1, 0]]
init = [0, 0]
goal = [len(grid)-1, len(grid[0])-1]
cost = 1 delta = [[-1, 0], # up
[ 0,-1], # left
[ 1, 0], # down
[ 0, 1]] # right delta_name = ['^', '<', 'v', '>'] cost = 1 def search(grid,init,goal,cost):
closed = [[0 for row in range(len(grid[0]))] for col in range(len(grid))] #为了避免重复展开，定义一个与网格大小相同的空网格
closed[init[0]][init[1]] = 1#将起始位置标志为关闭
expand = [[-1] * len(grid[0]) for i in grid]
action = [[-1] * len(grid[0]) for i in grid]
#置初值
x = init[0]
y = init[1]
g = 0
h = heuristic[x][y]
f = g + h
open = [[g, x, y]] open = [[f, g, h, x, y]]
expand[x][y] = g
count = 0
#做两个标志值
found = False #找到目标时found置true
resign = False #找不到目标并且没有其他地方可展开resign置true while not found and not resign:
检查open列表里是否有元素
if len(open) == 0:
resign = True
return "fail"
else:
#删除open最小g值以继续展开
open.sort() #递增排列
open.reverse() #颠转，因为pop是从头部弹出
next = open.pop() #弹出最小
#给要展开的x,y赋值，g必须在最前面
x = next[1] x = next[3] # x从0开始
y = next[2] y = next[4] # y从1开始
g = next[0] g = next[1]
expand[x][y] = count #将下面的那句改成这样
count += 1 # 将下面的这句移到这
expand[x][y] = expand_counter
#检测是否达到目标
if x == goal[0] and y == goal[1]:
found = True
else: #这是没有达到，也是核心部分
for i in range(len(delta)):
#遍历将每个动作都赋值给x，y
x2 = x + delta[i][0]
y2 = y + delta[i][1]
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 <len(grid[0]): #如果赋了动作的x,y仍然在网格内
if closed[x2][y2] == 0 and grid[x2][y2] == 0: #并且尚未标记(通过检查close),还有这个单元可以走
g2 = g + cost
h2 = heuristic[x2][y2]
f2 = g2 + h2
open.append([f2，g2, x2, y2])
closed[x2][y2] = 1
count += 1
action[x2][y2] = i#用[x2][y2]而不用[x][y]是因为后者代表扩展，前者代表回溯
policy = [[' '] * len(grid[0]) for i in grid]#action包含障碍物的所有方格的方向，非想要，所以要另外初始化一个用''填充的表格
x = goal[0] #标明终点并用*表示
y = goal[1]
policy[x][y] = '*'
while x != init[0] or y != init[1]: #从终点位置返回直到到达初始点为止
x2 = x - delta[action[x][y]][0]#通过从当前状态做减法来坐标回溯
y2 = y - delta[action[x][y]][1]
policy[x2][y2] = delta_name[action[x][y]]#将用二维向量表示的方向图像化
x = x2
y = y2
for row in policy:
print row
return policy 以下是打印代码 #print 'new open list:'
#for i in range(len(open))
# print '',open[i]
#print '---------------------' #for i in range(len(expand)):
# print expand[i]
#print '---------------------'
expend step table
Expand是一个与网格大小相同的表格，它保存的是节点在哪个步骤展开的信息。从未展开过的标为-1，当一个节点展开时它会得到一个这个表格中唯一的步骤序号，比如下面这个例子步骤序号是0~22：
修改函数search（），以便返回一个名为expand的值的表，里面的元素是g，即此表将跟踪每个节点扩展时是在第几步。做少量修改，以下：
1.初始化一个与网格大小相同且初值全为-1的列表。
2.还需要一个名为count的变量来保持扩展步骤的数量。
3.在结束时，将expand [x] [y]的值设置为count的值，并将count递增1。
已在原代码中以红色代码添加
Print Path
打印出最终解决方案的路径，为此实现一个新的数据结构。
已在源代码中以橙色代码添加
A*算法
search算法的一种变种，比扩展每个节点更高效。这种算法使用了heuristic()走最短的路径，得出来的值小于，或者最好等于真实距离：
h(x,y) ≤ actual goal distance from x,y；
这也意味着heuristic()不精确；heuristic()如下图所示，它能指导search：
在这里，仍然有open列表，但在这个列表里的元素除了g，还有g加上heuristic()的值h(x,y)，用f调用：
f = g + h(x,y)
每次移除f最小的节点，这有一个例子过程，最终确定了一条最短路径：
完成A*算法，-1表示障碍物和未扩展的节点。
已在源代码中用紫色代码添加
在上面粒子中少访问的格子可能微不足道，但当数据非常庞大时就有巨大的不同，特别是出现一个非常深的死胡同效率区别就非常明显
在这个模型中障碍物会随着车辆的移动被传感器感知而扩展，A*树规划的路径会时刻更新不断取消之前的A*树，每一次重新规划的时间都在10微秒之内， 完成这个跟完成A*的区别是是否能完成它的 运动模型，去转向和直走，最明显的区别是能否能沿原轨迹返回。
动态规划
这是路径规划可选择的模型之一，给它一个map和一个或更多目的地，它能返回到任意位置的最佳路径，最重要的是它的起始位置可以是任意!
policy是一个函数将空白小格遍历变成动作，给一个这样的图和一个目的地，就能输出带路标的格子
这样图片就包含了到达任意位置的步数
完成这样的编程，给一个地图，1代表障碍，0代表空位；函数能计算到每一格的最短步数值，并且返回包含这些值的表，障碍用99表示（足够大不会混淆步数）
def compute_value():
value = [[99 for row in range(len(grid[0]))] for col in range(len(grid))]
change = True #标志change在有实际updata时置True，循环中置False
while change:
change = False
for x in range(len(grid)):
for y in range(len(grid[0])):
if goal[0] == x and goal[1] == y: #如果出发点就是目标点，并考虑到意外目标值非0便置0，change置True表示updata
if value[x][y] > 0:
value[x][y] = 0
policy[x][y] = ‘*’
change = True
elif grid[x][y] == 0: #出发点不是目标点情况，
for a in range(len(delta)):
x2 = x + delta[a][0]
y2 = y + delta[a][1]
if x2 >= 0 and x2 < len(grid) and y2 >=0 and y2 < len(grid[0]): #如果赋了动作的x,y还在大网格内（在空格子里并非障碍物）
v2 = value[x2][y2] + cost_step #加一个成本
if v2 < value[x][y]: #取值更小的路径，updata
change = True
value[x][y] = v2
policy[x][y] = delta_name[a] #以下是policy打印代码
for i in range(len(value)):
print policy[i]
事实证明，通过动态规划找到最佳解决方案比A *更容易。将上述的表格转换成动作表，修改已用梅色添加
now do something fun！将动态规划应用于现实：3D的状态空间，每个小格是2D，添加一个方向维度
grid =[[1, 1, 1, 0, 0, 0],
[1, 1, 1, 0, 1, 0],
[0, 0, 0, 0, 0, 0],
[1, 1, 1, 0, 1, 1],
[1, 1, 1, 0, 1, 1]] forward = [[ 1, 0], # up
[ 0, 1], # left
[ 1, 0], # down
[ 0, 1]] # right goal = [2, 0] #从左上角算起
init = [4, 3, 0] cost = [2, 1, 20] # 为cost赋三个值：右转，不转，左转
action = [1, 0, 1]
action_name = ['R', '#', 'L'] def optimum_policy2D():
value = [[[999 for row in range(len(grid[0]))] for col inrange(len(grid))],
[[999 for row in range(len(grid[0]))] for col in range(len(grid))],
[[999 for row in range(len(grid[0]))] for col in range(len(grid))],
[[999 for row in range(len(grid[0]))] for col in range(len(grid))]] policy = [[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))],
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]
[[' ' for row in range(len(grid[0]))] for col in range(len(grid))]] policy2D = [[' ' for row in range(len(grid[0]))] for col in range(len(grid))] #这是最后要打印的 change = True
while change:
change = False
#遍历所有小格并且计算值
for x in range(len(grid)): #起点是目的地
for y in range(len(grid[0])):
for orientation in range(4):
if goal[0] == x and goal[1] == y:
if value[orientation][x][y] > 0:
value[orientation][x][y] = 0
policy[orientation][x][y] = '*'
change = True elif grid[x][y] == 0: #以空格开始
for i in range(3):#计算三种方法来传播值
o2 = (orientation + action[i]) % 4 #添加方向累加，取模4确保方向在3内，做一个缓冲区
x2 = x + forward[o2][0]
y2 = y + forward[o2][1]
if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
v2 = value[o2][x2][y2] + cost[i]
if v2 < value[orientation][x][y]: #留下最小值
change = True
value[orientation][x][y] = v2
policy[orientation][x][y]= action_name[i] #赋值动作标号 x = init[0]
y = init[1]
orientation = init[2] #这里方向是0
policy2D[x][y] = policy[orientation][x][y] #从3D表上copy到2D上来
while policy[orientation][x][y] != '*':#在没到目的地的情况下检查方向，并将方向代表的数字赋给o2
if policy[orientation][x][y] == '#':
o2 = orientation
elif policy[orientation][x][y] == 'R':
o2 = (orientation - 1) % 4
elif policy[orientation][x][y] == 'L':
o2 = (orientation + 1) % 4
x = x + forward[o2][0] #更新x、y、orientation
y = y + forward[o2][1]
orientation = o2
policy2D[x][y] = policy[orientation][x][y] #刚才那句赋的是起点，这里是剩下的全部
return policy2D
SIMULATION
程序能通过改变 cost functions 高效地驾驶这辆车
class世界是离散的，2个规划算法 - A *，它使用heuristic() 来找到one path和dynamic programming，找到一个完整的策略(policy)，即为每个位置制定一个规划。不仅完成这两，还在3D世界做了动态规划。
so far，需了解如何将其转化为实际的机器人动作。 了解连续状态空间以及用来使机器人移动的所谓“控制”。
>>>next...