无人车动态规划(Dynamic Programming)入门

Posted 2021-04-18 半杯茶的小酒杯

1、什么是动态规划(Dynamic Programming)

CS专业出身的人大抵没有人不知道动态规划(Dynamic Programming)的，该算法的本质就是把复杂的大问题分解成相互重叠的简单子问题，将子问题的最优解层层组合起来，就得到了复杂大问题的最优解。

能用动态规划解决的问题必须满足两个条件：一是最优子结构。即问题的最优解所包含的子问题的解也是最优的；二是子问题相互重叠。即是当使用递归进行自顶向下的求解时,每次产生的子问题不总是新的问题,而是已经被重复计算过的问题。

最典型的经常被拿来讲解Dynamic Programming的例子就是斐波那契数列(Fibonacci sequence)，它的数学定义如下:

可以看到斐波那契数列(Fibonacci sequence)带有明显的子问题拆分的特征，通过将F(n)的求解拆分为F(n-1)和F(n-2)两个子问题，重复的子问题只需要计算一次。

斐波那契数列(Fibonacci sequence)计算的Python实现如下：

def fib(n):
    f = [0] * (n + 1)
    f[0] = 0
    f[1] = 1

    for i in range(2, n + 1):
        f[i] = f[i - 1] + f[i - 2]
    return f[n]

2、动态规划算法在无人车中的应用

在如下的自动驾驶场景中，无人车在位置A处进行右转，目标是达到位置G处。理想的驾驶路径是:

(位置A处右转)->(进入车道C)->(变道进入车道B)->(左转到达目标位置G)

自动驾驶场景

但是由于环境是随机的，我们的无人车在实际上路时，可能遇到各种情况。比如当我们计划从车道C变道进入车道B时，发现左侧被一辆大卡车挡住了；如果停下来等大卡车驶过之后再变道，会被车道C上无人车后方的司机拼命用大喇叭催你，无奈之下，我们只好放弃左转，继续直行，再寻求其它路径达到目的地。

目标车道被阻塞的场景

所以最后的行驶路径可能就变成了:

(位置A处右转)->(进入车道C，直行)->(通过路口，进入车道D)->(连续右转，达到位置E)->(直行到达目标位置G)

这里可以看到，我们需要一种方法，使得在无人车放弃车道C到车道B的变道时继续前进时，能够快速找到下一条可通行路径。动态规划(Dynamic Programming)可以用来解决这类问题，它可以给出从任意一个位置出发到达目的地的最优路径。

2.1、更加简化的问题

为了应用动态规划(Dynamic Programming)算法，我们首先看下简化版的问题。如下图所示，我们将道路区域按照空间进行网格划分，带阴影线的网格表示不可通行区域，G表示目标位置。我们的目标是求解从任意一点到目标位置的路径规划策略。

图中的蓝色箭头表示车辆在该位置的规划策略，也是我们要求解的目标，可以认为我们的目标就是通过Policy函数，将位置(x，y)映射到车辆的运动Action上。我们假设车辆只有四个运动Action：向上、向下、向左、向右，我们可以将目标函数写为:

如何将(x,y)转化为具体的Action呢？为了计算Action，我们可以计算每个Cell到达目标姿态的最小Cost，一般情况下，cost的大小与该Cell距离目标姿态的路径长度有关系；有了每个Cell的Cost之后，Action的方向就是从Cost值大的Cell指向Cost值小的网格。

2.2 网格Value(Cost)的计算

每个Cell的Value的Value Function定义如下：

+ Cost

的取值为:  , ,  ,  ，即它的左、上、右、下四个方向的Cell；cost为Cell之间的Cost，这里取Cost为步长1。

可以看到，这是一个典型的动态规划的问题。我们看下如何使用动态规划(Dynamic Programming)算法求解每个Cell的Value。

# Map Represention
grid = [[0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 1, 0, 0, 0, 0],
        [0, 0, 0, 0, 1, 0]]
goal = [len(grid)-1, len(grid[0])-1]

# the cost associated with moving from a cell to an adjacent one
cost = 1 

delta = [[-1, 0 ], # go up
         [ 0, -1], # go left
         [ 1, 0 ], # go down
         [ 0, 1 ]] # go right

