自动驾驶规划算法（一）：A*算法原理和代码（c++与python）

1. A*算法简介

A*算法（A-star algorithm）诞生于1968年，由彼得·哈特（Peter Hart）、尼尔森·尼尔森（Nils Nilsson）和伯特·拉波特（Bertram Raphael）三位计算机科学家提出。它的设计初衷是为了解决路径搜索问题，尤其是通过启发式函数的引导，使得算法能够高效地在图（graph）或网格（grid）结构中找到最优路径。

A*算法是人工智能与路径搜索领域中最为经典的图搜索算法之一，因其在许多实际应用中的高效性和灵活性广泛受到关注。A*算法可以看作是对传统搜索算法的扩展和改进，结合了广度优先搜索（Breadth-First Search，BFS）的系统性和贪心搜索（Greedy Best-First Search）的启发式策略。通过精巧的启发函数设计，A*算法不仅能够找到从起点到终点的最短路径，还能够在实践中保证较高的执行效率，极大地减少了搜索的计算复杂度。本文将深入探讨A算法的原理、流程、优势、局限性及其应用场景，系统性地解读这一算法。

2. A*算法原理

A*算法是一种基于图搜索的启发式搜索算法，用于找到从起点到目标点的最短路径。它结合了 Dijkstra算法 的代价搜索和 贪婪最佳优先搜索 的启发式搜索方法，是一种高效且常用的路径规划算法。A*算法在搜索过程中使用了两种代价函数：

g(n)：从起点到当前节点的实际代价，即路径的累计长度。

h(n)：从当前节点到目标节点的启发式估计代价（通常使用欧几里得距离或曼哈顿距离）。

A*算法的总代价函数是：其中，表示从起点经过当前节点到达目标节点的估计总代价。确保路径不偏离实际代价，而引导算法朝向目标节点。

3. A*算法的过程

3.1 算法流程

• 初始化：

  • 将起点加入开放列表（open set），即待探索的节点列表。

  • 初始化一个闭合列表（closed set），即已经探索过的节点。

• 从开放列表中选择下一个节点：

  • 从开放列表中选择一个代价最小的节点 n，即 f(n) 最小的节点。

• 检查目标节点：

  • 如果选中的节点 n 是目标节点，则搜索结束，路径已找到。

  • 否则，将该节点从开放列表移除，加入闭合列表。

• 扩展节点：

  • 计算当前节点 n 的邻居节点（即相邻节点）。

  • 对于每个邻居节点：

    • 如果该邻居节点在闭合列表中，则跳过。

    • 如果不在开放列表中，则将其加入开放列表，并记录当前节点为其父节点。

    • 如果该节点已经在开放列表中，检查从当前节点 n 到该邻居节点的路径是否比之前找到的路径更短。如果更短，则更新路径。

• 重复步骤2-4：

  • 重复从开放列表中选取代价最小的节点并进行扩展，直到找到目标节点或者开放列表为空（表示没有路径）。

• 生成路径：

  • 当找到目标节点时，可以通过追踪各个节点的父节点来生成从起点到目标节点的路径。

3.2 启发式函数的选择

启发式函数的选择至关重要，直接影响到A*算法的效率和搜索方向。常见的启发式函数有：

1. 曼哈顿距离（Manhattan Distance）：适用于网格移动，每次只能上下左右移动。

2. 欧几里得距离（Euclidean Distance）：适用于自由空间中任意方向的移动。

3. 对角线距离：适用于允许对角线移动的网格。

4. A*算法的优缺点

4.1 优点

• 最优性保证：在启发式函数是可加时，A*算法能够保证找到全局最优路径。这使得它在路径规划问题中广受青睐，特别是在需要找到确切最短路径的应用场景中。

• 高效性：A*算法通过启发式函数合理引导搜索过程，避免了无效的搜索路径，使得其效率远高于简单的遍历搜索算法。

• 灵活性：A*算法可以适应不同的启发式函数，针对不同问题场景可以设计出特定的启发函数，使得算法适应性强。

4.2 缺点

• 性能依赖启发式函数：A*算法的性能极大程度上取决于启发式函数的设计。如果启发式函数设计不当，可能会导致算法性能急剧下降，甚至退化为广度优先搜索。

• 空间复杂度高：A*算法需要存储开放列表和关闭列表中的大量节点信息，这使得其空间复杂度较高。在图的规模较大时，可能会遇到内存不足的问题。

5. A*算法的应用场景

5.1 自动导航与机器人路径规划

A算法被广泛应用于导航系统和机器人路径规划。通过高效的路径搜索能力，A算法能够为自动驾驶汽车、无人机、仓库机器人等设备规划出从起点到终点的最优路径。在这些应用中，启发式函数通常基于实际的物理距离或交通路况设计。

