老周是这座城市跑单最快的外卖骑手。同样是送一单餐，他有三件法宝：一部装着离线地图的手机、一把探索陌生小区用的飞镖、一排在快速路上只属于他的“专用轨道”。去熟悉的商圈，他打开手机导航，对着已知地图系统性地挑最短路线——这是 A* 算法；闯进没画过图的还建小区，他随机扔飞镖、就近长出一根根探索树枝——这是 RRT 算法；上了城市快速路，他不再自由散漫地走，而是沿着一条条预设的车道轨迹规规矩矩地“打格子”前进——这是 Lattice 格栅规划。三件法宝背后其实是同一件事：如何在“能走”的空间里，找出一条“好走”的路。本文就借老周的一天，把路径规划的原理、选型与实战讲清楚。

岗位关联

一句话价值：路径规划是自动驾驶“大脑”从“决定做什么”到“决定怎么动”的最后一跳。掌握 A*/RRT/Lattice 的算法思想与选型逻辑，是进入预测与规划方向、走向自动驾驶核心岗位的必修课。

目录
  1. 可视化演示
  2. 路径规划概述
  3. 搜索空间建模
  4. A*算法
  5. RRT随机树
  6. Lattice格栅
  7. 算法对比选型
  8. 工程实践优化
  9. 自动驾驶应用
  10. AI 应用场景
  11. Python 演示
  12. 常见误区
  13. 面试题

可视化演示

动态展示路径规划问题定义与算法分类、A*启发式图搜索、RRT采样式规划与Lattice Frenet坐标系规划。

本节配有交互动效（进入视口后自动加载，约 3KB）。

路径规划概述

老周接到一单从写字楼到医院的外卖。出发前他不会直接冲下楼，而是先在门口盘算：从哪个门出、走哪条街、要不要绕开修路路段。这个“出发前先想清楚怎么走”的过程，就是路径规划。对自动驾驶来说，路径规划就是让车辆从起点到终点找一条路，而且这条路必须同时满足两个条件：一是“能走”，不碰撞障碍物、不违反交规；二是“好走”，在某种评价标准下尽量优秀。这里的评价标准可以是路程最短、时间最少、更安全或者更平顺。

一个完整的路径规划问题包含四个基本要素：起点、终点、环境地图和约束条件。其中约束条件包括车辆动力学限制（转向半径、加速度）和交规约束（禁止压线、禁止逆行）。规划还有一个重要分层：全局规划在已知静态地图上算出一条宏观路线，计算频率较低；局部规划则在行驶中根据实时感知的动态障碍不断修正轨迹，通常要求 10Hz 以上才能保证安全。

评价一条规划路径，常用三类指标。第一是效率，看路径总长度和耗时；第二是安全，看与障碍物的最小距离是否足够；第三是舒适，看曲率变化是否平缓、加速度是否突兀。把这三类指标加权合并，就得到路径的总代价。

老周给自己的每个订单都打“总分”：距离短加分、红灯多扣分、路太颠扣分，总分最低的就是他的最优路线。自动驾驶里的代价函数，干的就是同一件事——用数字给一条路打分。

J(π)=∑i=0N−1c(si,si+1)\displaystyle J(\pi)=\sum_{i=0}^{N-1} c(s_{i}, s_{i+1})
公式解读：一条路径的总分，等于把路径切成 N 小段后，每一段的“花销”相加。规划的目标就是找到总分最小的那条路径。
  1. 路径 π整条路线由一串位姿点序列组成
    π={s0,s1,…,sN}\pi = \{s_{0}, s_{1}, \ldots, s_{N}\}
  2. 位姿 si第 i 个时刻车辆的位置与朝向
    si=(xi,yi,θi)s_{i} = (x_{i}, y_{i}, \theta_{i})
  3. 单段代价 c从 si 走到 si+1 的消耗，可以是距离、时间或风险
    c(si,si+1)c(s_{i}, s_{i+1})
  4. 总代价 J把所有小段代价累加起来，就是整条路径的分数
    J(π)=∑i=0N−1c(si,si+1)J(\pi)=\sum_{i=0}^{N-1} c(s_{i}, s_{i+1})

搜索空间建模

老周手机里的地图 App，本质是一张“哪里能走、哪里不能走”的简化图。他把城市抽象成可走区域和障碍区域两部分，这就是搜索空间建模——数学世界不会直接理解“街道”和“楼房”，它只认“自由的点”和“占据的点”。

