（5-6-01）D*算法：自动驾驶路径导航系统(1)

最新推荐文章于 2024-08-12 18:10:44 发布

码农三叔

最新推荐文章于 2024-08-12 18:10:44 发布

阅读量860

点赞数 22

分类专栏：《路径规划和决策算法--Python卷》文章标签：算法自动驾驶人工智能 python 路径规划机器人

本文链接：https://blog.csdn.net/asd343442/article/details/140433398

版权

《路径规划和决策算法--Python卷》专栏收录该内容

85 篇文章 4 订阅

订阅专栏

5.5 自动驾驶路径导航系统

本节的项目是一个基于 D* 算法实现的自动驾驶路径导航系统，利用高效的路径规划技术来为自动驾驶车辆提供动态、实时的路径生成与重规划能力。该系统通过智能分析高分辨率地图数据，能够快速识别并规避障碍物，同时在复杂环境中为车辆规划出一条安全、最优的行驶路径。通过交互式的用户界面，操作者可以轻松添加或更新障碍物信息，系统将即时响应并重新计算路径，确保自动驾驶车辆的导航既灵活又可靠。

实例5-4：基于 D* 算法的自动驾驶路径导航系统（codes/5/d_star.py）

5.5.1 背景介绍

随着自动驾驶技术的快速发展，对高效、可靠的路径规划算法的需求日益增长。自动驾驶车辆需要在复杂多变的道路环境中安全行驶，这不仅要求系统能够实时响应交通状况，还要能够处理未知或动态变化的障碍物。传统的路径规划方法在动态环境中往往难以快速准确地重新规划路径。为了解决这一问题，我们开发了基于 D* 算法的自动驾驶路径导航系统，旨在提供一种优化、灵活且高效的解决方案。

本项目的主要功能模块如下所示：

（1）地图网格化（MapGridding）

将输入的地图图像转换为网格化表示，便于算法处理。
通过二值化处理，智能识别并标记地图上的障碍物。

（2）障碍物处理（AddObstacle & DrawObstacle）

允许用户输入障碍物数量，并在地图上随机生成障碍物。
动态绘制障碍物，实时更新路径规划环境。

（3）路径搜索与优化（Planning & ProcessState）

实现 D* 算法的核心功能，包括启发式成本估计和路径搜索。
通过 ProcessState 方法不断更新开放列表和路径成本，寻找最优路径。

（4）动态障碍物响应（ModifyCost & CollisionFree）

当遇到动态障碍物时，能够快速重新计算路径成本，避开障碍。
确保路径规划的动态适应性和实时性。

（5）路径可视化（DrawPath）

将计算出的路径以可视化的方式呈现在地图上。
通过颜色和线条引导用户直观理解路径走向。

（6）用户交互（主入口点）

提供简单的文本界面，允许用户启动路径规划、添加障碍物和退出程序。
增强用户体验，使系统操作简便易懂。

（7）算法性能保证

通过精心设计的算法流程，确保在有限的计算资源下实现最优路径搜索。
考虑算法的时间复杂度和空间复杂度，优化性能。

本项目通过上述功能模块的协同工作，为自动驾驶车辆提供了一个强大的路径规划工具，能够在各种交通环境和条件下提供可靠的导航服务。

5.5.2 具体实现

文件d_star.py实现了基于D*算法的路径规划功能，适用于自动驾驶车辆的导航系统。通过地图图像网格化和D*算法，车辆能够从起点(qstart)到终点(qgoal)寻找最优路径。另外，还支持在行驶过程中动态处理新增障碍物，实时更新路径规划，确保车辆能够安全到达目的地，同时在地图上显示规划结果。文件d_star.py的具体实现流程如下所示。

（1）定义类State，用于表示路径规划的过程中每个节点都包含其位置pos、指向其前驱节点的指针b、状态类型t（初始为'NEW'）、调用值h、以及所有值k以及是否为阻塞物的标志is_obs。这个类用于存储和管理路径规划过程中每个节点的相关信息。

