（5-5）D*算法：探险家的行进路线

5.4  探险家的行进路线

本节的项目将解决一个问题：假设你是一名探险家，需要从地图的起点（红色）到达终点（绿色），但这不是一条普通的路径。地图上有一些障碍物（黑色区域），你需要在不触碰这些障碍物的情况下找到一条通往终点的道路。你的任务是使用Python设计D*算法，能够找到一条避开障碍物的最短路径。本项目的目标如下所示：

1. 编程绘制两幅图片：my_map.png 和 my_map_newobs.png，其中绘制的图片my_map.png包含了起点和终点，并且周围有多个不规则形状的黑色障碍物，空白区域表示可以走的路线。在绘制的图片my_map_newobs.png 中包含了黑色矩形障碍物。
2. 当机器人遇到新的障碍物时，可以根据当前地图状态及时进行路径调整，以避开障碍物。

实例5-3：探险家的行进路线（codes/5/map.py和dxing.py）

5.4.1  绘制地图

文件map.py生成了一个空白的地图，并在地图上绘制了起点（红色圆点）和终点（绿色圆点），然后通过在地图上绘制黑色的不规则形状来表示障碍物，包括矩形和五角星形状。最后，将生成的地图和包含障碍物的图像保存为名为"my_map.png"和"my_map_newobs.png"的文件。文件map.py的具体实现代码如下所示。

import cv2
import numpy as np

# 创建空白地图
map_size = (200, 200)  # 地图尺寸
map_img = np.zeros((map_size[0], map_size[1], 3), dtype=np.uint8)  # 创建空白地图
map_img[:, :] = (255, 255, 255)  # 设置地图为白色

# 绘制起点和终点
start_point = (20, 20)
end_point = (180, 180)
cv2.circle(map_img, start_point, 5, (0, 0, 255), -1)  # 绘制红色起点
cv2.circle(map_img, end_point, 5, (0, 255, 0), -1)  # 绘制绿色终点

# 绘制多个不规则形状的障碍物
# 矩形障碍物
rect1 = [(40, 40), (80, 80)]
rect2 = [(120, 40), (160, 80)]
cv2.rectangle(map_img, rect1[0], rect1[1], (0, 0, 0), -1)  # 绘制黑色矩形障碍物
cv2.rectangle(map_img, rect2[0], rect2[1], (0, 0, 0), -1)  # 绘制黑色矩形障碍物

# 五角星形障碍物
star = np.array([[(140, 160), (150, 140), (160, 160), (140, 145), (160, 145)]], np.int32)
cv2.fillPoly(map_img, [star], color=(0, 0, 0))  # 填充黑色五角星形障碍物

# 保存地图图片
cv2.imwrite("my_map.png", map_img)

# 创建包含黑色矩形障碍物的图像
obs_img = np.zeros((map_size[0], map_size[1], 3), dtype=np.uint8)  # 创建空白地图
obs_img[:, :] = (255, 255, 255)  # 设置地图为白色

# 绘制黑色矩形障碍物
obs_rect = [(80, 80), (120, 120)]
cv2.rectangle(obs_img, obs_rect[0], obs_rect[1], (0, 0, 0), -1)  # 绘制黑色矩形障碍物

# 保存障碍物图片
cv2.imwrite("my_map_newobs.png", obs_img)

5.4.2  实现D* 算法

文件dxing.py实现了 D* 算法，首先根据给定的地图信息，包括起点、终点和障碍物，通过图像识别的方式确定障碍物的位置，并在这些位置上更新地图信息。然后，利用 D* 算法搜索从起点到终点的最短路径，并在搜索过程中动态地处理新发现的障碍物，实时更新路径信息。最终，将搜索得到的路径标记在地图上，并输出结果图像。

（1）下面的代码定义了类State，用于表示路径规划中的状态。每个状态对象包含以下属性：