车辆有长有宽，如果直接拿车体形状去做搜索，很容易卡进缝隙里。聪明的做法是引入位形空间（Configuration Space）概念：把车辆缩成一个点，同时把障碍物按照车体尺寸向外膨胀。这样，一个“点”走过的路径，原尺寸的车辆也能安全通过。膨胀半径通常等于车宽的一半、安全间隙和定位误差余量的总和。

地图建模有三种常见形式。栅格地图把连续空间切成均匀小格子，每个格子标记为自由、占据或未知，实现简单，但分辨率决定精度；路图则把可走区域抽象成节点和边的拓扑结构，配合图搜索效率很高；还有一种是采样地图，按需在空间中撒点连线。障碍物膨化是所有方式的共同前置步骤。

老周心里那张“红绿地图”——红灯区是障碍，绿灯区是能走的路，而且边界永远画得比他车身大一圈——就是位形空间思想在生活中的样子。建模建得好，后面的搜索才能跑得快、走得稳。

C=Cfree∪Cobs,Cfree∩Cobs=∅\displaystyle \mathcal{C}=\mathcal{C}_{\text{free}}\cup\mathcal{C}_{\text{obs}},\qquad \mathcal{C}_{\text{free}}\cap\mathcal{C}_{\text{obs}}=\varnothing
公式解读：所有合法位姿被严格分成两半——能走的自由空间和不能走的障碍空间，二者互不重叠、合起来就是全集。
  1. 位形空间 C所有可能的车辆位姿组成的集合
    C\mathcal{C}
  2. 自由空间不会与障碍碰撞的位姿集合
    Cfree\mathcal{C}_{\text{free}}
  3. 占据空间会与障碍发生碰撞的位姿集合
    Cobs\mathcal{C}_{\text{obs}}
  4. 划分关系自由与占据互不重叠，并集覆盖整个位形空间
    Cfree∩Cobs=∅\mathcal{C}_{\text{free}}\cap\mathcal{C}_{\text{obs}}=\varnothing

A*算法

老周打开手机导航，发现每个路口都标着一个数字。这个数字由两部分组成：他已经骑过的距离，加上前方直线到顾客楼下的估计距离。两者相加最小的路口，他会优先通过。这个“已走 + 预估”的打分逻辑，正是 A* 算法的灵魂。

A* 把一个节点 n 的估价写成三部分的组合：g(n) 是从起点到节点 n 已经付出的实际代价；h(n) 是从节点 n 到终点的启发式估计代价；f(n) 则是两者之和，代表“经过 n 的这条路大概要花多少钱”。A* 维护一张“开启表”存放待考察的节点，每次从中取出 f 值最小的节点进行扩展，扩展完的节点放入“关闭表”不再重复考察。算法不断重复“取最小 f、扩展、更新邻居”，直到终点被取出。

A* 能保证找到最短路径，前提是启发式函数满足可采纳性：h(n) 永远不大于从 n 到终点的真实最小代价，也就是 h 不能“乐观过头”。此时 A* 既完备又最优。作为参照，Dijkstra 算法等价于 h(n)=0 的 A*，而贪心搜索只认 h 不看 g，虽然快但容易绕远。

老周发现导航推荐的路线，既不是纯粹“已走最少”，也不是“看起来最近”，而是两者权衡后的结果。生活中我们凭直觉这样做，A* 则把它变成了严谨的数学保证。

f(n)=g(n)+h(n)\displaystyle f(n)=g(n)+h(n)
公式解读：每个节点都打一个分——已经花的钱加上预估还要花的钱，A* 每次都优先扩展分数最小的节点。
  1. 实际代价 g(n)从起点走到节点 n 已经付出的代价
    g(n)=∑s0→ncg(n) = \sum_{s_{0}\to n} c
  2. 启发代价 h(n)从节点 n 到终点的估计剩余代价
    h(n)≈剩余距离的估计h(n)\approx \text{剩余距离的估计}
  3. 总估价 f(n)已走与预估相加，决定节点的扩展优先级
    f(n)=g(n)+h(n)f(n)=g(n)+h(n)
  4. 最优性条件h 不大于真实代价时，A* 保证找到最优解
    h(n)≤h∗(n)h(n)\leq h^{*}(n)