class State(object):
    def __init__(self, pos):
        self.pos = pos
        self.b = None
        self.t = 'NEW'
        self.h = float('inf')
        self.k = float('inf')
        self.is_obs = False
        return

（2）定义类，D_STAR，用于实现D*路径规划算法。函数__init__接受地图路径map_path、起点qstart、终点qgoal和网格参数grid_size，并初始化了各个网格可能移动的方向。通过调用MapGridding方法，并化网格地图，为路径规划预定良好的基础。

class D_STAR(object):
    def __init__(self, map_path, qstart, qgoal, grid_size):
        self.directions = [[-1, -1], [-1, 0], [-1, 1],
                           [0, -1], [0, 1],
                           [1, -1], [1, 0], [1, 1]]
        self.MapGridding(map_path, qstart, qgoal, grid_size)
        return

（3）定义方法MapGridding，作用是将一张地图图像转换成网格化的格式，用于路径规划算法（可能是D*算法或其变种）。以下是该方法的主要功能和步骤：

读取地图：使用cv2.imread读取指定路径的地图图像。
颜色空间转换：将图像从BGR颜色空间转换为灰度空间，使用cv2.cvtColor。
二值化处理：通过cv2.threshold将图像转换为二值图像，其中非障碍区域为白色（255），障碍区域为黑色（0）。
计算网格尺寸：根据提供的grid_size，计算地图可以被划分成多少行和列的网格。
初始化状态集合：创建一个状态集合S，每个网格点对应一个State对象，存储在二维列表中。
障碍物检测：遍历每个网格点，使用IsObstacle方法（未在代码中给出）判断是否为障碍物，并设置相应状态对象的is_obs属性。
设置起点和终点：根据提供的qstart和qgoal坐标，找到对应的网格点状态对象，并赋值给self.qstart和self.qgoal。
绘制网格：使用cv2.line在原地图图像上绘制网格线，以可视化网格结构。
显示和等待：通过cv2.imshow和cv2.waitKey显示网格化后的地图，并等待用户响应。

    def MapGridding(self, map_path, qstart, qgoal, grid_size, color=(0, 0, 0)):
        '''
        grid map 网格化地图
        '''
        self.src_map = cv2.imread(map_path)
        self.map = cv2.cvtColor(self.src_map, cv2.COLOR_BGR2GRAY)
        _, self.map = cv2.threshold(
            self.map, 0, 255, cv2.THRESH_BINARY_INV)

        self.map_shape = np.shape(self.map)
        self.grid_size = grid_size
        self.rows = int(self.map_shape[0] / grid_size)
        self.cols = int(self.map_shape[1] / grid_size)
        # 地图栅格, 初始化地图所有路径点
        self.S = []
        for row in range(self.rows):
            items_rows = []
            for col in range(self.cols):
                items_rows.append(State([row, col]))
            self.S.append(items_rows)
        for row in range(self.rows):
            for col in range(self.cols):
                if self.IsObstacle(self.S[row][col]):
                    self.S[row][col].is_obs = True
        self.qstart = self.S[int(qstart[0] / grid_size)][int(qstart[1] / grid_size)]
        self.qgoal = self.S[int(qgoal[0] / grid_size)][int(qgoal[1] / grid_size)]
        # 绘制地图栅格
        for row in range(self.rows + 1):
            pt1 = (0, int(row * self.grid_size))
            pt2 = (int(self.cols * self.grid_size), int(row * self.grid_size))
            cv2.line(self.src_map, pt1, pt2, color)
            cv2.imshow('D_STAR', self.src_map)
            cv2.waitKey(2)

        for col in range(self.cols + 1):
            pt1 = (int(col * self.grid_size), 0)
            pt2 = (int(col * self.grid_size), int(self.rows * self.grid_size))
            cv2.line(self.src_map, pt1, pt2, color)
            cv2.imshow('D_STAR', self.src_map)
            cv2.waitKey(2)

        cv2.imshow('D_STAR', self.src_map)
        cv2.waitKey(5)
        return

