Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现

在上一节中，介绍了 Dijkstra 算法的原理以及在图中的应用，这一节将一步步实现 Dijkstra 路径规划算法在二维环境中的路径规划，来进一步加深对 Dijkstra 算法的理解。

所需要用到的 python 库为 matplotlib 和 math。

二维环境的搭建

我们将搭建下图所示的二维环境，其中黑色原点围成的为墙壁障碍物，绿色点为起点（30, 30），红色点为目标点（70，70）。后面需要将所示环境地图转换为栅格地图，所以在此设置栅格地图中栅格的大小为：1.0，设置移动机器人的半径为： 2.0。

实现上述环境的代码如下：

# 设置起点，终点
sx, sy = 30, 30
gx, gy = 70, 70gird_size = 1.0          # 栅格的大小
robot_radius = 2.0        # 机器人的半径# 设置环境地图
ox, oy = [], []# 设置四条边
for i in range(20, 80):         # 下边ox.append(i)oy.append(20.0)
for i in range(20, 80):         # 右边ox.append(80.0)oy.append(i)
for i in range(20, 80):         # 上边ox.append(i)oy.append(80.0)
for i in range(20, 80):         # 左边ox.append(20)oy.append(i)# 设置内部的障碍物
for i in range(20, 60):ox.append(40)oy.append(i)
for i in range(40, 80):ox.append(60)oy.append(i)if show:plt.plot(ox, oy, '.k')plt.plot(sx, sy, 'og')plt.plot(gx, gy, 'or')plt.axis('equal')plt.show()

根据二维环境地图构建栅格地图

根据二维环境地图构建栅格地图的思路是：

由二维环境地图的数据，可以得到地图的四条边界值，左边界：min_x，右边界：max_x，上边界：max_y，下边界：min_y。
根据四条边界值以及我们设置的栅格的大小，可以的栅格地图中栅格的行数和列数，即每一行栅格的数量：y_grid_num，每一列栅格的数量：x_grid_num。
有了栅格的行数以及列数，可以将栅格地图初始化为一个二维数组，初始化每个栅格的占据状态都为 False，即没有被占据。
扫描二维环境，将障碍物占据的栅格设置为 True，表示该栅格被障碍物占据，机器人无法到达。栅格化后，机器人一步一个栅格，即一个栅格就表示机器人可能到达的一个位置，通过计算障碍物到栅格的距离（即障碍物到机器人的距离）与机器人的半径作比较，从而判断该栅格机器人能否到达。到达不了的栅格，将设置为 True。

def calc_obstacle_grid_map(self, ox, oy):""" 构建环境栅格地图 """# 1. 获取环境的 上、 下、 左、 右 四个边界值self.min_x = round(min(ox))self.max_x = round(max(ox))self.min_y = round(min(oy))self.max_y = round(max(oy))# 2. 根据四个边界值和栅格的大小计算 x, y 方向上 栅格的数量self.x_grid_num = round((self.max_x - self.min_x) / self.grid_size)self.y_grid_num = round((self.max_y - self.min_y) / self.grid_size)# 3. 初始化环境栅格地图self.obstacle_map = [[False for _ in range(self.x_grid_num)] for _ in range(self.y_grid_num)]# 4. 将障碍物占据栅格""" 遍历每一个 栅格（前两个 for 循环）以及 遍历每一个障碍物（后两个循环）, 并计算障碍物到栅格的距离比较该距离和机器人半径的大小，判断该栅格是否应该被障碍物占据"""for ix in range(self.x_grid_num):for iy in range(self.y_grid_num):x = self.calc_position(ix, self.min_x)y = self.calc_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.sqrt((iox - x)**2 + (ioy - y)**2)if d <= self.robot_radius:self.obstacle_map[ix][iy] = Truebreak

其中，calc_position() 函数计算的栅格在二维环境中的坐标，其代码如下：

def calc_position(self, index, min_p):""" 将栅格转化成在二维环境中的坐标 """pos = min_p + index * self.grid_sizereturn pos

机器人运动模式

