前面内容：从自由空间脱困到回归车道：Hybrid A* Freespace Pull Out 算法教程（1）
12. 启发函数：让搜索既懂障碍物，也懂车辆转弯
好的启发函数 h ( n ) h(n) h(n) 能显著减少搜索量。自由空间 Hybrid A* 使用两个互补的距离估计。
第一个是栅格自由空间距离。以目标格子为起点，在二维栅格上向外传播距离，只穿过非障碍且有基本横向净空的格子。传播时，格子至少要满足：
O ( i , j ) = 0 且 D obs ( i , j ) ≥ w 2 O(i,j)=0 \quad\text{且}\quad D_{\text{obs}}(i,j)\ge\frac{w}{2} O(i,j)=0且Dobs(i,j)≥2w
这条公式的目的，是在生成二维自由空间距离图时，先判断某个格子 ( i , j ) (i,j) (i,j) 能不能作为"可传播"的自由格子。它不是在判断某个位姿下整辆车一定能不能通过，而是在做一个比较便宜、比较保守的初筛。
逐项解释如下：
O ( i , j ) = 0 O(i,j)=0 O(i,j)=0
表示格子 ( i , j ) (i,j) (i,j) 本身不是障碍物。这里的 O ( i , j ) O(i,j) O(i,j) 是由占据栅格地图得到的二值结果：
O ( i , j ) = { 1 , 格子被认为是障碍物或未知区域 0 , 格子被认为是可通行区域 O(i,j)= \begin{cases} 1, & \text{格子被认为是障碍物或未知区域} \\ 0, & \text{格子被认为是可通行区域} \end{cases} O(i,j)={1,0,格子被认为是障碍物或未知区域格子被认为是可通行区域
所以 O ( i , j ) = 0 O(i,j)=0 O(i,j)=0 只说明"车辆参考点落在这个格子中心时，参考点没有直接压到障碍物"。但车辆不是一个点，而是有宽度的矩形。如果只检查参考点，可能出现这种情况：
参考点没撞障碍物，但车身侧面已经刮到障碍物 \text{参考点没撞障碍物，但车身侧面已经刮到障碍物} 参考点没撞障碍物，但车身侧面已经刮到障碍物
因此还要看第二个条件：
D obs ( i , j ) ≥ w 2 D_{\text{obs}}(i,j)\ge\frac{w}{2} Dobs(i,j)≥2w
其中 D obs ( i , j ) D_{\text{obs}}(i,j) Dobs(i,j) 表示格子 ( i , j ) (i,j) (i,j) 到最近障碍物的欧氏距离， w w w 是车辆宽度， w 2 \frac{w}{2} 2w 是车辆半宽。
可以把车辆参考点想象成车辆中心线上的一个点。即使这个点本身在自由格子里，车身还会向左右各占据半个车宽。如果最近障碍物距离参考点连半个车宽都不到，那么无论车头方向如何变化，这个格子附近都非常危险，至少在二维粗略判断里不应该把它当成很舒服的自由空间。
举个数字例子。假设车辆宽度为：
w = 2.0 m w=2.0\ \text{m} w=2.0 m
那么车辆半宽为：
w 2 = 1.0 m \frac{w}{2}=1.0\ \text{m} 2w=1.0 m
如果某个格子到最近障碍物的距离是：
D obs ( i , j ) = 0.6 m D_{\text{obs}}(i,j)=0.6\ \text{m} Dobs(i,j)=0.6 m
虽然这个格子本身可能满足：
O ( i , j ) = 0 O(i,j)=0 O(i,j)=0
也就是格子中心不是障碍物，但由于：
0.6 < 1.0 0.6<1.0 0.6<1.0
说明最近障碍物已经进入车辆半宽范围内。对于二维自由空间距离传播来说，这个格子就不应该继续传播。
反过来，如果：
D obs ( i , j ) = 1.4 m D_{\text{obs}}(i,j)=1.4\ \text{m} Dobs(i,j)=1.4 m
则：
1.4 ≥ 1.0 1.4\ge1.0 1.4≥1.0
说明这个格子至少在横向上留出了一个车辆半宽的基本净空，可以作为二维距离图的可传播格子。
所以这条公式可以读成一句话：
只有当格子本身不是障碍物，并且它离最近障碍物至少有半个车宽时，才允许二维距离向这里传播。 \text{只有当格子本身不是障碍物，并且它离最近障碍物至少有半个车宽时，才允许二维距离向这里传播。} 只有当格子本身不是障碍物，并且它离最近障碍物至少有半个车宽时，才允许二维距离向这里传播。
需要特别注意：这个条件仍然不是最终碰撞检测。它只考虑二维格子和最近障碍物距离，没有考虑车辆长度、车辆朝向、前后悬、旋转后的矩形 footprint 等因素。真正扩展 Hybrid A* 节点时，还要对完整车身矩形做碰撞检查。这里的作用只是让启发函数不要鼓励搜索穿过明显太窄、太贴近障碍物的区域。
对八邻域扩展而言，若从 ( i , j ) (i,j) (i,j) 到邻居 ( i + Δ i , j + Δ j ) (i+\Delta i,j+\Delta j) (i+Δi,j+Δj)，代价为：
c grid = r ∣ Δ i ∣ + ∣ Δ j ∣ c_{\text{grid}}= r\sqrt{|\Delta i|+|\Delta j|} cgrid=r∣Δi∣+∣Δj∣
这里每个变量的含义是：
c grid : 从当前格子走到邻居格子的二维栅格代价 c_{\text{grid}}: \text{从当前格子走到邻居格子的二维栅格代价} cgrid:从当前格子走到邻居格子的二维栅格代价
r : 栅格地图分辨率，也就是一个格子的实际边长 r: \text{栅格地图分辨率，也就是一个格子的实际边长} r:栅格地图分辨率，也就是一个格子的实际边长
Δ i , Δ j : 邻居格子相对当前格子的索引偏移 \Delta i,\Delta j: \text{邻居格子相对当前格子的索引偏移} Δi,Δj:邻居格子相对当前格子的索引偏移
在八邻域里， Δ i \Delta i Δi 和 Δ j \Delta j Δj 通常只会取 − 1 , 0 , 1 -1,0,1 −1,0,1。如果从 ( i , j ) (i,j) (i,j) 走到右侧格子 ( i + 1 , j ) (i+1,j) (i+1,j)，则：
Δ i = 1 , Δ j = 0 \Delta i=1,\quad \Delta j=0 Δi=1,Δj=0
所以：
∣ Δ i ∣ + ∣ Δ j ∣ = 1 |\Delta i|+|\Delta j|=1 ∣Δi∣+∣Δj∣=1
代价为：
c grid = r c_{\text{grid}}=r cgrid=r
这就是水平或竖直移动一个格子的代价。
如果从 ( i , j ) (i,j) (i,j) 走到右上角格子 ( i + 1 , j + 1 ) (i+1,j+1) (i+1,j+1)，则：
Δ i = 1 , Δ j = 1 \Delta i=1,\quad \Delta j=1 Δi=1,Δj=1
所以：
∣ Δ i ∣ + ∣ Δ j ∣ = 2 |\Delta i|+|\Delta j|=2 ∣Δi∣+∣Δj∣=2
代价为：
c grid = r 2 c_{\text{grid}}=r\sqrt{2} cgrid=r2
这就是对角移动一个格子的代价。这个公式本质上是在用欧氏距离近似格子之间的真实距离：直走一格是 r r r，斜走一格是对角线长度 2 r \sqrt{2}r 2 r。
举个数字例子。若地图分辨率为：
r = 0.2 m r=0.2\ \text{m} r=0.2 m
水平走一格的代价为：
c grid = 0.2 m c_{\text{grid}}=0.2\ \text{m} cgrid=0.2 m
对角走一格的代价为：
c grid = 0.2 2 ≈ 0.283 m c_{\text{grid}}=0.2\sqrt{2}\approx0.283\ \text{m} cgrid=0.22 ≈0.283 m
传播得到：
D free ( i , j ) = 从 ( i , j ) 到目标的二维自由空间最短距离估计 D_{\text{free}}(i,j)= \text{从 }(i,j)\text{ 到目标的二维自由空间最短距离估计} Dfree(i,j)=从 (i,j) 到目标的二维自由空间最短距离估计
这里的 D free ( i , j ) D_{\text{free}}(i,j) Dfree(i,j) 可以理解成一张"到目标还要绕多远"的二维表。它从目标格子开始向外传播，所以目标格子的值为：
D free ( i g , j g ) = 0 D_{\text{free}}(i_g,j_g)=0 Dfree(ig,jg)=0
离目标越远，或者中间需要绕开障碍物， D free D_{\text{free}} Dfree 就越大。
例如目标在 ( 4 , 2 ) (4,2) (4,2)，当前格子在 ( 0 , 0 ) (0,0) (0,0)。如果没有障碍物，二维最短距离大约是：
( 4 r ) 2 + ( 2 r ) 2 \sqrt{(4r)^2+(2r)^2} (4r)2+(2r)2
但如果中间有障碍物墙，传播不能穿墙，只能绕路，那么 D free ( 0 , 0 ) D_{\text{free}}(0,0) Dfree(0,0) 会变大。它知道障碍物分布，但不知道车辆朝向和最小转弯半径，所以它只能回答：
从地图上看，这个格子离目标绕路还有多远？ \text{从地图上看，这个格子离目标绕路还有多远？} 从地图上看，这个格子离目标绕路还有多远？
它不能回答：
车头朝这个方向时，车辆能不能用最小转弯半径开过去？ \text{车头朝这个方向时，车辆能不能用最小转弯半径开过去？} 车头朝这个方向时，车辆能不能用最小转弯半径开过去？
第二个是 Reeds-Shepp 距离。在直接阅读公式之前，需要先理解它解决的是什么问题。
12.1 为什么只有欧氏距离还不够
假设停车场里有一辆车。车辆当前位置和目标位置只相差：
2.0 m 2.0\ \text{m} 2.0 m
如果把车辆当成一个可以任意移动的小圆点，那么最短路径就是一条直线，距离也是：
2.0 m 2.0\ \text{m} 2.0 m
但真实车辆不能像棋子一样直接横向平移。车辆只能沿车头方向前进或倒车，并通过转动前轮逐渐改变车头朝向。
例如，车辆当前位姿为：
q = ( 0 , 0 , 0 ∘ ) q=(0,\ 0,\ 0^\circ) q=(0, 0, 0∘)
目标位姿为：
q g = ( 0 , 2 , 0 ∘ ) q_g=(0,\ 2,\ 0^\circ) qg=(0, 2, 0∘)
两个位置之间的欧氏距离只有：
D euclidean = ( 0 − 0 ) 2 + ( 2 − 0 ) 2 = 2.0 m D_{\text{euclidean}}=\sqrt{(0-0)^2+(2-0)^2}=2.0\ \text{m} Deuclidean=(0−0)2+(2−0)2 =2.0 m
可是目标点位于车辆正左侧，车辆不能原地向左平移 2.0 m 2.0\,\text{m} 2.0m。它可能需要先转弯前进，再倒车调整，最后把车头摆回目标方向。真实可行路径长度显然大于 2.0 m 2.0\,\text{m} 2.0m。
所以，位置之间的欧氏距离只能回答：
两点在地图上隔多远？ \text{两点在地图上隔多远？} 两点在地图上隔多远？
它不能回答：
一辆不能横移、转弯半径有限的车，真正至少要开多远？ \text{一辆不能横移、转弯半径有限的车，真正至少要开多远？} 一辆不能横移、转弯半径有限的车，真正至少要开多远？
Reeds-Shepp 距离就是为第二个问题服务的。
12.2 Reeds-Shepp 距离是什么
Reeds-Shepp 距离描述的是：在暂时不考虑障碍物的情况下，一辆满足以下约束的车辆，从起始位姿 q q q 到目标位姿 q g q_g qg，最短至少需要行驶多长的路径。
车辆约束包括：
- 车辆不能横向平移。
- 车辆的转弯半径不能无限小。
- 车辆既可以前进，也可以倒车。
- 起点和终点不仅有位置，还有车头朝向。
这里的"位姿"不是只有位置 ( x , y ) (x,y) (x,y)，而是：
q = ( x , y , θ ) q=(x,y,\theta) q=(x,y,θ)
其中：
x , y : 车辆基准点的位置 x,y: \text{车辆基准点的位置} x,y:车辆基准点的位置
θ : 车辆朝向 \theta: \text{车辆朝向} θ:车辆朝向
这意味着同一个停车位，如果要求车辆最终朝向不同，Reeds-Shepp 距离也会不同。
举个直观例子。假设车辆已经位于目标停车位中心：
( x , y ) = ( x g , y g ) (x,y)=(x_g,y_g) (x,y)=(xg,yg)
但车辆朝向与目标朝向相反：
θ = θ g + 180 ∘ \theta=\theta_g+180^\circ θ=θg+180∘
此时欧氏距离为：
D euclidean = 0 D_{\text{euclidean}}=0 Deuclidean=0
但车辆显然还没有完成任务。它必须开出去并进行若干次前进、倒车和转向，才能把车头转到正确方向。所以：
D RS > 0 D_{\text{RS}}>0 DRS>0
这正是 Reeds-Shepp 距离比欧氏距离更适合车辆路径规划的原因：它同时考虑位置、朝向和转弯能力。
12.3 Reeds-Shepp 路径由哪些基本动作组成
为了描述车辆能够执行的运动，可以先准备三种几何片段：
Left arc , Right arc , Straight \text{Left arc},\quad \text{Right arc},\quad \text{Straight} Left arc,Right arc,Straight
它们分别表示：
Left arc : 按固定半径向左转的一段圆弧 \text{Left arc}: \text{按固定半径向左转的一段圆弧} Left arc:按固定半径向左转的一段圆弧
Right arc : 按固定半径向右转的一段圆弧 \text{Right arc}: \text{按固定半径向右转的一段圆弧} Right arc:按固定半径向右转的一段圆弧
Straight : 沿当前车头方向行驶的一段直线 \text{Straight}: \text{沿当前车头方向行驶的一段直线} Straight:沿当前车头方向行驶的一段直线
通常也会把它们简写成：
L , R , S L,\quad R,\quad S L,R,S
每一段既可以前进执行，也可以倒车执行。当前进和倒车发生切换时，就会出现一个换挡点，也常称为 cusp。
例如，一条候选路径可以是：
L → S → R L\rightarrow S\rightarrow R L→S→R
表示先沿左转圆弧行驶，再直行，最后沿右转圆弧行驶。
另一条候选路径可能包含换挡：
L forward → R backward → L forward L_{\text{forward}} \rightarrow R_{\text{backward}} \rightarrow L_{\text{forward}} Lforward→Rbackward→Lforward
表示先向前左转，再换挡倒车右转，最后再次换挡向前左转。这类路径很像真实驾驶中的多次挪车。
把不同的 L L L、 R R R、 S S S 片段按照允许的规则组合起来，就得到不同的 Reeds-Shepp 路径族。所谓"路径族"，可以理解为不同的动作模板，例如：
L → S → R L\rightarrow S\rightarrow R L→S→R
R → S → L R\rightarrow S\rightarrow L R→S→L
L → R → L L\rightarrow R\rightarrow L L→R→L
那么这些模板集合是怎么来的？它不是从地图里搜索出来的，也不是运行时随便排列 L L L、 R R R、 S S S 得到的，而是来自车辆最短路径问题本身。
在不考虑障碍物时，车辆可以用下面的简化运动模型描述：
x ˙ = v cos θ \dot{x}=v\cos\theta x˙=vcosθ
y ˙ = v sin θ \dot{y}=v\sin\theta y˙=vsinθ
θ ˙ = v κ \dot{\theta}=v\kappa θ˙=vκ
其中：
v ∈ { − 1 , + 1 } v\in\{-1,+1\} v∈{−1,+1}
表示车辆可以倒车或前进；
κ ∈ − κ max , κ max \kappa\in-\\kappa_{\\max},\\kappa_{\\max} κ∈−κmax,κmax
表示车辆曲率受限，不能无限急转。曲率和转弯半径的关系是：
κ = 1 R \kappa=\frac{1}{R} κ=R1
如果要最小化行驶长度，最优路径通常不会使用"半左不左、半右不右"的连续变化转角，而会落在几类极端动作上：
κ = + κ max \kappa=+\kappa_{\max} κ=+κmax
对应最大左转圆弧 L L L；
κ = − κ max \kappa=-\kappa_{\max} κ=−κmax
对应最大右转圆弧 R R R；
κ = 0 \kappa=0 κ=0
对应直线段 S S S。
也就是说，最短路径可以看成由下面三种基本片段拼接：
{ L , R , S } \{L,\ R,\ S\} {L, R, S}
再加上 v v v 可以从 + 1 +1 +1 切换到 − 1 -1 −1，或者从 − 1 -1 −1 切换到 + 1 +1 +1，就允许路径中出现换挡点。因此 Reeds-Shepp 路径族可以理解为：
有限个由 L , R , S 和换挡点组成的最短路径模板 \text{有限个由 }L,R,S\text{ 和换挡点组成的最短路径模板} 有限个由 L,R,S 和换挡点组成的最短路径模板
更进一步，Reeds 和 Shepp 的理论结果说明：不需要枚举任意长的动作序列。最短路径一定属于一组有限的候选形式。常见实现会把这些形式整理成一个模板集合，例如：
Σ = { L S L , L S R , R S L , R S R , L R L , R L R , ... } \Sigma= \{ LSL,\ LSR,\ RSL,\ RSR,\ LRL,\ RLR,\ \dots \} Σ={LSL, LSR, RSL, RSR, LRL, RLR, ...}
这里的省略号表示包含前进/倒车切换的更多模板。不同库可能采用不同的等价压缩方式，但思想相同：先利用理论结果准备一张有限的"路径模板表"，然后对每个模板求出具体的圆弧角度和直线长度。
比如模板：
σ = L S R \sigma=LSR σ=LSR
不是一条已经确定长度的路径，而是一种路径形状：
先左转圆弧 → 再直线 → 最后右转圆弧 \text{先左转圆弧} \rightarrow \text{再直线} \rightarrow \text{最后右转圆弧} 先左转圆弧→再直线→最后右转圆弧
真正计算时，还要根据当前位姿 q q q 和目标位姿 q g q_g qg 解出：
α 1 , l , α 2 \alpha_1,\quad l,\quad \alpha_2 α1,l,α2
分别表示第一段左转圆弧角度、中间直线长度、最后右转圆弧角度。如果某个模板解不出来，或者得到的段长不满足约束，就丢弃；如果解得出来，就计算它的总长度。
所以 Σ \Sigma Σ 的来源可以概括为：
车辆运动学约束 → 最短路径只需由 L , R , S 和换挡组成 → 理论上可归纳为有限个候选模板 → Σ \text{车辆运动学约束} \rightarrow \text{最短路径只需由 }L,R,S\text{ 和换挡组成} \rightarrow \text{理论上可归纳为有限个候选模板} \rightarrow \Sigma 车辆运动学约束→最短路径只需由 L,R,S 和换挡组成→理论上可归纳为有限个候选模板→Σ
Reeds-Shepp 求解器会尝试满足约束的候选模板，并从中选出总长度最短的可行组合。
12.4 现在再引入 Reeds-Shepp 距离公式
有了前面的直觉，就可以阅读数学表达了。如果用于估计的平均转弯半径为 R avg R_{\text{avg}} Ravg，可以把 Reeds-Shepp 距离抽象写成：
D RS ( q , q g ) = R avg min σ ∈ Σ ( ∑ arc a ∈ σ ∣ α a ∣ + ∑ straight s ∈ σ ∣ l s ∣ R avg ) D_{\text{RS}}(q,q_g)= R_{\text{avg}} \min_{\sigma\in\Sigma} \left( \sum_{\text{arc }a\in\sigma}|\alpha_a|+ \sum_{\text{straight }s\in\sigma}\frac{|l_s|}{R_{\text{avg}}} \right) DRS(q,qg)=Ravgσ∈Σmin arc a∈σ∑∣αa∣+straight s∈σ∑Ravg∣ls∣
这条公式看起来复杂，但可以拆成一句话：
在所有可能的 Reeds-Shepp 路径形状里，选总长度最短的那一条。 \text{在所有可能的 Reeds-Shepp 路径形状里，选总长度最短的那一条。} 在所有可能的 Reeds-Shepp 路径形状里，选总长度最短的那一条。
逐个变量解释如下：
D RS ( q , q g ) : 从当前位姿 q 到目标位姿 q g 的 Reeds-Shepp 距离 D_{\text{RS}}(q,q_g): \text{从当前位姿 }q\text{ 到目标位姿 }q_g\text{ 的 Reeds-Shepp 距离} DRS(q,qg):从当前位姿 q 到目标位姿 qg 的 Reeds-Shepp 距离
q = ( x , y , θ ) : 当前车辆位姿 q=(x,y,\theta): \text{当前车辆位姿} q=(x,y,θ):当前车辆位姿
q g = ( x g , y g , θ g ) : 目标车辆位姿 q_g=(x_g,y_g,\theta_g): \text{目标车辆位姿} qg=(xg,yg,θg):目标车辆位姿
R avg : 用于估计的平均转弯半径 R_{\text{avg}}: \text{用于估计的平均转弯半径} Ravg:用于估计的平均转弯半径
Σ : 所有候选 Reeds-Shepp 路径族的集合 \Sigma: \text{所有候选 Reeds-Shepp 路径族的集合} Σ:所有候选 Reeds-Shepp 路径族的集合
这里的 Σ \Sigma Σ 就是上面说的"有限模板表"。它不是障碍物地图产生的，也不是 Hybrid A* 在栅格里一步步搜索出来的，而是 Reeds-Shepp 几何最短路径理论提前给出的候选动作模式集合。
σ : 其中某一种具体路径形状，例如左转-直行-右转 \sigma: \text{其中某一种具体路径形状，例如左转-直行-右转} σ:其中某一种具体路径形状，例如左转-直行-右转
α a : 某段圆弧的转过角度，单位通常是弧度 \alpha_a: \text{某段圆弧的转过角度，单位通常是弧度} αa:某段圆弧的转过角度，单位通常是弧度
l s : 某段直线的长度 l_s: \text{某段直线的长度} ls:某段直线的长度
为什么圆弧部分写成 R avg ∣ α a ∣ R_{\text{avg}}|\alpha_a| Ravg∣αa∣？因为圆弧长度满足：
圆弧长度 = 半径 × 圆心角 \text{圆弧长度}= \text{半径}\times\text{圆心角} 圆弧长度=半径×圆心角
也就是：
l arc = R avg ∣ α a ∣ l_{\text{arc}}=R_{\text{avg}}|\alpha_a| larc=Ravg∣αa∣
公式里把括号内部写成"角度 + 归一化直线长度"，最后再乘以 R avg R_{\text{avg}} Ravg，只是为了把圆弧段和直线段统一到同一个尺度里。
举个简化例子。假设某条候选路径由三段组成：
左转 30 ∘ → 直行 2.0 m → 右转 30 ∘ \text{左转 }30^\circ \rightarrow \text{直行 }2.0\ \text{m} \rightarrow \text{右转 }30^\circ 左转 30∘→直行 2.0 m→右转 30∘
且：
R avg = 5.0 m R_{\text{avg}}=5.0\ \text{m} Ravg=5.0 m
因为：
30 ∘ = π 6 30^\circ=\frac{\pi}{6} 30∘=6π
两段圆弧长度合计为：
5.0 ⋅ π 6 + 5.0 ⋅ π 6 = 5.0 ⋅ π 3 ≈ 5.24 m 5.0\cdot\frac{\pi}{6}+5.0\cdot\frac{\pi}{6}= 5.0\cdot\frac{\pi}{3} \approx5.24\ \text{m} 5.0⋅6π+5.0⋅6π=5.0⋅3π≈5.24 m
再加上直线段：
D RS ≈ 5.24 + 2.0 = 7.24 m D_{\text{RS}}\approx5.24+2.0=7.24\ \text{m} DRS≈5.24+2.0=7.24 m
真实 Reeds-Shepp 求解会枚举更多路径族，然后取最短的那个。这个距离不看障碍物，它只关心"按照车辆转弯半径，从姿态上最短需要多长"。
平均转弯半径可以由平均转角得到。若：
Δ δ = δ max N s \Delta\delta=\frac{\delta_{\max}}{N_s} Δδ=Nsδmax
则可取：
δ avg = Δ δ + δ max − Δ δ 2 \delta_{\text{avg}}= \Delta\delta+ \frac{\delta_{\max}-\Delta\delta}{2} δavg=Δδ+2δmax−Δδ
R avg = L tan δ avg R_{\text{avg}}= \frac{L}{\tan\delta_{\text{avg}}} Ravg=tanδavgL
这里的变量含义是：
δ max : 搜索允许使用的最大前轮转角 \delta_{\max}: \text{搜索允许使用的最大前轮转角} δmax:搜索允许使用的最大前轮转角
N s : 转角离散步数 N_s: \text{转角离散步数} Ns:转角离散步数
Δ δ : 相邻两个离散转角之间的角度间隔 \Delta\delta: \text{相邻两个离散转角之间的角度间隔} Δδ:相邻两个离散转角之间的角度间隔
δ avg : 用于估计 Reeds-Shepp 距离的代表性转角 \delta_{\text{avg}}: \text{用于估计 Reeds-Shepp 距离的代表性转角} δavg:用于估计 Reeds-Shepp 距离的代表性转角
L : 车辆轴距 L: \text{车辆轴距} L:车辆轴距
R avg : 由代表性转角换算得到的平均转弯半径 R_{\text{avg}}: \text{由代表性转角换算得到的平均转弯半径} Ravg:由代表性转角换算得到的平均转弯半径
这个半径来自自行车模型的基本关系：
R = L tan δ R=\frac{L}{\tan\delta} R=tanδL
转角越大， tan δ \tan\delta tanδ 越大，转弯半径 R R R 越小；转角越小，车辆越不容易转弯， R R R 越大。
例如：
L = 2.8 m , δ avg = 20 ∘ L=2.8\ \text{m},\quad \delta_{\text{avg}}=20^\circ L=2.8 m,δavg=20∘
则：
R avg = 2.8 tan 20 ∘ ≈ 7.69 m R_{\text{avg}}= \frac{2.8}{\tan20^\circ} \approx7.69\ \text{m} Ravg=tan20∘2.8≈7.69 m
这意味着在 Reeds-Shepp 估计里，车辆大致按 7.69 m 7.69\,\text{m} 7.69m 的转弯半径来计算姿态连接难度。
Reeds-Shepp 距离知道车辆运动学，但不知道障碍物。
因此启发函数取两者较大值：
h ( q ) = w h max ( D free ( i , j ) , D RS ( q , q g ) ) h(q)= w_h \max \left( D_{\text{free}}(i,j),\ D_{\text{RS}}(q,q_g) \right) h(q)=whmax(Dfree(i,j), DRS(q,qg))
这里：
h ( q ) : 当前位姿 q 到目标的启发代价 h(q): \text{当前位姿 }q\text{ 到目标的启发代价} h(q):当前位姿 q 到目标的启发代价
w h : 启发函数权重 w_h: \text{启发函数权重} wh:启发函数权重
D free ( i , j ) : 二维地图上绕开障碍物到目标的距离估计 D_{\text{free}}(i,j): \text{二维地图上绕开障碍物到目标的距离估计} Dfree(i,j):二维地图上绕开障碍物到目标的距离估计
D RS ( q , q g ) : 不考虑障碍物时，满足车辆转弯约束的姿态距离估计 D_{\text{RS}}(q,q_g): \text{不考虑障碍物时，满足车辆转弯约束的姿态距离估计} DRS(q,qg):不考虑障碍物时，满足车辆转弯约束的姿态距离估计
为什么取最大值，而不是相加？因为这两个距离是在描述同一个"到目标还需要付出的距离"，只是从两个角度估计它：一个看地图障碍，一个看车辆运动学。如果相加，可能会把同一段距离重复计算得过重；取最大值表示：
至少要付出这两个估计中更严格的那一个。 \text{至少要付出这两个估计中更严格的那一个。} 至少要付出这两个估计中更严格的那一个。
举个例子。某个节点的二维自由空间距离为：
D free = 12 m D_{\text{free}}=12\ \text{m} Dfree=12 m
Reeds-Shepp 距离为：
D RS = 8 m D_{\text{RS}}=8\ \text{m} DRS=8 m
说明从地图上看需要绕障碍物，障碍物约束更强，于是：
max ( 12 , 8 ) = 12 \max(12,8)=12 max(12,8)=12
如果：
w h = 2.0 w_h=2.0 wh=2.0
则启发代价为：
h = 2.0 × 12 = 24 h=2.0\times12=24 h=2.0×12=24
反过来，如果某个节点地图上离目标很近：
D free = 3 m D_{\text{free}}=3\ \text{m} Dfree=3 m
但车头方向很别扭，需要倒车转向才能对准目标：
D RS = 7 m D_{\text{RS}}=7\ \text{m} DRS=7 m
则：
h = 2.0 × 7 = 14 h=2.0\times7=14 h=2.0×7=14
这说明启发函数会提醒搜索：虽然位置近，但姿态连接并不容易。
这个设计很有意思：二维自由空间距离负责"别穿墙"，Reeds-Shepp 距离负责"别忘了车不能横移"。取最大值意味着节点至少要付出这两种估计中更严格的那一个。
当 w h > 1 w_h>1 wh>1 时，这更接近 Weighted A*。它通常搜索更快，但最优性保证会变弱。对起步脱困来说，实时性和可行性往往比严格最短路径更重要。
13. 实际代价 g g g：不是最短，而是更像好开的路径
如果只最小化路径长度，Hybrid A* 可能给出贴障碍物、频繁换挡、方向盘剧烈变化的路径。自由空间驶出需要的是"能开且好开"的路径，所以 g g g 由多个代价项组成。
设当前节点为 n n n，候选动作的转角索引为 u u u，上一段转角索引为 u p u_p up，运动方向为 m m m，上一段方向为 m p m_p mp，扩展距离为 d d d。则新节点代价为：
g ( n ′ ) = g ( n ) + C motion + C smooth + C obs + C lat + C switch g(n')= g(n)+ C_{\text{motion}}+ C_{\text{smooth}}+ C_{\text{obs}}+ C_{\text{lat}}+ C_{\text{switch}} g(n′)=g(n)+Cmotion+Csmooth+Cobs+Clat+Cswitch
这条公式是在说：候选新节点 n ′ n' n′ 的累计代价，不只是"到这里走了多远"，而是把多个驾驶偏好一起累加进去。
每个变量的含义是：
n : 当前正在扩展的节点 n: \text{当前正在扩展的节点} n:当前正在扩展的节点
n ′ : 由某个车辆动作生成的候选后继节点 n': \text{由某个车辆动作生成的候选后继节点} n′:由某个车辆动作生成的候选后继节点
g ( n ) : 从起点走到当前节点 n 的累计实际代价 g(n): \text{从起点走到当前节点 }n\text{ 的累计实际代价} g(n):从起点走到当前节点 n 的累计实际代价
g ( n ′ ) : 从起点走到候选节点 n ′ 的累计实际代价 g(n'): \text{从起点走到候选节点 }n'\text{ 的累计实际代价} g(n′):从起点走到候选节点 n′ 的累计实际代价
C motion : 本次动作的基础运动代价，包括距离、转弯、倒车惩罚 C_{\text{motion}}: \text{本次动作的基础运动代价，包括距离、转弯、倒车惩罚} Cmotion:本次动作的基础运动代价，包括距离、转弯、倒车惩罚
C smooth : 方向盘转角变化代价 C_{\text{smooth}}: \text{方向盘转角变化代价} Csmooth:方向盘转角变化代价
C obs : 离障碍物太近的软惩罚 C_{\text{obs}}: \text{离障碍物太近的软惩罚} Cobs:离障碍物太近的软惩罚
C lat : 接近目标时的横向对齐惩罚 C_{\text{lat}}: \text{接近目标时的横向对齐惩罚} Clat:接近目标时的横向对齐惩罚
C switch : 前进/倒车方向切换惩罚 C_{\text{switch}}: \text{前进/倒车方向切换惩罚} Cswitch:前进/倒车方向切换惩罚
可以先用一个非常粗的数字例子建立直觉。假设当前节点已经花了：
g ( n ) = 10 g(n)=10 g(n)=10
本次动作各项代价为：
C motion = 1.5 , C smooth = 0.2 , C obs = 0.4 , C lat = 0 , C switch = 0 C_{\text{motion}}=1.5,\quad C_{\text{smooth}}=0.2,\quad C_{\text{obs}}=0.4,\quad C_{\text{lat}}=0,\quad C_{\text{switch}}=0 Cmotion=1.5,Csmooth=0.2,Cobs=0.4,Clat=0,Cswitch=0
那么候选节点代价就是：
g ( n ′ ) = 10 + 1.5 + 0.2 + 0.4 = 12.1 g(n')=10+1.5+0.2+0.4=12.1 g(n′)=10+1.5+0.2+0.4=12.1
如果另一条候选路径距离差不多，但需要换挡且贴近障碍物，可能变成：
g ( n ′ ) = 10 + 1.5 + 0.2 + 1.5 + 0.8 = 14.0 g(n')=10+1.5+0.2+1.5+0.8=14.0 g(n′)=10+1.5+0.2+1.5+0.8=14.0
A* 就会更不愿意选这条路径。下面逐项解释每一个代价项。
14. 路径长度与转弯代价
直行和转弯都要付出距离代价，但转弯应稍贵一些。转角索引 u u u 的归一化转弯程度为：
η u = ∣ u ∣ N s \eta_u=\frac{|u|}{N_s} ηu=Ns∣u∣
这里：
u : 当前动作使用的转角索引 u: \text{当前动作使用的转角索引} u:当前动作使用的转角索引
N s : 最大转角索引，也就是从 0 到最大转角分成了多少步 N_s: \text{最大转角索引，也就是从 }0\text{ 到最大转角分成了多少步} Ns:最大转角索引，也就是从 0 到最大转角分成了多少步
η u : 当前转角相对最大转角的比例 \eta_u: \text{当前转角相对最大转角的比例} ηu:当前转角相对最大转角的比例
当 u = 0 u=0 u=0 时，车辆直行：
η u = 0 \eta_u=0 ηu=0
当 ∣ u ∣ = N s |u|=N_s ∣u∣=Ns 时，车辆使用最大离散转角：
η u = 1 \eta_u=1 ηu=1
例如 N s = 2 N_s=2 Ns=2，动作集的转角索引为：
u ∈ { − 2 , − 1 , 0 , 1 , 2 } u\in\{-2,-1,0,1,2\} u∈{−2,−1,0,1,2}
那么：
u = 1 ⇒ η u = 1 2 = 0.5 u=1\Rightarrow\eta_u=\frac{1}{2}=0.5 u=1⇒ηu=21=0.5
表示使用一半强度的转弯；而：
u = 2 ⇒ η u = 1 u=2\Rightarrow\eta_u=1 u=2⇒ηu=1
表示使用最大强度转弯。
转弯代价权重为：
C curve_weight = w c η u = w c ∣ u ∣ N s C_{\text{curve\_weight}} = w_c\eta_u = w_c\frac{|u|}{N_s} Ccurve_weight=wcηu=wcNs∣u∣
其中：
w c : 转弯惩罚权重 w_c: \text{转弯惩罚权重} wc:转弯惩罚权重
w c w_c wc 越大，搜索越不喜欢急转弯； w c w_c wc 越小，搜索越接近单纯按路径长度选路。
如果该动作是前进，则基础运动代价为：
C motion = ∣ d ∣ ( 1 + w c ∣ u ∣ N s ) C_{\text{motion}} = |d| \left( 1+w_c\frac{|u|}{N_s} \right) Cmotion=∣d∣(1+wcNs∣u∣)
这里：
d : 本次运动原语的行驶距离，前进为正，倒车为负 d: \text{本次运动原语的行驶距离，前进为正，倒车为负} d:本次运动原语的行驶距离，前进为正，倒车为负
∣ d ∣ : 本次动作实际走过的距离长度 |d|: \text{本次动作实际走过的距离长度} ∣d∣:本次动作实际走过的距离长度
为什么使用 ∣ d ∣ |d| ∣d∣？因为无论前进还是倒车，只要车辆移动了 1 m 1\,\text{m} 1m，路径长度都是 1 m 1\,\text{m} 1m。方向差异由后面的倒车惩罚单独处理。
如果该动作是倒车，则再乘以倒车惩罚：
C motion = ∣ d ∣ ( 1 + w c ∣ u ∣ N s ) ( 1 + w r ) C_{\text{motion}} = |d| \left( 1+w_c\frac{|u|}{N_s} \right) \left( 1+w_r \right) Cmotion=∣d∣(1+wcNs∣u∣)(1+wr)
其中：
w r : 倒车惩罚权重 w_r: \text{倒车惩罚权重} wr:倒车惩罚权重
若 w r = 1 w_r=1 wr=1，倒车同样距离的代价大约翻倍。这不会禁止倒车，但会让搜索优先选择前进可行路径。
举个数字例子。设：
∣ d ∣ = 1.0 m , w c = 0.5 , N s = 2 , u = 1 |d|=1.0\ \text{m},\quad w_c=0.5,\quad N_s=2,\quad u=1 ∣d∣=1.0 m,wc=0.5,Ns=2,u=1
因为：
∣ u ∣ N s = 1 2 = 0.5 \frac{|u|}{N_s}=\frac{1}{2}=0.5 Ns∣u∣=21=0.5
前进转弯动作的基础代价为：
C motion = 1.0 ( 1 + 0.5 × 0.5 ) = 1.25 C_{\text{motion}}= 1.0 \left( 1+0.5\times0.5 \right) =1.25 Cmotion=1.0(1+0.5×0.5)=1.25
如果同样动作是倒车，且：
w r = 1.0 w_r=1.0 wr=1.0
则：
C motion = 1.25 ( 1 + 1.0 ) = 2.5 C_{\text{motion}}= 1.25(1+1.0)=2.5 Cmotion=1.25(1+1.0)=2.5
这说明同样移动 1 m 1\,\text{m} 1m，倒车不是不能走，而是会被搜索认为更"贵"。
15. 转角变化代价：让方向盘不要跳变
即使两条路径长度相同，方向盘频繁从左打满切到右打满也很糟糕。因此加入平滑代价：
C smooth = w s ∣ u − u p ∣ 2 N s C_{\text{smooth}} = w_s \frac{|u-u_p|}{2N_s} Csmooth=ws2Ns∣u−up∣
这里：
w s : 转角变化惩罚权重 w_s: \text{转角变化惩罚权重} ws:转角变化惩罚权重
u : 当前动作的转角索引 u: \text{当前动作的转角索引} u:当前动作的转角索引
u p : 上一段动作的转角索引 u_p: \text{上一段动作的转角索引} up:上一段动作的转角索引
∣ u − u p ∣ : 这一次方向盘动作变化有多大 |u-u_p|: \text{这一次方向盘动作变化有多大} ∣u−up∣:这一次方向盘动作变化有多大
分母 2 N s 2N_s 2Ns 是最大可能转角索引变化。例如从 − N s -N_s −Ns 直接切到 + N s +N_s +Ns，有：
∣ u − u p ∣ = 2 N s |u-u_p|=2N_s ∣u−up∣=2Ns
此时代价达到 w s w_s ws。
举个例子。设：
N s = 2 , w s = 0.5 N_s=2,\quad w_s=0.5 Ns=2,ws=0.5
如果上一段直行：
u p = 0 u_p=0 up=0
当前动作轻微左转：
u = 1 u=1 u=1
则：
C smooth = 0.5 ⋅ ∣ 1 − 0 ∣ 4 = 0.125 C_{\text{smooth}}= 0.5\cdot\frac{|1-0|}{4} =0.125 Csmooth=0.5⋅4∣1−0∣=0.125
如果上一段右打满：
u p = − 2 u_p=-2 up=−2
当前动作左打满：
u = 2 u=2 u=2
则：
C smooth = 0.5 ⋅ ∣ 2 − ( − 2 ) ∣ 4 = 0.5 C_{\text{smooth}}= 0.5\cdot\frac{|2-(-2)|}{4} =0.5 Csmooth=0.5⋅4∣2−(−2)∣=0.5
这表示"方向盘从一侧打满瞬间切到另一侧打满"会被明显惩罚。
这个代价不会直接限制曲率连续，但会偏好转角变化更小的动作序列，使生成路径更容易被后续控制器跟踪。
16. 换挡代价：少做短距离前后挪车
自由空间脱困允许倒车，但频繁前进、倒车、前进会导致路径难执行，也会让乘坐体验变差。设当前动作方向与上一动作方向不同：
m ≠ m p m\neq m_p m=mp
就认为发生了一次方向切换。设从上一次方向切换以来已经行驶的距离为 D dir D_{\text{dir}} Ddir，则换挡代价为：
C switch = w sw ( 1 + 1 1 + D dir ) C_{\text{switch}} = w_{\text{sw}} \left( 1+ \frac{1}{1+D_{\text{dir}}} \right) Cswitch=wsw(1+1+Ddir1)
这里：
w sw : 换挡惩罚权重 w_{\text{sw}}: \text{换挡惩罚权重} wsw:换挡惩罚权重
D dir : 自上一次换挡以来，同一方向已经累计行驶的距离 D_{\text{dir}}: \text{自上一次换挡以来，同一方向已经累计行驶的距离} Ddir:自上一次换挡以来，同一方向已经累计行驶的距离
1 + 1 1 + D dir : 短距离内反复换挡的额外放大项 1+\frac{1}{1+D_{\text{dir}}}: \text{短距离内反复换挡的额外放大项} 1+1+Ddir1:短距离内反复换挡的额外放大项
这个公式有一个很自然的性质：
当刚换挡后很快又换挡， D dir ≈ 0 D_{\text{dir}}\approx0 Ddir≈0，代价接近：
C switch ≈ 2 w sw C_{\text{switch}}\approx2w_{\text{sw}} Cswitch≈2wsw
当同一方向已经走了较长距离后再换挡， D dir D_{\text{dir}} Ddir 较大，代价接近：
C switch ≈ w sw C_{\text{switch}}\approx w_{\text{sw}} Cswitch≈wsw
所以算法不是完全禁止多次换挡，而是强烈抑制"原地来回蹭"的解。
举个数字例子。设：
w sw = 1.5 w_{\text{sw}}=1.5 wsw=1.5
如果刚刚前进了很短一段就又要切到倒车：
D dir = 0.2 m D_{\text{dir}}=0.2\ \text{m} Ddir=0.2 m
则：
C switch = 1.5 ( 1 + 1 1 + 0.2 ) ≈ 1.5 ( 1.833 ) ≈ 2.75 C_{\text{switch}}= 1.5 \left( 1+\frac{1}{1+0.2} \right) \approx 1.5(1.833) \approx2.75 Cswitch=1.5(1+1+0.21)≈1.5(1.833)≈2.75
如果同一方向已经行驶了较长距离后再换挡：
D dir = 5.0 m D_{\text{dir}}=5.0\ \text{m} Ddir=5.0 m
则：
C switch = 1.5 ( 1 + 1 6 ) ≈ 1.75 C_{\text{switch}}= 1.5 \left( 1+\frac{1}{6} \right) \approx1.75 Cswitch=1.5(1+61)≈1.75
可以看到，两者都会有换挡惩罚，但短距离反复换挡更贵。
换挡后，新的方向累计距离从当前这段运动开始：
D dir ′ = ∣ d ∣ D_{\text{dir}}' = |d| Ddir′=∣d∣
如果方向没有切换，则继续累加：
D dir ′ = D dir + ∣ d ∣ D_{\text{dir}}' = D_{\text{dir}}+|d| Ddir′=Ddir+∣d∣
17. 障碍物距离代价：不碰撞还不够，还要留余量
碰撞检测只回答：
撞还是不撞 \text{撞还是不撞} 撞还是不撞
但在真实驾驶中，距离障碍物 5 cm 5\,\text{cm} 5cm 和距离障碍物 80 cm 80\,\text{cm} 80cm 完全不是一回事。自由空间搜索会用 EDT 给路径增加软约束。
设车辆基准点到最近障碍物距离为 D obs D_{\text{obs}} Dobs，最近障碍物方向为 ϕ obs \phi_{\text{obs}} ϕobs。车辆朝向为 θ \theta θ，则障碍物相对车身方向为：
α = normalize ( ϕ obs − θ ) \alpha=\operatorname{normalize}(\phi_{\text{obs}}-\theta) α=normalize(ϕobs−θ)
这里要注意符号约定。本文把 ϕ obs \phi_{\text{obs}} ϕobs 定义为"从车辆基准点指向最近障碍物的全局方向角"，把 θ \theta θ 定义为"车辆自身 x x x 轴，也就是车头方向，在全局坐标系下的方向角"。因此把障碍物方向从全局坐标系转换到车辆自身坐标系时，要减去车辆朝向：
α = ϕ obs − θ \alpha= \phi_{\text{obs}}-\theta α=ϕobs−θ
这和普通二维坐标旋转是一致的：从全局坐标转到车辆坐标，相当于把坐标系旋转 − θ -\theta −θ。
举个例子。车辆朝向全局 x x x 轴正方向：
θ = 0 ∘ \theta=0^\circ θ=0∘
障碍物在车辆左侧：
ϕ obs = 90 ∘ \phi_{\text{obs}}=90^\circ ϕobs=90∘
则：
α = 90 ∘ − 0 ∘ = 90 ∘ \alpha=90^\circ-0^\circ=90^\circ α=90∘−0∘=90∘
表示障碍物在车辆坐标系左侧。如果写成 θ − ϕ obs \theta-\phi_{\text{obs}} θ−ϕobs，则会得到 − 90 ∘ -90^\circ −90∘，含义会变成右侧，这与"左正右负"的常用车辆坐标约定相反。
不过在障碍物距离代价里，如果后续只用 ∣ α ∣ |\alpha| ∣α∣ 去计算矩形车身到障碍物方向的边界距离，那么 ϕ obs − θ \phi_{\text{obs}}-\theta ϕobs−θ 和 θ − ϕ obs \theta-\phi_{\text{obs}} θ−ϕobs 会得到相同的距离结果，因为左右两侧车身是对称的。也就是说，作为有符号方向解释时应使用 ϕ obs − θ \phi_{\text{obs}}-\theta ϕobs−θ；作为左右对称的距离计算时，符号反过来通常不影响最终距离。
这里：
D obs : 车辆基准点到最近障碍物的距离 D_{\text{obs}}: \text{车辆基准点到最近障碍物的距离} Dobs:车辆基准点到最近障碍物的距离
ϕ obs : 从车辆基准点指向最近障碍物的方向角 \phi_{\text{obs}}: \text{从车辆基准点指向最近障碍物的方向角} ϕobs:从车辆基准点指向最近障碍物的方向角
θ : 车辆当前朝向 \theta: \text{车辆当前朝向} θ:车辆当前朝向
α : 最近障碍物在车辆自身坐标系下的相对方向 \alpha: \text{最近障碍物在车辆自身坐标系下的相对方向} α:最近障碍物在车辆自身坐标系下的相对方向
normalize ( ⋅ ) \operatorname{normalize}(\cdot) normalize(⋅) 表示把角度规整到固定范围，例如：
( − π , π ] (-\pi,\pi] (−π,π]
这样 190 ∘ 190^\circ 190∘ 会被看成 − 170 ∘ -170^\circ −170∘，避免角度差因为跨过 ± π \pm\pi ±π 而突然变大。
为了估计障碍物到车身外轮廓的距离，需要先求从车辆基准点沿方向 α \alpha α 射到矩形边界的距离，记为：
r body ( α ) r_{\text{body}}(\alpha) rbody(α)
这个量的意义是：从车辆基准点朝障碍物方向打一条射线，这条射线先碰到车身矩形边界的位置，距离基准点有多远。
对矩形车辆，可以按射线打到前边、侧边、后边分段计算。令半宽为：
h = w 2 h=\frac{w}{2} h=2w
这里 h h h 只是为了简化书写，它不是启发函数 h ( q ) h(q) h(q)，而是车辆半宽。
则一个直观表达是：
r body ( α ) = min { t > 0 ∣ ( t cos α , t sin α ) ∈ ∂ B } r_{\text{body}}(\alpha) = \min \left\{ t>0 \mid (t\cos\alpha,\ t\sin\alpha)\in\partial\mathcal{B} \right\} rbody(α)=min{t>0∣(tcosα, tsinα)∈∂B}
∂ B \partial\mathcal{B} ∂B 就是"车身外壳那一圈边"。
于是车身外轮廓到障碍物的近似距离为：
D clear = max ( D obs − r body ( α ) , 0 ) D_{\text{clear}} = \max \left( D_{\text{obs}}-r_{\text{body}}(\alpha),\ 0 \right) Dclear=max(Dobs−rbody(α), 0)
这里：
D clear : 估计的车身外轮廓到最近障碍物的净空距离 D_{\text{clear}}: \text{估计的车身外轮廓到最近障碍物的净空距离} Dclear:估计的车身外轮廓到最近障碍物的净空距离
如果 D obs D_{\text{obs}} Dobs 是"基准点到障碍物"的距离，而 r body ( α ) r_{\text{body}}(\alpha) rbody(α) 是"基准点到车身边界"的距离，那么二者相减就是"车身边界到障碍物"的距离。外面再套一层 max ( ⋅ , 0 ) \max(\cdot,0) max(⋅,0)，是为了避免数值上出现负净空：
D obs < r body ( α ) ⇒ D clear = 0 D_{\text{obs}}<r_{\text{body}}(\alpha) \Rightarrow D_{\text{clear}}=0 Dobs<rbody(α)⇒Dclear=0
这通常意味着障碍物已经非常接近，甚至从近似模型看已经进入车身范围。
设无惩罚安全距离为 d free d_{\text{free}} dfree，默认可理解为 1 m 1\,\text{m} 1m 级别，则障碍物距离代价为：
C obs = w o max ( 1 − D clear d free , 0 ) C_{\text{obs}} = w_o \max \left( 1-\frac{D_{\text{clear}}}{d_{\text{free}}}, 0 \right) Cobs=womax(1−dfreeDclear,0)
这里：
w o : 障碍物距离惩罚权重 w_o: \text{障碍物距离惩罚权重} wo:障碍物距离惩罚权重
d free : 希望保留的无惩罚安全净空 d_{\text{free}}: \text{希望保留的无惩罚安全净空} dfree:希望保留的无惩罚安全净空
这个公式可以读成：
净空越小，惩罚越大；净空达到安全距离后，惩罚为 0 \text{净空越小，惩罚越大；净空达到安全距离后，惩罚为 }0 净空越小，惩罚越大；净空达到安全距离后，惩罚为 0
当车身外轮廓离障碍物超过 d free d_{\text{free}} dfree 时：
C obs = 0 C_{\text{obs}}=0 Cobs=0
当车身已经非常接近障碍物时：
C obs → w o C_{\text{obs}}\rightarrow w_o Cobs→wo
另外，如果障碍物离车辆基准点已经比"车辆最大尺寸加安全距离"还远，就可以直接令该代价为零：
D obs > d max + d free ⇒ C obs = 0 D_{\text{obs}}> d_{\max}+d_{\text{free}} \Rightarrow C_{\text{obs}}=0 Dobs>dmax+dfree⇒Cobs=0
这既符合直觉，也减少了计算量。
举个数字例子。设：
w o = 1.75 , d free = 1.0 m w_o=1.75,\quad d_{\text{free}}=1.0\ \text{m} wo=1.75,dfree=1.0 m
如果车身外轮廓到障碍物还有：
D clear = 0.8 m D_{\text{clear}}=0.8\ \text{m} Dclear=0.8 m
则：
C obs = 1.75 ( 1 − 0.8 1.0 ) = 0.35 C_{\text{obs}}= 1.75 \left( 1-\frac{0.8}{1.0} \right) =0.35 Cobs=1.75(1−1.00.8)=0.35
如果净空只有：
D clear = 0.2 m D_{\text{clear}}=0.2\ \text{m} Dclear=0.2 m
则：
C obs = 1.75 ( 1 − 0.2 1.0 ) = 1.4 C_{\text{obs}}= 1.75 \left( 1-\frac{0.2}{1.0} \right) =1.4 Cobs=1.75(1−1.00.2)=1.4
所以两条路径都不碰撞时，搜索仍然会更偏好离障碍物远的那一条。
18. 目标附近横向代价：回到路上时要摆正
自由空间路径最终要接回道路中心线。如果车辆只是"碰到目标附近"，但横向偏差明显，接回中心线时会产生不自然的折线。因此在接近目标时加入横向偏差代价。
设参考目标位姿为 q ref q_{\text{ref}} qref。对正向搜索，它就是道路回归目标 q g q_g qg；对反向搜索，则参考关系会相应反过来。
把当前位姿 q q q 表达到参考目标坐标系下：
q rel = T ref − 1 T q = ( x rel , y rel , θ rel ) q_{\text{rel}} = T_{\text{ref}}^{-1}T_q = (x_{\text{rel}},y_{\text{rel}},\theta_{\text{rel}}) qrel=Tref−1Tq=(xrel,yrel,θrel)
这里：
T q : 当前车辆位姿对应的二维刚体变换 T_q: \text{当前车辆位姿对应的二维刚体变换} Tq:当前车辆位姿对应的二维刚体变换
T ref : 参考目标位姿对应的二维刚体变换 T_{\text{ref}}: \text{参考目标位姿对应的二维刚体变换} Tref:参考目标位姿对应的二维刚体变换
T ref − 1 T q : 把当前位姿从全局坐标系转换到参考目标坐标系 T_{\text{ref}}^{-1}T_q: \text{把当前位姿从全局坐标系转换到参考目标坐标系} Tref−1Tq:把当前位姿从全局坐标系转换到参考目标坐标系
转换后：
x rel : 当前点在目标坐标系下的纵向偏差 x_{\text{rel}}: \text{当前点在目标坐标系下的纵向偏差} xrel:当前点在目标坐标系下的纵向偏差
y rel : 当前点在目标坐标系下的横向偏差 y_{\text{rel}}: \text{当前点在目标坐标系下的横向偏差} yrel:当前点在目标坐标系下的横向偏差
θ rel : 当前车头相对目标车头的角度偏差 \theta_{\text{rel}}: \text{当前车头相对目标车头的角度偏差} θrel:当前车头相对目标车头的角度偏差
当前点到参考目标欧氏距离为：
d goal = ( x − x ref ) 2 + ( y − y ref ) 2 d_{\text{goal}}= \sqrt{(x-x_{\text{ref}})^2+(y-y_{\text{ref}})^2} dgoal=(x−xref)2+(y−yref)2
这里的 d goal d_{\text{goal}} dgoal 只看平面位置距离，不看朝向。
只有当：
d goal ≤ d near d_{\text{goal}}\le d_{\text{near}} dgoal≤dnear
才启用目标横向代价：
C lat = w lat ∣ y rel ∣ C_{\text{lat}} = w_{\text{lat}}|y_{\text{rel}}| Clat=wlat∣yrel∣
这里：
w lat : 目标附近横向偏差惩罚权重 w_{\text{lat}}: \text{目标附近横向偏差惩罚权重} wlat:目标附近横向偏差惩罚权重
∣ y rel ∣ : 当前点相对目标中心线的横向偏差绝对值 |y_{\text{rel}}|: \text{当前点相对目标中心线的横向偏差绝对值} ∣yrel∣:当前点相对目标中心线的横向偏差绝对值
如果离目标还很远，则：
C lat = 0 C_{\text{lat}}=0 Clat=0
举个例子。设：
d near = 5.0 m , w lat = 5.0 d_{\text{near}}=5.0\ \text{m},\quad w_{\text{lat}}=5.0 dnear=5.0 m,wlat=5.0
如果当前节点距离目标还有：
d goal = 8.0 m d_{\text{goal}}=8.0\ \text{m} dgoal=8.0 m
即使横向偏差为：
∣ y rel ∣ = 0.3 m |y_{\text{rel}}|=0.3\ \text{m} ∣yrel∣=0.3 m
也暂时不惩罚：
C lat = 0 C_{\text{lat}}=0 Clat=0
因为此时还很远，过早要求横向对齐可能会限制绕障能力。
如果当前节点已经进入目标附近：
d goal = 3.0 m d_{\text{goal}}=3.0\ \text{m} dgoal=3.0 m
且：
∣ y rel ∣ = 0.3 m |y_{\text{rel}}|=0.3\ \text{m} ∣yrel∣=0.3 m
则：
C lat = 5.0 × 0.3 = 1.5 C_{\text{lat}}= 5.0\times0.3=1.5 Clat=5.0×0.3=1.5
这会鼓励最后几米逐渐贴近道路中心线。
这个设计避免算法在远处过早贴向目标横向线，同时鼓励最后一段以更自然的方式进入道路中心线附近。
19. 完整节点评分公式
把上述项合起来，候选节点的累计代价可以写成：
g ( n ′ ) = g ( n ) + ∣ d ∣ ( 1 + w c ∣ u ∣ N s ) ( 1 + w r 1 m = backward ) g(n') = g(n) + |d| \left( 1+w_c\frac{|u|}{N_s} \right) \left( 1+w_r\mathbf{1}_{m=\text{backward}} \right) g(n′)=g(n)+∣d∣(1+wcNs∣u∣)(1+wr1m=backward)
- w s ∣ u − u p ∣ 2 N s + C obs + C lat + 1 m ≠ m p w sw ( 1 + 1 1 + D dir ) \quad + w_s\frac{|u-u_p|}{2N_s} + C_{\text{obs}} + C_{\text{lat}} + \mathbf{1}{m\neq m_p} w{\text{sw}} \left( 1+\frac{1}{1+D_{\text{dir}}} \right) +ws2Ns∣u−up∣+Cobs+Clat+1m=mpwsw(1+1+Ddir1)
为了避免这条公式显得太长，可以把它分成五块来看。
第一块：
g ( n ) g(n) g(n)
表示已经走到当前节点 n n n 的历史累计代价。
第二块：
∣ d ∣ ( 1 + w c ∣ u ∣ N s ) ( 1 + w r 1 m = backward ) |d| \left( 1+w_c\frac{|u|}{N_s} \right) \left( 1+w_r\mathbf{1}_{m=\text{backward}} \right) ∣d∣(1+wcNs∣u∣)(1+wr1m=backward)
表示本次动作的运动代价。其中：
1 m = backward = { 1 , m = backward 0 , m = forward \mathbf{1}_{m=\text{backward}}= \begin{cases} 1, & m=\text{backward}\\ 0, & m=\text{forward} \end{cases} 1m=backward={1,0,m=backwardm=forward
也就是说，只有倒车动作才会乘上倒车惩罚；前进动作不会被这项放大。
第三块：
w s ∣ u − u p ∣ 2 N s w_s\frac{|u-u_p|}{2N_s} ws2Ns∣u−up∣
表示方向盘转角变化代价。
第四块：
C obs + C lat C_{\text{obs}}+C_{\text{lat}} Cobs+Clat
表示障碍物净空代价和目标附近横向对齐代价。
第五块：
1 m ≠ m p w sw ( 1 + 1 1 + D dir ) \mathbf{1}{m\neq m_p} w{\text{sw}} \left( 1+\frac{1}{1+D_{\text{dir}}} \right) 1m=mpwsw(1+1+Ddir1)
表示换挡代价。其中：
1 m ≠ m p = { 1 , 当前动作方向和上一动作方向不同 0 , 当前动作方向和上一动作方向相同 \mathbf{1}_{m\neq m_p}= \begin{cases} 1, & \text{当前动作方向和上一动作方向不同}\\ 0, & \text{当前动作方向和上一动作方向相同} \end{cases} 1m=mp={1,0,当前动作方向和上一动作方向不同当前动作方向和上一动作方向相同
所以只有发生前进/倒车切换时，这一项才会生效。
来看一个完整数字例子。假设当前节点已有代价：
g ( n ) = 20 g(n)=20 g(n)=20
本次动作参数为：
∣ d ∣ = 1.0 , u = 1 , u p = 0 , N s = 2 |d|=1.0,\quad u=1,\quad u_p=0,\quad N_s=2 ∣d∣=1.0,u=1,up=0,Ns=2
权重为：
w c = 0.5 , w r = 1.0 , w s = 0.5 , w sw = 1.5 w_c=0.5,\quad w_r=1.0,\quad w_s=0.5,\quad w_{\text{sw}}=1.5 wc=0.5,wr=1.0,ws=0.5,wsw=1.5
并且本次是前进动作：
m = forward , m p = forward m=\text{forward},\quad m_p=\text{forward} m=forward,mp=forward
障碍物和横向代价为：
C obs = 0.35 , C lat = 0 C_{\text{obs}}=0.35,\quad C_{\text{lat}}=0 Cobs=0.35,Clat=0
则运动代价为：
1.0 ( 1 + 0.5 ⋅ 1 2 ) ( 1 + 1.0 ⋅ 0 ) = 1.25 1.0 \left( 1+0.5\cdot\frac{1}{2} \right) \left( 1+1.0\cdot0 \right) =1.25 1.0(1+0.5⋅21)(1+1.0⋅0)=1.25
转角变化代价为：
0.5 ⋅ ∣ 1 − 0 ∣ 4 = 0.125 0.5\cdot\frac{|1-0|}{4}=0.125 0.5⋅4∣1−0∣=0.125
没有换挡，所以换挡代价为：
0 0 0
于是：
g ( n ′ ) = 20 + 1.25 + 0.125 + 0.35 + 0 + 0 = 21.725 g(n')= 20+1.25+0.125+0.35+0+0 =21.725 g(n′)=20+1.25+0.125+0.35+0+0=21.725
如果另一个候选动作几何位置类似，但它是倒车且发生换挡，设：
m = backward , m p = forward , D dir = 0.5 m=\text{backward},\quad m_p=\text{forward},\quad D_{\text{dir}}=0.5 m=backward,mp=forward,Ddir=0.5
则倒车会让运动代价变成：
1.25 ( 1 + 1.0 ) = 2.5 1.25(1+1.0)=2.5 1.25(1+1.0)=2.5
换挡代价为：
1.5 ( 1 + 1 1 + 0.5 ) = 2.5 1.5 \left( 1+\frac{1}{1+0.5} \right) =2.5 1.5(1+1+0.51)=2.5
即使其他项不变：
g ( n ′ ) = 20 + 2.5 + 0.125 + 0.35 + 0 + 2.5 = 25.475 g(n')= 20+2.5+0.125+0.35+0+2.5 =25.475 g(n′)=20+2.5+0.125+0.35+0+2.5=25.475
这就是代价函数的作用：不是把倒车或换挡禁止掉，而是让搜索在有选择时更倾向于平顺、少换挡、离障碍物远的路径。
最终优先级为：
f ( n ′ ) = g ( n ′ ) + w h max ( D free ( i ′ , j ′ ) , D RS ( q ′ , q g ) ) f(n') = g(n') + w_h \max \left( D_{\text{free}}(i',j'),\ D_{\text{RS}}(q',q_g) \right) f(n′)=g(n′)+whmax(Dfree(i′,j′), DRS(q′,qg))
这里：
f ( n ′ ) : 候选节点在优先队列中的最终排序分数 f(n'): \text{候选节点在优先队列中的最终排序分数} f(n′):候选节点在优先队列中的最终排序分数
g ( n ′ ) : 已经真实付出的累计代价 g(n'): \text{已经真实付出的累计代价} g(n′):已经真实付出的累计代价
w h max ( D free , D RS ) : 预计从 n ′ 到目标还需要付出的代价 w_h \max \left( D_{\text{free}}, D_{\text{RS}} \right): \text{预计从 }n'\text{ 到目标还需要付出的代价} whmax(Dfree,DRS):预计从 n′ 到目标还需要付出的代价
继续沿用上面的例子，若：
g ( n ′ ) = 21.725 g(n')=21.725 g(n′)=21.725
且：
D free = 6.0 , D RS = 4.5 , w h = 2.0 D_{\text{free}}=6.0,\quad D_{\text{RS}}=4.5,\quad w_h=2.0 Dfree=6.0,DRS=4.5,wh=2.0
则启发代价为：
2.0 max ( 6.0 , 4.5 ) = 12.0 2.0\max(6.0,4.5)=12.0 2.0max(6.0,4.5)=12.0
最终：
f ( n ′ ) = 21.725 + 12.0 = 33.725 f(n')=21.725+12.0=33.725 f(n′)=21.725+12.0=33.725
A* 的优先队列会把不同候选节点按 f f f 从小到大排序，优先扩展看起来"已经走得不错，并且未来也有希望"的节点。
这条公式几乎概括了 Hybrid A* Freespace Pull Out 的全部偏好：
短路径 + 少急转 + 少倒车 + 少换挡 + 远离障碍物 + 接近目标时横向摆正 \text{短路径} + \text{少急转} + \text{少倒车} + \text{少换挡} + \text{远离障碍物} + \text{接近目标时横向摆正} 短路径+少急转+少倒车+少换挡+远离障碍物+接近目标时横向摆正
它不是一个单纯的"最短路"算法，而是一个把驾驶可执行性写入搜索代价的规划器。