（4）定义方法MapGridding，用于将输入的地图图像进行网格化处理，为路径规划准备数据。首先，将地图图像并转换为灰度图像，然后进行二值化处理以创建地图的二进制反转版本。接着，根据指定的网格大小，将地图划分为网格，并初始化每个网格点为路径规划中的状态对象。对于每个网格点，检查其中是否存在障碍物，并将其标记为障碍物状态。确定起点和终点在网格中的位置后，使用一些已绘制的地图的栅格线在图像上显示，以便Visual Basic地图的网格化处理过程。

    def DrawObstacle(self, obs_set, color):
        '''
        画出添加障碍的节点
        '''
        for obs in obs_set:
            # 原图和二值化后的图片尺寸反的
            # 这里将边界缩小1个像素，防止出现障碍扩充到隔壁格子
            pt1 = (int(obs[1] * self.grid_size + 1),
                   int(obs[0] * self.grid_size + 1))
            pt2 = (int((obs[1] + 1) * self.grid_size - 1),
                   int((obs[0] + 1) * self.grid_size - 1))
            cv2.rectangle(self.map, pt1, pt2, 255, cv2.FILLED)
            cv2.rectangle(self.src_map, pt1, pt2, color, cv2.FILLED)
            cv2.imshow('D_STAR', self.src_map)
            cv2.waitKey(5)
        return

（5）定义方法AddObstacle，作用是在某个环境中随机生成指定数量的障碍物。方法AddObstacle首先初始化一个空列表用于存储障碍物的位置和障碍物对象。接着，它通过循环 num 次来生成障碍物。在每次循环中，它随机选择一个位置，检查这个位置是否与起点或终点重合，如果是，则跳过该位置并重新生成。如果位置有效，它将位置添加到列表中，并将对应位置的障碍物对象标记为障碍物，然后将其添加到障碍物列表中。最后，方法AddObstacle调用 DrawObstacle 方法来在界面上绘制这些障碍物，并返回生成的障碍物列表。

    def AddObstacle(self, num):
        '''
        产生 num 个随机障碍
        '''
        obs_pos = []
        obstacles = []
        color = (0, 0, 0)

        for i in range(num):
            # 排除起始点和终止点
            pos = [random.randint(0, self.rows - 1),
                   random.randint(0, self.cols - 1)]
            if pos == self.qstart.pos or pos == self.qgoal.pos:
                i -= 1
                continue
            obs_pos.append(pos)
            obs = self.S[pos[0]][pos[1]]
            obs.is_obs = True
            obstacles.append(obs)
        self.DrawObstacle(obs_pos, color)
        return obstacles

（6）定义方法IsObstacle，用于检查在给定的 x 点为中心的方格内是否存在障碍物。方法IsObstacle通过计算以 x 点位置为起始点，按照每个方格的大小 grid_size 所覆盖的区域，从地图 map 中提取相应的像素区域。然后，使用 NumPy 库的 sum 函数对这个区域的像素值求和。如果求和结果非零，意味着该区域内存在障碍物（因为障碍物的像素值通常不为零），方法返回 True 表示存在障碍；如果求和结果为零，则返回 False 表示该方格内没有障碍物。这是一种基于图像像素值来判断障碍物存在与否的方法。

    def IsObstacle(self, x):
        '''
        检查以x点为中心的方格是否有障碍物
        '''
        # 反二值化图片，所以判断是否有障碍，就对这个区域求和即可
        row_start = x.pos[0] * self.grid_size
        col_start = x.pos[1] * self.grid_size
        area = self.map[row_start: row_start +
                        self.grid_size, col_start: col_start + self.grid_size]
        if np.sum(area):
            return True
        return False

（7）定义方法MinState，其功能是从一个名为 open_list 的列表中找到具有最小 k 值的状态对象。k 值通常用于表示状态的优先级或成本，例如在 A* 算法中，k 值可能是一个状态的总成本，包括从起点到该状态的路径成本和预估到达终点的成本。

    def MinState(self):
        if not self.open_list:
            return None

        min_state = min(self.open_list, key=lambda x: x.k)
        return min_state