def compute_value(grid,goal,cost):
  # If a cell is a wall or it is impossible to reach the goal from a cell,assign that cell a value of 99.
  value = [[99 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])):
        if goal[0] == x and goal[1] == y:
          if value[x][y] > 0:
            value[x][y] = 0
            change = True
        
        elif grid[x][y] == 0:
          for a in range(len(delta)):
            x2 = x + delta[a][0]
            y2 = y + delta[a][1]
            print((x2, y2))
                        
            if x2 >= 0 and x2 < len(grid) and y2 >= 0 and y2 < len(grid[0]) and grid[x2][y2] == 0:
              v2 = value[x2][y2] + cost
                
              if v2 < value[x][y]:
                change = True
                value[x][y] = v2
                                
  return value

最后我们可以得到每个Cell的Value值如下，它的每个值代表了从该位置到达目标姿态的最小Cost，99表示该位置不可达。

[[11, 99, 7, 6, 5,  4],
 [10, 99, 6, 5, 4,  3],
 [9,  99, 5, 4, 3,  2],
 [8,  99, 4, 3, 2,  1],
 [7,  6,  5, 4, 99, 0]]

将Value值映射为Policy，就得到从(x,y)到Policy的映射表：

 [['v', ' ', 'v', 'v', 'v', 'v'],
  ['v', ' ', 'v', 'v', 'v', 'v'],
  ['v', ' ', 'v', 'v', 'v', 'v'],
  ['v', ' ', '>', '>', '>', 'v'],
  ['>', '>', '^', '^', ' ', '*']]

2.3 应用到车辆运动中

用稍微不那么简化的方式来展示Dynamic Programming的应用。如下图所示，红色是车辆的当前位置，蓝色是车辆的目标姿态。假设车辆的运动方向 只有四个选择{Up, Down, Left, Right}， 车辆的运动只有三个选择: 左转、直行、右转。

我们首先构建地图信息：

# 0 = navigable space
# 1 = unnavigable space
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]]

给定车辆的起始位置、结束位置和车辆的运动约束：

init = [4, 3, 0] # given in the form [row,col,direction]
# direction = 0: up
# 1: left
# 2: down
# 3: right
                
goal = [2, 0] # given in the form [row,col]

forward = [[-1,  0], # go up
           [ 0, -1], # go left
           [ 1,  0], # go down
           [ 0,  1]] # go right
forward_name = ['up', 'left', 'down', 'right']

# action has 3 values: right turn, no turn, left turn
action = [-1, 0, 1]
action_name = ['R', '#', 'L']

cost = [2, 1, 20] # cost has 3 values, corresponding to making a right turn, no turn, and a left turn

基于Dynamic Programming计算从起点到终点的车辆运动路径。计算的过程如前所述，先计算Policy，在从Policy中搜索从起点到目标位置的可行驶路径。

def optimum_policy2D(grid,init,goal,cost):
    value = [[[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))],
             [[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
                            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]:
                                    value[orientation][x][y] = v2
                                    policy[orientation][x][y] = action_name[i]
                                    change = True
    
    x = init[0]
    y = init[1]
    orientation = init[2]
    
    policy2D[x][y] = policy[orientation][x][y]
    while policy[orientation][x][y] != '*':
        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]
        y = y + forward[o2][1]
        orientation = o2
        
        policy2D[x][y] = policy[orientation][x][y]
            
    return policy2D

最终输出的路径结果如下:

[[' ', ' ', ' ', 'R', '#', 'R'],
 [' ', ' ', ' ', '#', ' ', '#'],
 ['*', '#', '#', '#', '#', 'R'],
 [' ', ' ', ' ', '#', ' ', ' '],
 [' ', ' ', ' ', '#', ' ', ' ']]

在上面的实现中，我们将左转的Cost设置为20，比较高的左转代价使得车辆更倾向于直行和右转，所以规划路径的效果如下：

我们将将左转的Cost降低到2，看看会发生什么效果？

[[' ', ' ', ' ', ' ', ' ', ' '],
 [' ', ' ', ' ', ' ', ' ', ' '],
 ['*', '#', '#', 'L', ' ', ' '],
 [' ', ' ', ' ', '#', ' ', ' '],
 [' ', ' ', ' ', '#', ' ', ' ']]

可以看到，车辆路径规划在路口位置选择了左转，规划效果如下：

参考资料

本文内容源自Udacity的免费课程：Dynamic Programming - Artificial Intelligence for Robotics

Youtube链接: https://www.youtube.com/watch?v=r2bPY2s9wII&t=12s

推荐阅读

点击左下角【阅读原文】加入我的知识星球！后台回复“优惠券”可以领取优惠券哦！