RRT随机树

有一回老周进了没有地图的还建小区，导航完全失灵。他干脆骑到路口，闭眼朝随机方向扔飞镖，落点不确定，但他从自己身边最近的点朝飞镖方向走一小步，画下一根“探索树枝”。扔得够多，树枝就织成一张网，总有一枝能够到目的地。这就是他口中的“乱拳打法”。

这一招对应 RRT 算法（Rapidly-exploring Random Tree，快速扩展随机树）。算法每轮做三件事：先在空间里随机采样一个点 q_rand；再在已有树上找到离它最近的节点 q_near；最后从 q_near 向 q_rand 方向前进固定步长 ε，生成新节点 q_new 并挂到树上。反复迭代，树就从起点“生长”出去，逐渐覆盖整个空间，直到终点所在的区域被树枝碰到。

RRT 的核心特性是概率完备：当随机采样次数趋于无穷时，找到可行路径的概率趋近于 1，但它不保证在有限时间内找到解，也不保证找到的路径最优。为了逼近最优，RRT* 算法在加入新节点后，会对邻域内的既有节点做“重新接线”——如果绕经新节点的代价更小，就替换父节点，从而让整棵树不断“自我优化”，最终具备渐进最优性。

老周把这招叫“乱拳打法”：不知道哪里能走时，就到处探一探；探得多了，路自然就出来了。RRT 正是用“大量随机试探 + 就近连接”，把未知环境一点点织成通路。

qnew=qnear+ε⋅qrand−qnear∥qrand−qnear∥\displaystyle q_{\text{new}}=q_{\text{near}}+\varepsilon\cdot\frac{q_{\text{rand}}-q_{\text{near}}}{\|q_{\text{rand}}-q_{\text{near}}\|}
公式解读：新节点等距离开在“最近节点朝随机点方向”上，步长固定，方向由两个点的差值归一化得到。
  1. 随机采样点在空间里随机撒一个目标点
    qrandq_{\text{rand}}
  2. 最近树节点从树上找出离采样点最近的节点
    qnear=arg⁡min⁡q∈T∥qrand−q∥q_{\text{near}}=\arg\min_{q\in T}\|q_{\text{rand}}-q\|
  3. 单位方向两个节点差向量除以长度，得到方向
    qrand−qnear∥qrand−qnear∥\frac{q_{\text{rand}}-q_{\text{near}}}{\|q_{\text{rand}}-q_{\text{near}}\|}
  4. 步长扩展沿方向走固定距离 ε 生成新节点
    qnew=qnear+ε⋅qrand−qnear∥qrand−qnear∥q_{\text{new}}=q_{\text{near}}+\varepsilon\cdot\frac{q_{\text{rand}}-q_{\text{near}}}{\|q_{\text{rand}}-q_{\text{near}}\|}

Lattice格栅

上了城市快速路，老周不再自由走位，而是守规矩地沿着一条条预设的车道轨迹行驶。每条轨迹的弯度多大、加速多快、偏离中线多少，都是事先定好的。他要做的不是找路，而是从这些候选轨迹里挑出最合适的一条——像地铁司机在既定轨道里选线，而不是在山野里开路。

这就是 Lattice（格栅/栅格采样）规划。它不是在二维平面上画均匀网格，而是在完整的状态空间里撒“状态格点”。状态不仅包含位置，还包含朝向角、曲率甚至速度。每个状态格点对应一组“车辆真实开得出来”的轨迹，这些轨迹连在一起构成一张“轨迹网络”，规划就在这张网络上搜索。

Lattice 的关键在于轨迹采样必须满足动力学约束：最小转弯半径限制、最大加速度限制、速度与曲率的匹配关系。每一条候选轨迹都附带一个代价分数，通常由三部分加权组成——安全代价：离障碍越近惩罚越高；舒适代价：加速度和曲率变化越剧烈惩罚越高；效率代价：行驶时间越长惩罚越高。三个权重体现了系统策略：是更保守还是更激进。

老周觉得 Lattice 就像地铁轨道：轨道是提前设计好的，他要做的不是“找路”而是“选轨”。结构化道路上的自动驾驶，正是这种思路——把无穷无尽的连续空间，收敛成有限条可以执行的轨迹。