有了栅格地图，接着设置机器人的运动模式，即机器人如何在栅格地图中运动以及运动的消耗是多少。在此设置机器人可以朝着 8 个方向运动，即上、下、左、右、右上、右下、左上、左下。代码实现如下：

def get_motion_model():# dx, dy, costmodel = [[0, 1, 1],      # 上[0, -1, 1],     # 下[-1, 0, 1],     # 左[1, 0, 1],      # 右[1, 1, math.sqrt(2)],    # 右上[1, -1, math.sqrt(2)],   # 右下[-1, -1, math.sqrt(2)],  # 左下[-1, 1, math.sqrt(2)]    # 左上]return model

至此，环境的搭建以及机器人的属性都已设置完毕。为了方便后序进行路径规划，将每一个栅格表示为一个结点，代码实现如下：

class Node:def __init__(self, x, y, cost, parent_index):self.x = x      # 栅格的 x 轴索引self.y = y      # 栅格的 y 轴索引self.cost = cost        # g(n)self.parent_index = parent_index       # 当前节点的父节点

接下来就可以进行 Dijkstra 路径规划了。

Dijkstra 路径规划

在此回顾上节中所说的代码框架。

实现代码如下：

def planning(self, sx, sy, gx, gy):""" 进行路径规划 """# 1. 将机器人的坐标进行结点化sx_index = self.calc_xy_index(sx, self.min_x)sy_index = self.calc_xy_index(sy, self.min_y)gx_index = self.calc_xy_index(gx, self.min_x)gy_index = self.calc_xy_index(gy, self.min_y)start_node = self.Node(sx_index, sy_index, 0.0, -1)goal_node = self.Node(gx_index, gy_index, 0.0, -1)# 2. 初始化 open_set, close_set,并将起点放进 open_set 中open_set, close_set = dict(), dict()open_set[self.calc_index(start_node)] = start_node# 3.开始循环while True:# (1). 取 open_set 中 cost 最小的结点c_id = min(open_set, key=lambda o: open_set[o].cost)current = open_set[c_id]if show:  # 展现路径规划的过程plt.plot(self.calc_position(current.x, self.min_x),self.calc_position(current.y, self.min_y), "xc")if len(close_set.keys()) % 10 == 0:plt.pause(0.001)# (2). 判断该节点是否为终点if current.x == goal_node.x and current.y == goal_node.y:print('Find Goal!')goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# (3). 将该节点从 open_set 中取出，并加入到 close_set 中del open_set[c_id]close_set[c_id] = current# (4). 根据机器人的运动模式，在栅格地图中探索当前位置出发到达的下一可能位置for move_x, move_y, move_cost in self.robot_motion:node = self.Node(current.x + move_x,current.y + move_y,current.cost + move_cost, c_id)n_id = self.calc_index(node)if n_id in close_set:continueif not self.verify_node(node):continueif n_id not in open_set:open_set[n_id] = node   # 发现新的结点else:if open_set[n_id].cost >= node.cost:# 当前节点的路径到目前来说是最优的，进行更新open_set[n_id] = noderx, ry = self.calc_final_path(goal_node, close_set)return rx, ry