5.2 游戏开发

在许多实时战略游戏、角色扮演游戏中，A*算法用于角色的路径规划，使得角色能够在复杂的游戏场景中智能地避开障碍物，找到最短的移动路线。

5.3 图像处理

在图像处理领域，A算法可以用于图像中的边缘检测、分割、形状匹配等问题。例如，在医学影像分析中，A算法可以用来自动检测图像中的某些特定区域，帮助医生进行诊断。

5.4 网络路由优化

A算法还可以用于网络中的路由规划，帮助选择数据包从源地址到目的地址的最优传输路径。通过设计合适的代价函数，A算法可以有效地处理不同网络拓扑结构下的路由问题。

6. A*算法的优化策略

随着A*算法在实际应用中的普及，人们不断提出对该算法的改进与优化策略，以提高其效率和适应性。常见的优化方法包括：

6.1 记忆化搜索

通过将已计算的节点信息存储在哈希表或其他数据结构中，避免重复计算，能够显著提高算法的效率。这一方法在状态空间较大、代价计算复杂的问题中尤为有效。

6.2 双向A*算法

双向A*算法通过同时从起点和终点两个方向进行搜索，在两条路径相遇时停止，从而减少搜索空间，缩短计算时间。

6.3 启发式函数改进

针对具体问题场景，通过设计更具针对性的启发式函数，能够有效减少无效搜索，提高搜索效率。例如，在机器人路径规划问题中，可以引入障碍物代价或动态环境代价，使得算法更加贴近实际应用需求。

7. 代码示例

python代码如下：

import math
import matplotlib.pyplot as plt

show_animation = True

