基于搜索的路径生成

• BFS (广度优先搜索)
  • 1. 核心思想：逐层扩散
  • 2. 核心保证：最短路径
  • 3. BFS 的三大法宝（核心数据结构）
  • 4. 算法执行伪代码
  • 5. 算法特性总结
  • 6. 算法缺陷
• Dijkstra 算法 学习笔记
  • 1. 核心思想：成本优先
  • 2. 核心保证：成本最优
  • 3. Dijkstra 的“升级工具” (Upgraded Tools)
  • 4. 算法执行伪代码 (专业版)
  • 5. 算法特性总结
  • 6. 算法缺陷
    • 为什么负权重边会““破坏” (Break) Dijkstra？
      • 举例说明：
• A* (A-Star)
  • 1. 核心思想：Dijkstra + “方向感”
  • 2. A* 的核心公式
  • 3. A* 的“法宝” (数据结构)
  • 4. 算法执行伪代码 (专业版)
  • 5. 算法特性总结
  • 6. A* 的缺陷与局限
• 搜索算法对比：BFS vs Dijkstra vs A*
  • 1. 算法的““升级””与““降级””关系
    • 1. BFS -> Dijkstra 的“升级”
    • 2. Dijkstra -> A* 的“升级”
  • 2. 三种算法优缺点对比
• 搜索代码分析
  • 1.安全走廊构建
    • 概述
    • 步骤一：构建“龙骨” - 平滑的中心参考线
    • 步骤二：构建“墙壁” - 筛选可见的路沿
  • 连接成廊 - 形成最终边界
    • 总结：输出
  • 搜索任务准备与并行化
    • 1. 概述
    • 2. 步骤一：定义搜索的起点和终点
    • 3. 步骤二：加载障碍物与边界
    • 4. 步骤三：创建并行搜索任务 (多车道搜索)
    • 5. Part 2 总结：输出
  • 混合A*搜索算法
    • 1. 概述
    • 2. 状态、栅格化 (Gridding) 与搜索列表
    • 3. A* 搜索循环
    • 4. 步骤一：节点扩展 (Node Expansion) - “Hybrid”的核心
    • 5. 步骤二：碰撞检测 (Collision Checking)
    • 6. 步骤三：代价计算 ($g$ 和 $h$)
      • $g(n)$ - 真实路径代价
      • $h(n)$ - 启发式函数: HoloObstacleHeuristic
    • 输出
  • 轨迹平滑与优化
    • 1. 概述
    • 2. 步骤一：路径加密 (Densification)
    • 3. 步骤二：填充优化约束 (Constraint Filling)
    • 4. 步骤三：B样条二次规划 (B-Spline QP) 平滑
    • 5. 步骤四：轨迹选择与最终输出
    • 6. Part 4 总结：最终轨迹
• 附录：关键车辆模型详解
  • 1. 阿克曼转向 (Ackermann Steering) - “为什么需要模型”
    • 核心思想
  • 2. 自行车模型 (Bicycle Model) - “如何模拟转向”
    • 核心思想
    • 如何在代码中使用 (Next_node_generator)
  • 3. 纵向动力学 (Longitudinal Dynamics) - “如何规划速度”
    • 纵向动力学模型如何工作
  • 总结：三者关系

本文先从 BFS 算法开始，介绍基于搜索的路径生成。然后再引导至当前实际工程中的搜索方法。

BFS (广度优先搜索)

1. 核心思想：逐层扩散

BFS 的核心思想可以比喻为“水波纹”：

• 它从一个 start (起点) 开始。
• 像波纹一样，一层一层地、均匀地向外探索。
• 它会先访问所有距离起点为1的节点（第1层），然后才访问所有距离为2的节点（第2层），以此类推。

2. 核心保证：最短路径

• 最优性：在无权重图（或所有边权重都为1）中，BFS 保证能找到步数最短的路径。
• 原因：由于它是逐层探索的，当它第一次到达 goal (终点) 时，所经过的“层数”必然是最少的。

3. BFS 的三大法宝（核心数据结构）

BFS 的实现依赖三个关键工具：

4. 算法执行伪代码

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33

function BFS(start, goal):
  # 1. 初始化三大法宝
  queue = new Queue()     // 放入起点
  queue.add(start)
  
  visited = new Set()     // 标记起点
  visited.add(start)
  
  parent = new Map()      // 记录起点
  parent.set(start, null) 

  # 2. 循环，直到队列为空
  while queue.is_not_empty():
    
    current_node = queue.pop_front() # 从队列头部取出

    # 3. 检查是否到达终点
    if current_node == goal:
      return reconstruct_path(parent, start, goal) # 成功！

    # 4. 遍历邻居
    for neighbor in get_neighbors(current_node):
      
      # 5. 关键检查：是否是“新”节点
      if neighbor not in visited:
        
        # 6. 标记、入队、记录
        visited.add(neighbor)         # 标记为已访问
        parent.set(neighbor, current_node)  # 记录父节点
        queue.add_back(neighbor)      # 放入队尾，等待探索
        
  # 7. 队列为空，仍未找到
  return "No Path Found"

5. 算法特性总结

完备性 (Completeness): 是。只要路径存在，BFS 一定能找到。
最优性 (Optimality): 是 (仅限无权重图)。
时间复杂度 (Time): $O(V + E)$$V$ = 节点 (Vertex) 数量。$E$ = 边 (Edge) 数量。(每个节点和每条边最多被访问一次)。
空间复杂度 (Space): $O(V)$(在最坏情况下，Queue 和 visited 集合可能需要存储所有 $V$ 个节点)。

6. 算法缺陷

不懂成本： 找不到“最快”或“最短”的路，只能找到“转弯最少”的路。

搜索盲目： 像水波纹一样向所有方向搜索，在城市地图上会“撑爆”内存和算力。

反应僵硬： 无法应对实时路况（如行人、堵车），必须从头重算，太慢。

Dijkstra 算法 学习笔记

1. 核心思想：成本优先

Dijkstra (迪杰斯特拉) 算法是 BFS 的“升级版”。

• BFS 的目标：找到步数最少的路径。
• Dijkstra 的目标：在带权重的图中，找到累计成本最低的路径。

