第四章：全局路径规划：A* 与 Hybrid A*，Lattice Planner，Lanelet2 地图下的路线生成

全局路径规划，说白了就是给车画一条从起点到终点的「大路线」。你想想看，车要自己跑，总得知道往哪走吧？这一章我们聊的，就是怎么在 Lanelet2 这种高精地图上，用 A*、Hybrid A* 和 Lattice Planner 把路线算出来。

我个人习惯把全局规划分成两类：一类是「纯几何路径」，不考虑车能不能拐过去；另一类是「运动学可行路径」，得让车真的能开。嗯，这里要注意，很多初学者上来就调 A*，结果车到了路口发现转不过去——这就是没搞懂区别。

4.1 A* 算法：最基础的搜索

A* 大家应该都熟，就是启发式搜索。我当年在学校写作业时觉得这玩意儿太简单了，直到在项目中遇到 10 公里 x 10 公里的地图……嗯，网格一多，搜索效率直接崩了。

核心公式就一个：f(n) = g(n) + h(n)。g 是实际代价，h 是预估代价。我建议 h 用欧几里得距离，别用曼哈顿——因为车可以斜着走，曼哈顿会高估代价，导致搜索变慢。

避坑指南： 我曾经在项目中用曼哈顿距离做 h，结果搜索范围大了 30%。后来换成欧几里得，速度明显提升。记住：h 不能高估实际代价，否则 A* 退化成 Dijkstra。

代码实现其实不复杂，核心就这几步：

def a_star(start, goal, grid):
    open_set = PriorityQueue()
    open_set.put((0, start))
    came_from = {}
    g_score = {start: 0}
    f_score = {start: heuristic(start, goal)}
    
    while not open_set.empty():
        current = open_set.get()[1]
        if current == goal:
            return reconstruct_path(came_from, current)
        
        for neighbor in get_neighbors(current, grid):
            tentative_g = g_score[current] + distance(current, neighbor)
            if tentative_g < g_score.get(neighbor, float('inf')):
                came_from[neighbor] = current
                g_score[neighbor] = tentative_g
                f_score[neighbor] = tentative_g + heuristic(neighbor, goal)
                open_set.put((f_score[neighbor], neighbor))
    return None

但注意，A* 出来的路径是「网格路径」，不是「车能开的路径」。拐角处全是 90 度折线，你得后续做平滑。我一般用 B 样条或者 Dubins 曲线来后处理。

4.2 Hybrid A*：让路径真的能开

Hybrid A* 是 A* 的升级版。它不是在网格上搜，而是在连续空间里搜。说白了，它考虑了车的转弯半径、朝向角这些运动学约束。

我记得第一次看 Hybrid A* 论文时，觉得这东西太巧妙了——它把状态空间离散化，但路径是连续的。每个节点不光有 (x, y)，还有 heading 角 θ。搜索时用 Reeds-Shepp 曲线或者 Dubins 曲线来连接节点。

我的经验： Hybrid A* 的启发式函数很关键。我建议用两种启发式：一种是「不考虑障碍物的 Dubins 距离」，另一种是「考虑障碍物的 2D A* 距离」。取最大值，这样既保证最优性，又不会太保守。

代码上，Hybrid A* 比 A* 复杂不少。核心区别在于扩展节点时，不是上下左右走，而是用车辆运动模型生成一系列可能的轨迹：

def hybrid_a_star(start, goal, map):
    open_set = PriorityQueue()
    open_set.put((0, start))
    visited = set()
    
    while not open_set.empty():
        current = open_set.get()[1]
        if is_near_goal(current, goal):
            return smooth_path(current)
        
        # 用车辆模型生成下一步
        for steering in [-max_steer, 0, max_steer]:
            for speed in [forward_speed, reverse_speed]:
                next_state = simulate_motion(current, steering, speed)
                if is_collision_free(next_state, map):
                    cost = current.g + motion_cost(steering, speed)
                    if next_state not in visited or cost < visited[next_state]:
                        visited[next_state] = cost
                        open_set.put((cost + heuristic(next_state, goal), next_state))
    return None

这里有个坑：Hybrid A* 的搜索空间比 A* 大得多。我曾经在密集城区场景下，搜索时间飙到了 5 秒以上。后来加了「剪枝策略」——只保留每个网格单元中 cost 最小的那个状态，才把时间压到 0.5 秒以内。

4.3 Lattice Planner：结构化道路的利器

Lattice Planner 的思路跟 A* 完全不同。它不是在网格上搜，而是在「时空轨迹空间」里采样。说白了，就是生成一堆候选轨迹，然后挑一条最好的。

