参考：ChatGPT、autoware

对应源码文件: autoware_devel/src/universe/autoware_universe/planning/behavior_path_planner/autoware_behavior_path_start_planner_module/src/freespace_pull_out.cpp

从自由空间脱困到回归车道：Hybrid A* Freespace Pull Out 算法教程

Freespace Pull Out 解决的是一个很具体、也很棘手的起步问题：车辆停在路肩、停车位边缘、障碍物旁边或车道外侧，普通的“沿车道中心线向前走”已经不可行；基于车道的平滑横移、圆弧驶出等方法也找不到安全路径。这时车辆需要临时把自己看作一个在二维占据栅格里运动的非完整约束车辆，先在自由空间中绕开障碍物，再回到道路中心线。

这篇教程专门讲 Freespace Pull Out 中的 Hybrid A* 思想。它不按源码顺序解释，而是从零建立算法：什么是自由空间驶出、为什么普通 A* 不够、怎样把车辆位姿离散成可搜索状态、怎样用自行车模型生成运动原语、怎样做矩形车身碰撞检测、怎样设计启发函数和代价函数、怎样判定到达目标、怎样把自由空间轨迹重新接回车道。读完后，即使不了解 Autoware，也应该能独立理解这类算法如何工作。

1. 先理解 Freespace Pull Out 的任务

车辆从路边起步时，理想情况是沿着目标车道方向平滑合入车道中心线。但现实里可能出现下面的情况：

1. 车辆前方中心线方向被障碍物挡住。
2. 车辆与道路方向夹角较大，简单横移不满足转向半径。
3. 路肩、停车区或障碍物形成狭窄空间，需要先倒车再前进。
4. 车道几何本身不足以描述车辆当前可行驶区域。

因此，自由空间驶出不再把路径限制在车道中心线附近，而是解决下面的问题：

$$\text{给定起点 }{q}_s=(x_s,y_s,{\theta }_s)\text{ 和道路上的回归目标 }{q}_g=(x_g,y_g,{\theta }_g),\text{ 在障碍物栅格中寻找一条可行车辆轨迹}$$

这里的 $q=(x,y,\theta )$ 是车辆的二维位姿，$x,y$ 表示平面位置，$\theta$ 表示车头朝向。所谓“可行车辆轨迹”，必须同时满足四类约束：

$$\text{不撞障碍物}$$

$$\text{不驶出规划栅格边界}$$

$$\text{满足车辆最小转弯半径}$$

$$\text{尽量少倒车、少换挡、少贴近障碍物，并最终回到道路方向}$$

从行为规划视角看，Freespace Pull Out 不是最终一路开到终点的全局路径规划。它更像一个“脱困段”：

$$\text{当前位置}\to \text{自由空间搜索}\to \text{道路中心线上的回归点}\to \text{继续沿道路中心线行驶}$$

这就是本算法的主线。

2. 为什么需要 Hybrid A*

在讲 Hybrid A* 之前，先把普通 A* 讲清楚。A* 本质上是一种“带方向感的最短路搜索”。它面对的问题是：

$$\text{给定起点 }s\text{ 和终点 }g,\text{ 在一张图中找到总代价尽可能小的路径}$$

这里的“图”不是图片，而是由节点和边组成的数学对象：

$$\mathcal{G}=(\mathcal{V},\mathcal{E})$$

其中 $\mathcal{V}$ 是节点集合，$\mathcal{E}$ 是边集合。若从节点 $a$ 可以走到节点 $b$，就存在一条边：

$$(a,b)\in \mathcal{E}$$

每条边都有一个非负代价：

$$c(a,b)\ge 0$$

在二维栅格地图里，可以把每个空闲格子看成一个节点，把相邻格子之间的移动看成边。比如从一个格子向右走一格，代价可以是 $1$；斜向走一格，代价可以是 $\sqrt{2}$。这样，找路径就变成了在图中找一串节点：

$$s=n_0\to n_1\to \cdots \to n_K=g$$

路径总代价为：

$$J=\sum _{i=0}^{K-1}c(n_i,n_{i+1})$$

A* 的目标是找到 $J$ 尽可能小的路径。

如果完全不知道终点在哪里，可以用 Dijkstra 算法。Dijkstra 每次选择“从起点走到这里已经花费最小”的节点继续扩展。它的评价函数只有：

$$f(n)=g(n)$$

其中：

$$g(n)=\text{从起点到节点 }n\text{ 已经花掉的真实代价}$$

Dijkstra 很稳，但它没有目标方向感。它像水波一样从起点向四周扩散，直到碰到终点。

另一种极端是贪心最佳优先搜索。它只看节点离终点还剩多远：

$$f(n)=h(n)$$

其中：

$$h(n)=\text{从节点 }n\text{ 到终点的估计代价}$$

贪心搜索方向感很强，但它可能为了看起来更接近终点而绕进死胡同。

A* 正好把这两种思想合在一起：

$$f(n)=g(n)+h(n)$$

这个公式可以用一句话理解：

$$\text{评价一个节点，不仅看已经走了多贵，也看预计还要走多贵}$$

其中 $g(n)$ 负责“脚踏实地”，因为它是真实已经付出的代价；$h(n)$ 负责“抬头看路”，因为它给搜索一个朝向目标的趋势。

对新手来说，可以把 A* 想象成每次都问：

$$\text{如果我从起点走到这里，再从这里走到终点，总共大概要花多少？}$$

这个“大概要花多少”就是 $f(n)$。

普通栅格 A* 通常把 $h(n)$ 设成当前格子到目标格子的几何距离。若节点 $n$ 的坐标为 $(i,j)$，目标为 $(i_g,j_g)$，四邻域移动时常用曼哈顿距离：

$$h_{\text{manhattan}}(n)=|i-i_g|+|j-j_g|$$

八邻域或连续平面里更常用欧氏距离：

$$h_{\text{euclidean}}(n)=\sqrt{(i-i_g{)}^2+(j-j_g{)}^2}$$

启发函数有一个重要概念：如果 $h(n)$ 从不高估真实剩余代价，就称为可采纳启发：

$$0\le h(n)\le h^{\ast }(n)$$

其中 $h^{\ast }(n)$ 是从 $n$ 到目标的真实最优代价。可采纳启发的直觉是：可以乐观，但不能吹牛。如果 $h$ 可采纳，并且边代价非负，经典 A* 能保证找到最优路径。

但自动驾驶里的实时规划常常会使用加权启发：

$$f(n)=g(n)+w_{h}h(n)$$

当 $w_h>1$ 时，搜索会更“急着朝目标走”，速度通常更快，但严格最优性会变弱。自由空间起步规划更重视有限时间内找到可行、可执行、足够好的路径，所以会接受这种折中。