它的核心规则是：永远优先探索从起点出发，累计成本最低的那个““前沿” (frontier) 节点。

2. 核心保证：成本最优

• 最优性：是。只要图中没有负权重的边，Dijkstra 保证能找到从起点到所有其他节点的最低成本路径。
• 原因：由于它总是优先弹出累计成本最低的节点，所以当它第一次弹出 goal (终点) 时，它所记录的成本必然是全局最低的。（任何其他可能的路径，其成本在队列中时就已经高于这个值了）。

3. Dijkstra 的“升级工具” (Upgraded Tools)

Dijkstra 升级了 BFS 的数据结构，使其能够处理“成本” (cost)。

4. 算法执行伪代码 (专业版)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37

function Dijkstra(start, goal):
  # 1. 初始化
  pq = new PriorityQueue()
  heapq.heappush(pq, (0, start)) # (cost, node)
  
  cost_so_far = {start: 0}
  parent = {start: None}

  # 2. 循环，直到队列为空
  while pq.is_not_empty():
    
    current_cost, current_node = heapq.heappop(pq) # 弹出 *成本最低* 的

    # 3. (关键优化) 处理“过时”节点
    if current_cost > cost_so_far[current_node]:
      continue # 这是一条旧的、更贵的路径，跳过

    # 4. 检查终点
    if current_node == goal:
      return reconstruct_path(parent, start, goal) # 成功！

    # 5. 遍历邻居
    for neighbor in get_neighbors(current_node):
      
      # 6. 计算新路径的 *累计成本*
      new_cost = current_cost + get_cost(neighbor)
      
      # 7. (Dijkstra 的灵魂) 检查是否是条 *更好的* 路径
      if neighbor not in cost_so_far or new_cost < cost_so_far[neighbor]:
        
        # 8. 是！更新所有记录
        cost_so_far[neighbor] = new_cost
        parent[neighbor] = current_node
        heapq.heappush(pq, (new_cost, neighbor))
        
  # 9. 队列为空，仍未找到
  return "No Path Found"

5. 算法特性总结

完备性 (Completeness): 是。只要路径存在，一定能找到。
最优性 (Optimality): 是 (前提：图中没有负权重的边)。
时间复杂度 (Time): $O((E+V) \log V)$ 或 $O(E \log V)$$V$ = 节点数, $E$ = 边数。复杂度主要取决于优先队列 (heapq) 的性能。
空间复杂度 (Space): $O(V)$cost_so_far 和 parent 最多存储 $V$ 个节点。pq 在最坏情况下也可能存储 $O(V)$ 个节点。

6. 算法缺陷

Dijkstra 最大的缺陷是效率，而不是正确性（在无负权边的情况下）。

它是一个“盲目搜索” (Blind Search) 算法。

• 缺陷描述：Dijkstra 算法虽然很“聪明”，会优先探索累计成本低的路径，但它没有**“方向感” (direction)**。
• 具体表现：如果您的终点 G 在地图的东边，Dijkstra 算法在向东探索的同时，也会花费同样多的精力去探索西边、南边和北边的所有路径（只要它们的成本也很低）。
• 后果：它会探索大量“不相关” (irrelevant) 的区域，导致在大地图上效率低下。

一句话总结缺陷： Dijkstra 找到了最优的路径，但代价是盲目地探索了太多的区域，效率不高。

(这正是我们下一个要学的 A* 算法 所要解决的问题，A* 通过引入“启发函数” (heuristic) 解决了“盲目” (blindness) 问题。)

为什么负权重边会““破坏” (Break) Dijkstra？

因为负权重边打破了 Dijkstra 算法的根本前提。

• Dijkstra 的根本前提（假设）：
  “一条路径的成本只会随着它的变长而增加（或保持不变）。它永远不会减少。
  一旦我找到了一个到 A 节点成本为 10 的路径，我就最终确定了 cost_so_far[A] = 10。我永远不需要再回头看 A，因为任何未来能到达 A 的新路径，都必然是绕了更远的路，成本必定 > 10。”

• 负权重边如何打破这个前提：
  负权重边意味着“捷径” (shortcuts) 或“回扣” (rebates)，它允许你“时间倒流”，在未来找到一条更短的路径。

举例说明：

想象一个简单的地图，有三个节点：S (起点), A, B。

路径 1： S -> A

• 成本 = 2

路径 2： S -> B

• 成本 = 5

路径 3： B -> A

• 成本 = -4 (一个强大的““漩涡”)

Dijkstra 的执行过程：

1. 初始化：

   • pq = [ (0, S) ]
   • cost_so_far = { S: 0 }

2. Step 1：

   • 弹出 (0, S)。
   • 探索 S 的邻居 A 和 B。
   • 找到 A：新成本是 0 + 2 = 2。
   • 找到 B：新成本是 0 + 5 = 5。
   • 更新法宝：
     • pq = [ (2, A), (5, B) ] (A 在前，成本更低)
     • cost_so_far = { S: 0, A: 2, B: 5 }

3. Step 2：

   • 弹出成本最低的 (2, A)。
   • Dijkstra 在此刻““敲定” (finalizes)：“A 的最低成本绝对是 2。”
   • (假设 A 没有邻居，探索结束)

4. Step 3：

   • 弹出 (5, B)。
   • 探索 B 的邻居 A。
   • 找到 A：新成本是 current_cost (5) + cost(B->A) (-4) = 1。
   • 发现矛盾！ 算法发现了一条到 A 成本为 1 的新路径。

问题所在：
Dijkstra 算法在 Step 2 时，已经错误地““承诺” (finalized) A 的成本是 2。它（在简单实现中）不会再回头去更新 A 和 A 的所有邻居。它已经基于一个错误的前提（A=2）继续工作了。

一句话总结： 负权重边允许““未来” (later) 路径推翻““早期” (earlier) 的最优解，这违背了 Dijkstra 算法的贪心（Greedy）假设。

(注：专门用于处理负权重边的算法叫 Bellman-Ford，但它比 Dijkstra 慢得多。)

如果有负边会在那里来回走

A* (A-Star)

