【路径规划】原理及实现

路径规划（Path Planning）是指在给定地图、起始点和目标点的情况下，确定应该采取的最佳路径。常见的路径规划算法包括A* 算法、Dijkstra 算法、RRT（Rapidly-exploring Random Tree）等。

目录

一.A*

1.算法原理

2.实例

二.Dijkstra

1.算法原理

2.实例

三.RRT

1.算法原理

2.实例

一.A*

A*算法找到的1条由图中节点以及边组成的路径。一般而言，A*算法基于BFS(Breath First Search,广度优先遍历)，同时也可以认为是对BFS搜索算法的优化。

1.算法原理

具体执行过程如下：

(1) 搜索区域

以2维平面坐标下的简单搜索为例。

绿色方框代表起点和终点，红色方框代表不可经过的点。

(2) 搜索起点

A*算法从起点开始向四周检索相邻方格并进行扩展，直到扩展扫描到终点，算法结束。

流程如下：

① 从起点S开始，把S作为一个等待检查的方格，将其放入open_list中。

open_list：待走的位置。相当于候选点集合，每到1点，将周围点放入候选集合中，往后有可能往候选点去走。

闭集合：已经走过的位置，不会再被考虑。

② 寻找起点S周围可以到达的方格(最多8个)，并将它们加入open_list，同时设置他们的父方格为S;

③ 从open_list中删除起点S，同时将S加入close_list;

④ 计算每个周围方格的F值(F=G+H);

G:从起点S按照生成的路径移动到指定方格的移动代价。有2种距离计算方式：欧拉距离、曼哈顿距离。

H：从指定方格移动到终点的估算成本。使用Manhattan方法，忽略路径中的障碍物，计算当前方格横向或纵向移动到达目标所经过的方格数。

⑤ 从open_list中选取F值最小的方格a，并将其从open_list中删除，放入close_list中;

⑥ 继续检查a的邻接方格：

a). 忽略不可经过的方格以及被标记在close_list中的方格，将剩余的方格加入open_list，并计算这些方格的F值，同时设置父方格为a；

b). 如果某邻接的方格c已经在open_list中，则需计算新的路径从S到c的值G，判断是否需要更新：

如果新的G值更小一些，则修改父方格为方格a，重新计算F值。注意：H值不需要改变，因为方格到达终点的预计消耗是固定的，不需要重复计算。

如果新的G值更大一些，那么说明新路径不是一条更优的路径，则不需要更新值。

⑦ 继续从open_list中找出F值最小的，跳跃至第⑤步继续循环执行。

⑧ 当open_list出现终点E时，代表路径已经被找到，搜索成功。

如果open_list为空，那么说明没有找到合适的路径，搜索失败。

2.实例

'''A*'''
import time
import numpy as np
from matplotlib.patches import Rectangle
import matplotlib.pyplot as plt
import heapq


# 地图上的每1个点都是1个Point对象，用于记录该点的类别、代价等信息
class Point:
    def __init__(self, x=0, y=0):
        self.x = x
        self.y = y
        # 0代表可通行，1代表障碍物
        self.val = 0 
        self.cost_g = 0  
        self.cost_h = 0
        self.cost_f = 0
        self.parent = None
        # 0：不在开集合  1：在开集合  -1：在闭集合
        self.is_open = 0

    # 用于heapq小顶堆的比较
    def __lt__(self, other):
        return self.cost_f < other.cost_f


class Map:
    def __init__(self, map_size):
        self.map_size = map_size
        self.width = map_size[0]
        self.height = map_size[1]
        self.map = [[Point(x, y) for y in range(self.map_size[1])] for x in range(self.map_size[0])]

    # 手动设置障碍物，可多次调用设置地图
    def set_obstacle(self, topleft, width, height):
        for x in range(topleft[0], topleft[0] + width):
            for y in range(topleft[1], topleft[1] + height):
                self.map[x][y].val = 1


