（6-3-03）D* Lite 算法：基于 D* Lite的路径规划器(3) 实现路径规划算法

最新推荐文章于 2024-08-14 11:57:07 发布

码农三叔

最新推荐文章于 2024-08-14 11:57:07 发布

阅读量796

点赞数 10

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

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

版权

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

85 篇文章 4 订阅

订阅专栏

（11）定义方法 UInsert，用于将节点 u 插入到优先队列 U 中。该方法接收两个参数，节点 u 和其对应的键值 key。首先，将节点 u 的键值 k 设置为参数 key。然后，将节点 u 添加到类成员变量 U 表示的优先队列中。这个方法在路径规划算法中常用，用于管理和维护优先级队列中的节点，以确保在算法执行中能够快速访问和处理优先级最高的节点。

    def UInsert(self, u, key):
        '''
        将节点u 插入到优先队列中
        '''
        u.k = key
        self.U.add(u)
        return

（12）定义方法 UpdateVertex，用于在D* Lite路径规划算法中更新节点 u 的状态和优先级队列。具体功能如下：

首先，如果节点 u 不是目标节点 qgoal：
计算节点 u 的新路径代价 rhs，通过考虑其所有后继节点的路径代价和当前路径代价来确定最小的路径成本，并更新节点 u 的 rhs 和其父节点 u.p。
然后，如果节点 u 存在于优先级队列 U 中，则从队列中移除。
最后，如果节点 u 的实际路径代价 g 不等于其估计路径代价 rhs，则将节点 u 插入优先级队列 U 中，并按照计算的关键值调整其优先级顺序。

    def UpdateVertex(self, u):
        if u != self.qgoal:
            new_rhs = float('inf')
            for succ in self.Successors(u):
                tmp_rhs = self.Cost(u, succ) + succ.g
                if tmp_rhs < new_rhs:
                    new_rhs = tmp_rhs
                    u.p = succ
            u.rhs = new_rhs
        if u in self.U:
            self.U.remove(u)

        if u.g != u.rhs:
            self.UInsert(u, self.CalculateKey(u))
        return

（13）方法ComputeShortestPath实现了D* Lite路径规划算法的核心逻辑，通过维护优先级队列 U 和更新节点的路径代价来实现从起始节点到目标节点的最短路径计算。算法不断地更新节点的路径代价，并根据当前路径估计调整节点的优先级，直到找到最优路径或确定无法到达目标节点为止。

    def ComputeShortestPath(self):
        '''
        计算最短路径
        '''
        while len(self.U) != 0 and ((self.TopKey() < self.CalculateKey(self.qstart)) or
                                    (self.qstart.rhs != self.qstart.g)):
            k_old = self.TopKey()
            u = min(self.U, key=lambda x: x.k)
            self.U.remove(u)
            k_u = self.CalculateKey(u)
            if k_old < k_u:
                self.UInsert(u, k_u)
            elif u.g > u.rhs:
                u.g = u.rhs
                for succ in self.Successors(u):
                    self.UpdateVertex(succ)
            else:
                u.g = float('inf')
                self.UpdateVertex(u)
                for pred in self.Predessors(u):
                    self.UpdateVertex(pred)
        return self.qstart.rhs != float('inf')

（14）定义方法 DrawGrid，用于在图像上绘制网格线。该方法通过遍历网格的行和列，利用OpenCV的函数在地图图像 self.src_map 上绘制水平和垂直的网格线条，每条线条的间隔由 self.grid_size 决定。在绘制过程中通过 cv2.imshow 函数实时显示更新后的图像，并使用函数cv2.waitKey来控制显示的频率，以便观察网格绘制的过程。

    def DrawGrid(self, color=(0, 0, 0)):
        '''
        画出网格
        '''
        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_LITE', self.src_map)
            cv2.waitKey(50)

        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_LITE', self.src_map)
            cv2.waitKey(50)

        cv2.imshow('D_STAR_LITE', self.src_map)
        cv2.waitKey(50)
        return

（15）定义方法DrawObstacle，用于在地图图像上绘制障碍物节点。方法DrawObstacle接收两个参数 obs_set 和 color，其中 obs_set 是一个包含障碍物位置的列表或集合，每个障碍物位置由其行列坐标组成。方法遍历 obs_set 中的每个障碍物位置，并在地图图像 self.src_map 和二值化地图 self.map 上绘制矩形障碍物区域。绘制时考虑到像素边界问题，使用偏移量来避免障碍物扩展到相邻格子。在绘制完成后，通过 cv2.imshow 函数实时显示更新后的地图图像，并通过 cv2.waitKey 函数控制显示频率，以便观察障碍物添加的过程。

    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_LITE', self.src_map)
            cv2.waitKey(5)
        return

（16）方法AddObstacle用于在地图中随机生成指定数量的障碍物，并在地图图像上标记这些障碍物的位置，确保它们不包括起始点和目标点。

    def DrawPath(self, color=None):
        '''
        路径结果， 这里坐标和cv的图片坐标相反的
        '''
        if color is None:
            color = (random.randint(0, 255),
                     random.randint(0, 255),
                     random.randint(0, 255))

        mid_node = self.qstart
        while mid_node != self.qgoal:
            # 画出网格连线
            pt1 = (int(mid_node.pos[1] * self.grid_size + self.grid_size / 2),
                   int(mid_node.pos[0] * self.grid_size + self.grid_size / 2))
            pt2 = (int(mid_node.p.pos[1] * self.grid_size + self.grid_size / 2),
                   int(mid_node.p.pos[0] * self.grid_size + self.grid_size / 2))
            cv2.line(self.src_map, pt1, pt2, color, 2)
            cv2.imshow('D_STAR_LITE', self.src_map)
            cv2.waitKey(50)

            mid_node = mid_node.p

        return