1. 核心思想：Dijkstra + “方向感”

A* 算法是 Dijkstra 算法的“智能”升级版。

• Dijkstra 的问题：它很“聪明”，总能找到成本最低的路径，但它很“盲目”。它会向所有方向探索“廉价”的路径，即使那个方向离终点十万八千里。
• A* 的解决方案：A* 在 Dijkstra 的“成本意识” (Cost) 基础上，增加了一个“指南针” (Compass)，这个指南针永远指向终点。

这个“指南针”就是 启发函数 (Heuristic)。

2. A* 的核心公式

A* 通过一个新公式 $f(n)$ 来决定优先队列的顺序：

• $g(n)$ (G-Cost)：从起点 S 走到节点 n 的**“实际累计成本”**。

  • 作用：与 Dijkstra 一样，这是我们永久记录在 cost_so_far 字典中的“真实花费”。
  • 地位：算法的**“事实依据”**。

• $h(n)$ (H-Cost)：从节点 n 走到终点 G 的**“估计成本”** (Heuristic)。

  • 作用：这就是“指南针”。它为算法提供了“方向感”。
  • 最常用的：曼哈顿距离 ( abs(n.x - G.x) + abs(n.y - G.y) )。
  • 地位：算法的**“最佳猜测”**。

• $f(n)$ (F-Cost)：从起点经过 n 到达终点的**“估计总成本”**。

  • 作用：它的唯一作用，就是作为 heapq 优先队列的**“排序标签”**。
  • 地位：算法的**“决策依据”**。

3. A* 的“法宝” (数据结构)

A* 的法宝与 Dijkstra 几乎一样，但用途被严格分开了：

A* 最关键的原则：G-Cost ($g$) 用于永久记录，F-Cost ($f$) 用于队列排序。

4. 算法执行伪代码 (专业版)

1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49

function A_Star(start, goal):
  # 1. 初始化
  # g_cost_so_far 只记录 G-Cost
  g_cost_so_far = {start: 0}
  parent = {start: None}
  
  # pq (优先队列) 只记录 F-Cost
  pq = new PriorityQueue()
  h_start = heuristic(start, goal)
  f_start = g_cost_so_far[start] + h_start
  heapq.heappush(pq, (f_start, start)) # (F-Cost, node)

  # 2. 循环
  while pq.is_not_empty():
    
    current_f_cost, current_node = heapq.heappop(pq)
    
    # 3. 检查终点
    if current_node == goal:
      return reconstruct_path(parent, start, goal) # 成功！
      
    # (专业优化：可以在这里检查弹出的节点是否“过时”)
    # if current_f_cost > g_cost_so_far[current_node] + heuristic(current_node, goal):
    #   continue
      
    # 4. 遍历邻居
    for neighbor in get_neighbors(current_node):
      
      # 5. 计算 *新* 的 G-Cost
      # g(n) = g(current) + step_cost
      step_cost = get_cost(neighbor) # (+ wall_penalty, etc.)
      new_g_cost = g_cost_so_far[current_node] + step_cost
      
      # 6. (A* 的灵魂) 检查 G-Cost 记录
      if neighbor not in g_cost_so_far or new_g_cost < g_cost_so_far[neighbor]:
        
        # 7. 更新 G-Cost 记录 (永久记录)
        g_cost_so_far[neighbor] = new_g_cost
        
        # 8. 计算 F-Cost (用于排序)
        h_cost = heuristic(neighbor, goal)
        f_cost = new_g_cost + h_cost
        
        # 9. 推入队列
        heapq.heappush(pq, (f_cost, neighbor))
        parent[neighbor] = current_node
        
  # 10. 队列为空
  return "No Path Found"

5. 算法特性总结

• 完备性 (Completeness): 是。只要路径存在，一定能找到。
• 最优性 (Optimality): 是，当且仅当启发函数 $h(n)$ 是“可接受的” (Admissible)，即它永远不会高估到终点的实际成本。
• 时间复杂度 (Time): $O((E+V) \log V)$。在实践中，由于 $h(n)$ 的引导，它远远快于 Dijkstra。
• 空间复杂度 (Space): $O(V)$。

6. A* 的缺陷与局限

A* 已经非常接近“完美”的网格搜索算法，但它仍有局限：

1. 静态算法：和 BFS/Dijkstra 一样，它假设地图是固定不变的。如果路径中途突然出现障碍物（如行人），它必须从头重新规划。 (注：D* Lite 等算法专门解决这个问题)。
2. 路径“质量”问题：
   • A* 找到的路径是“成本最低”的，但不一定是“最平滑”或“最安全”的。
   • 如您所见，它会紧贴墙壁拐弯。我们必须手动修改成本函数（如增加 WALL_PENALTY）来“诱导”它走得更平滑，这不是算法自带的功能。
3. 网格的诅咒：
   • 它找到的路径是由网格点组成的，充满了 90 度的“锯齿”转弯。
   • 在自动驾驶中，还需要一个“后处理”步骤（如路径平滑）才能将这些点变成车辆可以实际执行的平滑曲线。
4. 启发函数的依赖：
   • 如果 $h(n) = 0$，A* 退化为 Dijkstra（聪明但盲目）。
   • 如果 $h(n)$ 高估了成本（“不可接受”），A* 会变得很快，但不再保证找到最优路径。

搜索算法对比：BFS vs Dijkstra vs A*

这三种算法是“基于搜索”的路径规划的基石，它们之间有着清晰的““升级””关系。

1. 算法的““升级””与““降级””关系

您可以将这三者视为一个不断进化的工具：

BFS (广度优先搜索) -> (升级) -> Dijkstra -> (升级) -> A*

1. BFS -> Dijkstra 的“升级”

• BFS 的缺陷：
  它只懂““步数””，不懂““成本””。在它看来，走 1 公里的高速公路（1步）和走 1 公里的沼泽（1步）代价相同。
• Dijkstra 如何“升级”：
  1. 引入 $g(n)$ (G-Cost)：Dijkstra 引入了**““累计实际成本””的概念。它不再关心走了几步，只关心““到这里一共花了多少钱””**。
  2. 升级数据结构：它将 BFS 的 Queue (先进先出队列) 升级为 PriorityQueue (最小堆)，使其总是优先探索当前 g(n) 最低的节点。