class AStar:
    def __init__(self, map, start_point, end_point, connect_num=4):
        self.map: Map = map
        self.start_point = start_point
        self.end_point = end_point
        # 开集合，先放入起点，从起点开始遍历
        self.open_set = [self.start_point]

        self.start_point.is_open = 1 
        # 连通数，目前支持4连通或8连通 
        self.connect_num = connect_num
        self.diffuse_dir = [(1, 0), (-1, 0), (0, 1), (0, -1)]
    def g_cost(self, p):
        '''
        计算 g 代价:当前点与父节点的距离 + 父节点的 g 代价（欧氏距离）
        '''
        x_dis = abs(p.parent.x - p.x)
        y_dis = abs(p.parent.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2) + p.parent.cost_g

    def h_cost(self, p):
        '''
        计算 h 代价:当前点与终点之间的距离（欧氏距离）
        '''
        x_dis = abs(self.end_point.x - p.x)
        y_dis = abs(self.end_point.y - p.y)
        return np.sqrt(x_dis ** 2 + y_dis ** 2)

    def is_valid_point(self, x, y):
        # 无效点：超出地图边界或为障碍物
        if x < 0 or x >= self.map.width:
            return False
        if y < 0 or y >= self.map.height:
            return False
        return self.map.map[x][y].val == 0

    def search(self):
        self.start_time = time.time()
        p = self.start_point
        while not (p == self.end_point):
            # 弹出代价最小的开集合点，若开集合为空，说明没有路径
            try:
                p = heapq.heappop(self.open_set)
            except:
                raise 'No path found, algorithm failed!!!'

            p.is_open = -1

            # 遍历周围点
            for i in range(self.connect_num):
                dir_x, dir_y = self.diffuse_dir[i]
                self.diffusion_point(p.x + dir_x, p.y + dir_y, p)
        return self.build_path(p)  # p = self.end_point

    def diffusion_point(self, x, y, parent):
        # 无效点或者在闭集合中，跳过
        if not self.is_valid_point(x, y) or self.map.map[x][y].is_open == -1:
            return
        p = self.map.map[x][y]
        pre_parent = p.parent
        p.parent = parent
        # 先计算出当前点的总代价
        cost_g = self.g_cost(p)
        cost_h = self.h_cost(p)
        cost_f = cost_g + cost_h
        # 如果在开集合中，判断当前点和开集合中哪个点代价小，换成小的，相同x,y的点h值相同，g值可能不同
        if p.is_open == 1:
            if cost_f < p.cost_f:
                # 如果从当前parent遍历过来的代价更小，替换成当前的代价和父节点
                p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            else:
                # 如果从之前父节点遍历过来的代价更小，保持之前的代价和父节点
                p.parent = pre_parent
        else:
            # 如果不在开集合中，说明之间没遍历过，直接加到开集合里就好
            p.cost_g, p.cost_h, p.cost_f = cost_g, cost_h, cost_f
            heapq.heappush(self.open_set, p)
            p.is_open = 1

    def build_path(self, p):
        print('search time: ', time.time() - self.start_time, ' seconds')
        # 回溯完整路径
        path = []
        while p != self.start_point:
            path.append(p)
            p = p.parent
        print('search time: ', time.time() - self.start_time, ' seconds')
        # 打印开集合、闭集合的数量
        print('open set count: ', len(self.open_set))
        close_count = 0
        for x in range(self.map.width):
            for y in range(self.map.height):
                close_count += 1 if self.map.map[x][y].is_open == -1 else 0
        print('close set count: ', close_count)
        print('total count: ', close_count + len(self.open_set))
        # path = path[::-1]  # path为终点到起点的顺序，可使用该语句翻转
        return path

if __name__ == '__main__':
    map = Map((6, 6))

    # 用于显示plt图
    ax = plt.gca()
    ax.xaxis.set_ticks_position('top')
    ax.set_xlim([0, map.width])
    ax.set_ylim([0, map.height])
    plt.tight_layout()
    plt.grid(True, linestyle="--", color="black", linewidth="1", axis='both')
    ax.xaxis.set_ticks_position('top')
    ax.invert_yaxis()

    # 设置障碍物
    map.set_obstacle([2, 2], 3, 1)
    map.set_obstacle([2, 3], 1, 2)

    # 设置起始点和终点，并创建astar对象
    start_point = map.map[1][1]
    end_point = map.map[4][4]
    # 将障碍物及起点、终点显示到plt上
    ax.add_patch(Rectangle([2, 2], width=3, height=1, color='red'))
    ax.add_patch(Rectangle([2, 3], width=1, height=2, color='red'))
    ax.add_patch(Rectangle([1, 1], width=1, height=1, color='green'))
    ax.add_patch(Rectangle([4, 4], width=1, height=1, color='green'))
    astar = AStar(map, start_point, end_point)
    path = astar.search()


    # 可视化起点到终点完整路径
    for p in path:
        if p!=end_point:
            ax.add_patch(Rectangle([p.x, p.y], width=1, height=1, color='blue'))

    plt.savefig('res.jpg') 
    plt.show()

二.Dijkstra