J(u)=w1Jsafe(u)+w2Jcomfort(u)+w3Jeffi(u)\displaystyle J(u)=w_{1}J_{\text{safe}}(u)+w_{2}J_{\text{comfort}}(u)+w_{3}J_{\text{effi}}(u)
公式解读：每条候选轨迹的分数由安全、舒适、效率三个子分数加权相加，权重表达“更看重哪头”。
  1. 候选控制量 u一组控制输入，如加速度与方向盘转角
    u=(a, δ)u=(a,\ \delta)
  2. 安全代价离障碍物越近，惩罚分数越高
    Jsafe(u)J_{\text{safe}}(u)
  3. 舒适代价加速度与曲率变化越剧烈，分数越差
    Jcomfort(u)J_{\text{comfort}}(u)
  4. 效率代价行驶时间和路程越长，分数越差
    Jeffi(u)J_{\text{effi}}(u)
  5. 加权求和用权重决定策略偏好，选总分数最小的轨迹
    J(u)=w1Jsafe(u)+w2Jcomfort(u)+w3Jeffi(u)J(u)=w_{1}J_{\text{safe}}(u)+w_{2}J_{\text{comfort}}(u)+w_{3}J_{\text{effi}}(u)

算法对比选型

送完几单后老周发现，三件法宝没有绝对的优劣，只看路况。熟悉商圈用导航，陌生小区用飞镖，快速路用轨道，他甚至会把它们组合起来用。算法选型，本质上也是这个道理。

先看完备性。A* 在有限离散图上具有确定性完备性：只要路径存在，就一定能找到。RRT 是概率完备：采样次数足够多时几乎必然找到解，但无法保证有限时间内完成。Lattice 在采样足够密集时完备，采样稀疏则可能漏掉可行解。再看效率。A* 的效率取决于启发式质量和地图规模；RRT 在高维空间随机探索效率很高，但结果粗糙；Lattice 可以离线预计算轨迹簇，在线推理很快，代价是内存占用大。

场景适配是选型的核心逻辑：静态已知地图且要求最优解，选 A*；环境未知或维度高（机械臂、泊车），选 RRT；结构化道路且要满足动力学，选 Lattice。工程上更常见的是混合策略：全局用 A* 确定方向，局部用 Lattice 处理高速工况，遇到突发障碍再用 RRT 快速重探索。

老周的工具箱从不同时只带一件：手机没电时用飞镖探路，飞镖探到路再换导航精修。成熟的自动驾驶系统同样是“多算法混搭”，用各自的优势覆盖不同场景。

lim⁡N→∞P(搜索到解)=1\displaystyle \lim_{N\to\infty} P(\text{搜索到解})=1
公式解读：这是“概率完备”的数学表达——采样次数趋于无穷时，找到解的概率趋近于 1，但注意它不等于“一定能很快找到”。
  1. 概率 P随机算法在前 N 次采样内找到可行解的概率
    P(搜索到解)P(\text{搜索到解})
  2. 采样次数 N随机采样的迭代次数趋近于无穷
    N→∞N\to\infty
  3. 概率完备极限值为 1，但无法给出有限时间内的保证
    lim⁡N→∞P(搜索到解)=1\lim_{N\to\infty}P(\text{搜索到解})=1

工程实践优化

老周车技再好，路线也得“磨”：把急转抹成缓弯、把时间算得更快、给刹车留出提前量。这正好对应路径规划落地时的三类工程优化——让算法从“理论能跑”变成“车上好用”。

第一是混合 A*（Hybrid A*）。普通 A* 搜出来的是折线，车辆根本开不出来。混合 A* 把搜索放到连续状态空间，节点不仅包含位置，还包含朝向角，节点之间用满足车辆运动学模型的弧线连接，从而保证搜出来的路径“车能跟着走”。第二是路径平滑，用多项式曲线或 B 样条把折线拟合成曲率连续的光滑曲线，提升乘坐舒适性，也减少轮胎磨损。第三是安全约束，包括障碍物膨胀、逐段碰撞检测、速度与加速度限幅，确保任何时刻都有安全余量。

实时性优化同样关键：降低搜索分辨率、复用上一帧的规划结果、多线程并行计算、用“从终点反向做一遍 Dijkstra”预计算启发式，这些手段共同把规划周期压进百毫秒甚至几十毫秒。在高速场景下，规划器必须在车辆移动一小段距离内给出新轨迹，否则后果不堪设想。