class AStarPlanner:
    def __init__(self, ox, oy, resolution, rr):
        """
        初始化网格地图用于A*规划

        ox: 障碍物的x坐标列表 [m]
        oy: 障碍物的y坐标列表 [m]
        resolution: 网格的分辨率 [m]
        rr: 机器人半径 [m]
        """
        self.resolution = resolution
        self.rr = rr
        self.min_x, self.min_y = min(ox), min(oy)
        self.max_x, self.max_y = max(ox), max(oy)
        self.motion = self.get_motion_model()
        self.x_width = round((self.max_x - self.min_x) / self.resolution)
        self.y_width = round((self.max_y - self.min_y) / self.resolution)
        self.obstacle_map = self.calc_obstacle_map(ox, oy)

    class Node:
        """ A*算法中的节点定义，包含节点位置、代价和父节点信息 """
        def __init__(self, x, y, cost, parent_index):
            self.x = x
            self.y = y
            self.cost = cost
            self.parent_index = parent_index

        def __str__(self):
            return f"Node({self.x}, {self.y}, {self.cost}, {self.parent_index})"

    def planning(self, sx, sy, gx, gy):
        """
        A*搜索路径

        输入：
            sx, sy: 起点的x、y坐标 [m]
            gx, gy: 终点的x、y坐标 [m]
        
        输出：
            rx: 最终路径的x坐标列表
            ry: 最终路径的y坐标列表
        """
        start_node = self.Node(self.calc_xy_index(sx, self.min_x),
                               self.calc_xy_index(sy, self.min_y), 0.0, -1)
        goal_node = self.Node(self.calc_xy_index(gx, self.min_x),
                              self.calc_xy_index(gy, self.min_y), 0.0, -1)

        open_set = {self.calc_grid_index(start_node): start_node}  # 开放列表
        closed_set = {}  # 闭合列表

        while open_set:
            c_id = min(open_set, key=lambda o: open_set[o].cost + self.calc_heuristic(goal_node, open_set[o]))
            current = open_set[c_id]

            # 动画显示
            if show_animation:
                self.plot_current_node(current)

            # 如果到达目标节点
            if current.x == goal_node.x and current.y == goal_node.y:
                goal_node.parent_index = current.parent_index
                goal_node.cost = current.cost
                break

            del open_set[c_id]
            closed_set[c_id] = current

            # 扩展当前节点的邻居
            for move_x, move_y, move_cost in self.motion:
                node = self.Node(current.x + move_x, current.y + move_y,
                                 current.cost + move_cost, c_id)
                n_id = self.calc_grid_index(node)

                if not self.verify_node(node) or n_id in closed_set:
                    continue

                if n_id not in open_set or open_set[n_id].cost > node.cost:
                    open_set[n_id] = node  # 更新或添加新节点

        return self.calc_final_path(goal_node, closed_set)

    def calc_final_path(self, goal_node, closed_set):
        """ 生成最终路径 """
        rx, ry = [self.calc_grid_position(goal_node.x, self.min_x)], [self.calc_grid_position(goal_node.y, self.min_y)]
        parent_index = goal_node.parent_index
        while parent_index != -1:
            node = closed_set[parent_index]
            rx.append(self.calc_grid_position(node.x, self.min_x))
            ry.append(self.calc_grid_position(node.y, self.min_y))
            parent_index = node.parent_index
        return rx[::-1], ry[::-1]  # 翻转路径，确保从起点到终点

    def calc_heuristic(self, n1, n2):
        """ 启发式函数，使用欧氏距离计算 """
        return math.hypot(n1.x - n2.x, n1.y - n2.y)

    def calc_grid_position(self, index, min_position):
        """ 计算网格节点的实际坐标 """
        return index * self.resolution + min_position

    def calc_xy_index(self, position, min_pos):
        """ 根据实际位置计算网格索引 """
        return round((position - min_pos) / self.resolution)

    def calc_grid_index(self, node):
        """ 计算网格索引，用于在开放列表和闭合列表中存储 """
        return (node.y - self.min_y) * self.x_width + (node.x - self.min_x)

    def verify_node(self, node):
        """ 验证节点是否在地图范围内，且不在障碍物内 """
        if node.x < 0 or node.x >= self.x_width or node.y < 0 or node.y >= self.y_width:
            return False
        if self.obstacle_map[node.x][node.y]:  # 碰撞检测
            return False
        return True

    def calc_obstacle_map(self, ox, oy):
        """ 生成障碍物地图 """
        obstacle_map = [[False for _ in range(self.y_width)] for _ in range(self.x_width)]
        for ix in range(self.x_width):
            x = self.calc_grid_position(ix, self.min_x)
            for iy in range(self.y_width):
                y = self.calc_grid_position(iy, self.min_y)
                for iox, ioy in zip(ox, oy):
                    if math.hypot(iox - x, ioy - y) <= self.rr:
                        obstacle_map[ix][iy] = True
                        break
        return obstacle_map

    @staticmethod
    def get_motion_model():
        """ 机器人运动模型，定义了可能的移动方向及代价 """
        return [[1, 0, 1], [0, 1, 1], [-1, 0, 1], [0, -1, 1],
                [-1, -1, math.sqrt(2)], [-1, 1, math.sqrt(2)],
                [1, -1, math.sqrt(2)], [1, 1, math.sqrt(2)]]

    def plot_current_node(self, node):
        """ 绘制当前节点用于动画展示 """
        plt.plot(self.calc_grid_position(node.x, self.min_x),
                 self.calc_grid_position(node.y, self.min_y), "xc")
        plt.gcf().canvas.mpl_connect('key_release_event',
                                     lambda event: [exit(0) if event.key == 'escape' else None])
        if int(node.cost) % 10 == 0:  # 改为根据cost进行判断
            plt.pause(0.001)

def main():
    # 起点和终点
    sx, sy = 10.0, 10.0  # [m]
    gx, gy = 50.0, 50.0  # [m]
    grid_size = 2.0  # [m]
    robot_radius = 1.0  # [m]

    # 设置障碍物
    ox, oy = [], []
    for i in range(-10, 60):
        ox.append(i)
        oy.append(-10.0)
    for i in range(-10, 60):
        ox.append(60.0)
        oy.append(i)
    for i in range(-10, 61):
        ox.append(i)
        oy.append(60.0)
    for i in range(-10, 61):
        ox.append(-10.0)
        oy.append(i)
    for i in range(-10, 40):
        ox.append(20.0)
        oy.append(i)
    for i in range(0, 40):
        ox.append(40.0)
        oy.append(60.0 - i)

    # 动画初始化
    if show_animation:
        plt.plot(ox, oy, ".k")
        plt.plot(sx, sy, "og")
        plt.plot(gx, gy, "xb")
        plt.grid(True)
        plt.axis("equal")

    a_star = AStarPlanner(ox, oy, grid_size, robot_radius)
    rx, ry = a_star.planning(sx, sy, gx, gy)

    # 显示最终路径
    if show_animation:
        plt.plot(rx, ry, "-r")
        plt.pause(0.001)
        plt.show()

if __name__ == '__main__':
    main()

c++代码如下：

#include <iostream>
#include <vector>
#include <cmath>
#include <map>
#include <queue>
#include <algorithm>
#include <matplotlibcpp.h>  // 用于绘图

namespace plt = matplotlibcpp;

using namespace std;