路径规划是在结点的基础上进行的，而机器人的起点给的是在二维环境中的坐标，所以首先需要将机器人在二维环境中的坐标转化为栅格地图中坐标，然后再转化成结点表示。其中，二维环境的坐标转化为栅格地图中的坐标的函数为：calc_xy_index()，其代码实现如下：
```
def calc_xy_index(self, pos, min_p):""" 将机器人在二维环境地图中的坐标转化成栅格地图中的坐标 """index = round((pos - min_p) / self.grid_size)return index
```
初始化 open_set，closed_set，并将起点放入到 open_set 中。第一步中已经得到了栅格结点化后的起点，为了方便在 open_set 以及 closed_set 中索引查找，在此再次对栅格结点进行标号索引，标号方式为从左下角向右一行一行进行编号索引。实现函数为：calc_index(),具体实现代码如下：
```
def calc_index(self, node):"""将栅格结点化后的地图进行编号索引，从左下角向右一行一行进行编号索引,如下面示例共 9 个节点，编号方式为：[7, 8, 9][4, 5, 6][1, 2, 3]"""index = node.y * self.x_grid_num + node.xreturn index
```
开始循环查找路径。
- 取出 open_set 中 cost 最小的结点。
- 判断该结点是否为终点。如果是，则说明找到目标点，退出循环。
- 将第一步从 open_set 中取出的结点加入到 closed_set 中。
- 根据机器人的运动模式，在栅格地图中探索当前位置出发到达的下一可能位置，并更新 open_set。其中，在探索可能到达的下一位置时，需要判断下一位置是否有效，即是否在所给的环境当中以及是否处在障碍物上。这个验证函数为：verify_node()，实现代码如下：
```
def verify_node(self, node):""" 验证机器人的当前位置是否合理 """px = self.calc_position(node.x, self.min_x)py = self.calc_position(node.y, self.min_y)# 检查当前位置是否在环境内if px < self.min_x or px > self.max_x:return Falseif py < self.min_x or py > self.max_y:return False# 检查当前位置是否处于障碍物中if self.obstacle_map[node.x][node.y]:return Falsereturn True
```

找到目标点后，进行路径回溯。从目标点开始向前回溯，直到回溯到起点。回溯函数为：calc_final_path(),实现代码如下：

