2. 全局路径规划基础：A*算法原理与实现、基于网格的路径搜索、在ROS2中发布全局路径

全局路径规划，说白了就是给自动驾驶的车子画一条从起点到终点的「大路线」。我刚开始做规划时，总觉得这步很简单——不就是找条路吗？后来在园区测试时才发现，一条不合理的全局路径，能让后面的控制模块直接「崩溃」。今天我们就来聊聊最经典的A*算法，以及怎么在ROS2里把它跑起来。

2.1 A*算法的核心思想

A*算法本质上是一种启发式搜索。它不像Dijkstra那样「无头苍蝇」式地乱找，而是带着一个「指南针」——也就是启发函数。我个人习惯把A*拆成三个关键部分：

g(n)：从起点到当前节点的实际代价。说白了就是已经走了多远。
h(n)：从当前节点到终点的估计代价。这是启发函数，我常用曼哈顿距离或欧几里得距离。
f(n) = g(n) + h(n)：总代价。A*每次选f(n)最小的节点去扩展。

你想想看，如果h(n)始终为0，A*就退化成Dijkstra了。如果h(n)比实际值大很多，那就变成了贪心最佳优先搜索，容易走弯路。我在项目中遇到过一个问题：用欧几里得距离做启发函数，结果在迷宫场景下搜索效率极低。后来换成曼哈顿距离，速度直接翻倍。

关键点：A*算法的完备性和最优性取决于启发函数。如果h(n)是可采纳的（即不大于实际代价），A*一定能找到最短路径。

2.2 基于网格的路径搜索实现

实际工程中，我们通常把环境离散成网格地图。每个格子要么是「可通行」，要么是「障碍物」。嗯，这里要注意：网格的分辨率直接影响规划效果。分辨率太高，计算量爆炸；分辨率太低，路径可能穿墙。

下面是我常用的一个A*实现框架，用Python写的伪代码，但逻辑和C++版本完全一致：

def a_star_search(grid, start, goal):
    open_set = PriorityQueue()
    open_set.put(start, 0)
    
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
    
    while not open_set.empty():
        current = open_set.get()
        
        if current == goal:
            return reconstruct_path(came_from, current)
        
        for neighbor in get_neighbors(grid, current):
            # 跳过障碍物
            if grid[neighbor.x][neighbor.y] == OBSTACLE:
                continue
                
            tentative_g = g_score[current] + 1  # 假设相邻格子代价为1
            
            if neighbor not in g_score or tentative_g < g_score[neighbor]:
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
                open_set.put(neighbor, f_score[neighbor])
    
    return None  # 无路可走

这段代码里，get_neighbors函数决定了搜索的连通性。我建议用8邻域（上下左右+对角线），但要注意对角线移动的代价应该是√2，而不是1。我曾经因为偷懒统一用1，结果规划出来的路径在拐角处「切西瓜」，车子根本走不了。

避坑指南：我曾经在动态障碍物场景下直接用静态网格做A*，结果路径频繁重规划，车子在原地打转。后来加了「膨胀层」——把障碍物周围的格子也标记为不可通行，路径就平滑多了。

2.3 在ROS2中发布全局路径

ROS2里，全局路径通常以nav_msgs/msg/Path消息类型发布。我个人习惯把A*规划出来的网格路径，先做一次「路径平滑」，再发布出去。为什么？因为网格路径全是锯齿状的折线，直接给控制模块，车子会像喝醉了酒一样左右摇摆。

下面是一个简单的发布节点示例：

import rclpy
from rclpy.node import Node
from nav_msgs.msg import Path
from geometry_msgs.msg import PoseStamped

class GlobalPlanner(Node):
    def __init__(self):
        super().__init__('global_planner')
        self.publisher_ = self.create_publisher(Path, 'global_path', 10)
        
    def publish_path(self, waypoints):
        # waypoints是A*规划出的路径点列表
        path_msg = Path()
        path_msg.header.frame_id = 'map'
        path_msg.header.stamp = self.get_clock().now().to_msg()
        
        for wp in waypoints:
            pose = PoseStamped()
            pose.header = path_msg.header
            pose.pose.position.x = wp.x
            pose.pose.position.y = wp.y
            pose.pose.orientation.w = 1.0  # 默认朝向
            path_msg.poses.append(pose)
        
        self.publisher_.publish(path_msg)
        self.get_logger().info('全局路径已发布')

这里有个细节：frame_id一定要设置正确。我见过有人把路径发布在base_link坐标系下，结果控制模块收到后直接「起飞」。全局路径通常用map坐标系。

2.4 知识体系总览

为了让你更直观地理解这一章的内容，我画了一张流程图。它展示了从环境建模到路径发布的完整链路：

2.5 实战中的几个坑

最后分享几个我在实际调试中踩过的坑：

网格地图的更新频率：如果地图更新太慢，A*规划出的路径可能已经撞上动态障碍物。我一般把地图更新频率设在10Hz以上。
路径的连续性：A*输出的路径点之间距离可能不均匀。我习惯用三次样条插值做后处理，保证路径的一阶导数连续。
多目标点场景：如果终点在障碍物内部，A*会直接返回None。我建议在规划前先做一次「可达性检查」。

警告：千万不要在路径发布后就不管了。全局路径只是「大方向」，实际行驶中还需要局部规划模块做实时避障。我曾经见过有人把全局路径当轨迹跟踪，结果车子直接撞上路边的锥桶。

好了，这一章的内容就到这里。A*算法虽然基础，但它是整个规划控制模块的「地基」。地基打不牢，后面盖再高的楼也是白搭。希望你能动手把代码跑一遍，感受一下从网格到路径的完整流程。

可可精品资料库

📚 课程目录

2. 全局路径规划基础：A*算法原理与实现、基于网格的路径搜索、在ROS2中发布全局路径

2.1 A*算法的核心思想

2.2 基于网格的路径搜索实现

2.3 在ROS2中发布全局路径

2.4 知识体系总览

2.5 实战中的几个坑