方法MinState首先检查 open_list 是否为空，如果为空则返回 None，表示没有可用的状态。如果不为空，它使用 Python 的 min 函数，并通过一个 lambda 函数指定 key 参数，该 lambda 函数返回每个状态对象的 k 属性值。min 函数将根据这些 k 值找到并返回具有最小 k 值的状态对象，这个状态对象可以被认为是在当前所有候选状态中成本最低的，因此在搜索算法中通常是下一步要探索的状态。

（8）定义方法GetKMin，目的是从 open_list 中找到所有状态对象的最小 k 值。k 值通常代表状态的某种成本或优先级，例如在路径搜索算法中，它可能代表从起点到当前状态的路径成本加上预估的到终点的成本。

    def GetKMin(self):
        if not self.open_list:
            return -1
        k_min = min([X.k for X in self.open_list])
        return k_min

（9）定义方法Delete，作用是从 `open_list` 中删除指定的状态对象 `x`，并将该状态对象标记为 'CLOSED'。`open_list` 通常是一个存储待探索状态的列表，而 'CLOSED' 状态表示该状态已经被探索过，不再需要进一步考虑。方法Delete首先检查状态对象 `x` 是否存在于 `open_list` 中。如果存在，它使用 `remove` 方法将其从列表中移除，这意味着该状态不再被视为待探索的候选项。接着，无论 `x` 是否在列表中，方法都将其 `t` 属性设置为 'CLOSED'，这通常用于状态的分类或标记，以便于在搜索过程中跟踪状态的状态。这个过程是路径搜索算法中常见的操作，用于管理状态的探索状态。

    def Delete(self, x):
        if x in self.open_list:
            self.open_list.remove(x)
        x.t = 'CLOSED'

（10）定义方法Neighbors，功能是获取指定状态 x 周围8个方向上的邻居节点。这个方法通常用于网格搜索算法中，如 A* 或 Dijkstra 算法，用于探索从当前状态出发可以到达的所有可能的下一步状态。方法Neighbors首先初始化一个空列表 neighbors 来存储找到的邻居节点。然后，它遍历一个名为 directions 的列表，该列表包含了所有可能的移动方向（通常是8个方向：上、下、左、右以及对角线方向）。对于每个方向，方法计算新的位置坐标 row 和 col。如果计算出的新坐标超出了网格的边界（即行或列的索引小于0或大于最大值），则跳过当前方向，不添加任何节点到邻居列表中。如果坐标在有效范围内，方法将对应位置的节点从网格 S 中取出，并将其添加到 neighbors 列表中。最后，方法返回这个包含所有有效邻居节点的列表。

    def Neighbors(self, x):
        '''
        获取接邻的8个节点
        '''
        neighbors = []
        for direction in self.directions:
            row = x.pos[0] + direction[0]
            if row < 0 or row >= self.rows:
                continue

            col = x.pos[1] + direction[1]
            if col < 0 or col >= self.cols:
                continue
            neighbors.append(self.S[row][col])
        return neighbors

未完待续

码农三叔

关注

22
点赞
踩
25

收藏

觉得还不错? 一键收藏
打赏
0
评论
（5-6-01）D*算法：自动驾驶路径导航系统(1)

随着自动驾驶技术的快速发展，对高效、可靠的路径规划算法的需求日益增长。自动驾驶车辆需要在复杂多变的道路环境中安全行驶，这不仅要求系统能够实时响应交通状况，还要能够处理未知或动态变化的障碍物。传统的路径规划方法在动态环境中往往难以快速准确地重新规划路径。为了解决这一问题，我们开发了基于 D* 算法的自动驾驶路径导航系统，旨在提供一种优化、灵活且高效的解决方案。本项目的主要功能模块如下所示：（1）地图网格化（MapGridding）将输入的地图图像转换为网格化表示，便于算法处理。
复制链接

扫一扫