结论：Dijkstra 就是““一个能处理带权重图（成本）的 BFS””。

““降级””：如果在一个 Dijkstra 算法中，你把所有的““成本”” (cost) 都设为 1，那么 Dijkstra 的行为将和 BFS 完全一致。

2. Dijkstra -> A* 的“升级”

• Dijkstra 的缺陷：
  它虽然““聪明””（懂成本），但依旧““盲目””。它没有方向感。如果终点在东边，它会同时向西边探索，只要西边的路也足够便宜。
• A* 如何“升级”：
  1. 引入 $h(n)$ (H-Cost)：A* 引入了**““启发函数””（Heuristic），即一个指向终点的““指南针””。这是一个对““未来””成本的““最佳猜测””**。
  2. 升级决策公式：它不再像 Dijkstra 那样只根据 $g(n)$ 来排优先级，而是根据一个新的公式 $f(n) = g(n) + h(n)$ 来决定优先级。这使得它会有方向地、优先探索那些““看起来离终点更近””的节点。

结论：A* 就是““一个带有‘方向感’（启发函数）的 Dijkstra 算法””。

““降级””：如果在一个 A* 算法中，你把““启发函数”” $h(n)$ 恒定设为 0，那么 $f(n) = g(n) + 0$，A* 算法就完全退化成了 Dijkstra 算法。

2. 三种算法优缺点对比

搜索代码分析

1.安全走廊构建

概述

在混合A* (Hybrid A*) 算法开始搜索之前，系统必须首先理解其运行的“世界模型”。这个世界模型就是“安全走廊”。

安全走廊的构建是 SafetyCorridorGenerator 类的核心职责。它的目标是利用来自地图和传感器的原始数据（如路沿、车道线），处理成一个清晰的、可供A*算法使用的“硬边界”（路沿）和“软边界”（车道线）。

此过程在 SafetyCorridorGenerator::process 函数中被调用。

步骤一：构建“龙骨” - 平滑的中心参考线

算法的第一步不是找边界，而是先确定一条平滑的中心“龙骨”，后续所有的边界都将相对这条“龙骨”来定义。

此步骤在 pre_process 函数中通过调用 setDiscreteReferencePoints 完成。

1. 输入：函数接收一个包含多条原始参考折线的数据 ref_polylinesylines。
2. 定位车辆：计算自车 (ego_state_) 在这些原始折线上的投影位置 (ego_proj_len)，以确定车辆的当前进度。
3. 截取与采样：根据车辆的投影位置，在车辆前后截取一定范围（例如 corridor_re_Min_dist_ 到 corridor_re_Max_dist_）的参考线段，并以5米的间隔进行粗略采样 (samplePtsBetween)。
4. B样条平滑：
   • A*搜索不能使用粗糙的折线，因此系统必须对其进行平滑。
   • 它调用 shape_point_smoother_new_->Solve。
   • 这个 ShapePointSmoother (来自 shape_point_smoother.cpp) 是一个基于 B样条的OSQP（二次规划）优化器。
   • 优化器的目标是找到一条B样条曲线，该曲线在尽可能贴近粗采样点的同时，保持自身的高度平滑（最小化曲率、加加速度等）。
5. 输出 (龙骨)：平滑后的结果被存储在 disref_polyline_ 和 disref_polyline_pwg_ (PiecewiseGeo 格式) 中。这条线就是后续所有计算的基准。

步骤二：构建“墙壁” - 筛选可见的路沿

有了中心“龙骨”后，系统开始构建走廊的“硬墙壁”，即路沿 (Road Edges)。系统不会使用所有路沿，而是通过一个精妙的“可见性”算法来筛选出最内侧的边界。

此步骤在 computeVisiblePoints 函数中执行。

1. 遍历所有路沿：函数遍历从上游获取的 roadedge_table_ 中的每一条路沿线。
2. 判断左/右：
   • 对于每条路沿线，LineDirectionjudge 函数被调用。
   • 此函数判断这条路沿线整体位于“龙骨” (disref_polyline_pwg_) 的左侧还是右侧。
3. “视线”可见性检查：
   • 这是最核心的逻辑。系统遍历路沿上的每一个点。
   • 调用 checkpointIsVisible 函数。
   • 此函数会从当前路沿点 point 向它在“龙骨”上的投影点 disref_polyline_pwg_.project(point).proj 画一条虚拟“视线”。
   • 它会检查这条“视线”是否与 polylines_table 中的任何其他路沿线段相交。
4. 筛选：
   • 如果“视线”被遮挡（即相交），意味着这个点point不是最内侧的边界（例如，它是外侧车道或匝道口的边界），该点被丢弃。
   • 只有“可见”的点（即未被遮挡的点）才被认为是构成安全走廊的有效点。
5. 存储：所有可见的左侧点被添加到 ref_to_vis_points_table_Left_，右侧点添加到 ref_to_vis_points_table_Right_。

连接成廊 - 形成最终边界

最后一步是将筛选出的离散的“可见点”连接成连续的走廊边界。

此步骤在 connectPointsToPolygon 函数中执行。

1. 排序：
   • 系统获取所有可见的左侧点 (sortedVec_left) 和右侧点 (sortedVec_right)。
   • 它根据点在“龙骨”上的投影距离 (dis_s) 对这些点进行排序。
2. 连接：
   • 系统按排序顺序，将所有左侧可见点连接起来，形成连续的折线 corridor_polyline_left_。
   • 系统按排序顺序（通常是逆序），将所有右侧可见点连接起来，形成 corridor_polyline_right_。
   • 为了防止点过于稀疏，如果相邻点之间距离大于1.5米，还会进行插值 (samplePtsBetween)。

总结：输出

经过以上步骤，安全走廊的“硬边界”构建完成。系统生成了两个关键的成员变量：

• corridor_polyline_left_：安全走廊的左侧“墙壁”（硬边界）。
• corridor_polyline_right_：安全走廊的右侧“墙壁”（硬边界）。