老周的“压弯技巧”其实就是平滑：同样的路，会压弯的人跑起来又快又不洒汤。工程优化的目标，正是让算法也学会“压弯”——在保证安全的前提下，把路径磨得又快又稳。

Jsmooth=wc∫κ2(s) ds  +  wj∫(dκds)2ds\displaystyle J_{\text{smooth}}=w_{c}\int \kappa^{2}(s)\,ds\;+\;w_{j}\int\left(\frac{d\kappa}{ds}\right)^{2}ds
公式解读：平滑惩罚等于“总弯曲量惩罚”加上“方向盘猛打惩罚”，两项加权越小，轨迹越丝滑。
  1. 曲率 κ(s)路径在弧长位置 s 处的弯曲程度
    κ(s)\kappa(s)
  2. 弯曲总量曲率平方沿路径积分，弯曲越少越平滑
    ∫κ2(s) ds\int \kappa^{2}(s)\,ds
  3. 曲率变化率方向盘转动的快慢，影响舒适性
    ∫(dκds)2ds\int\left(\frac{d\kappa}{ds}\right)^{2}ds
  4. 加权合并两项分别乘权重后相加成平滑代价
    Jsmooth=wc∫κ2(s) ds+wj∫(dκds)2dsJ_{\text{smooth}}=w_{c}\int \kappa^{2}(s)\,ds+w_{j}\int\left(\frac{d\kappa}{ds}\right)^{2}ds

自动驾驶应用

老周的一天，其实是“感知—预测—规划—控制”的完整流水线：眼睛看路是感知，判断旁边电动车要往哪走是预测，决定自己怎么走是规划，手脚配合执行是控制。路径规划，就是流水线里“决定怎么走”的关键一环。

规划层之上还有一个行为决策层。决策层输出的是语义指令：“跟车”“变道”“让行”“左转待转”；路径规划则把这些指令翻译成一条条可执行的轨迹。决策与规划必须紧密协同：决策层判断“该让行了”，规划层才能在几十毫秒内生成一条减速等待的平滑曲线，两者配合才能应对加塞、鬼探头等复杂博弈。

典型场景管线各有侧重：高速公路上结构强、车速快，常用 Lattice 或 Frenet 坐标系下的轨迹采样；城区道路复杂、目标多，常用混合 A* 结合道路拓扑进行规划；泊车场景空间狭窄，常用 RRT 探索入库轨迹再用曲线平滑。业界案例中，百度 Apollo 的 EM Planner 采用横向纵向解耦做轨迹采样，Waymo 与特斯拉也采用分层规划架构。趋势上，模仿学习与端到端模型正在与经典算法融合，让规划既保底又智能。

老周把一天骑得又快又稳，靠的不是某一项单独技能，而是一整套环环相扣的流程。自动驾驶的路径规划同样只是流程里的一环，但这一环的输出质量，直接决定了乘客的安全感与舒适度。

π∗=arg⁡min⁡π∈ΠJ(π)s.t.π˙∈D, π∩O=∅\displaystyle \pi^{*}=\arg\min_{\pi\in\Pi} J(\pi) \quad \text{s.t.} \quad \dot{\pi}\in\mathcal{D},\ \pi\cap\mathcal{O}=\varnothing
公式解读：在所有“动力学可行且不碰障碍”的候选轨迹里，挑出代价最小的那一条，就是最终执行的轨迹。
  1. 候选轨迹集 Π规划器一次生成的所有备选轨迹
    Π={π1,π2,…,πn}\Pi=\{\pi_{1},\pi_{2},\ldots,\pi_{n}\}
  2. 动力学可行轨迹的导数属于车辆可达的动态集合
    π˙∈D\dot{\pi}\in\mathcal{D}
  3. 安全无碰撞轨迹与障碍物集合不相交
    π∩O=∅\pi\cap\mathcal{O}=\varnothing
  4. 最优轨迹在满足约束的候选里取代价最小者
    π∗=arg⁡min⁡π∈ΠJ(π)\pi^{*}=\arg\min_{\pi\in\Pi}J(\pi)

AI 应用场景

Robotaxi 城区路口规划

阶段：预测与规划（运动规划）