class AStarPlanner {
public:
    AStarPlanner(const vector<double>& ox, const vector<double>& oy, double resolution, double rr, bool show_animation = true) {
        this->resolution = resolution;
        this->rr = rr;
        this->show_animation = show_animation;

        // 计算地图的最小和最大边界
        min_x = static_cast<int>(round(*min_element(ox.begin(), ox.end())));
        min_y = static_cast<int>(round(*min_element(oy.begin(), oy.end())));
        max_x = static_cast<int>(round(*max_element(ox.begin(), ox.end())));
        max_y = static_cast<int>(round(*max_element(oy.begin(), oy.end())));

        // 计算网格的宽度和高度
        x_width = static_cast<int>(round((max_x - min_x) / resolution));
        y_width = static_cast<int>(round((max_y - min_y) / resolution));

        // 获取运动模型
        motion = get_motion_model();

        // 计算障碍物地图
        calc_obstacle_map(ox, oy);
    }

    struct Node {
        int x;  // 网格索引x
        int y;  // 网格索引y
        double cost;  // 从起点到当前节点的代价
        int parent_index;  // 父节点的索引

        Node(int x_, int y_, double cost_, int parent_index_)
            : x(x_), y(y_), cost(cost_), parent_index(parent_index_) {}

        Node() : x(0), y(0), cost(0.0), parent_index(-1) {}  // 默认构造函数

        bool operator<(const Node& other) const {
            return cost > other.cost;  // 用于优先级队列，代价小的优先级高
        }
    };

    pair<vector<double>, vector<double>> planning(double sx, double sy, double gx, double gy) {
        // 起始节点和目标节点
        Node start_node(calc_xy_index(sx, min_x), calc_xy_index(sy, min_y), 0.0, -1);
        Node goal_node(calc_xy_index(gx, min_x), calc_xy_index(gy, min_y), 0.0, -1);

        // 开放列表和闭合列表
        map<int, Node> open_set;
        map<int, Node> closed_set;
        int start_index = calc_grid_index(start_node);
        open_set[start_index] = start_node;

        while (!open_set.empty()) {
            // 从开放列表中选取代价最小的节点
            int c_id = min_cost_node_index(open_set, goal_node);
            Node current = open_set[c_id];

            // 动画显示
            if (show_animation) {
                plot_current_node(current);
            }

            // 如果到达目标节点
            if (current.x == goal_node.x && current.y == goal_node.y) {
                goal_node.parent_index = current.parent_index;
                goal_node.cost = current.cost;
                break;
            }

            // 从开放列表移除，并加入闭合列表
            open_set.erase(c_id);
            closed_set[c_id] = current;

            // 扩展当前节点的邻居节点
            for (auto& m : motion) {
                Node node(current.x + static_cast<int>(m[0]),
                          current.y + static_cast<int>(m[1]),
                          current.cost + m[2], c_id);
                int n_id = calc_grid_index(node);

                if (!verify_node(node)) {
                    continue;
                }

                if (closed_set.find(n_id) != closed_set.end()) {
                    continue;
                }

                if (open_set.find(n_id) == open_set.end() || open_set[n_id].cost > node.cost) {
                    open_set[n_id] = node;  // 更新或添加新节点
                }
            }
        }

        // 生成最终路径
        return calc_final_path(goal_node, closed_set);
    }

private:
    double resolution;
    double rr;
    int min_x, min_y;
    int max_x, max_y;
    int x_width, y_width;
    vector<vector<bool>> obstacle_map;
    vector<vector<double>> motion;
    bool show_animation;

    void calc_obstacle_map(const vector<double>& ox, const vector<double>& oy) {
        obstacle_map.resize(x_width, vector<bool>(y_width, false));
        for (int ix = 0; ix < x_width; ix++) {
            double x = calc_grid_position(ix, min_x);
            for (int iy = 0; iy < y_width; iy++) {
                double y = calc_grid_position(iy, min_y);
                for (size_t i = 0; i < ox.size(); i++) {
                    double d = hypot(ox[i] - x, oy[i] - y);
                    if (d <= rr) {
                        obstacle_map[ix][iy] = true;
                        break;
                    }
                }
            }
        }
    }

    pair<vector<double>, vector<double>> calc_final_path(Node goal_node, map<int, Node>& closed_set) {
        vector<double> rx, ry;
        rx.push_back(calc_grid_position(goal_node.x, min_x));
        ry.push_back(calc_grid_position(goal_node.y, min_y));
        int parent_index = goal_node.parent_index;
        while (parent_index != -1) {
            Node n = closed_set[parent_index];
            rx.push_back(calc_grid_position(n.x, min_x));
            ry.push_back(calc_grid_position(n.y, min_y));
            parent_index = n.parent_index;
        }
        reverse(rx.begin(), rx.end());
        reverse(ry.begin(), ry.end());
        return make_pair(rx, ry);
    }