这两个变量将作为不可穿越的障碍物，在下一阶段被送入混合A*搜索器中。

搜索任务准备与并行化

1. 概述

在安全走廊（硬边界）构建完毕后，系统并不会立即开始搜索。它首先需要定义A*搜索的具体任务，包括：起点、终点、障碍物，以及最重要的——并行搜索的目标。

此过程主要在 SafetyCorridorGenerator::setSearchInputdata 函数中完成。

2. 步骤一：定义搜索的起点和终点

A*搜索需要一个明确的起点 (start_point) 和终点 (end_point)。

1. 起点 (Start Point)：

   • 冷启动：如果这是第一次运行，或者上一帧的轨迹无效 (stitch_info_.last_refline_valid == false)，起点被设置为车辆的当前局部坐标，即 (0, 0, 0)。
   • 热启动 (轨迹缝合)：在连续运行时，为了保证轨迹的平滑过渡，起点被设置为上一帧计算结果的最后一个点。这个点从 stitch_info_.points.back() 中获取。

2. 终点 (End Point)：

   • 终点是动态计算的，而不是一个固定目标。
   • 系统使用在 Part 1 中生成的平滑中心参考线 (disref_polyline_pwg_)。
   • 它沿着这条参考线，从自车当前投影点 (ego_rel_s_) 向前推进一个固定的目标距离 (search_point_end_dist_)。
   • 终点 ep 就是这个推进距离在参考线上的对应点，其朝向也由参考线的切线方向决定。

3. 步骤二：加载障碍物与边界

系统需要将 Part 1 构建的安全走廊和所有其他感知信息转换为A*算法可以理解的格式。

1. 加载硬边界 (路沿)：

   • Part 1 中生成的 corridor_polyline_left_ 和 corridor_polyline_right_ 被加载。
   • 它们被转换并存储在 search_data_current.obstacles 中。在A*搜索期间，这些是不可穿越的（在 ValidityCheck 中检查）。

2. 加载软边界 (车道线)：

   • 系统遍历所有的车道线 (laneline_table_)。
   • 车道线被加载到 search_data_current.soft_boundary 中。
   • 这些边界在A搜索中被视为“软”障碍物。A算法可以穿越它们，但在穿越时会受到巨大的代价惩罚 (soft_boundary_penalty_)，使其倾向于不压线行驶。

3. 加载粗参考线：

   • Part 1 中生成的 disref_polyline_ 被加载到 search_data_current.coarse_refline。
   • 这条线将用于在A搜索期间计算启发式函数 (Heuristic)，引导A算法沿着“正确的”方向前进。

4. 步骤三：创建并行搜索任务 (多车道搜索)

这是本系统的一个高级特性。为了应对变道或大型障碍物，系统不会只在当前车道搜索一条路径，而是会同时创建并评估三条可能的路径。

1. 创建三个数据副本：

   • search_data_current：用于搜索当前车道的轨迹。
   • search_data_left：用于搜索向左变道后的轨迹。
   • search_data_right：用于搜索向右变道后的轨迹。
     它们都继承了相同的终点和障碍物信息。

2. 检查变道可行性：

   • 对于左/右变道任务，系统需要检查“立即变道”是否可行（例如，是否会立即撞上隔离带或另一条车道线）。
   • startpointcheck_inglobal 函数被调用。
   • 此函数会从当前起点向左/右侧（偏移一个车道宽度 shift_lane_width）画一条虚拟的线，检查这条线是否会立即与路沿 (corridor_polyline_left_) 或车道线 (laneline_table_) 相交。

3. 设置变道起点：

   • 如果可行 (startpointcheck_inglobal 返回 true)：左/右搜索任务的起点会被立即设置为偏移一个车道宽度的位置 (search_data_left.start_point.y() = -shift_offset_left)。
   • 如果不可行：is_valid 标志被设为 false，该搜索任务后续将被跳过。

5. Part 2 总结：输出

此过程完成后，系统生成了 search_data_vec_，这是一个包含三个搜索任务（Astar_Search_data）的向量。

• 任务1 (左)：is_valid = true/false，起点可能已偏移。
• 任务2 (中)：is_valid = true，起点为当前位置。
• 任务3 (右)：is_valid = true/false，起点可能已偏移。

这三个任务将被提交到线程池 (search_ThreadPool)，在下一阶段被并行执行。

混合A*搜索算法

1. 概述

HybridAStar::performAstarSearch 函数 是混合A*算法的具体实现。它在 Part 1 构建的安全走廊内，根据 Part 2 定义的起止点，搜索一条运动学可行（Kinematically Feasible）的粗糙路径。

它严格遵循A*算法 $f = g + h$ 的框架，但对其所有关键组件进行了“混合”定制。

2. 状态、栅格化 (Gridding) 与搜索列表

混合A*的“混合”特性体现在它如何在连续空间中搜索，同时又用离散栅格来管理搜索进度。

1. 连续状态：

   • 搜索的真实状态是连续的 $(x, y, \theta)$ 位姿。
   • 节点 (Node3d) 存储的是高精度的 double 型 $x, y, \phi$ 值。

2. 离散栅格化 (Gridding)：

   • 为了管理“已访问”列表（open_set_ 和 close_set_），A*算法必须知道两个节点是否“几乎在同一位置”。
   • 系统使用 xy_grid_resolution_（例如0.5米）和 phi_grid_resolution_（例如0.05弧度）作为分辨率。
   • 当一个 Node3d 被创建时，它的连续 $(x, y, \theta)$ 状态会被栅格化（类似于“四舍五入”），以生成一个唯一的离散索引ID（Node3d::GetIndex()）。
   • 例如：$(1.2, 0.6, 0.1)$ 和 $(1.3, 0.7, 0.12)$ 这两个连续状态，在栅格化后可能共享同一个索引ID：“X_1_Y_0_Phi_2”。

3. 搜索列表 (Open/Close Sets)：

   • open_pq_：一个优先队列，存储连续的 Node3d 对象。它根据 $f$ 代价（$f=g+h$）自动排序，保证A*总是先探索 $f$ 代价最低的节点。
   • open_set_ 和 close_set_：两个哈希表 (unordered_map)，它们存储的是离散的索引ID。它们用于快速检查“这个栅格我们是否已经访问过？”。