Dijkstra是1种适用于非负权值网络的单源最短路径算法，可以给出从指定节点到图中其他节点的最短路径，以及任意2点的最短路径。

1.算法原理

下面用1个示例讲述算法的原理过程，如图所示，1个有向权值图，利用Dijkstra算法找到从节点1到节点6的最短路径。

(1) 首先进行初始化，初始化1个空的open list，close list 以及parent。将起点及其距离信息放入到open list 中，将起点及其父亲节点放入parent中，起点的父节点为None。

open list ：存放已经访问的从该节点到起点有路径的结点(有路径但不一定是最优路径)
close list ：存放那些已经找到最优路径的结点。
parent ：存放结点的父子关系，方便后面路径回溯。

(2) 按步骤执行：

(3) 按步骤执行：

(4) 按步骤执行：

(5) 按步骤执行：

(6) 按步骤执行：

(7) 按步骤执行：

(8) 路径回溯

根据parent中的继承关系，从终点向起点回溯，得到从起点到终点的最短路径为：1=>4=>7=>6，最短路径长为：6。

2.实例

import heapq

def dijkstra(graph,start):
    distances = {vertex: float('infinity') for vertex in graph}
    distances[start] = 0
    # 初始化父亲节点
    parent = {vertex: None for vertex in graph}
    priority_queue = [(0,start)]

    while priority_queue:
        # 弹出堆中距离最小的节点
        current_distance, current_vertex = heapq.heappop(priority_queue)
        # 如果当前距离已经大于已知的最短距离，则跳过
        if current_distance > distances[current_vertex]:
            continue
        # 更新相邻节点的距离
        for neighbor, weight in graph[current_vertex].items():
            distance = current_distance + weight
            # 如果找到更短的路径，则更新距离，并将节点加入优先队列
            if distance < distances[neighbor]:
                distances[neighbor] = distance
                parent[neighbor] = current_vertex
                heapq.heappush(priority_queue, (distance, neighbor))
                # print("加入后的队列:",priority_queue)
    return distances, parent

# 输出路径回溯
def get_path(parent,end):
    path = []
    current = end
    while current is not None:
        path.append(current)
        current = parent[current]
    path.reverse()
    return path

graph = {
    '1': {'2': 2, '4': 1},
    '2': {'4': 3, '5': 11 },
    '3': {'1': 4, '6': 5},
    '4': {'5': 2, '6': 8, '7': 4},
    '5': {'7': 6},
    '6':{},
    '7': {'6': 1},
}
start_node='1'
end_node = '6'
distances, parent = dijkstra(graph, start_node)
path = get_path(parent, end_node)
print(f"从节点 {start_node} 到节点 {end_node} 的路径:", path)

三.RRT

1.算法原理

快速随机树算法（Rapid Random Tree,RRT）以初始的1个根节点，通过随机采样的方法在空间搜索，然后添加叶节点来不断扩展随机树。当目标点进入随机树里面后，随机树扩展立即停止，此时能找到1条从起始点到目标点的路径。

算法的计算过程如下：

(1) 初始化随机树。设置起点，目标点和搜索空间；将起点作为随机树搜索的起点，此时树中只包含1个节点即根节点； 

(2)在中随机采样。在中随机采样1个点，若不在障碍物范围内，则计算随机树中所有节点到的欧式距离，并找到距离最近的节点；若在障碍物范围内则重新采样；

(3)生成新节点。在和连线方向，由固定步长距离生成1个新的节点，并判断是否在障碍物范围内，若不在障碍物范围内则将添加到随机树中，并将的父节点设置为；否则的话返回(2)重新对环境进行随机采样；

(4)停止搜索。重复步骤(2)、(3)，当和之间的距离小于设定的阈值时，则代表随机树已经到达了目标点，将作为最后1个路径节点加入到随机树中，算法结束并得到所规划的路径 。

2.实例

import math
import random
import matplotlib.pyplot as plt
import numpy as np
from celluloid import Camera