    double calc_heuristic(Node n1, Node n2) {
        // 启发式函数，使用欧氏距离
        return hypot(n1.x - n2.x, n1.y - n2.y);
    }

    double calc_grid_position(int index, int min_pos) {
        // 计算网格节点的实际坐标
        return index * resolution + min_pos;
    }

    int calc_xy_index(double position, int min_pos) {
        // 根据实际位置计算网格索引
        return static_cast<int>(round((position - min_pos) / resolution));
    }

    int calc_grid_index(Node node) {
        // 计算网格索引，用于在开放列表和闭合列表中存储
        return (node.y - min_y) * x_width + (node.x - min_x);
    }

    bool verify_node(Node node) {
        // 验证节点是否在地图范围内，且不在障碍物内
        int px = node.x;
        int py = node.y;

        if (px < 0 || py < 0 || px >= x_width || py >= y_width) {
            return false;
        }

        if (obstacle_map[px][py]) {
            return false;
        }

        return true;
    }

    vector<vector<double>> get_motion_model() {
        // 机器人运动模型，定义了可能的移动方向及代价
        return {
            {1, 0, 1},
            {0, 1, 1},
            {-1, 0, 1},
            {0, -1, 1},
            {-1, -1, sqrt(2)},
            {-1, 1, sqrt(2)},
            {1, -1, sqrt(2)},
            {1, 1, sqrt(2)}
        };
    }

    int min_cost_node_index(map<int, Node>& open_set, Node& goal_node) {
        // 从开放列表中找到代价最小的节点索引
        double min_cost = numeric_limits<double>::max();
        int min_index = -1;
        for (auto& item : open_set) {
            double cost = item.second.cost + calc_heuristic(item.second, goal_node);
            if (cost < min_cost) {
                min_cost = cost;
                min_index = item.first;
            }
        }
        return min_index;
    }

    void plot_current_node(Node& node) {
        // 绘制当前节点用于动画展示
        double x = calc_grid_position(node.x, min_x);
        double y = calc_grid_position(node.y, min_y);
        plt::plot({x}, {y}, "xc");
        plt::pause(0.001);
    }
};

int main() {
    // 起点和终点
    double sx = 10.0;  // [m]
    double sy = 10.0;  // [m]
    double gx = 50.0;  // [m]
    double gy = 50.0;  // [m]
    double grid_size = 2.0;  // [m]
    double robot_radius = 1.0;  // [m]
    bool show_animation = true;  // 是否显示动画

    // 设置障碍物
    vector<double> ox, oy;
    for (int i = -10; i < 60; i++) {
        ox.push_back(i);
        oy.push_back(-10.0);
    }
    for (int i = -10; i < 60; i++) {
        ox.push_back(60.0);
        oy.push_back(i);
    }
    for (int i = -10; i <= 60; i++) {
        ox.push_back(i);
        oy.push_back(60.0);
    }
    for (int i = -10; i <= 60; i++) {
        ox.push_back(-10.0);
        oy.push_back(i);
    }
    for (int i = -10; i < 40; i++) {
        ox.push_back(20.0);
        oy.push_back(i);
    }
    for (int i = 0; i < 40; i++) {
        ox.push_back(40.0);
        oy.push_back(60.0 - i);
    }

    // 创建A*规划器
    AStarPlanner a_star(ox, oy, grid_size, robot_radius, show_animation);

    // 如果显示动画，初始化绘图
    if (show_animation) {
        // 绘制障碍物
        plt::plot(ox, oy, ".k");
        // 绘制起点和终点
        plt::plot(vector<double>{sx}, vector<double>{sy}, "og");
        plt::plot(vector<double>{gx}, vector<double>{gy}, "xb");
        plt::grid(true);
        plt::axis("equal");
        plt::pause(0.001);
    }

    // 进行路径规划
    pair<vector<double>, vector<double>> result = a_star.planning(sx, sy, gx, gy);

    // 输出路径坐标
    vector<double> rx = result.first;
    vector<double> ry = result.second;

    // 绘制最终路径
    if (show_animation) {
        // 绘制最终的路径
        plt::plot(rx, ry, "-r");
        plt::pause(0.001);
        plt::show();
        plt::close();
    } else {
        cout << "找到的路径坐标：" << endl;
        for (size_t i = 0; i < rx.size(); i++) {
            cout << "(" << rx[i] << ", " << ry[i] << ")" << endl;
        }
    }

    return 0;
}

搜索结果如下：

参考文献：

https://github.com/AtsushiSakai/PythonRobotics/tree/master