Discrete-Path-Planning-Practice-Code.ipynb

{
 "cells": [
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "本笔记记录了源自Udactiy自动驾驶课程中关于路径规划的入门级算法。主要针对“离散”的环境下的路径规划问题。涉及了A*，以及动态规划Dyanamic Programming两个典型算法。"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# A*"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### 离散环境的说明：\n",
    "构建一个5*6的矩阵grid，代表整个“地图”,其中“0”表示可以移动到的位置，“1”则表示障碍物。该离散环境如下\n",
    "```\n",
    "[[0, 1, 0, 0, 0, 0],\n",
    "[0, 1, 0, 0, 0, 0],\n",
    "[0, 1, 0, 0, 0, 0],\n",
    "[0, 1, 0, 0, 0, 0],\n",
    "[0, 0, 0, 0, 1, 0]]\n",
    "```\n",
    "其他变量的定义如下：\n",
    "* init 为起始位置\n",
    "* goal 为目标位置\n",
    "* cost 表示每一动一步需要的代价\n",
    "* delta表 示移动的方向，[-1,0]代表上，[0，-1]代表左移，[1，0]代表下移，[0,1]代表右移\n",
    "* delta_name 用符号标出具体的移动方向"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "### A*算法说明\n",
    "A*算法相比于一般的搜索算法，通过启发函数使得搜索的方向更接近于指向终点的方向，从而能更好地找到最短的路径。其关键在于需建立一个启发函数 heuristic function： \n",
    "* 启发函数的尺寸与地图相当。\n",
    "* 每个单元格的数字代表了离终点的距离，终点本身标记为0，移动一个能到达终点则为1，移动2格能到达终点则为2，以此类推。 \n",
    "\n",
    "为了得到最短的路径，要求在计算每一个步骤 action 的g值时（可以简单理解为从起点走到当前位置时已经经过的步数），需要多增加一个f值，既对应的启发函数（矩阵）中对应的数值\n",
    "\n",
    "A*算法的大致步骤为：\n",
    "1. 对各个变量进行初始化，其中将g值初始化为0\n",
    "1. 判断开放节点列表len(open)是否为空，如为空，且已经到终点found=True，则结束循环e。如为空，且未到达终点,则无可行路径，返回\"fail\"\n",
    "2. 如开放节点列表不为空，则里取f值最小的位置 [f,g,h,x,y]. \n",
    "3. 根据delta找到相邻的可以移动到的位置[x2,y2]，并计算移动到该位置的g2值\n",
    "4. 通过启发函数（矩阵）得到新位置[x2,y2]的h2值\n",
    "5. 将g2与h2值相加得到f2值\n",
    "6. 将[f2,g2,h2,x2,y2]加入开放节点列表 open_list中\n",
    "7. 将[x2,y2]标记为关闭节点\n",
    "8. 进行下一次循环\n"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 1,
   "metadata": {},
   "outputs": [],
   "source": [
    "# 定义地图，0为可移动的位置，1为障碍物\n",
    "grid = [[0, 1, 0, 0, 0, 0],\n",
    "        [0, 1, 0, 0, 0, 0],\n",
    "        [0, 1, 0, 0, 0, 0],\n",
    "        [0, 1, 0, 0, 0, 0],\n",
    "        [0, 0, 0, 0, 1, 0]]\n",
    "\n",
    "#定义在离散地图中的移动方向\n",
    "delta = [[-1, 0 ], # go up\n",
    "         [ 0, -1], # go left\n",
    "         [ 1, 0 ], # go down\n",
    "         [ 0, 1 ]] # go right\n",
    "delta_name = ['^', '<', 'v', '>']\n",
    "\n",
    "# 根据目标位置定义启发函数，目标位置值为0，距离越远值越大\n",
    "goal = [len(grid)-1, len(grid[0])-1]\n",
    "heuristic = [[9, 8, 7, 6, 5, 4],\n",
    "             [8, 7, 6, 5, 4, 3],\n",
    "             [7, 6, 5, 4, 3, 2],\n",
    "             [6, 5, 4, 3, 2, 1],\n",
    "             [5, 4, 3, 2, 1, 0]]\n",
    "\n",
    "# 设置初始位置\n",
    "init = [0, 0]\n",
    "\n",
    "#设置移动消耗\n",
    "cost = 1"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 2,
   "metadata": {},
   "outputs": [],
   "source": [
    "def a_star_search(grid,init,goal,cost,heuristic):\n",
    "\n",
    "    # 构造expand矩阵，用于记录从起点起步，到达每个单元格的步数。初始化为-1，表示该点不可达。\n",
    "    expand = [[-1 for col in range(len(grid[0]))] for row in range(len(grid))]\n",
    "\n",
    "    # 构造close矩阵，表示已经走过过的节点，与地图grid相同，所有位置值初始化为0，起始位置初始值为1\n",
    "    closed = [[0 for col in range(len(grid[0]))] for row in range(len(grid))]\n",
    "    closed[init[0]][init[1]] = 1\n",
    "\n",
    "    #初始化变量\n",
    "    #初始化起点坐标\n",
    "    x = init[0]\n",
    "    y = init[1]\n",
    "    #初始化起点g值\n",
    "    g = 0\n",
    "    #初始化h值，并计算f值\n",
    "    h = heuristic[x][y]\n",
    "    f = g + h\n",
    "    #初始化开放节点，加入起始点init\n",
    "    open = [[f,g,h,x,y]]\n",
    "\n",
    "    #构建循环判断变量\n",
    "    #found初始设置为false，表示还未找到终点\n",
    "    found = False\n",
    "    #resign初始设置为false，表示探索尚未中止（开放节点不为空）\n",
    "    resign = False\n",
    "    \n",
    "    #初始化步数count为0\n",
    "    count = 0\n",
    "\n",
    "    #开始循环\n",
    "    while found is False and resign is False: #判断是否为终点，以及是否中止\n",
    "\n",
    "        #判断是否还有open的节点，如果没有，则将resign设为真，回到while循环开头，判断resign为真后退出循环\n",
    "        if len(open) == 0: \n",
    "            resign = True\n",
    "            print(\"fail\") \n",
    "\n",
    "        #open的节点不为空，则继续探索open的节点\n",
    "        else:\n",
    "            #对open的节点进行排序，默认为从小到大，排序后需进行reverse改为从大到小排，通过pop取出最后一个元素，则为f最小的节点\n",
    "            open.sort()\n",
    "            open.reverse()\n",
    "            next = open.pop()\n",
    "\n",
    "            #取处开放节点的坐标x,y和当前的g值\n",
    "            x = next[3]\n",
    "            y = next[4]\n",
    "            g = next[1]\n",
    "            \n",
    "            #在expand中把该位置改写为当前经过的步骤        \n",
    "            expand[x][y] = count\n",
    "            count += 1 \n",
    "\n",
    "            #判断如果当前位置是目标位置，则把found定义为真，回到while循环初始位置后跳出while循环\n",
    "            if x==goal[0] and y==goal[1]:\n",
    "                found =True\n",
    "            \n",
    "            #如果当前位置不是目标位置，则进入主要的更新步骤\n",
    "            else:\n",
    "                #依次探索四个移动方向\n",
    "                for i in range(len(delta)):\n",
    "                    x2 = x + delta[i][0]\n",
    "                    y2 = y + delta[i][1]\n",
    "                    #首先判断个方向是否在地图范围内\n",
    "                    if x2 >= 0 and x2 <= len(grid)-1 and y2 >=0 and y2 <= len(grid[0])-1:\n",
    "                        #再判断下一个位置是不是还没被探索，且没有障碍物\n",
    "                        if closed[x2][y2] == 0 and grid[x2][y2] == 0:\n",
    "                            #如果下一个位置是可达的，则将g值+1,并计算新的h和f值\n",
    "                            g2 = g + cost\n",
    "                            h2 = heuristic[x2][y2]\n",
    "                            f2 = g2 + h2\n",
    "                            #将这个相邻的节点以及其f值加入开放列表\n",
    "                            open.append([f2, g2, h2, x2, y2])\n",
    "                            #并将它标记为已探索\n",
    "                            closed[x2][y2] = 1\n",
    "\n",
    "    return expand"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 3,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[[0, -1, -1, -1, -1, -1],\n",
       " [1, -1, -1, -1, -1, -1],\n",
       " [2, -1, -1, -1, -1, -1],\n",
       " [3, -1, 8, 9, 10, 11],\n",
       " [4, 5, 6, 7, -1, 12]]"
      ]
     },
     "execution_count": 3,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "a_star_search(grid,init,goal,cost,heuristic)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 动态规划 Dynamic Programming\n",
    "\n",
    "动态规划算法可以以任意点为起点，得到最佳路径。其目标是计算在每一个位置上的最佳的运动方向。得到的这种策略与起点的位置往往时无关的  \n",
    "动态规划算法的关键在于构建价值函数 value function。其含义是每个位置到达终点位置的最短距离。"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 29,
   "metadata": {},
   "outputs": [],
   "source": [
    "def dp_search(grid,init,goal,cost): \n",
    "\n",
    "    #定义value函数（矩阵）用于储存各个点到终点的最小步数        \n",
    "    value = [[99 for col in range(len(grid[0]))] for row in range(len(grid))]\n",
    "\n",
    "    #定义policy测率函数，用于储存再各个点上，为了到达终点的最佳移动方向\n",
    "    policy = [[\"\" for col in range(len(grid[0]))] for row in range(len(grid))]    \n",
    "\n",
    "    #定义update变量并初始化为True，用于标记value函数是否还需要更新    \n",
    "    update = True\n",
    "    while update:\n",
    "        #循环开始时，先将update定义为False，后面如果没有动作，则不再更新\n",
    "        update = False\n",
    "\n",
    "        #依次取地图上的每一个点作为探索的中心点        \n",
    "        for x in range(len(grid)):\n",
    "            for y in range(len(grid[0])):\n",
    "\n",
    "                #判断是否为终点，如为终点，则将其value值设为0，并把policy值设为*\n",
    "                if goal[0] == x and goal[1] == y:\n",
    "                    if value[x][y] > 0:\n",
    "                        value[x][y] = 0\n",
    "                        policy[x][y] = \"*\"\n",
    "                        update = True\n",
    "\n",
    "                #如果不是终点，则判断是否为可达的点\n",
    "                elif grid[x][y] == 0:\n",
    "                    #如果时可达的点，则在该中心点上往4个方向，依次移动探索\n",
    "                    for a in range(len(delta)):\n",
    "                        x2 = x + delta[a][0]\n",
    "                        y2 = y + delta[a][1]\n",
    "                        \n",
    "                        #如果周围的的点，在地图范围内，且是可达的\n",
    "                        if x2 >= 0 and x2 <= len(grid)-1 and y2 >=0 and y2 <= len(grid[0])-1 \\\n",
    "                            and grid[x2][y2] == 0:\n",
    "\n",
    "                            #则更新一下周边点的value值，加上移动的cost\n",
    "                            v2 = value[x2][y2] + cost\n",
    "\n",
    "                            #如果周边的点的value值小于还小于中心点的value值\n",
    "                            #说明目前中心点的value比周边被探索点的value大了2cost以上\n",
    "                            #应用周边点的value值替换中心点的value值，这样可保证相邻value值得差保持在1个cost\n",
    "                            if v2 < value[x][y]:\n",
    "                                update = True                                \n",
    "                                value[x][y] = v2                                     \n",
    "                                policy[x][y] = delta_name[a]\n",
    "                                \n",
    "    return value, policy"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 30,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "([[11, 99, 7, 6, 5, 4],\n",
       "  [10, 99, 6, 5, 4, 3],\n",
       "  [9, 99, 5, 4, 3, 2],\n",
       "  [8, 99, 4, 3, 2, 1],\n",
       "  [7, 6, 5, 4, 99, 0]],\n",
       " [['v', '', 'v', 'v', 'v', 'v'],\n",
       "  ['v', '', 'v', 'v', 'v', 'v'],\n",
       "  ['v', '', 'v', 'v', 'v', 'v'],\n",
       "  ['v', '', '>', '>', '>', 'v'],\n",
       "  ['>', '>', '^', '^', '', '*']])"
      ]
     },
     "execution_count": 30,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "dp_search(grid,init,goal,cost)"
   ]
  },
  {
   "cell_type": "markdown",
   "metadata": {},
   "source": [
    "# 左转策略\n",
    "\n",
    "在实际驾驶中，左转相对于其他动作更加复杂，因此需将左转的cost设置的更高。\n",
    "* 在规划路线时，则需要在2D的value和policy矩阵上增加一个维度，用于储存动作信息\n",
    "* 增加action变量，表示车辆保持直行，还是左转或者右转，和forwad变量的不同在于，action是再车辆坐标系下"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 32,
   "metadata": {},
   "outputs": [],
   "source": [
    "forward = [[-1,  0], # go up\n",
    "           [ 0, -1], # go left\n",
    "           [ 1,  0], # go down\n",
    "           [ 0,  1]] # go right\n",
    "forward_name = ['up', 'left', 'down', 'right']\n",
    "\n",
    "# action has 3 values: right turn, no turn, left turn\n",
    "action = [-1, 0, 1]\n",
    "action_name = ['R', '#', 'L']\n",
    "\n",
    "grid = [[1, 1, 1, 0, 0, 0],\n",
    "        [1, 1, 1, 0, 1, 0],\n",
    "        [0, 0, 0, 0, 0, 0],\n",
    "        [1, 1, 1, 0, 1, 1],\n",
    "        [1, 1, 1, 0, 1, 1]]\n",
    "\n",
    "init = [4, 3, 0] # given in the form [row,col,direction]\n",
    "                 # direction = 0: up\n",
    "                 #             1: left\n",
    "                 #             2: down\n",
    "                 #             3: right\n",
    "                \n",
    "goal = [2, 0] # given in the form [row,col]\n",
    "\n",
    "cost = [2, 1, 20] # cost has 3 values, corresponding to making \n",
    "                  # a right turn, no turn, and a left turn"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 33,
   "metadata": {},
   "outputs": [],
   "source": [
    "def left_turn_policy(grid,init,goal,cost): \n",
    "        \n",
    "    value = [[[999 for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "             [[999 for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "             [[999 for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "             [[999 for col in range(len(grid[0]))] for row in range(len(grid))]]\n",
    "    \n",
    "    policy = [[[\" \" for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "              [[\" \" for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "              [[\" \" for col in range(len(grid[0]))] for row in range(len(grid))],\n",
    "              [[\" \" for col in range(len(grid[0]))] for row in range(len(grid))]]\n",
    "    \n",
    "    policy2D = [[\" \" for col in range(len(grid[0]))] for row in range(len(grid))]\n",
    "    \n",
    "    change = True\n",
    "    while change:\n",
    "        change = False\n",
    "        \n",
    "        for x in range(len(grid)):\n",
    "            for y in range(len(grid[0])):\n",
    "                for orientation in range(4):\n",
    "\n",
    "                    if goal[0] == x and goal[1] == y:\n",
    "                        if value[orientation][x][y] > 0:\n",
    "                            value[orientation][x][y] = 0\n",
    "                            policy[orientation][x][y] = \"*\"\n",
    "                            change = True\n",
    "\n",
    "                    elif grid[x][y] == 0:\n",
    "                        for i in range(3):\n",
    "                            o2 = (orientation + action[i]) % 4\n",
    "                            x2 = x + forward[o2][0]\n",
    "                            y2 = y + forward[o2][1]\n",
    "\n",
    "                            if x2 >= 0 and x2 <= len(grid)-1 and y2 >=0 and y2 <= len(grid[0])-1 \\\n",
    "                                and grid[x2][y2] == 0:\n",
    "                                v2 = value[o2][x2][y2] + cost[i]\n",
    "\n",
    "                                if v2 < value[orientation][x][y]:\n",
    "                                    change = True\n",
    "                                    value[orientation][x][y] = v2                               \n",
    "                                    policy[orientation][x][y] = action_name[i]\n",
    "\n",
    "    x = init[0]\n",
    "    y = init[1]\n",
    "    orientation = init[2]\n",
    "    \n",
    "    policy2D[x][y]=policy[orientation][x][y]\n",
    "\n",
    "    while policy[orientation][x][y] != \"*\":\n",
    "        if policy[orientation] == \"#\":\n",
    "            o2 = orientation\n",
    "        elif policy[orientation][x][y] == \"R\":\n",
    "            o2 = (orientation -1) % 4\n",
    "        elif policy[orientation][x][y] == \"L\":\n",
    "            o2 = (orientation +1) % 4\n",
    "        x = x + forward[o2][0]\n",
    "        y = y + forward[o2][1]\n",
    "        orientation = o2\n",
    "        policy2D[x][y] = policy[orientation][x][y]\n",
    "                                    \n",
    "    return policy2D"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 34,
   "metadata": {},
   "outputs": [
    {
     "data": {
      "text/plain": [
       "[[' ', ' ', ' ', 'R', '#', 'R'],\n",
       " [' ', ' ', ' ', '#', ' ', '#'],\n",
       " ['*', '#', '#', '#', '#', 'R'],\n",
       " [' ', ' ', ' ', '#', ' ', ' '],\n",
       " [' ', ' ', ' ', '#', ' ', ' ']]"
      ]
     },
     "execution_count": 34,
     "metadata": {},
     "output_type": "execute_result"
    }
   ],
   "source": [
    "left_turn_policy(grid,init,goal,cost)"
   ]
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "base",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.8.12"
  },
  "orig_nbformat": 4,
  "vscode": {
   "interpreter": {
    "hash": "d9d7cfc533f939c64e31c9003c6f3f9e0a6a96a7a727e48c37a8912fa84f4b35"
   }
  }
 },
 "nbformat": 4,
 "nbformat_minor": 2
}