解决的问题：城市路口车流密集、目标众多，传统规则式规划“见招拆招”反应慢。行为决策层给出“直行/让行”的语义指令后，规划层用 Lattice 或混合 A* 在道路坐标系下生成多条候选轨迹，实时评估安全、舒适与效率，选出最优轨迹。

具体案例：Robotaxi 在十字路口左转时，感知系统发现对向有直行车辆高速逼近。规划器在几十毫秒内重新采样一组轨迹，选择“先减速等待、待对向车通过后再完成左转”的平滑曲线，全程无急刹。

仓储物流 AGV 导航

阶段：机器人导航规划

解决的问题：仓库地图频繁变动、货架密集，单一算法难以兼顾全局效率与局部避障。工程上常用“全局 A* + 局部 RRT”的组合：A* 在栅格地图上算出跨区域的干线路径，遇到动态障碍时用 RRT 快速扩展绕行树枝。

具体案例：某电商仓库的 AGV 接到搬运任务后，先用 A* 算出从充电区到拣货台的干线路线；途经通道时发现临时堆放的货箱，立即用 RRT 就地探索出一条绕行支线，平滑接回原路径，全程不停顿。

自主代客泊车

阶段：低速运动规划

解决的问题：停车场空间狭窄、障碍物密集，泊车轨迹必须满足车辆最小转弯半径。RRT 特别适合在狭窄空间里快速探索可行入库通道，再用曲线平滑把探索出的折线磨成可执行的弧线轨迹。

具体案例：车辆驶入地下停车场后，系统先用 RRT 在目标车位周边随机探索，生成一条“倒车入库”的可行参考轨迹，再做曲率平滑与碰撞复核，最终一把入库，耗时不超过两秒。

Python 演示