1. x 和 y：表示状态在地图上的坐标位置。
2. parent：指向当前状态的父状态，用于构建路径。
3. state：表示状态的类型，包括普通格子（.）、新路径（@）、障碍（#）、新障碍（%）、第一次搜索的路径（+）、起点（S）和终点（E）。
4. t：表示状态的标记，初始为 "new"。
5. h：表示从起点到当前状态的路径长度。
6. k：表示状态的优先级。

此外，在类State中还定义了以下方法：

1. cost(self, state)：计算当前状态到另一个状态的代价。
2. set_state(self, state)：设置当前状态的类型，如果类型不合法，则不进行设置。

class State(object):
    def __init__(self, x, y):
        self.x = x
        self.y = y
        self.parent = None
        self.state = "."
        self.t = "new"
        self.h = 0
        self.k = 0

    def cost(self, state):
        if self.state == "#" or state.state == "#" or self.state == "%" or state.state == "%":
            return maxsize
        return math.sqrt(pow(self.x - state.x, 2) + pow(self.y - state.y, 2))

    def set_state(self, state):
        # .普通格子  @ 新路径  # 障碍  % 新障碍   + 第一次搜索的路径 S 起点 E 终点
        if state not in [".", "@", "#", "%", "+", "S", "E"]:
            return
        self.state = state

（2）下面的代码定义了类Map，用于表示地图，每个地图对象包含如下所示的属性。

1. row 和 col：表示地图的行数和列数。
2. map：表示地图的二维状态数组，初始化时调用 init_map 方法创建。

另外，在类 Map中还定义了如下所示的方法：

1. init_map(self)：初始化地图，创建一个二维状态数组，数组中的每个元素是 State 类的实例。
2. get_neighbors(self, state)：获取给定状态周围的邻居状态列表。
3. set_obstacle(self, point_list)：在地图上设置障碍物，接受一个包含障碍物坐标的列表作为输入。

class Map(object):
    def __init__(self, row, col):
        self.row = row
        self.col = col
        self.map = self.init_map()

    def init_map(self):
        map_list = []
        for i in range(self.row):
            temp = []
            for j in range(self.col):
                temp.append(State(i, j))
            map_list.append(temp)
        return map_list

    def get_neighbors(self, state):
        state_list = []
        for i in [-1, 0, 1]:
            for j in [-1, 0, 1]:
                if i == 0 and j == 0:
                    continue
                if state.x + i < 0 or state.x + i >= self.row:
                    continue
                if state.y + j < 0 or state.y + j >= self.col:
                    continue
                state_list.append(self.map[state.x + i][state.y + j])
        return state_list

    def set_obstacle(self, point_list):
        for i, j in point_list:
            if i < 0 or i >= self.row or j < 0 or j >= self.col:
                continue
            self.map[i][j].set_state("#")

（3）定义类 DStar，功能是定义了用于实现路径规划的D*算法。该类的主要功能如下所示。

1. 初始化方法 __init__(self, maps)：接受一个地图对象作为参数，并初始化 D* 算法的实例。其中，maps 是一个 Map 类的实例，用于表示地图。
2. process_state(self) 方法：处理状态，根据当前状态更新状态值，并调整优先级队列。
3. min_state(self) 方法：获取优先级队列中优先级最高的状态。
4. min_k_value(self) 方法：获取优先级队列中优先级最高的状态的优先级值。
5. insert_node(self, state, h_new) 方法：向优先级队列中插入新的状态。
6. remove(self, state) 方法：从优先级队列中移除状态。
7. modify_cost(self, state) 方法：修改状态的代价值。
8. run(self, start, end, obs_pic_path) 方法：运行 D* 算法进行路径规划。接受起点、终点和新障碍物图像的路径作为输入，生成路径规划结果。

