自动驾驶规划算法（一）：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*算法的总代价函数是： $f(n) = g(n) + h(n)$ 其中， $f(n)$ 表示从起点经过当前节点到达目标节点的估计总代价。 $g(n)$ 确保路径不偏离实际代价，而 $h(n)$ 引导算法朝向目标节点。

3. A*算法的过程

3.1 算法流程

初始化：
- 将起点加入开放列表（open set），即待探索的节点列表。
- 初始化一个闭合列表（closed set），即已经探索过的节点。
从开放列表中选择下一个节点：
- 从开放列表中选择一个代价最小的节点 n，即 f(n) 最小的节点。
检查目标节点：
- 如果选中的节点 n 是目标节点，则搜索结束，路径已找到。
- 否则，将该节点从开放列表移除，加入闭合列表。
扩展节点：
- 计算当前节点 n 的邻居节点（即相邻节点）。
- 对于每个邻居节点：
  - 如果该邻居节点在闭合列表中，则跳过。
  - 如果不在开放列表中，则将其加入开放列表，并记录当前节点为其父节点。
  - 如果该节点已经在开放列表中，检查从当前节点 n 到该邻居节点的路径是否比之前找到的路径更短。如果更短，则更新路径。
重复步骤2-4：
- 重复从开放列表中选取代价最小的节点并进行扩展，直到找到目标节点或者开放列表为空（表示没有路径）。
生成路径：
- 当找到目标节点时，可以通过追踪各个节点的父节点来生成从起点到目标节点的路径。

3.2 启发式函数的选择

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

1. 曼哈顿距离（Manhattan Distance）：适用于网格移动，每次只能上下左右移动。 $h(n) = |x_{\text{current}} - x_{\text{goal}}| + |y_{\text{current}} - y_{\text{goal}}|$

2. 欧几里得距离（Euclidean Distance）：适用于自由空间中任意方向的移动。 $h(n) = \sqrt{(x_{\text{current}} - x_{\text{goal}})^2 + (y_{\text{current}} - y_{\text{goal}})^2}$

3. 对角线距离：适用于允许对角线移动的网格。 $h(n) = \max(|x_{\text{current}} - x_{\text{goal}}|, |y_{\text{current}} - y_{\text{goal}}|)$

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 pltshow_animation = Trueclass AStarPlanner:def __init__(self, ox, oy, resolution, rr):"""初始化网格地图用于A*规划ox: 障碍物的x坐标列表 [m]oy: 障碍物的y坐标列表 [m]resolution: 网格的分辨率 [m]rr: 机器人半径 [m]"""self.resolution = resolutionself.rr = rrself.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 = xself.y = yself.cost = costself.parent_index = parent_indexdef __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_indexgoal_node.cost = current.costbreakdel 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:continueif 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_indexwhile 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_indexreturn 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_positiondef 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 Falseif self.obstacle_map[node.x][node.y]:  # 碰撞检测return Falsereturn Truedef 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] = Truebreakreturn obstacle_map@staticmethoddef 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;  // 网格索引xint y;  // 网格索引ydouble 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;
}

搜索结果如下：