3. A* 搜索循环

performAstarSearch 函数的主体是一个 while 循环。

1. 初始化：将 start_node_ 放入 open_pq_ 和 open_set_。
2. 循环条件：while (!open_pq_.empty())。
   • 循环会一直运行，直到找到路径，或者 open_pq_ 变空（搜索失败），或者达到了最大探索节点数 (max_explored_num)。
3. 弹出最优节点：
   • current_node = open_pq_.top().first：从优先队列中取出 $f$ 代价最低的节点。
   • open_pq_.pop()。
4. 加入关闭列表：将 current_node 的索引ID加入 close_set_，表示“这个栅格我们已经处理过了”。
5. 检查终点：调用 ChecknodeIsFinal。在您的代码中，这不仅是检查是否到达 $(x,y)$，而是检查车辆的纵向投影 (GetProjLength()) 是否已经超过了终点的投影 (endp_proj_len_) 且航向角接近。如果满足，则 final_node_ 被设置，搜索趋于结束。
6. 扩展节点：对 current_node 进行节点扩展（见下文）。

4. 步骤一：节点扩展 (Node Expansion) - “Hybrid”的核心

这是混合A与传统A的根本区别。它不探索 $(x, y)$ 栅格的8个邻居，而是模拟车辆的驾驶动作。

此步骤在 Next_node_generator 函数中实现。

1. 生成离散动作：
   • 循环 next_node_num_ 次（例如9次）。
   • 生成一组离散的转向角，例如 steering = {-20°, -10°, 0°, 10°, 20°...}。
   • 同时生成前进（traveled_distance = step_size_）和后退（traveled_distance = -step_size_）两种动作。