class RRT:
    class Node:
        def __init__(self, x, y):
            self.x = x
            self.y = y
            self.path_x = []
            self.path_y = []
            self.parent = None

    class AreaBounds:

        def __init__(self, area):
            self.xmin = float(area[0])
            self.xmax = float(area[1])
            self.ymin = float(area[2])
            self.ymax = float(area[3])

    def __init__(self,
                 start,
                 goal,
                 obstacle_list,
                 rand_area,
                 expand_dis=1.0,
                 goal_sample_rate=5,
                 max_iter=500,
                 play_area=None,
                 robot_radius=0.0,
                 ):
        """
        Setting Parameter
        start:起点 [x,y]
        goal:目标点 [x,y]
        obstacleList:障碍物位置列表 [[x,y,size],...]
        rand_area: 采样区域 x,y ∈ [min,max]
        play_area: 约束随机树的范围 [xmin,xmax,ymin,ymax]
        robot_radius: 机器人半径
        expand_dis: 扩展的步长
        goal_sample_rate: 采样目标点的概率，百分制.default: 5，即表示5%的概率直接采样目标点
        """
        self.start = self.Node(start[0], start[1])  # 根节点(0,0)
        self.end = self.Node(goal[0], goal[1])  # 终点(6,10)
        self.min_rand = rand_area[0]  # -2  树枝生长区域xmin
        self.max_rand = rand_area[1]  # 15  xmax

        if play_area is not None:
            self.play_area = self.AreaBounds(play_area)  # 树枝生长区域，左下(-2,0)==>右上(12,14)
        else:
            self.play_area = None  # 数值无限生长

        self.expand_dis = expand_dis  # 树枝一次的生长长度
        self.goal_sample_rate = goal_sample_rate  # 多少概率直接选终点
        self.max_iter = max_iter  # 最大迭代次数
        self.obstacle_list = obstacle_list  # 障碍物的坐标和半径
        self.node_list = []  # 保存节点
        self.robot_radius = robot_radius  # 随机点的搜索半径

    # 路径规划
    def planning(self, animation=True, camara=None):
        # 先在节点列表中保存起点
        self.node_list = [self.start]
        for i in range(self.max_iter):
            # 随机选取一个节点
            rnd_node = self.sample_free()
            # 从已知节点中选择和目标节点最近的节点
            nearest_ind = self.get_nearest_node_index(self.node_list, rnd_node)
            nearest_node = self.node_list[nearest_ind]
            new_node = self.steer(nearest_node, rnd_node, self.expand_dis)

            # 判断新点是否在规定的树的生长区域内，新点和最近点之间是否存在障碍物
            if self.is_inside_play_area(new_node, self.play_area) and \
                    self.obstacle_free(new_node, self.obstacle_list, self.robot_radius):
                # 都满足才保存该点作为树节点
                self.node_list.append(new_node)

            # 如果此时得到的节点到目标点的距离小于扩展步长，则直接将目标点作为xrand。
            if self.calc_dist_to_goal(self.node_list[-1].x, self.node_list[-1].y) <= self.expand_dis:
                # 以新点为起点，向终点画树枝
                final_node = self.steer(self.node_list[-1], self.end, self.expand_dis)
                # 如果最新点和终点之间没有障碍物True
                if self.obstacle_free(final_node, self.obstacle_list, self.robot_radius):
                    return self.generate_final_course(len(self.node_list) - 1)

            if animation and i % 5 == 0:
                self.draw_graph(rnd_node, camara)

        return None  # cannot find path

    # 距离最近的已知节点坐标，随机坐标，从已知节点向随机节点的延展的长度
    def steer(self, from_node, to_node, extend_length=float("inf")):
        # d已知点和随机点之间的距离，theta两个点之间的夹角
        d, theta = self.calc_distance_and_angle(from_node, to_node)
        # 如果树枝的生长长度超出了随机点，就用随机点位置作为新节点
        if extend_length >= d:
            new_x = to_node.x
            new_y = to_node.y
        # 如果树生长长度没达到随机点长度，就截取长度为extend_length的节点作为新节点
        else:
            new_x = from_node.x + math.cos(theta) * extend_length
            new_y = from_node.y + math.sin(theta) * extend_length

        new_node = self.Node(new_x, new_y)
        new_node.path_x = [from_node.x]
        new_node.path_y = [from_node.y]
        new_node.path_x.append(new_x)
        new_node.path_y.append(new_y)
        new_node.parent = from_node

        return new_node

    def generate_final_course(self, goal_ind):
        path = [[self.end.x, self.end.y]]
        node = self.node_list[goal_ind]
        while node.parent is not None:
            path.append([node.x, node.y])
            node = node.parent
        path.append([node.x, node.y])

        return path

    def calc_dist_to_goal(self, x, y):
        """计算(x,y)离目标点的距离"""
        dx = x - self.end.x
        dy = y - self.end.y
        return math.hypot(dx, dy)

    def sample_free(self):
        if random.randint(0, 100) > self.goal_sample_rate:
            rnd = self.Node(
                random.uniform(self.min_rand, self.max_rand),
                random.uniform(self.min_rand, self.max_rand))
        else:  # goal point sampling
            rnd = self.Node(self.end.x, self.end.y)
        return rnd

    # 绘制搜索过程
    def draw_graph(self, rnd=None, camera=None):
        if camera == None:
            plt.clf()
        # 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 rnd is not None:
            plt.plot(rnd.x, rnd.y, "^k")
            if self.robot_radius > 0.0:
                self.plot_circle(rnd.x, rnd.y, self.robot_radius, '-r')
        # 画生成的树
        for node in self.node_list:
            if node.parent:
                plt.plot(node.path_x, node.path_y, "-g")

        # 画障碍物
        for (ox, oy, size) in self.obstacle_list:
            self.plot_circle(ox, oy, size)

        # 画出可行区域
        if self.play_area is not None:
            plt.plot([self.play_area.xmin, self.play_area.xmax,
                      self.play_area.xmax, self.play_area.xmin,
                      self.play_area.xmin],
                     [self.play_area.ymin, self.play_area.ymin,
                      self.play_area.ymax, self.play_area.ymax,
                      self.play_area.ymin],
                     "-k")

        # 画出起点和目标点
        plt.plot(self.start.x, self.start.y, "xr")
        plt.plot(self.end.x, self.end.y, "xr")
        plt.axis("equal")
        plt.axis([-2, 15, -2, 15])
        plt.grid(True)
        plt.pause(0.01)
        if camera != None:
            camera.snap()

    @staticmethod
    def plot_circle(x, y, size, color="-b"):
        deg = list(range(0, 360, 5))
        deg.append(0)
        xl = [x + size * math.cos(np.deg2rad(d)) for d in deg]
        yl = [y + size * math.sin(np.deg2rad(d)) for d in deg]
        plt.plot(xl, yl, color)

    @staticmethod
    def get_nearest_node_index(node_list, rnd_node):
        # 计算所有已知节点和随机节点之间的距离
        dlist = [(node.x - rnd_node.x) ** 2 + (node.y - rnd_node.y) ** 2
                 for node in node_list]
        minind = dlist.index(min(dlist))

        return minind

    @staticmethod
    def is_inside_play_area(node, play_area):
        if play_area is None:
            return True
        if node.x < play_area.xmin or node.x > play_area.xmax or \
                node.y < play_area.ymin or node.y > play_area.ymax:
            return False
        else:
            return True

    @staticmethod
    def obstacle_free(node, obstacleList, robot_radius):
        # 目标点，障碍物中点和半径，移动时的占地半径
        if node is None:
            return False

        for (ox, oy, size) in obstacleList:
            dx_list = [ox - x for x in node.path_x]
            dy_list = [oy - y for y in node.path_y]
            d_list = [dx * dx + dy * dy for (dx, dy) in zip(dx_list, dy_list)]

            if min(d_list) <= (size + robot_radius) ** 2:
                return False

        return True

    @staticmethod
    def calc_distance_and_angle(from_node, to_node):
        """计算两个节点间的距离和方位角"""
        dx = to_node.x - from_node.x
        dy = to_node.y - from_node.y
        d = math.hypot(dx, dy)
        theta = math.atan2(dy, dx)
        return d, theta


def main(gx=6.0, gy=10.0):
    print("start " + __file__)
    fig = plt.figure(1)

    camera = Camera(fig) # 保存动图时使用
    show_animation = True
    obstacleList = [(5, 5, 1), (3, 6, 2), (3, 8, 2), (3, 10, 2), (7, 5, 2),
                    (9, 5, 2), (8, 10, 1)]  # [x, y, radius]
    # Set Initial parameters
    rrt = RRT(
        start=[0, 0],
        goal=[12, 12],
        rand_area=[-2, 15],
        obstacle_list=obstacleList,
        play_area=[-2, 15, 0, 15],
        # 搜索半径
        robot_radius=0.2
    )
    path = rrt.planning(animation=show_animation, camara=camera)

    if path is None:
        print("Cannot find path")
    else:
        print("found path!!")

        # 绘制最终路径
        if show_animation:
            print('start drawing the final path')
            rrt.draw_graph(camera=camera)
            plt.grid(True)
            plt.pause(0.01)
            plt.plot([x for (x, y) in path], [y for (x, y) in path], '-r')
            if camera != None:
                camera.snap()
                animation = camera.animate()
                animation.save('trajectory_res.gif')
            plt.show()


if __name__ == '__main__':
    main()