普通栅格 A* 通常搜索二维点：

$$s=(i,j)$$

每次从当前格子扩展到上下左右或八邻域格子。这种方法适合点机器人，却不适合汽车。汽车至少有三个关键限制：

首先，汽车有朝向。两个车辆虽然中心点相同，但车头方向不同，下一步能去的地方完全不同：

$$(x,y,{\theta }_1)\ne (x,y,{\theta }_2)$$

其次，汽车不能横向平移。车辆的速度方向大致沿车身纵向，无法从一个格子直接侧向移动到另一个格子。

第三，汽车有最小转弯半径。若轴距为 $L$，前轮转角为 $\delta$，曲率为：

$$\kappa =\frac{\tan \delta }{L}$$

转弯半径为：

$$R=\frac{1}{|\kappa |}=\frac{L}{|\tan \delta |}$$

当 $|\delta |\le {\delta }_{\max }$ 时，最小转弯半径为：

$$R_{\min }=\frac{L}{\tan {\delta }_{\max }}$$

普通 A* 的“斜着走一格”并不保证满足这个半径约束。

Hybrid A* 的核心思想是：搜索索引是离散的，但节点之间的运动是连续车辆运动学积分出来的。也就是说，它把状态空间写成：

$$q=(x,y,\theta )$$

再把它映射到离散索引：

$$\mathrm{index}(q)=(i,j,k)$$

其中 $i,j$ 是栅格位置索引，$k$ 是航向角索引。搜索时，优先队列、闭集、重复访问判断都基于 $(i,j,k)$；但每一条边不是简单的邻接格，而是一小段车辆可执行的前进或倒车圆弧。

这就是 “Hybrid” 的含义：

$$\text{连续车辆运动学}+\text{离散图搜索}=\text{Hybrid A*}$$

它比普通 A* 更懂车辆，比纯连续优化更稳定，也更适合在占据栅格上做脱困搜索。

把普通 A* 和 Hybrid A* 对照起来看，会更容易理解：

普通 A* 的节点是一个二维格子：

$$n=(i,j)$$

Hybrid A* 的节点是一个带朝向的车辆位姿：

$$n=(x,y,\theta )$$

普通 A* 的节点身份由二维格子决定：

$$\mathrm{id}(n)=(i,j)$$

Hybrid A* 的节点身份由位置格子和角度桶决定：

$$\mathrm{id}(n)=(i,j,k)$$

普通 A* 的边是“从当前格子走到相邻格子”：

$$(i,j)\to (i+\Delta i,j+\Delta j)$$

Hybrid A* 的边是“车辆用某个转角行驶一小段距离”：

$$q^{\prime }=F(q,\delta ,d)$$

普通 A* 的碰撞检查通常只看目标格子是否被占据：

$$O(i,j)=0$$

Hybrid A* 的碰撞检查要看完整车身矩形是否与障碍物重叠：

$$\mathcal{B}(q)\cap \mathcal{O}=\varnothing$$

因此，Hybrid A* 并不是抛弃 A*，而是把 A* 的节点、边、代价和碰撞检查全部替换成适合汽车的版本。

3. 自由空间驶出的整体流程

完整流程可以理解为一条递进链路：

$$\text{判断是否需要自由空间脱困}\to \text{在道路中心线上采样回归目标}\to \text{把代价地图变成占据栅格}\to \text{用 Hybrid A* 搜索起点到目标}\to \text{按前进/倒车切分轨迹}\to \text{接回道路中心线}$$

自由空间规划并不是一直运行。它通常作为兜底方案出现：车辆已经停止，普通起步路径没有找到，且有新鲜的代价地图可用时，才尝试自由空间搜索。

更具体地说，它适合在下面的状态下启动：

$$\text{车辆处于起步规划场景}$$

$$\text{车辆已经停止}$$

$$\text{常规停车等待或车道语义驶出没有找到可行路径}$$

$$\text{代价地图存在且时间足够新}$$

如果代价地图过旧，搜索即使找到路径也可能对应已经失效的障碍物环境；如果车辆还没有停止，自由空间脱困段的低速几何搜索也不一定适合作为当前运动的直接接管路径。

设车辆当前位置为 $q_s$，它在当前道路中心线上的弧长坐标为 $s_{\text{ego}}$。算法不会随便选一个远处目标，而是在车辆前方道路中心线上生成候选回归点：

$$s_{\text{start}}=\max (0,s_{\text{ego}}+d_{\text{start}})$$

$$s_{\text{end}}=s_{\text{ego}}+d_{\text{end}}$$

然后按间隔 $\Delta s$ 采样：

$$s_m=s_{\text{start}}+m\Delta s,s_m\le s_{\text{end}}$$

每个 $s_m$ 对应道路中心线上的一个目标位姿：

$$q_g^{(m)}=\left(x_c(s_m),y_c(s_m),{\theta }_c(s_m)\right)$$

其中 $C(s)=(x_c(s),y_c(s))$ 是道路中心线，${\theta }_c(s)$ 是中心线切向角。

这一步非常关键。自由空间搜索不是漫无目的地“找一条路”，而是问：

$$\text{我能否从当前位置安全地开到前方某个道路中心线点？}$$

通常会从较近的候选回归点开始尝试，找到第一条可行路径就停止。这样得到的路径不会过度绕行，也更像一个起步脱困动作。

在正式搜索前，还必须检查起点和候选目标本身是否有效：

$$\mathrm{collision}(q_s)=0$$

$$\mathrm{collision}(q_g^{(m)})=0$$

只要起点或目标已经与障碍物、未知区域或地图边界冲突，该候选问题就没有继续搜索的意义。

4. 代价地图、占据栅格与坐标变换

Hybrid A* 在代价地图上搜索。代价地图可以看成一个二维栅格：

$$M\in {\mathbb{Z}}^{H\times W}$$

每个格子存一个代价值 $M_{ij}$。值越大，越可能是障碍物；未知区域通常也会被当作不可通行区域。

给定障碍物阈值 ${\tau }_{\text{obs}}$，可以定义占据函数：