我一般在高速场景用 Lattice Planner。为什么？因为高速上车道线清晰，车辆行为相对规则。你想想看，在高速上变道，无非就是横向偏移 + 纵向速度调整，用 Lattice 采样特别自然。

Lattice Planner 的核心步骤：

  1. Frenet 坐标系转换：把 (x, y) 转成 (s, d)，s 是沿车道线的纵向距离，d 是横向偏移。
  2. 采样：在 s-t 空间和 d-t 空间分别采样，生成一堆轨迹候选。
  3. 代价评估：对每条轨迹算代价，包括舒适性、效率、安全性。
  4. 选择最优：挑代价最小的轨迹，同时要保证无碰撞。
关键点： Lattice Planner 的采样密度直接影响效果。我建议纵向采样 5-10 个点，横向采样 3-5 个点。采样太密计算量大，太稀又可能漏掉最优解。

代码示例（Frenet 转 Cartesian）：

def frenet_to_cartesian(s, d, reference_line):
    # 找到参考线上最近的 s 位置
    ref_point = reference_line.get_point_at_s(s)
    # 计算法向量
    tangent = ref_point.get_tangent()
    normal = np.array([-tangent[1], tangent[0]])
    # 转换
    x = ref_point.x + d * normal[0]
    y = ref_point.y + d * normal[1]
    return x, y

嗯，这里要注意：Lattice Planner 生成的轨迹是「时空轨迹」，也就是说它不光规划路径，还规划了速度。这在动态场景下特别有用——你可以提前减速让行，或者加速超车。

4.4 Lanelet2 地图下的路线生成

Lanelet2 是 Autoware 用的高精地图格式。它把道路分成一个个 Lanelet（车道段），每个 Lanelet 有左右边界、中心线、速度限制等信息。

在 Lanelet2 地图上做全局规划，说白了就是「图搜索」——Lanelet 是节点，Lanelet 之间的连接是边。我一般用 Dijkstra 或者 A* 在 Lanelet 图上搜，找到一条由 Lanelet 序列组成的路线。

我的做法： 先用 Lanelet2 的 routing graph 生成 Lanelet 级别的路径，然后再在每个 Lanelet 内部用 Hybrid A* 或者 Lattice Planner 生成具体的轨迹。这样既保证了全局最优性，又保证了局部可行性。

Lanelet2 地图的加载和查询：

import lanelet2

# 加载地图
projector = lanelet2.projection.UtmProjector(lanelet2.io.Origin(0, 0))
map = lanelet2.io.load("map.osm", projector)

# 获取 routing graph
traffic_rules = lanelet2.traffic_rules.create(lanelet2.traffic_rules.Locations.Germany, 
                                              lanelet2.traffic_rules.Participants.Vehicle)
routing_graph = lanelet2.routing.RoutingGraph(map, traffic_rules)

# 找路径
start_lanelet = find_nearest_lanelet(map, start_pose)
goal_lanelet = find_nearest_lanelet(map, goal_pose)
route = routing_graph.getRoute(start_lanelet, goal_lanelet)

这里有个常见的坑：Lanelet2 地图的拓扑关系可能不完整。我曾经遇到过一个场景，两个 Lanelet 明明物理上连着，但地图里没有定义连接关系。结果 routing graph 返回空路径。后来我加了一个「手动连接」的 fallback 逻辑，才解决了这个问题。

警告： Lanelet2 地图的质量直接影响规划效果。我建议在加载地图后，先做一次拓扑完整性检查。如果发现断头路或者孤立 Lanelet，要及时修复或者做 fallback 处理。

4.5 三种方法的对比与选择

说了这么多，到底该用哪个？我列个表，方便你对比：

方法 适用场景 优点 缺点
A* 低速、简单场景 实现简单、速度快 路径不可开、需要后处理
Hybrid A* 复杂场景、停车场 运动学可行、能倒车 搜索空间大、速度较慢
Lattice Planner 高速、结构化道路 时空联合规划、舒适性好 依赖参考线、采样密度难调

我个人习惯是：城区低速用 Hybrid A*，高速用 Lattice Planner，简单场景用 A* 加后处理。你想想看，每种方法都有自己的脾气，选对了事半功倍。

最后说一句：全局路径规划不是一锤子买卖。实际项目中，车跑着跑着可能遇到临时封路、动态障碍物，这时候需要重新规划。所以规划器的「重规划频率」和「计算耗时」也是关键指标。我一般把重规划频率设在 1-2 Hz，既能保证实时性，又不会太耗算力。

嗯，这一章就到这里。下一章我们聊局部路径规划，看看车怎么在动态环境中实时避障。

← 上一章 📖 返回目录 下一章 →