class DStar(object):
    def __init__(self, maps):
        self.map = maps
        self.open_list = set()

    def process_state(self):
        x = self.min_state()
        if x is None:
            return -1

        old_k = self.min_k_value()
        self.remove(x)

        print("In process_state:(", x.x, ", ", x.y, ", ", x.k, ")")

        # raise状态的点，包含两种情况：①有障碍信息，不知道怎么去终点的点  ②找到了到终点的路径，但是这条路径比最初没障碍的路径长，还要考察一下
        if old_k < x.h:
            for y in self.map.get_neighbors(x):
                if old_k >= y.h and x.h > y.h + x.cost(y):
                    x.parent = y
                    x.h = y.h + x.cost(y)

        # low状态的点
        if old_k == x.h:  # 注意这个开头的if，不可以是elif，不然算法就不对了。某网上资源是有错误的
            for y in self.map.get_neighbors(x):
                if ((y.t == "new") or
                        (y.parent == x and y.h != x.h + x.cost(y)) or
                        (y.parent != x and y.h > x.h + x.cost(y))):
                    y.parent = x
                    self.insert_node(y, x.h + x.cost(y))
        else:  # raise状态的点
            for y in self.map.get_neighbors(x):
                if (y.t == "new") or (y.parent == x and y.h != x.h + x.cost(y)):
                    y.parent = x
                    self.insert_node(y, x.h + x.cost(y))
                else:
                    if y.parent != x and y.h > x.h + x.cost(y):
                        x.k = x.h
                        self.insert_node(x, x.h)
                    else:
                        if y.parent != x and x.h > y.h + x.cost(y) and y.t == "close" and y.h > old_k:
                            self.insert_node(y, y.h)
        return self.min_k_value()

    def min_state(self):
        if not self.open_list:
            print("Open_list is NULL")
            return None
        result = min(self.open_list, key=lambda x: x.k)  # 获取openlist中k值最小对应的节点
        if result.k >= maxsize / 2:
            return None
        return result

    def min_k_value(self):
        if not self.open_list:
            return -1
        result = min([x.k for x in self.open_list])  # 获取openlist表中值最小的k
        if result >= maxsize / 2:
            return -1
        return result

    def insert_node(self, state, h_new):
        if state.t == "new":
            state.k = h_new
        elif state.t == "open":
            state.k = min(state.k, h_new)
        elif state.t == "close":
            state.k = min(state.k, h_new)
        state.h = h_new
        state.t = "open"
        self.open_list.add(state)

    def remove(self, state):
        if state.t == "open":
            state.t = "close"

        self.open_list.remove(state)

    def modify_cost(self, state):
        if state.t == "close":
            self.insert_node(state, state.h)

    def run(self, start, end, obs_pic_path):
        self.insert_node(end, 0)
        while True:
            temp_min_k = self.process_state()
            if start.t == "close" or temp_min_k == -1:
                break
        start.set_state("S")
        s = start
        while s != end:
            s = s.parent
            if s is None:
                print("No route!")
                return
            s.set_state("+")
        s.set_state("E")

        # 添加噪声障碍点！！
        # rand_obs = set()
        # while len(rand_obs) < 1000:
        #     rand_obs.add((int(random.random()*self.map.row), int(random.random()*self.map.row)))

        # 根据图片添加障碍点
        rand_obs = set()
        # new_obs_pic = cv2.imread("./my_map_newobs.png")
        new_obs_pic = cv2.imread(obs_pic_path)
        for i in range(new_obs_pic.shape[0]):
            for j in range(new_obs_pic.shape[0]):
                if new_obs_pic[i][j][0] == 0 and new_obs_pic[i][j][1] == 0 and new_obs_pic[i][j][2] == 0:
                    rand_obs.add((i, j))

        temp_step = 0  # 当前机器人走了多少步(一步走一格)
        temp_s = start
        is_noresult = False  # 新障碍物是不是导致了无解
        while temp_s != end:
            if temp_step == NEW_OBS_STEP:
                # 观察到新障碍
                for i, j in rand_obs:
                    if self.map.map[i][j].state == "#":
                        continue
                    else:
                        self.map.map[i][j].set_state("%")  # 新增障碍物
                        self.modify_cost(self.map.map[i][j])

            k_min = self.min_k_value()
            while not k_min == -1:
                k_min = self.process_state()
                if k_min >= temp_s.h or k_min == -1:
                    # 条件之所以是>=x_c.h，是因为如果从x_c找到了到达目的地的路，那么自身的h就会下降，当搜索到k_min比这个h小时，
                    # 证明需要超过“从当前点到终点的最短路径”的代价的路径已经找完了，再搜也只会找到更长的路径，所以没必要找了，其他
                    # 解都不如这个优。如果一直找不到解，那么x_c.h会是一直是无穷，那么就是说整个openlist都会被搜索完，导致其变为
                    # 空。所以就是无解
                    is_noresult = (k_min == -1)
                    break

            if is_noresult:
                break

            if temp_step == NEW_OBS_STEP:
                draw_s = temp_s
                while not draw_s == end:
                    draw_s.set_state("@")
                    draw_s = draw_s.parent

            temp_s = temp_s.parent
            temp_step += 1

        temp_s.set_state("E")