def calc_final_path(self, goal_node, close_set):""" 从终点开始进行回溯，生成从起点到终点的最优路径 """rx = [self.calc_position(goal_node.x, self.min_x)]ry = [self.calc_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = close_set[parent_index]rx.append(self.calc_position(n.x, self.min_x))ry.append(self.calc_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, ry

各个地图之间坐标的转化关系为：

地图表示示例如下：

最终的效果如下：

完整代码如下：

import matplotlib.pyplot as plt
import mathclass Dijkstra:def __init__(self, ox, oy, grid_size, robot_radius):# 初始化地图的情况self.min_x = Noneself.max_x = Noneself.min_y = Noneself.max_y = Noneself.x_grid_num = Noneself.y_grid_num = Noneself.obstacle_map = Noneself.grid_size = grid_sizeself.robot_radius = robot_radiusself.calc_obstacle_grid_map(ox, oy)         # 构建环境栅格地图self.robot_motion = self.get_motion_model()def calc_obstacle_grid_map(self, ox, oy):""" 构建环境栅格地图 """# 1. 获取环境的 上、 下、 左、 右 四个边界值self.min_x = round(min(ox))self.max_x = round(max(ox))self.min_y = round(min(oy))self.max_y = round(max(oy))# 2. 根据四个边界值和栅格的大小计算 x, y 方向上 栅格的数量self.x_grid_num = round((self.max_x - self.min_x) / self.grid_size)self.y_grid_num = round((self.max_y - self.min_y) / self.grid_size)# 3. 初始化环境栅格地图self.obstacle_map = [[False for _ in range(self.x_grid_num)] for _ in range(self.y_grid_num)]# 4. 将障碍物占据栅格""" 遍历每一个 栅格（前两个 for 循环）以及 遍历每一个障碍物（后两个循环）, 并计算障碍物到栅格的距离比较该距离和机器人半径的大小，判断该栅格是否应该被障碍物占据"""for ix in range(self.x_grid_num):for iy in range(self.y_grid_num):x = self.calc_position(ix, self.min_x)y = self.calc_position(iy, self.min_y)for iox, ioy in zip(ox, oy):d = math.sqrt((iox - x)**2 + (ioy - y)**2)if d <= self.robot_radius:self.obstacle_map[ix][iy] = Truebreakdef planning(self, sx, sy, gx, gy):""" 进行路径规划 """## 1. 将机器人的坐标进行结点化sx_index = self.calc_xy_index(sx, self.min_x)sy_index = self.calc_xy_index(sy, self.min_y)gx_index = self.calc_xy_index(gx, self.min_x)gy_index = self.calc_xy_index(gy, self.min_y)start_node = self.Node(sx_index, sy_index, 0.0, -1)goal_node = self.Node(gx_index, gy_index, 0.0, -1)# 2. 初始化 open_set, close_set,并将起点放进 open_set 中open_set, close_set = dict(), dict()open_set[self.calc_index(start_node)] = start_node# 3.开始循环while True:# (1). 取 open_set 中 cost 最小的结点c_id = min(open_set, key=lambda o: open_set[o].cost)current = open_set[c_id]if show:  # pragma: no coverplt.plot(self.calc_position(current.x, self.min_x),self.calc_position(current.y, self.min_y), "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(close_set.keys()) % 10 == 0:plt.pause(0.001)# (2). 判断该节点是否为终点if current.x == goal_node.x and current.y == goal_node.y:print('Find Goal!')goal_node.parent_index = current.parent_indexgoal_node.cost = current.costbreak# (3). 将该节点从 open_set 中取出，并加入到 close_set 中del open_set[c_id]close_set[c_id] = current# (4). 根据机器人的运动模式，在栅格地图中探索当前位置出发到达的下一可能位置for move_x, move_y, move_cost in self.robot_motion:node = self.Node(current.x + move_x,current.y + move_y,current.cost + move_cost, c_id)n_id = self.calc_index(node)if n_id in close_set:continueif not self.verify_node(node):continueif n_id not in open_set:open_set[n_id] = node   # 发现新的结点else:if open_set[n_id].cost >= node.cost:# 当前节点的路径到目前来说是最优的，进行更新open_set[n_id] = noderx, ry = self.calc_final_path(goal_node, close_set)return rx, rydef calc_final_path(self, goal_node, close_set):""" 从终点开始进行回溯，生成从起点到终点的最优路径 """rx = [self.calc_position(goal_node.x, self.min_x)]ry = [self.calc_position(goal_node.y, self.min_y)]parent_index = goal_node.parent_indexwhile parent_index != -1:n = close_set[parent_index]rx.append(self.calc_position(n.x, self.min_x))ry.append(self.calc_position(n.y, self.min_y))parent_index = n.parent_indexreturn rx, ryclass Node:def __init__(self, x, y, cost, parent_index):self.x = x      # 栅格的 x 轴索引self.y = y      # 栅格的 y 轴索引self.cost = cost        # g(n)self.parent_index = parent_index       # 当前节点的父节点## def __str__(self):#     return str(self.x) + "," + str(self.y) + "," + str(self.cost) + "," + str(self.parent_index)def calc_index(self, node):"""将栅格化后的地图进行编号索引，从左下角向右一行一行进行编号索引,如下面示例[7, 8, 9][4, 5, 6][1, 2, 3]"""index = node.y * self.x_grid_num + node.xreturn indexdef calc_xy_index(self, pos, min_p):""" 将机器人在二维环境地图中的坐标转化成栅格地图中的坐标 """index = round((pos - min_p) / self.grid_size)return indexdef calc_position(self, index, min_p):""" 将栅格地图的坐标转化成在真实环境中的坐标 """pos = min_p + index * self.grid_sizereturn posdef verify_node(self, node):""" 验证机器人的当前位置是否合理 """px = self.calc_position(node.x, self.min_x)py = self.calc_position(node.y, self.min_y)# 检查当前位置是否在环境内if px < self.min_x or px > self.max_x:return Falseif py < self.min_x or py > self.max_y:return False# 检查当前位置是否处于障碍物中if self.obstacle_map[node.x][node.y]:return Falsereturn True@staticmethoddef get_motion_model():# dx, dy, costmodel = [[0, 1, 1],      # 上[0, -1, 1],     # 下[-1, 0, 1],     # 左[1, 0, 1],      # 右[1, 1, math.sqrt(2)],    # 右上[1, -1, math.sqrt(2)],   # 右下[-1, -1, math.sqrt(2)],  # 左下[-1, 1, math.sqrt(2)]    # 左上]return modeldef main():# 设置起点，终点sx, sy = 30, 30gx, gy = 70, 70gird_size = 1.0       # 栅格的大小robot_radius = 2.0        # 机器人的半径# 设置环境地图ox, oy = [], []# 设置四条边for i in range(20, 80):         # 下边ox.append(i)oy.append(20.0)for i in range(20, 80):         # 右边ox.append(80.0)oy.append(i)for i in range(20, 80):         # 上边ox.append(i)oy.append(80.0)for i in range(20, 80):         # 左边ox.append(20)oy.append(i)# 设置内部的障碍物for i in range(20, 60):ox.append(40)oy.append(i)for i in range(40, 80):ox.append(60)oy.append(i)if show:plt.plot(ox, oy, '.k')plt.plot(sx, sy, 'og')plt.plot(gx, gy, 'or')# plt.grid('True')plt.axis('equal')# plt.show()dijkstra = Dijkstra(ox, oy, gird_size, robot_radius)rx, ry = dijkstra.planning(sx, sy, gx, gy)if show:plt.plot(rx, ry, '-r')plt.pause(0.01)plt.show()if __name__ == '__main__':show = Truemain()

如果本文对您有帮助，记得在下方点赞呦！欢迎在评论区留言讨论！

Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现相关推荐

RRT路径规划算法在二维仿真环境中的应用 -- Python代码实现
在上一节中,介绍了 RRT 算法的原理,这一节将一步步实现 RRT 路径规划算法在二维环境中的路径规划,来进一步加深对 RRT 算法的理解. 二维环境的搭建我们将搭建下图所示的二维环境,绿色点为起点 ...
RRT* 算法原理以及在二维仿真环境中的实现 -- Python代码实现
RRT* 算法是在 RRT 的基础上做出了一些改进,主要改进的点有两点: 新结点生成后,优化其父结点. 在生成新结点 new_node 后,首先设置一个搜索区域的半径,搜索该区域中的树结点,并计算其中 ...
doa的matlab算法,基于MATLAB的DOA估计算法的二维仿真建模
基本描述: DOA算法使用T形或L形天线接收微波信号,计算到达角,然后对二维坐标建模. 要解决的问题: 实际环境中存在多个相干源(例如多径效应,信号反射等),并给出了最佳DOA估计算法. 功能描述: ...
Dijkstra 路径规划算法原理详解及 Python 代码实现
荷兰数学家 E.W.Dijkstra 于 1959 年提出了 Dijkstra 算法,它是一种适用于非负权值网络的单源最短路径算法,同时也是目前求解最短路径问题的理论上最完备.应用最广的经典算法 ...
【路径规划】 Optimal Trajectory Generation in Frenet阅读记录 (附python代码实例)
参考与前言 2010年,论文 Optimal Trajectory Generation for Dynamic Street Scenarios in a Frenet Frame 地址:https ...
基于群智能的路径规划算法（三）------遗传算法
本系列文章主要记录学习基于群智能的路径规划算法过程中的一些关键知识点,并按照理解对其进行描述和进行相关思考. 主要学习资料是来自小黎的Ally 的 <第2期课程-基于群智能的三维路 ...
基于群智能的路径规划算法（四）------人工蜂群算法
本系列文章主要记录学习基于群智能的路径规划算法过程中的一些关键知识点,并按照理解对其进行描述和进行相关思考. 主要学习资料是来自小黎的Ally 的 <第2期课程-基于群智能的三维路 ...
【自动驾驶】基于采样的路径规划算法——PRM（含python实现）
文章目录参考资料 1. 基本概念 1.1 基于随机采样的路径规划算法 1.2 概率路图算法(Probabilistic Road Map, PRM) 1.3 PRM算法的优缺点 1.4 PRM算法伪 ...
a算法和a*算法的区别_机器人路径规划算法，全局路径规划与局部路径规划究竟有哪些区别？...
若步智能移动这一简单动作,对于人类来说相当容易,但对机器人而言就变得极为复杂,说到机器人移动就不得不提到路径规划,路径规划是移动机器人导航最基本的环节,指的是 ...

Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现

Dijkstra 路径规划算法在二维仿真环境中的应用 -- Python代码实现相关推荐

最新文章

热门文章