$$O(i,j)=\{\begin{array}{ll}1,& M_{ij}<0\text{ 或 }{M}_{ij}\ge {\tau }_{\text{obs}}\\ 0,& \text{otherwise}\end{array}$$
$M_{ij}<0$：未知区域，当成障碍物；
$M_{ij}\ge {\tau }_{\text{obs}}$：代价值超过障碍物阈值，也当成障碍物；
其他情况：认为可以通行。
$O(i,j)=1$ 表示该栅格不可通行。

代价地图有自己的局部坐标系。若地图原点在世界坐标中的位姿为 $T_M^W$，车辆世界位姿为 $T_B^W$，先把车辆变换到地图坐标：

$$T_B^M=(T_M^W{)}^{-1}{T}_B^W$$

得到局部位姿：

$$q^M=(x^M,y^M,{\theta }^M)$$

若栅格分辨率为 $r$，则位置索引采用四舍五入：

$$i=\mathrm{round}\left(\frac{x^M}{r}\right)$$

$$j=\mathrm{round}\left(\frac{y^M}{r}\right)$$

航向角被离散成 $N_{\theta }$ 个桶。角分辨率为：

$$\Delta \theta =\frac{2\pi }{N_{\theta }}$$

将角度归一化到 $[0,2\pi )$ 后：

$$k=\mathrm{round}\left(\frac{{\mathrm{normalize}}_{[0,2\pi )}(\theta )}{\Delta \theta }\right)\mathrm{mod}{N}_{\theta }$$

于是连续位姿 $q$ 对应离散索引：

$$\mathrm{index}(q)=(i,j,k)$$

搜索图中的唯一节点编号可以写成：

$$\mathrm{id}(i,j,k)=(jW+i)N_{\theta }+k$$

这说明 Hybrid A* 并不只是在二维地图上找路。它实际上在三维离散空间中搜索：

S = { 0 , … , W − 1 } × { 0 , … , H − 1 } × { 0 , … , N θ − 1 } \mathcal{S}= \{0,\dots,W-1\} \times \{0,\dots,H-1\} \times \{0,\dots,N_\theta-1\} S={0,…,W−1}×{0,…,H−1}×{0,…,Nθ​−1}

当默认 $N_{\theta }=120$ 时，角分辨率为：

$$\Delta \theta =\frac{360^{\circ }}{120}=3^{\circ }$$

所以同一个位置上，车头朝向相差约 $3^{\circ }$ 就可能进入不同的搜索状态。

5. 车辆不是一个点：矩形车身模型

自由空间规划必须考虑车身尺寸。设车辆矩形以后轴中心或基准点为参考，车长为 $l$，车宽为 $w$，基准点到车尾距离为 $l_b$，到车头距离为：

$$l_f=l-l_b$$

车辆在自身坐标系中的矩形区域为：

$$\mathcal{B}=\left\{(u,v)\mid -l_b\le u\le l_f,-\frac{w}{2}\le v\le \frac{w}{2}\right\}$$

当车辆位姿为 $q=(x,y,\theta )$ 时，车身占据区域为：

$$\mathcal{B}(q)=\left\{\left[\begin{array}{c}x\\ y\end{array}\right]+R(\theta )\left[\begin{array}{c}u\\ v\end{array}\right]\mid (u,v)\in \mathcal{B}\right\}$$

其中旋转矩阵为：

$$R(\theta )=\left[\begin{array}{cc}\cos \theta & -\sin \theta \\ \sin \theta & \cos \theta \end{array}\right]$$

如果 $\mathcal{B}(q)$ 中任意一点落入障碍物格子，则该位姿不可行：

$$\exists p\in \mathcal{B}(q),O(\mathrm{grid}(p))=1\Rightarrow \mathrm{collision}(q)=1$$

实际搜索不可能对连续矩形里每个点都检测，因为矩形内部有无穷多个点。如果每扩展一个 Hybrid A* 节点，都重新对整车矩形做连续几何求交，计算会非常重。更实用的做法是：把“车辆矩形覆盖哪些栅格”提前离散化。

先固定一个航向桶 $k$（“第几个朝向区间”）。此时车辆相对栅格坐标系的朝向已经确定，车身矩形相对车辆基准点的形状也就确定。然后在车身矩形内部按小步长采样，例如使用 $\frac{r}{2}$，其中 $r$ 是 costmap 分辨率。对每个采样点 $(u,v)$，先旋转到当前航向，再映射成相对车辆基准点的栅格偏移，（u，v）是相对于车辆坐标系下的车辆基准点偏移，（$\Delta x，\Delta y$）是相对于地图坐标系的基准点偏移）：

$$
\begin{bmatrix}
\Delta x\
\Delta y
\end{bmatrix}

R(\theta_k)
\begin{bmatrix}
u\
v
\end{bmatrix}
$$

$$\Delta i=\mathrm{round}\left(\frac{\Delta x}{r}\right),\Delta j=\mathrm{round}\left(\frac{\Delta y}{r}\right)$$

不同采样点可能落到同一个栅格，所以还要去重。最终，每个航向桶 $k$ 都可以预先生成一组车身覆盖格子：

I body ( k ) = { ( Δ i , Δ j ) } \mathcal{I}_{\text{body}}(k)= \left\{ (\Delta i,\Delta j) \right\} Ibody​(k)={(Δi,Δj)}

这组 ${\mathcal{I}}_{\text{body}}(k)$ 的含义是：

$$\text{如果车辆基准点位于某个栅格中心，那么车身矩形会覆盖这些相对偏移格子}$$

举个小例子会更直观。假设车辆基准点在车身中心附近，车长 $4\text{m}$，车宽 $2\text{m}$，costmap 分辨率是 $1\text{m}$。如果我们在车身矩形内部按 $0.5\text{m}$ 的步长取点，那么车身坐标系里会有很多采样点，比如：

( u , v ) ∈ { − 1.5 , − 1.0 , − 0.5 , 0 , 0.5 , 1.0 , 1.5 } × { − 1.0 , − 0.5 , 0 , 0.5 , 1.0 } (u,v)\in \{ -1.5,-1.0,-0.5,0,0.5,1.0,1.5 \} \times \{ -1.0,-0.5,0,0.5,1.0 \} (u,v)∈{−1.5,−1.0,−0.5,0,0.5,1.0,1.5}×{−1.0,−0.5,0,0.5,1.0}

其中，$u$ 表示车身前后方向，$v$ 表示车身左右方向。像 $(u,v)=(1.5,0.5)$ 这样的点，代表“车身前部偏右的一块区域”；$(u,v)=(-1.0,-0.5)$ 代表“车身后部偏左的一块区域”。

如果当前航向桶对应的角度是 $90^{\circ }$，那么这个采样点会先旋转到地图坐标系，再变成某个栅格偏移。也就是说，原来在车身右上角的点，转到地图里后，可能落到车辆基准点的上方或右方，具体取决于当前朝向。

所以这些采样点并不是“随便挑的一些点”，而是在做这件事：

$$\text{用一批离散代表点，近似整辆车的连续矩形占据区域}$$

因此，真正检测某个位姿时，不需要重新采样矩形。只需把这些偏移索引平移到车辆基准格子 $(i,j)$：

$$(i^{\prime },j^{\prime })=(i+\Delta i,j+\Delta j)$$

然后检查是否越界或占据：

$$\mathrm{collision}(i,j,k)=\exists (\Delta i,\Delta j)\in {\mathcal{I}}_{\text{body}}(k),O(i+\Delta i,j+\Delta j)=1$$

直观地说，完整检查就是问：

$$\text{车辆矩形覆盖的所有格子里，有没有任何一个是障碍物？}$$

如果有，碰撞；如果没有，不碰撞。

但是，Hybrid A* 搜索会产生大量候选节点。如果每个候选节点都遍历完整的 ${\mathcal{I}}_{\text{body}}(k)$，仍然会花很多时间。为了进一步加速，可以借助欧氏距离变换，也就是 EDT。

设 $D_{\text{obs}}(i,j)$ 表示车辆基准格子 $(i,j)$ 到最近障碍物格子的欧氏距离。也就是说：

$$
D_{\text{obs}}(i,j)

\text{车辆基准点附近最近障碍物离它有多远}
$$

注意这里的距离是从“车辆基准点”量到“最近障碍物”，还不是从“车身外轮廓”量到障碍物。

对车辆矩形而言，基准点到车身边界的最小距离近似为：

$$d_{\min }=\min \left(\frac{w}{2},l_b,l_f\right)$$

这个 $d_{\min }$ 可以理解成：从车辆基准点向任何方向出发，最快可能碰到车身边界的距离。因为车辆矩形围绕基准点展开，离基准点最近的边界可能是左侧边、右侧边、后边或前边，所以取：

$$d_{\min }=\min \left(\frac{w}{2},l_b,l_f\right)$$

基准点到最远车身角点的距离为：

$$d_{\max }=\max \left(\sqrt{l_f^2+{\left(\frac{w}{2}\right)}^2},\sqrt{l_b^2+{\left(\frac{w}{2}\right)}^2}\right)$$

这个 $d_{\max }$ 可以理解成：从车辆基准点到整车矩形最远角点的距离。也就是说，整个车身一定被包含在以车辆基准点为圆心、半径为 $d_{\max }$ 的圆里：

B ( q ) ⊆ { p ∣ ∥ p − p base ∥ ≤ d max ⁡ } \mathcal{B}(q) \subseteq \left\{ p\mid \|p-p_{\text{base}}\|\le d_{\max} \right\} B(q)⊆{p∣∥p−pbase​∥≤dmax​}

同时，以车辆基准点为圆心、半径为 $d_{\min }$ 的圆一定被包含在车身矩形内部：

{ p ∣ ∥ p − p base ∥ < d min ⁡ } ⊆ B ( q ) \left\{ p\mid \|p-p_{\text{base}}\|<d_{\min} \right\} \subseteq \mathcal{B}(q) {p∣∥p−pbase​∥<dmin​}⊆B(q)

于是有两个非常有用的快速结论。

第一种情况：障碍物足够远。

若：

$$D_{\text{obs}}(i,j)>d_{\max }$$

最近障碍物比车身任何部分都远，可以直接判定无碰撞。

原因是：整个车辆都被包在半径 $d_{\max }$ 的圆里。如果最近障碍物比这个圆还远，那么障碍物不可能碰到车身。

第二种情况：障碍物足够近。

若：

$$D_{\text{obs}}(i,j)<d_{\min }$$

最近障碍物已经进入基准点附近的必撞区域，可以直接判定碰撞。

原因是：半径 $d_{\min }$ 的圆一定在车身矩形内部。如果最近障碍物已经进入这个圆，那么它必然也进入了车身矩形。

只有中间情况才需要检查矩形采样格子：

$$d_{\min }\le D_{\text{obs}}(i,j)\le d_{\max }$$

这个中间区域的含义是：仅凭最近障碍物距离还判断不出来。障碍物可能在车身矩形内，也可能在矩形外但仍落在外接圆内。所以必须回到精确一点的做法，遍历 ${\mathcal{I}}_{\text{body}}(k)$。

来看一个具体例子。假设车辆参数为：

$$w=2.0\text{m},l_b=1.0\text{m},l_f=3.0\text{m}$$

则：

$$
d_{\min}

\min(1.0,1.0,3.0)

1.0,\text{m}
$$

$$
d_{\max}

\max
\left(
\sqrt{3.0^2+1.02},
\sqrt{1.0^2+1.02}
\right)

\sqrt{10}
\approx
3.16,\text{m}
$$

现在看三种情况。

若最近障碍物距离为：

$$D_{\text{obs}}=4.0\text{m}$$

因为：

$$4.0>d_{\max }=3.16$$

所以障碍物比车辆最远角点还远，可以直接判定无碰撞，不需要遍历车身覆盖格子。

若最近障碍物距离为：

$$D_{\text{obs}}=0.5\text{m}$$

因为：

$$0.5<d_{\min }=1.0$$

所以障碍物已经进入车身内部必然覆盖的区域，可以直接判定碰撞。

若最近障碍物距离为：

$$D_{\text{obs}}=2.0\text{m}$$

因为：

$$1.0\le 2.0\le 3.16$$

这时不能只靠距离判断。比如障碍物可能在车辆前方矩形内部，也可能在车辆斜侧方、落在外接圆内但不在矩形内。此时才需要遍历：

$$(\Delta i,\Delta j)\in {\mathcal{I}}_{\text{body}}(k)$$

逐个检查车身覆盖格子是否占据。

所以这套加速逻辑可以总结成：

$$\text{太远：直接安全}\text{太近：直接碰撞}\text{中间：再做矩形格子精查}$$

这让 Hybrid A* 能在大量节点扩展时保持较高效率。

6. 欧氏距离变换：既服务碰撞，也服务代价

欧氏距离变换，也就是 EDT，给每个非障碍格子计算到最近障碍物的距离：

$$D_{\text{obs}}(i,j)=\underset{(a,b):O(a,b)=1}{\min }r\sqrt{(i-a{)}^2+(j-b{)}^2}$$

同时还可以记录最近障碍物相对当前格子的方向角：

$${\varphi }_{\text{obs}}(i,j)=atan2(y_b-y_j,x_b-x_i)$$

这个距离图有三种用途。

第一，用于碰撞检测加速。上一节已经说明，距离太远一定无碰撞，距离太近一定碰撞。

第二，用于自适应扩展步长。离障碍物近时，车辆应当用更短的运动原语细致搜索；空旷区域可以用更长步长快速推进。

第三，用于障碍物距离代价。即使没有碰撞，贴着障碍物走也不舒服、不稳健，因此路径代价应鼓励车辆留出余量。

注意，EDT 不是简单的“障碍物膨胀”。障碍物膨胀通常只给出可行与不可行；EDT 给出连续距离，因此可以构造更细腻的软代价。

7. 运动原语：用自行车模型生成下一批节点

Hybrid A* 的每一次扩展不是走到八邻域，而是尝试若干车辆运动原语。车辆运动学采用常见的运动学自行车模型：

$$\dot{x}=v\cos \theta$$

$$\dot{y}=v\sin \theta$$

$$\dot{\theta }=\frac{v}{L}\tan \delta$$

其中 $L$ 是轴距，$\delta$ 是前轮转角。

用路径弧长 $s$ 替代时间，满足：

$$\frac{d\theta }{ds}=\frac{\tan \delta }{L}=\frac{1}{R}$$

其中：

$$R=\frac{L}{\tan \delta }$$

如果一段运动原语中转角 $\delta$ 保持不变，行驶距离为 $d$，则角度变化为：

$$\beta =\frac{d}{R}$$

当 $|\delta |$ 很小时，可以认为车辆直行：

$$x^{\prime }=x+d\cos \theta$$

$$y^{\prime }=y+d\sin \theta$$

$${\theta }^{\prime }=\theta$$

当 $|\delta |$ 不为零时，车辆沿圆弧运动，积分得到：

先把这个过程想成“沿着固定半径的圆走了一小段”。因为转角 $\delta$ 保持不变，所以转弯半径 $R$ 是常数，角速度也就恒定。此时车辆朝向满足：

$$\theta (s)={\theta }_0+\frac{s}{R}$$

这里 $s$ 是沿轨迹前进的弧长。若从初始位置开始积分，设初始位姿为 $(x_0,y_0,{\theta }_0)$，那么由几何关系可知：

$$\frac{dx}{ds}=\cos \theta (s)$$

$$\frac{dy}{ds}=\sin \theta (s)$$

把 $\theta (s)={\theta }_0+\frac{s}{R}$ 代进去，就得到：

$$x(s)=x_0+{\int }_0^d\cos \left({\theta }_0+\frac{s}{R}\right)ds$$

$$y(s)=y_0+{\int }_0^d\sin \left({\theta }_0+\frac{s}{R}\right)ds$$

现在对第一个积分做变量替换。令：

$$u={\theta }_0+\frac{s}{R}$$

则：

$$ds=Rdu$$

当 $s=0$ 时，$u={\theta }_0$；当 $s=d$ 时，$u={\theta }_0+\frac{d}{R}={\theta }_0+\beta$。于是：

$$x^{\prime }=x_0+R{\int }_{{\theta }_0}^{{\theta }_0+\beta }\cos udu$$

而：

$$\int \cos udu=\sin u$$

所以：

$$x^{\prime }=x_0+R\left(\sin ({\theta }_0+\beta )-\sin {\theta }_0\right)$$

同理，

$$y^{\prime }=y_0+R{\int }_{{\theta }_0}^{{\theta }_0+\beta }\sin udu$$

而：

$$\int \sin udu=-\cos u$$

所以：

$$y^{\prime }=y_0+R\left(\cos {\theta }_0-\cos ({\theta }_0+\beta )\right)$$

把初始角度写回 $\theta$，把初始位置写回 $x,y$，就得到源码里那组圆弧积分公式：

$$x^{\prime }=x+R\left(\sin (\theta +\beta )-\sin \theta \right)$$

$$y^{\prime }=y+R\left(\cos \theta -\cos (\theta +\beta )\right)$$

$${\theta }^{\prime }=\theta +\beta$$

如果你想检查这个结果是否合理，可以看两个极端：

当 $d=0$ 时，$\beta =0$，于是

$$x^{\prime }=x,y^{\prime }=y$$

这表示车没有移动，符合直觉。

当 $\beta$ 很小的时候，用一阶近似：

$$\sin (\theta +\beta )\approx \sin \theta +\beta \cos \theta$$

$$\cos (\theta +\beta )\approx \cos \theta -\beta \sin \theta$$

代回去可得：

$$x^{\prime }\approx x+R\beta \cos \theta =x+d\cos \theta$$

$$y^{\prime }\approx y+R\beta \sin \theta =y+d\sin \theta$$

这就退化成了直行公式。也就是说，直行并不是另一套完全不同的模型，而是圆弧模型在“转角趋近于 0”时的极限情况。

这组公式是 Hybrid A* 的运动学核心。它保证每条边都是车辆可以实际跟踪的局部轨迹。

8. 转角离散：从连续方向盘到有限动作集

这一节要解决的问题很直接：

$$\text{当前节点到底允许尝试哪些转向动作？}$$

如果不做离散化，方向盘角度是连续的，理论上有无穷多个值，A* 根本没法枚举。于是 Hybrid A* 把“连续转角”压缩成少量代表动作：

• 左转
• 直行
• 右转

更一般地说，车辆最大前轮转角为 ${\delta }_{\text{vehicle}}$。为了留出控制余量，自由空间搜索通常只使用其中一部分：

$${\delta }_{\max }=\rho {\delta }_{\text{vehicle}}$$

其中 $\rho \in (0,1]$ 是最大转角使用比例。

这一步的目的不是“把车转得更猛”，而是反过来：

$$\text{只用车辆物理上更稳、更容易跟踪的那一部分转角}$$

若转角步数为 $N_s$，则转角分辨率为：

$$\Delta \delta =\frac{{\delta }_{\max }}{N_s}$$

运动原语使用整数转角索引：

u ∈ { − N s , − N s + 1 , … , 0 , … , N s − 1 , N s } u\in\{-N_s,-N_s+1,\dots,0,\dots,N_s-1,N_s\} u∈{−Ns​,−Ns​+1,…,0,…,Ns​−1,Ns​}

对应真实转角：

$${\delta }_u=u\Delta \delta$$

每个节点最多会尝试：

$$2N_s+1$$

个前进原语。如果允许倒车，则再尝试同样数量的倒车原语。因此每个节点的最大分支数为：

$$B=\{\begin{array}{ll}2N_s+1,& \text{仅前进}\\ 2(2N_s+1),& \text{允许倒车}\end{array}$$

默认 $N_s=1$ 时，动作集非常简洁：

δ ∈ { − δ max ⁡ , 0 , + δ max ⁡ } \delta\in\{-\delta_{\max},0,+\delta_{\max}\} δ∈{−δmax​,0,+δmax​}

如果允许倒车，一个节点最多扩展 $6$ 个动作：前进左转、前进直行、前进右转、倒车左转、倒车直行、倒车右转。

所以这一节的本质不是“讲一个参数”，而是在定义 Hybrid A* 的第一层动作空间：

$$\text{每个节点有哪些可尝试的方向盘动作}$$

这个动作空间必须足够小，A* 才能算得动；又必须足够像车，路径才有意义。

一种很有用的理解方式是：

$$\text{转角离散}=\text{把连续方向盘变成有限几个代表姿态}$$

这样，搜索树每次扩展时就不是在“随机试方向”，而是在从一个有限集合里选下一个动作。

9. 扩展距离：为什么步长不能固定得太死

这一节要解决的问题是：

$$\text{选定了转角以后，这一步到底走多远？}$$

如果步长固定得太死，Hybrid A* 会有两个极端问题：

• 步长太长：狭窄通道里容易一脚跨过可行解，或者直接撞障碍物
• 步长太短：节点数量暴涨，搜索变慢

所以这里不是为了追求某个“神奇精确值”，而是在做一件更实际的事：

$$\text{让步长随场景变化，在效率和精度之间折中}$$

每个运动原语需要一个行驶距离 $d_{\text{exp}}$。

因此可以定义最小步长：

$$d_{\min }=\max \left(d_{\text{param}},1.5r\right)$$

其中 $d_{\text{param}}$ 是配置的基础扩展距离，$r$ 是栅格分辨率。乘以 $1.5$ 是为了让每次扩展至少跨过足够的栅格距离，避免原地反复落入同一索引。

这句话的意思可以再翻译一次：

$$\text{不能只走到“还在同一个格子附近”的程度，否则搜索会空转}$$

最大步长与轴距相关：

$$d_{\max }=\max \left(0.5L,d_{\min }\right)$$

若启用自适应扩展，当前节点到目标的欧氏距离为 $d_g$，到最近障碍物的距离为 $d_o$，则：

$$d_{\text{raw}}=\min \left(0.15d_g,0.3d_o\right)$$

最终扩展距离为：

$$d_{\text{exp}}=\mathrm{clip}\left(d_{\text{raw}},d_{\min },d_{\max }\right)$$

这个公式体现了两个直觉：

离目标越近，步长越短，方便精确进入目标容差。

离障碍物越近，步长越短，方便在狭窄空间里细致调整。

在开阔且离目标较远的区域，步长可以增大，提高搜索效率。

所以这一节的目的不是“算一个固定长度”，而是给每一次动作配一个合适的前进距离，让搜索既不会太笨，也不会太慢。

10. 前进、倒车与搜索方向

这一节要解决的是最现实的驾驶问题：

$$\text{这一步是前进，还是倒车？}$$

车辆起步脱困经常需要倒车，所以状态里不仅记录位姿，还记录当前运动方向：

m ∈ { forward , backward } m\in\{\text{forward},\text{backward}\} m∈{forward,backward}

运动原语距离带符号：

$$d=\{\begin{array}{ll}+d_{\text{exp}},& \text{前进}\\ -d_{\text{exp}},& \text{倒车}\end{array}$$

把这个 $d$ 代入自行车模型，就能同时得到前进和倒车的下一个位姿。

也就是说，方向不是一个附加装饰，而是运动学的一部分：

$$\text{同样的转角，同样的距离，前进和倒车会走出不同的轨迹}$$

还存在“正向搜索”和“反向搜索”的概念。正向搜索从车辆当前位置扩展到目标；反向搜索从目标扩展到车辆当前位置。两者得到的几何路径可以相同，但节点展开方向不同。反向搜索有时能更容易从道路目标处长出一条回到起点的轨迹，尤其当目标姿态约束很强时。

这一层的目的可以概括成两句话：

1. 让 Hybrid A* 能搜索包含倒车的脱困路径。
2. 让后续控制器知道每一段路是前进还是倒车。

无论搜索方向如何，最终输出都要转换成从车辆当前位置到道路回归点的顺序，并且每个路点要带上它属于前进段还是倒车段。这个方向标记很重要，因为后续会在换挡处把路径切分成多个子路径。

11. A* 的基本框架

这一节先不急着讲 Hybrid A*。先用一个普通二维栅格案例把 A* 讲清楚，再把每个概念映射回车辆规划。

设有一张小地图。S 是起点，G 是目标，X 是障碍物，. 是可通行格子：

y=4   .  .  .  .  .
y=3   .  .  .  .  .
y=2   .  .  X  .  G
y=1   .  .  X  .  .
y=0   S  .  X  .  .
      x=0 1  2  3  4

起点和目标是：

$$S=(0,0),G=(4,2)$$

障碍物是：

$$X=\{(2,0),(2,1),(2,2)\}$$

为了让例子简单，先假设普通 A* 只能上下左右移动，每走一格代价为 $1$。也就是：

$$c(n,n^{\prime })=1$$

这里的“节点”就是一个格子坐标，例如：

$$n=(x,y)$$

A* 给每个节点记三类代价：

$$f(n)=g(n)+h(n)$$

其中：

$$g(n)=\text{从起点到 }n\text{ 的累计实际代价}$$

$$h(n)=\text{从 }n\text{ 到目标的估计代价}$$

在这个案例里，$g(n)$ 很具体：从 $S$ 走到 $n$ 已经走了几格。

例如，从 $S=(0,0)$ 走到 $(1,0)$，只走了一步，所以：

$$g(1,0)=1$$

从 $S$ 走到 $(1,1)$，可能走：

$$(0,0)\to (1,0)\to (1,1)$$

所以：

$$g(1,1)=2$$

$h(n)$ 是“估计还要走多远”。在只能上下左右走的栅格里，常用曼哈顿距离：

$$h(x,y)=|x-x_G|+|y-y_G|$$

比如目标是 $G=(4,2)$，则：

$$h(1,0)=|1-4|+|0-2|=5$$

$$h(1,1)=|1-4|+|1-2|=4$$

于是：

$$f(1,0)=g(1,0)+h(1,0)=1+5=6$$

$$f(1,1)=g(1,1)+h(1,1)=2+4=6$$

$f(n)$ 的意思就是：

$$\text{如果路径经过 }n,\text{ 从起点到目标大约总共要花多少代价}$$

所以 A* 每次优先扩展 $f$ 最小的节点。它不是只看“已经走了多远”，也不是只看“离目标多近”，而是两者相加。

节点状态分三类：

$$\text{None}\to \text{Open}\to \text{Closed}$$

含义分别是：

$$\text{None}:\text{尚未发现}$$

$$\text{Open}:\text{已发现，等待扩展}$$

$$\text{Closed}:\text{已扩展，不再重复处理}$$

在这个案例里，可以这样理解：

$$\text{None}:\text{这个格子还没有被搜索看见}$$

$$\text{Open}:\text{这个格子已经被发现，但还没轮到它向外扩展}$$

$$\text{Closed}:\text{这个格子已经向外扩展过，它的邻居已经被检查}$$

下面用这个小地图走几轮 A*。

初始时，只有起点在 Open 集合中：

$$g(S)=0$$

$$h(S)=|0-4|+|0-2|=6$$

$$f(S)=6$$

此时：

Open = { S } , Closed = ∅ \text{Open}=\{S\},\qquad \text{Closed}=\varnothing Open={S},Closed=∅

第一轮，A* 从 Open 中取出 $f$ 最小的 $S$，把它放入 Closed，然后扩展它的邻居。$S=(0,0)$ 的可行邻居是：

$$(1,0),(0,1)$$

于是：

$$g(1,0)=1,h(1,0)=5,f(1,0)=6$$

$$g(0,1)=1,h(0,1)=5,f(0,1)=6$$

并记录父节点：

$$\mathrm{parent}(1,0)=S$$

$$\mathrm{parent}(0,1)=S$$

此时：

$$\text{Open}=\{(1,0),(0,1)\}$$

Closed = { S } \text{Closed}=\{S\} Closed={S}

第二轮，两个节点的 $f$ 都是 $6$，谁先扩展取决于优先队列的同价处理。假设先扩展 $(1,0)$。它的邻居有：

$$(0,0),(2,0),(1,1)$$

其中 $(0,0)$ 已经 Closed，跳过；$(2,0)$ 是障碍物，跳过；只剩：

$$(1,1)$$

于是：

$$g(1,1)=g(1,0)+1=2$$

$$h(1,1)=|1-4|+|1-2|=4$$

$$f(1,1)=6$$

并记录：

$$\mathrm{parent}(1,1)=(1,0)$$

这一步展示了 A* 的第一个关键动作：过滤不可行邻居。

对应到 Hybrid A*，过滤的内容会更复杂：

1. 检查 $n^{\prime }$ 是否越界。
2. 检查 $n^{\prime }$ 的基准格子是否为障碍物。
3. 检查完整矩形车身是否碰撞。

普通 A* 只看一个格子有没有障碍；Hybrid A* 要检查整辆车的矩形 footprint。

第三轮，再扩展 $(0,1)$。这里需要特别说明：第二轮结束时，Open 集合里至少有两个同样优先级的节点：

$$(0,1):g=1,h=5,f=6$$

$$(1,1):g=2,h=4,f=6$$

因为二者的 $f$ 值相同，A* 的主规则无法区分谁更应该先扩展。本教程这个例子采用一种常见的平局处理方式：如果 $f$ 相同，就先扩展更早进入 Open 集合的节点。$(0,1)$ 是第一轮从起点 $S$ 直接加入 Open 的，而 $(1,1)$ 是第二轮才从 $(1,0)$ 加入 Open 的，所以第三轮先研究 $(0,1)$。

如果某个实现采用另一种平局规则，例如 $f$ 相同时优先选择 $h$ 更小、看起来离目标更近的节点，那么第三轮也可能先扩展 $(1,1)$。这不改变 A* 的核心思想，只会影响同分节点的展开顺序。

现在继续看 $(0,1)$ 的扩展。它的可行邻居里可能也能到达 $(1,1)$：

$$(0,1)\to (1,1)$$

候选代价为：

$$g_{\text{new}}(1,1)=g(0,1)+1=2$$

但是 $(1,1)$ 之前已经有旧代价：

$$g_{\text{old}}(1,1)=2$$

新旧相同，不更好，所以不需要更新父节点。

如果某次出现：

$$g_{\text{new}}(n^{\prime })<g_{\text{old}}(n^{\prime })$$

才会更新：

$$g(n^{\prime })\leftarrow g_{\text{new}}(n^{\prime })$$

$$f(n^{\prime })\leftarrow g(n^{\prime })+h(n^{\prime })$$

$$\mathrm{parent}(n^{\prime })\leftarrow n$$

这个动作叫“松弛”。它的意思很朴素：

$$\text{如果我发现了一条到达某节点更便宜的路，就改用这条路}$$

为了把这个例子完整走完，我们继续沿着同一张小地图展开。下面给出一种可能的 A* 展开顺序。注意：当多个节点 $f$ 相同时，不同实现的优先队列可能选择不同节点；这里选择其中一种顺序来讲清楚过程，不影响 A* 的核心逻辑。

为了让表格更紧凑，记号写成：

$$(x,y):g,h,f$$

例如：

$$(1,0):1,5,6$$

表示：

$$g(1,0)=1,h(1,0)=5,f(1,0)=6$$

完整过程如下。

轮次	从 Open 取出	主要检查与更新
初始化	无	Open 只有 $S:0,6,6$
1	$S=(0,0)$	加入 $(1,0):1,5,6$，加入 $(0,1):1,5,6$；它们的 parent 都是 $S$
2	$(1,0)$	$(0,0)$ 已 Closed，跳过；$(2,0)$ 是障碍物，跳过；加入 $(1,1):2,4,6$，parent 为 $(1,0)$
3	$(0,1)$	与 $(1,1)$ 同为 $f=6$，但 $(0,1)$ 更早进入 Open，所以本例先取它；$(0,0)$ 已 Closed，跳过；到 $(1,1)$ 的新代价仍是 $2$，不更优；加入 $(0,2):2,4,6$，parent 为 $(0,1)$
4	$(1,1)$	$(2,1)$ 是障碍物，跳过；加入 $(1,2):3,3,6$，parent 为 $(1,1)$
5	$(0,2)$	到 $(1,2)$ 的新代价仍是 $3$，不更优；加入 $(0,3):3,5,8$，parent 为 $(0,2)$
6	$(1,2)$	$(2,2)$ 是障碍物，跳过；加入 $(1,3):4,4,8$，parent 为 $(1,2)$
7	$(0,3)$	到 $(1,3)$ 的新代价仍是 $4$，不更优；加入 $(0,4):4,6,10$，parent 为 $(0,3)$
8	$(1,3)$	加入 $(2,3):5,3,8$，parent 为 $(1,3)$；加入 $(1,4):5,5,10$
9	$(2,3)$	$(2,2)$ 是障碍物，跳过；加入 $(3,3):6,2,8$，parent 为 $(2,3)$；加入 $(2,4):6,4,10$
10	$(3,3)$	加入 $(4,3):7,1,8$，parent 为 $(3,3)$；加入 $(3,2):7,1,8$；加入 $(3,4):7,3,10$
11	$(4,3)$	发现目标 $G=(4,2):8,0,8$，parent 为 $(4,3)$；但目标此时只是进入 Open
12	$G=(4,2)$	目标从 Open 中被取出，判定到达，搜索结束

这张表里有几个关键细节。

第一，A* 不是看到障碍物就停，而是绕过障碍物。比如 $(2,0)$、$(2,1)$、$(2,2)$ 都是障碍物，所以搜索自然被迫走到上方通道：

$$(1,3)\to (2,3)\to (3,3)$$

第二，一个节点可能被不同路径再次发现。例如第 3 轮，$(0,1)$ 也可以走到 $(1,1)$：

$$(0,1)\to (1,1)$$

但候选代价是：

$$g_{\text{new}}(1,1)=g(0,1)+1=2$$

而旧代价已经是：

$$g_{\text{old}}(1,1)=2$$

所以不更新 parent。若某次真的发现更便宜路线，比如：

$$g_{\text{old}}(C)=7$$

但新路线为：

$$g_{\text{new}}(C)=g(n)+c(n,C)=4+1=5$$

因为：

$$5<7$$

A* 才会把 $C$ 的父节点改成 $n$，并重新计算 $f(C)$。这就是“松弛”的具体含义。

第三，目标不是一被发现就立刻结束。第 11 轮只是把 $G$ 放进 Open：

$$G:8,0,8$$

第 12 轮，当 $G$ 成为当前 $f$ 最小节点并被取出时，才判定搜索完成。这与源码中的逻辑一致：

$$\text{pop from Open}\to \text{isGoal}\to \text{setPath}$$

搜索结束后，靠 parent 回溯路径。根据上表，目标的父链是：

$$G\leftarrow (4,3)\leftarrow (3,3)\leftarrow (2,3)\leftarrow (1,3)\leftarrow (1,2)\leftarrow (1,1)\leftarrow (1,0)\leftarrow S$$

把它反过来，就得到完整路径：

$$S\to (1,0)\to (1,1)\to (1,2)\to (1,3)\to (2,3)\to (3,3)\to (4,3)\to G$$

这就是从起点到终点的完整 A* 案例。你可以看到，A* 的每一步都只有三件事：

1. 从 Open 里取出当前最值得扩展的节点。
2. 检查它的邻居，过滤障碍物和无效节点。
3. 对有效邻居更新 $g$、$f$、parent，再放回 Open。

这就是 A* 的核心循环：

1. 把起点放入 Open 集合，令 $g(s)=0$。
2. 从 Open 集合里取出 $f$ 最小的节点 $n$。
3. 如果 $n$ 是目标，结束搜索。
4. 把 $n$ 标记为 Closed，表示它已经扩展过。
5. 枚举 $n$ 的所有后继 $n^{\prime }$。
6. 对每个 $n^{\prime }$ 做越界、障碍物、碰撞等检查。
7. 用 $g_{\text{new}}(n^{\prime })=g(n)+c(n,n^{\prime })$ 判断是否找到更好的到达方式。
8. 如果更好，就更新 $g$、$f$ 和父节点，并把 $n^{\prime }$ 放回 Open 集合。
9. 如果 Open 集合为空还没找到目标，则搜索失败。

再用一个更小的选择例子，看 $g$、$h$、$f$ 如何影响扩展顺序。假设 Open 里有两个候选节点 $A$ 和 $B$：

$$g(A)=2,h(A)=8$$

$$g(B)=5,h(B)=2$$

则：

$$f(A)=g(A)+h(A)=10$$

$$f(B)=g(B)+h(B)=7$$

虽然 $B$ 已经走过的路更贵，因为 $g(B)>g(A)$，但它离目标估计更近，所以总估计代价更小：

$$f(B)<f(A)$$

A* 会优先扩展 $B$。这说明 A* 不是简单贪图“眼前便宜”，也不是只看“离目标近”，而是在两者之间做平衡。

如果令：

$$h(n)=0$$

A* 就退化成 Dijkstra：

$$f(n)=g(n)$$

如果让 $h(n)$ 占主导，搜索会更接近贪心。Hybrid A* 中的启发权重 $w_h$ 就是在调节这种平衡：

$$f(n)=g(n)+w_{h}h(n)$$

$w_h$ 越大，搜索越愿意朝目标方向冲；$w_h$ 越小，搜索越像均匀探索，通常更稳但更慢。

现在把这个二维小案例映射回 Hybrid A*。

普通 A* 里，后继节点来自相邻格子：

$$(x,y)\to (x+1,y)$$

$$(x,y)\to (x,y+1)$$

Hybrid A* 里，后继节点来自车辆动作。当前车辆位姿为：

$$q=(x,y,\theta )$$

动作由两个量组成：

$$a=({\delta }_u,d)$$

其中 ${\delta }_u$ 是离散转角，$d$ 是带符号行驶距离。运动模型给出：

$$q^{\prime }=F(q,{\delta }_u,d)$$

所以 Hybrid A* 的“邻居”不是旁边那个格子，而是：

$$\text{车辆按某个转角前进或倒车一小段后到达的新位姿}$$

一次扩展可以理解为下面这条链：

$$\text{取出当前节点}\to \text{选择前进或倒车}\to \text{选择一个转角}\to \text{用自行车模型积分出新位姿}\to \text{把新位姿离散成 }(i,j,k)\to \text{检查碰撞}\to \text{计算 }g,h,f\to \text{更新 Open 集合}$$

然后将 $q^{\prime }$ 离散化：

$$\mathrm{index}(q^{\prime })=(i^{\prime },j^{\prime },k^{\prime })$$

如果这个索引越界、落在障碍物上，或者完整车身矩形发生碰撞，就丢弃该候选：

$$\mathrm{invalid}(q^{\prime })=1\Rightarrow \text{discard}$$

否则继续计算：

$$g(q^{\prime })=g(q)+C(q,a,q^{\prime })$$

$$f(q^{\prime })=g(q^{\prime })+h(q^{\prime })$$

这里的 $C(q,a,q^{\prime })$ 不只是距离，还包含转弯、倒车、换挡、障碍物距离和目标对齐等代价。于是 Hybrid A* 的 A* 框架没变，变化的是节点生成方式和边代价定义。

最后再看 Open、Closed、parent 在 Hybrid A* 里的具体含义：

$$\text{Open}:\text{已经生成，但还没展开车辆动作的位姿节点}$$

$$\text{Closed}:\text{已经展开过车辆动作的位姿节点}$$

$$\mathrm{parent}(q^{\prime }):\text{哪一个旧位姿通过哪次车辆动作到达了 }{q}^{\prime }$$

还有一个容易忽略的小细节：当车辆刚从前进切到倒车，或刚从倒车切到前进时，搜索会避免立刻沿着同一个转角原路退回父节点。否则搜索会浪费大量节点在“刚走出去又走回来”的局部循环上。用直觉说，就是不要把第一步扩展用来撤销上一步动作。

后续内容：从自由空间脱困到回归车道：Hybrid A* Freespace Pull Out 算法教程（2）