（4）下面代码是 D* 算法的应用示例，主要完成以下功能：

1. 读取初始地图和新障碍物图像。
2. 创建地图对象，并在地图上标记初始障碍物位置。
3. 定义起点和终点。
4. 初始化 DStar 类的实例，并运行路径规划算法。
5. 根据规划结果，将不同类型的路径标记在地图上：红色标记新障碍物后重规划的路径，蓝色标记第一次搜索的路径，紫色标记新发现的障碍物。
6. 显示带有路径标记的地图，并保存结果地图图像。

if __name__ == "__main__":
    NEW_OBS_STEP = 38  # 在走了多少步之后发现新障碍
    map_png = "my_map.png"  # 初始地图png图片
    obs_png = "my_map_newobs.png"  # 新增障碍物png图片，要跟初始地图相同大小，黑色认为是新障碍物

    my_map = cv2.imread(map_png)
    _x_range_max = my_map.shape[0]
    _y_range_max = my_map.shape[1]
    mh = Map(_x_range_max, _y_range_max)
    for i in range(_x_range_max):
        for j in range(_y_range_max):
            if my_map[i][j][0] == 0 and my_map[i][j][1] == 0 and my_map[i][j][2] == 0:
                # png图片里黑色的点认为是障碍物
                mh.set_obstacle([[i, j]])

    start = mh.map[0][0]  # 起点为（0,0）,左上角
    end = mh.map[_x_range_max - 1][_y_range_max - 1]  # 终点为（max_x-1,max_y-1），右下角

    dstar = DStar(mh)
    dstar.run(start, end, obs_png)
    for i in range(_x_range_max):
        for j in range(_y_range_max):
            if dstar.map.map[i][j].state[0] == "@":  # 红色格子为发现新障碍物重规划后的路径
                my_map[i][j][0] = 0
                my_map[i][j][1] = 0
                my_map[i][j][2] = 255
            elif dstar.map.map[i][j].state[0] == "+":  # 蓝色格子为第一次搜索的路径
                my_map[i][j][0] = 255
                my_map[i][j][1] = 0
                my_map[i][j][2] = 0
            elif dstar.map.map[i][j].state[0] == "%":  # 紫色为新发现的障碍物
                my_map[i][j][0] = 255
                my_map[i][j][1] = 0
                my_map[i][j][2] = 255
            elif dstar.map.map[i][j].state[0] == "E":  # 终点
                my_map[i][j][0] = 128
                my_map[i][j][1] = 0
                my_map[i][j][2] = 128

    cv2.imshow("xx", my_map)
    cv2.waitKey()
    cv2.imwrite("my_map_result.png", my_map)

上述代码的主要目的是演示 D* 算法在实际场景中的应用，通过对地图中的障碍物动态更新，实现路径规划的实时调整和优化。执行后会绘制可视化图，展示没有添加新障碍物前的路线和添加新障碍物后的新路线，如图5-3所示。蓝色路线表示第一次搜索的路径，红色路线表示发现新障碍物后重规划的路径。

图5-3  路线可视化图