（17）方法DrawPath用于在地图上绘制从起始点到目标点的最短路径，使用OpenCV库在图像上画出连接路径的线条，并根据给定的颜色参数或随机颜色来标识路径。

    def DrawPath(self, color=None):
        '''
        路径结果， 这里坐标和cv的图片坐标相反的
        '''
        if color is None:
            color = (random.randint(0, 255),
                     random.randint(0, 255),
                     random.randint(0, 255))

        mid_node = self.qstart
        while mid_node != self.qgoal:
            # 画出网格连线
            pt1 = (int(mid_node.pos[1] * self.grid_size + self.grid_size / 2),
                   int(mid_node.pos[0] * self.grid_size + self.grid_size / 2))
            pt2 = (int(mid_node.p.pos[1] * self.grid_size + self.grid_size / 2),
                   int(mid_node.p.pos[0] * self.grid_size + self.grid_size / 2))
            cv2.line(self.src_map, pt1, pt2, color, 2)
            cv2.imshow('D_STAR_LITE', self.src_map)
            cv2.waitKey(50)

            mid_node = mid_node.p

        return

（18）方法Planning实现了路径规划的整体流程。首先，初始化优先队列 U，设置目标点 qgoal 的初始 rhs 值为0，并将目标点插入到优先队列中。然后，通过用户输入添加障碍物，并调用 ComputeShortestPath 方法计算最短路径。如果找到了最短路径，则绘制路径；否则输出“Not Found”并返回。接着，进入循环，允许用户继续添加障碍物并更新路径，直到找到新的最短路径或者用户终止输入。

    def Planning(self):
        self.U = set()
        self.qgoal.rhs = 0
        self.UInsert(self.qgoal, [self.HScore(self.qgoal), 0])
        num = input("input obstacle numbers: ")
        try:
            input_num = eval(num)
            if type(input_num) == int:
                obstacle_set = self.AddObstacle(input_num)
            else:
                return
        except:
            return
        if self.ComputeShortestPath():
            print('Found')
            self.DrawPath()
        else:
            print('Not Found')
            return
        while True:
            num = input("input obstacle numbers: ")
            try:
                input_num = eval(num)
                if type(input_num) == int:
                    obstacle_set = self.AddObstacle(input_num)
                    for obs in obstacle_set:
                        successors = self.Successors(obs)
                        for succ in successors:
                            self.UpdateVertex(succ)
                        for succ in successors:
                            if succ.p is obs:
                                succ.p = None
                    if self.ComputeShortestPath():
                        print('Found')
                        self.DrawPath()
                    else:
                        print('Not Found')
                        return
                else:
                    return
            except:
                return

（19）下面这段代码是一个示例程序的主程序部分，用来演示如何使用 D_STAR_LITE 类进行路径规划。首先，设置地图文件路径 map_path、起始点 qstart 和目标点 qgoal 的位置、最大步数 max_steps 和网格大小 grid_size。然后，创建类D_STAR_LITE的实例 d_star_lite，并传入这些参数进行初始化。接着，程序等待用户输入任意键，然后调用 d_star_lite.Planning() 方法进行路径规划。一旦规划完成，程序再次等待用户输入任意键，最后退出程序。

if __name__ == "__main__":
    map_path = 'map/map500-500.png'
    qstart = [10, 10]
    qgoal = [490, 490]
    max_steps = 1000
    grid_size = 20

    d_star_lite = D_STAR_LITE(map_path, qstart, qgoal, grid_size)
    input('press any key to start planning:')
    d_star_lite.Planning()
    input('press any key to quit:')

上述代码的功能是通过图形界面展示地图路径规划的过程，用户可以交互式地添加障碍物和查看规划路径的变化。运行程序，例如输入障碍物数量为50后会打印输出下面的内容，并绘制找到的最短路径可视化图，如图6-2所示。

press any key to start planning:
input obstacle numbers: 50
Found
input obstacle numbers:

图6-2 最短路径可视化图

码农三叔

关注

10
点赞
踩
16

收藏

觉得还不错? 一键收藏
打赏
0
评论
（6-3-03）D* Lite 算法：基于 D* Lite的路径规划器(3) 实现路径规划算法

在绘制完成后，通过 cv2.imshow 函数实时显示更新后的地图图像，并通过 cv2.waitKey 函数控制显示频率，以便观察障碍物添加的过程。如果找到了最短路径，则绘制路径；该方法通过遍历网格的行和列，利用OpenCV的函数在地图图像 self.src_map 上绘制水平和垂直的网格线条，每条线条的间隔由 self.grid_size 决定。（17）方法DrawPath用于在地图上绘制从起始点到目标点的最短路径，使用OpenCV库在图像上画出连接路径的线条，并根据给定的颜色参数或随机颜色来标识路径。
复制链接

扫一扫