python85 行
import heapqimport mathimport random GRID_W, GRID_H = 12, 8obstacles = {(3, 2), (3, 3), (3, 4), (6, 5), (7, 5), (8, 5)}start, goal = (0, 4), (11, 4) def heuristic(a, b):    return abs(a[0] - b[0]) + abs(a[1] - b[1]) def neighbors(node):    x, y = node    for dx, dy in ((1, 0), (-1, 0), (0, 1), (0, -1)):        nx, ny = x + dx, y + dy        if 0 <= nx < GRID_W and 0 <= ny < GRID_H and (nx, ny) not in obstacles:            yield (nx, ny) def astar(start_node, goal_node):    open_heap = [(0, start_node)]    g_score = {start_node: 0}    came_from = {}    while open_heap:        _, cur = heapq.heappop(open_heap)        if cur == goal_node:            path = []            while cur is not None:                path.append(cur)                cur = came_from.get(cur)            return path[::-1]        for nxt in neighbors(cur):            new_g = g_score[cur] + 1            if new_g < g_score.get(nxt, float('inf')):                g_score[nxt] = new_g                came_from[nxt] = cur                heapq.heappush(open_heap, (new_g + heuristic(nxt, goal_node), nxt))    return [] def rrt(start_node, goal_node, max_iter=3000, step=0.7):    tree = {start_node: None}     def distance(p, q):        return math.hypot(p[0] - q[0], p[1] - q[1])     def sample():        if random.random() < 0.15:            return goal_node        return (random.uniform(0, GRID_W), random.uniform(0, GRID_H))     def nearest(rand):        return min(tree, key=lambda p: distance(p, rand))     def steer(near, rand):        dist = distance(near, rand)        if dist < step:            return rand        return (near[0] + step * (rand[0] - near[0]) / dist,                near[1] + step * (rand[1] - near[1]) / dist)     for _ in range(max_iter):        rand_pt = sample()        near_pt = nearest(rand_pt)        new_pt = steer(near_pt, rand_pt)        if any(distance(new_pt, ob) < 0.5 for ob in obstacles):            continue        tree[new_pt] = near_pt        if distance(new_pt, goal_node) < step:            tree[goal_node] = new_pt            path = []            cur = goal_node            while cur is not None:                path.append(cur)                cur = tree.get(cur)            return path[::-1]    return [] if __name__ == '__main__':    path = astar(start, goal)    print('A* path:', path)    print('A* steps:', len(path))    random.seed(7)    rrt_path = rrt(start, goal)    print('RRT path points:', len(rrt_path))    if rrt_path:        print('RRT first:', rrt_path[0], 'last:', rrt_path[-1])
目的：在带障碍的栅格地图上，分别用 A* 和 RRT 从起点规划到终点，输出路径点与步数，直观对比两类算法的结果形态。
  1. open_heap = [(0, start_node)]用最小堆存放待扩展节点，堆顶永远是 f 值最小的候选
  2. heapq.heappush(open_heap, (new_g + heuristic(nxt, goal_node), nxt))关键：把 g+h 作为优先级入堆，这就是 A* 的估价灵魂
  3. if new_g < g_score.get(nxt, float('inf')):只有找到更优的已走代价才更新节点，避免重复走冤枉路
  4. if random.random() < 0.15:目标偏置：以 15% 概率直接采样终点，引导随机树向目标生长
  5. return min(tree, key=lambda p: distance(p, rand))在已长成的树上找离随机点最近的节点
  6. return (near[0] + step * (rand[0] - near[0]) / dist,沿随机点方向走固定步长，生成新节点
  7. if any(distance(new_pt, ob) < 0.5 for ob in obstacles):碰撞检查：离障碍中心太近的新节点直接丢弃
  8. cur = came_from.get(cur)从终点沿父指针一路回溯，倒推出完整路径

常见误区

误区听说 A* 是 Dijkstra 的“升级版”，于是认为不管什么地图，A* 都会更快。

正解A* 只是多了一个启发式估计 h(n)。当 h=0 时它就退化成 Dijkstra；当 h 带信息量时，它能减少扩展节点数。但如果 h 与真实代价偏差很大，A* 反而可能扩展更多节点。快不快，取决于启发式函数的质量，而不是“用了 A*”这个事实本身。

为何常见：Dijkstra 是教科书里的基础算法，A* 看起来只是“加了启发式”，容易让人想当然地认为升级版必然更快。

误区RRT 随机探索后找到了一条能走的路径，就认为它是最短或最优路径。

正解RRT 只保证概率完备——时间足够长时几乎肯定能找到一条可行路径，但随机性导致路径通常很绕。真正有渐进最优性的是 RRT*：它在扩展后做“重新接线”，不断用更优代价修正父节点，迭代无穷多次才能收敛到最优解。

为何常见：名字听上去像“A* 的随机版”，直觉上就以为它继承了 A* 的最优性。

误区Lattice 规划不过是在地图上画更密的网格，然后用 A* 搜索。

正解普通 A* 的网格节点只有位置 (x,y)，边是直线段；Lattice 的节点是包含朝向和曲率的完整状态 (x,y,θ,κ)，边是满足车辆运动学模型的可行轨迹。它搜的不是“格子”，而是“状态—轨迹网络”，两者本质不同。

为何常见：“格栅”这个译名容易让人联想到网格，忽略了状态维度和动力学约束这两个关键差异。

误区膨胀让本来能走的路变少、路径变绕，觉得工程上没必要。

正解真实车辆有体积，定位有误差，控制有延迟，规划出来的路径必须留安全裕度。把障碍物向外膨胀，等于把“车体半径＋安全间隙＋误差余量”一次性预留在几何层，后续碰撞检测才又高效又安全。

为何常见：仿真里用“点机器人”看似能贴边通过，忽略了真实车辆的尺寸与不确定性。

误区认为路径规划的目标就是“最短”，越短越好。

正解自动驾驶还要考虑可行性（转向半径、加速度）、舒适性（曲率变化）、安全性（离障碍距离）和法规（不压线、不逆行）。很多场景的最优解是“较长但更安全平稳”的轨迹，代价函数里的各项权重正是用来做这种权衡的。

为何常见：日常生活中导航默认推荐最短或最快路线，这种惯性思维被带到了算法理解中。

面试题

Q1. 在 A* 的估价函数 f(n)=g(n)+h(n) 中，h(n) 表示什么？

单选题 · easy

  • A.起点到当前点的实际代价
  • B.当前点到终点的估计代价← 正确答案
  • C.起点到终点的实际总代价
  • D.完全随机生成的代价值

答案：B。h 是启发式估计代价，代表从当前节点到终点的乐观预估。g 是起点到当前节点的实际代价，f 是两者之和。h 越接近真实代价，A* 扩展的节点越少。

Q2. Dijkstra 算法可以看作 A* 在哪种条件下的特例？

单选题 · easy

  • A.启发函数 h 恒为最大值时
  • B.实际代价 g 恒为零的时候
  • C.启发函数 h 恒等于零时← 正确答案
  • D.地图上完全没有障碍的时候

答案：C。当 h(n)=0 时，f(n)=g(n)，A* 不再利用任何终点方向信息，退化为按已走代价扩展的 Dijkstra。反过来也说明，A* 的加速全部来自启发式的质量。

Q3. 所谓“可采纳的启发式函数”，指的是？

单选题 · easy

  • A.估计值与真实代价完全无关
  • B.估计值总大于等于真实值
  • C.估计值恰好等于真实代价
  • D.估计值总小于等于真实值← 正确答案

答案：D。可采纳性要求 h(n) 不大于从 n 到终点的真实最小代价。只有满足这个条件，A* 才能保证找到最优解。若 h 偏大，可能提前放弃真正最优的路径。

Q4. RRT 算法属于哪一类路径规划方法？

单选题 · mid

  • A.基于状态格栅采样的方法
  • B.确定性网格广度优先搜索
  • C.基于随机采样的快速扩展树← 正确答案
  • D.基于人工势场吸引的方法

答案：C。RRT 即快速扩展随机树，核心是随机采样加就近生长。状态格栅采样属于 Lattice 规划，广度优先搜索属于离散图搜索，人工势场是另一类连续空间方法。

Q5. 关于 RRT 算法的完备性，下列说法正确的是？

单选题 · hard

  • A.既不完备也不是渐进最优
  • B.完备且每次都能找到最短路
  • C.具备确定性完备并且最优
  • D.具备概率完备而不具备最优← 正确答案

答案：D。RRT 是概率完备的：采样次数趋于无穷时找到解的概率趋近 1，但不保证有限时间找到解，也不保证路径最优。追求渐进最优需要 RRT* 的重新接线机制。

Q6. Lattice 规划相比普通 A* 的最大优势是？

单选题 · mid

  • A.计算量一定比 A* 算法更低
  • B.不需要地图也可以进行规划
  • C.生成的轨迹一定长度最短
  • D.生成的轨迹满足动力学约束← 正确答案

答案：D。Lattice 的节点包含朝向与曲率，边是满足车辆运动学模型的可行轨迹，因此规划结果可直接执行。普通 A* 的网格边是直线段，不考虑转向半径，搜出的折线车开不出来。

Q7. Hybrid A* 与普通 A* 的核心区别在于？

单选题 · hard

  • A.仅使用栅格离散化的节点
  • B.节点为连续位姿并考虑转向← 正确答案
  • C.完全放弃启发式搜索策略
  • D.只适用于高速公路的规划

答案：B。混合 A* 在连续状态空间搜索，节点携带朝向角，节点间用满足运动学的弧线连接，因此解出的轨迹车辆可执行。它仍然使用启发式，也不限于高速场景。

Q8. 下列哪些属于路径规划中常用的评价指标？

多选题 · mid

  • A.路径的总长度← 正确答案
  • B.规划算法的耗时← 正确答案
  • C.安全冗余裕度← 正确答案
  • D.车型品牌颜色

答案：ABC。路径规划的评价通常覆盖效率（长度与耗时）、实时性（算法耗时）与安全（安全裕度）。车型品牌与外观颜色与规划质量无关，是干扰项。

Q9. 下列关于算法性质的描述，正确的有？

多选题 · hard

  • A.RRT* 具有渐进最优性← 正确答案
  • B.混合 A* 节点含连续位姿← 正确答案
  • C.Lattice 轨迹满足运动学← 正确答案
  • D.RRT 一定找到最短路径

答案：ABC。RRT* 通过重连机制渐进收敛到最优；混合 A* 在连续状态空间搜索；Lattice 的边是满足运动学的轨迹。普通 RRT 只保证概率完备，不保证最短。

Q10. 在自动驾驶路径规划中，对障碍物做膨胀处理的原因包括？

多选题 · mid

  • A.补偿定位误差带来的风险← 正确答案
  • B.大幅度减小运算量加快速度
  • C.为车辆外廓留出安全间隙← 正确答案
  • D.让规划出来的路径更漂亮

答案：AC。膨胀半径包含车体尺寸、安全间隙与定位误差余量，能在几何层一次性处理安全问题。膨胀并不会减小搜索空间，也不以美观为目的。