2. 运动学模拟：
   • 对于每一个动作（如“前进，转向10度”），函数会根据自行车运动学模型计算出新的连续位姿 $(x', y', \theta')$。
   • next_phi = last_phi + traveled_distance / wheel_base * tan(steering)。
   • next_x = last_x + traveled_distance * cos(next_phi)。
   • next_y = last_y + traveled_distance * sin(next_phi)。
3. 创建新节点：用 $(x', y', \theta')$ 创建一个新的 Node3d 对象 (next_node)。

5. 步骤二：碰撞检测 (Collision Checking)

对于 Next_node_generator 生成的每一个 next_node，系统都会进行严格的碰撞检测。

此步骤在 ValidityCheck 函数中实现。

1. 获取包围盒 (Bounding Box)：Node3d::GetBoundingBox 函数根据车辆的 vehicle_param_（车长、车宽）和 next_node 的 $(x, y, \theta)$ 位姿，计算出车辆的2D矩形包围盒。
2. 检查重叠：
   • ValidityCheck 遍历 Part 1 中构建的所有硬边界线段 (obstacles_linesegments_vec_)，这包括了 corridor_polyline_left_ 和 corridor_polyline_right_。
   • 它检查车辆包围盒是否与任何一条硬边界线段发生重叠 (bounding_box.HasOverlap(linesegment))。
3. 丢弃节点：如果发生重叠（碰撞），ValidityCheck 返回 false，A*搜索循环会立即丢弃这个 next_node，不再对其进行代价计算。

6. 步骤三：代价计算 ($g$ 和 $h$)

如果 next_node 通过了碰撞检测，并且其栅格索引不在 close_set_ 中，系统就会调用 CalculateNodeCost 来计算其 $f$ 代价。

$g(n)$ - 真实路径代价

1. 运动代价 (TrajCost)：

   • 方向惩罚：后退的代价远高于前进（traj_back_penalty_ > traj_forward_penalty_）。
   • 换挡惩罚：如果动作从前进变为后退（或反之），施加 traj_gear_switch_penalty_。
   • 转向惩罚：惩罚大幅度转向（traj_steer_penalty_ * steer^2）。
   • 转向变化惩罚：惩罚方向盘的快速变化（traj_steer_change_penalty_ * (steer - last_steer)^2）。

2. 软边界代价 (LineCrossCost)：

   • 此函数检查从 current_node 到 next_node 的连线是否穿越了任何软边界（车道线 soft_laneline_linesegments_vec_）。
   • 如果穿越，施加一个巨大的代价 (soft_boundary_penalty_ * 100)。
   • 这使得A*算法会“极不情愿”地压线，但如果这是唯一的路径，它仍然会选择压线（与硬边界的“不可穿越”形成对比）。

$h(n)$ - 启发式函数: HoloObstacleHeuristic

您的代码中 包含了两种实现：

1. 实现方式一 (被注释掉): 2D栅格搜索 (真正的“全向-障碍物”启发)
这是最经典、最智能的启发函数，通过 grid_a_star_heuristic_generator_ 和 grid_search.cpp 实现。

• 预计算: 在混合A开始前，先运行一个2D栅格A (GenerateDpMap)。
• 反向搜索: 这个2D A*从终点开始，反向搜索并绕过所有硬障碍物（路沿）。
• 生成DP Map: 它生成一个代价地图 (dp_map_)，图中每个单元格的值是“从该点到终点的最短无障碍距离”。
• 查询: 混合A*在运行时，通过 CheckDpMap 查询 dp_map_ 即可得到 $h$ 值。
• 优点: 完美考虑了障碍物，引导性最强。

2. 实现方式二 (代码中实际使用): 基于粗轨迹 (“全向-无障碍物”启发)
您的代码实际运行的是一个更简单、针对车道环境高度优化的版本。它完全忽略障碍物，仅依赖 Part 1 中生成的平滑中心参考线 (rough_line_pwg_)。

1
2
3
4
5
6
7
8
9
10
11
12
13
14

double HybridAStar::HoloObstacleHeuristic(std::shared_ptr<Node3d> next_node) {
  // ... (2D Grid Search被注释掉了)

  // 1. 获取纵向距离
  double dist_long = endp_proj_len_ - next_node->GetProjLength();

  // 2. 获取横向距离
  double dist_lat = next_node->GetProjDis();
  
  // 3. 计算最终代价
  res = heuristic_lat_w1_ * (cost_from_lat) + heuristic_long_w2_ * dist_long;

  return res;
}

• 工作原理：它将“全向”代价分解为：
  1. 横向代价：车辆“横移”到参考线上的代价，基于 dist_lat。
  2. 纵向代价：车辆沿参考线行驶到终点的代价，即 dist_long。
• 最终 $h$ = $w_1 \times (\text{横向代价}) + w_2 \times (\text{纵向代价})$。
• 优点: 计算极快（一次投影）。在有清晰参考线的道路上，会强烈引导A*算法贴着参考线前进。
• 缺点: 完全无视障碍物，在复杂路口或障碍物遮挡参考线时，引导性会下降。

输出

当 while 循环结束（因为找到了 final_node_），GetNodeResult 函数会被调用。

1. 路径回溯：GetNodeResult 从 final_node_ 开始，通过 GetPreNode() 指针链条反向回溯到 start_node_。
2. 输出：它将回溯路径上的所有 $(x, y, \phi)$ 坐标点收集起来，存储在 result->x, result->y, result->phi 中。

此阶段的输出是一条粗糙的、由短线段和圆弧组成的路径。

• 优点：它100%保证了运动学可行性（车能开），并且100%避开了硬边界（路沿）。
• 缺点：它在转向连接处是不平滑的（曲率不连续），不能直接用于车辆控制。

这条粗糙路径将作为“强参考”，在 Part 4 中被送入B样条优化器进行最终的平滑处理。

轨迹平滑与优化

1. 概述

混合A*搜索（Part 3）的输出 result->x, result->y 是一条由圆弧和直线段组成的粗糙路径。它虽然避开了所有硬障碍，但驾驶体验会非常糟糕。

本阶段的目标是利用这条A*路径作为“强参考”，通过B样条二次规划 (QP) 优化，生成一条最终的、平滑的、可执行的轨迹。

此过程在 HybridAStar::performAstarSearch 函数的后半部分 和 SafetyCorridorGenerator::performSearch 的末尾执行。

2. 步骤一：路径加密 (Densification)

A*算法输出的节点是稀疏的（例如每1米一个点）。为了给优化器提供更丰富的参考信息，系统首先对其进行加密。

• 函数: HybridAStar::Densification_result。
• 操作: 它沿着A*的粗糙路径进行插值，生成一个点更密集的路径（例如，每0.5米一个点）。

3. 步骤二：填充优化约束 (Constraint Filling)

这是将“物理世界”转译为“数学问题”的关键一步。系统需要告诉优化器，这条轨迹必须在哪些边界内。

1. 函数: HybridAStar::FillSearchResultToOptimizer。
2. 约束池: 此函数使用 constraint_pool_ (来自 constraint_pool.cpp)。
3. 查询边界:
   • 它遍历加密后的A*路径上的每一个点。
   • 对于每个点，它调用 constraint_pool_.queryConstraineLineSeg。
   • constraint_pool_ 会在 Part 1 构建的安全走廊（corridor_polyline_left_ 和 corridor_polyline_right_）以及软边界（车道线）中，为这个点动态查询出离它最近的左、右边界线段。
4. 输出: 生成一个完整的约束集，其中A*路径上的每个点都“绑定”了它必须遵守的局部边界。

4. 步骤三：B样条二次规划 (B-Spline QP) 平滑

这是轨迹生成的最后一步，也是计算量最大的一步。

1. 调用求解器: 系统调用 search_result_smoother_->Solve()。
2. 求解器身份: search_result_smoother_ (来自 shape_point_smoother.cpp) 实际上是一个OSQP（二次规划）求解器的接口 (newOsqpInterface)。这与 Part 1 中用于平滑参考线的优化器是同一类。

OSQP求解器会解一个复杂的数学优化问题：

• 目标函数 (Cost Function) - CalculateKernel

  • “找到一条B样条曲线，使其总代价最小”。
  • 总代价 =
    • $w_1 \times$ (平滑后轨迹**偏离A*参考路径**的距离) (`weight_p_`)
    • • $w_2 \times$ (曲线的速度/一阶导数) (`weight_v_`)
    • • $w_3 \times$ (曲线的加速度/二阶导数) (`weight_a_`)
    • • $w_4 \times$ (曲线的加加速度Jerk/三阶导数) (`weight_j_`)
    • • $w_5 \times$ (曲线的加加加速度Snap/四阶导数) (`weight_s_`)
  • 核心思想: 权重 $w_1$ (位置权重 weight_p_) 保证了轨迹不会偏离A*找到的安全路径。其他权重 $w_2$ 到 $w_5$ 保证了轨迹本身是极其平滑、舒适、易于控制的。

• 约束条件 (Constraints) - CalculateAffineConstraint

  • “在满足以下硬约束的前提下，最小化上述总代价”：
  • 边界约束: B样条曲线上的所有点，必须位于 FillSearchResultToOptimizer (步骤二) 提供的左、右硬边界（路沿）和软边界（车道线）之间。

5. 步骤四：轨迹选择与最终输出

至此，Part 2 中提交的三个并行搜索任务（左、中、右）均已完成（或因无效而被跳过）。

此步骤在 SafetyCorridorGenerator::performSearch 的末尾执行。

1. 收集结果: 系统现在手握最多三个平滑且可行的轨迹方案（存储在 search_thread_map 中）。
2. 选择最优轨迹:
   • 系统需要从这三个方案中选出一个“最佳”方案。
   • 它使用一个简单的“最近”逻辑：遍历所有有效轨迹上的所有点，找出离车辆当前位置 (car_current_pos) 最近的那个点。
   • 包含这个“最近点”的整条轨迹，被选为最终轨迹 (nearst_road_id)。
3. 最终组装 (Output):
   • 系统将上一帧的缝合轨迹 (stitch_info_) 与新选出的这条最优轨迹 (result.optimized_x, result.optimized_y 等) 拼接在一起。
   • 最终结果被存储在 disref_positon_, disref_heading_, disref_curvature_ 中，作为最终的执行轨迹被发送给控制模块。

6. Part 4 总结：最终轨迹

整个流程的最终输出是一条：

• 连续且平滑 (B样条优化)
• 严格遵守硬边界 (A*碰撞检测 + QP约束)
• 运动学可行 (A*运动学扩展)
• 高可用 (并行多车道搜索)

的轨迹，完美地结合了混合A*的路径查找能力和二次规划的平滑优化能力。

附录：关键车辆模型详解

在整个轨迹生成流程中，车辆的物理特性通过几个关键模型被抽象和应用。理解这些模型是理解混合A*和后续优化的基础。

1. 阿克曼转向 (Ackermann Steering) - “为什么需要模型”

这个概念是现实世界中的物理原理。它描述了汽车在转弯时，为了防止轮胎侧滑（想象一下车轮在地上横着“搓”），四个轮子必须满足的几何条件。

核心思想

当汽车转弯时，所有四个车轮都必须围绕一个共同的圆心旋转。这个点我们称之为瞬时旋转中心 (Instantaneous Center of Rotation, ICR)。

• 看内侧和外侧：想象一下，在转弯时，内侧前轮走的圆圈半径 R_inner 总是小于外侧前轮走的圆圈半径 R_outer。
• 关键结论：为了让它们都指向同一个ICR，内侧前轮的转向角度（$\delta_{inner}$）必须大于外侧前轮的转向角度（$\delta_{outer}$）。
• 实现：在您的真实汽车上，这是通过一个精巧的机械结构（称为“转向梯形”）来实现的。

（这个我们玩乐高和besiege的时候都很熟悉了hhh）

阿克曼转向几何

2. 自行车模型 (Bicycle Model) - “如何模拟转向”

自行车模型是对阿克曼转向原理的数学简化。它是您代码中混合A*算法的核心运动学模型。

核心思想

我们不模拟四个轮子，太复杂了。我们把车简化成一辆自行车：

1. 把两个后轮合并成一个“虚拟后轮”。
2. 把两个前轮合并成一个“虚拟前轮”。

这辆“自行车”的状态（位姿）可以用三个变量来描述：

• $(x, y)$：虚拟后轮中心的位置。
• $\theta$ (theta)：自行车的朝向（航向角）。

如何在代码中使用 (Next_node_generator)

A*搜索不是基于时间 $t$ 和速度 $v$ 的，而是基于一个固定的步长 $s_{\text{step}}$（即 traveled_distance）。

代码将描述自行车运动的微分公式改写为“离散”的差分公式，用于计算A*的下一个节点：

1. 关键参数：

   • $L$：车辆轴距（前后轮之间的距离）。在您的代码中是 `vehicle_param_.wheel_base()`。
   • $\delta$ (delta)：虚拟前轮的转向角。在您的代码中是 `steering`。
   • $(x_k, y_k, \phi_k)$：当前节点（`current_node`）的位姿。

2. 计算新朝向 $\phi_{k+1}$：

   • 新朝向角的变化 $\Delta\phi \approx (s_{\text{step}} / L) \times \tan(\delta)$
   • 计算公式： $$\phi_{k+1} = \phi_k + \frac{s_{\text{step}}}{L} \tan(\delta)$$

3. 计算新位置 $(x_{k+1}, y_{k+1})$：

   • 新位置的变化 $\Delta x \approx s_{\text{step}} \times \cos(\phi)$
   • 新位置的变化 $\Delta y \approx s_{\text{step}} \times \sin(\phi)$
   • 计算公式（您的代码中使用 $\phi_{k+1}$ 进行积分，这是一种欧拉积分法）： $$x_{k+1} = x_k + s_{\text{step}} \cos(\phi_{k+1})$$ $$y_{k+1} = y_k + s_{\text{step}} \sin(\phi_{k+1})$$

总结：自行车模型是一个数学公式，它能精确地告诉A*算法：“如果我在 $(x_k, y_k, \phi_k)$，打 $\delta$ 这么多方向，前进 $s_{\text{step}}$ 这么远，我会到达哪个新位姿 $(x_{k+1}, y_{k+1}, \phi_{k+1})$”。这就是混合A*实现“运动学可行”的根本。

自行车运动学模型

3. 纵向动力学 (Longitudinal Dynamics) - “如何规划速度”

这个概念与“转向”（横向）无关，它只关心沿路径前进和后退（纵向）。

在自动驾驶规划中，一个非常聪明且常见的技巧是**“解耦”**（Decoupled）规划：

1. Step 1. 路径规划 (Path Planning)：

   • 任务：先不管速度，只用自行车模型（运动学）找到一条几何上可行的 $(x, y, \theta)$ 路径。
   • 执行者：HybridAStar::performAstarSearch。

2. Step 2. 速度规划 (Speed Profile)：

   • 任务：把Step 1找到的路径当成一条固定的“1D轨道”。现在，只在这条轨道上规划 $(v, a, j)$（速度, 加速度, 加加速度）。
   • 执行者：HybridAStar::GenerateSCurveSpeedAcceleration。

纵向动力学模型如何工作

您的代码没有使用复杂的发动机、刹车或轮胎力模型。它使用了一个更简单、更强大的基于约束的优化模型。

GenerateSCurveSpeedAcceleration 所做的事情是，调用 PiecewiseJerkSpeedProblem 求解器，并给它一组规则（动力学约束）：

• 规则1 (速度限制)：你的速度 $v$ 永远不能超过 max_forward_v_ 或 max_reverse_v_。
• 规则2 (加速度限制)：你的加速度 $a$ 永远不能超过 max_forward_acc_ 或 max_reverse_acc_。这代表了车辆“油门踩到底”或“刹车踩到底”的能力。
• 规则3 (Jerk限制)：你的加加速度 $j$ (Jerk) 永远不能超过 max_acc_jerk_。Jerk代表了你踩油门/刹车的“快慢”。限制Jerk是为了防止顿挫，保证乘客舒适性。

优化目标：求解器 PiecewiseJerkSpeedProblem 的目标是，在严格遵守上述所有动力学规则的前提下，找到一条从 $v=0$ 到 $v=0$ 的速度曲线，使其总Jerk最小（即最平滑、最舒适）。

总结：三者关系