前面内容：从自由空间脱困到回归车道：Hybrid A* Freespace Pull Out 算法教程（2）
20. 目标判定：不是必须精确压到同一个点
搜索空间是离散的，车辆运动又受转弯半径约束，所以要求轨迹精确到达目标位姿几乎不现实。Hybrid A* 使用目标容差。
设候选节点位姿为 q n q_n qn，目标位姿为 q g q_g qg。先把节点表达在目标坐标系下：
q rel = T g − 1 T n = ( x rel , y rel , θ rel ) q_{\text{rel}} = T_g^{-1}T_n = (x_{\text{rel}},y_{\text{rel}},\theta_{\text{rel}}) qrel=Tg−1Tn=(xrel,yrel,θrel)
这里的变量含义是：
T n : 候选节点位姿 q n 对应的二维刚体变换 T_n: \text{候选节点位姿 }q_n\text{ 对应的二维刚体变换} Tn:候选节点位姿 qn 对应的二维刚体变换
T g : 目标位姿 q g 对应的二维刚体变换 T_g: \text{目标位姿 }q_g\text{ 对应的二维刚体变换} Tg:目标位姿 qg 对应的二维刚体变换
T g − 1 T n : 把候选节点从全局坐标系转换到目标坐标系 T_g^{-1}T_n: \text{把候选节点从全局坐标系转换到目标坐标系} Tg−1Tn:把候选节点从全局坐标系转换到目标坐标系
转换后的三个分量可以这样理解：
x rel : 候选点相对目标点的前后偏差 x_{\text{rel}}: \text{候选点相对目标点的前后偏差} xrel:候选点相对目标点的前后偏差
y rel : 候选点相对目标点的左右偏差 y_{\text{rel}}: \text{候选点相对目标点的左右偏差} yrel:候选点相对目标点的左右偏差
θ rel : 候选车头方向相对目标车头方向的偏差 \theta_{\text{rel}}: \text{候选车头方向相对目标车头方向的偏差} θrel:候选车头方向相对目标车头方向的偏差
为什么要放到目标坐标系里判断？因为"前后"和"左右"应该跟目标车道方向有关，而不是跟地图的东西南北方向有关。目标车头朝东时， x rel x_{\text{rel}} xrel 表示东西方向偏差；目标车头朝北时， x rel x_{\text{rel}} xrel 表示南北方向偏差。
若节点和目标落在同一个离散索引中：
index ( q n ) = index ( q g ) \operatorname{index}(q_n)=\operatorname{index}(q_g) index(qn)=index(qg)
这里的 index ( ⋅ ) \operatorname{index}(\cdot) index(⋅) 会把连续位姿离散成：
( i , j , k ) (i,j,k) (i,j,k)
其中 ( i , j ) (i,j) (i,j) 是栅格索引， k k k 是航向桶索引。如果候选节点和目标落在同一个离散状态里，说明在搜索分辨率意义下它们已经足够接近，可以直接认为到达。否则继续检查连续容差。
配置给出的纵向、横向、角度目标范围通常表示完整窗口，因此实际使用半宽：
ϵ x = L goal 2 \epsilon_x=\frac{L_{\text{goal}}}{2} ϵx=2Lgoal
ϵ y = W goal 2 \epsilon_y=\frac{W_{\text{goal}}}{2} ϵy=2Wgoal
ϵ θ = Θ goal 2 \epsilon_\theta=\frac{\Theta_{\text{goal}}}{2} ϵθ=2Θgoal
其中角度范围通常以角度制给出，计算时转成弧度：
ϵ θ = π 180 Θ goal 2 \epsilon_\theta = \frac{\pi}{180} \frac{\Theta_{\text{goal}}}{2} ϵθ=180π2Θgoal
这里：
L goal : 目标判定窗口的纵向总长度 L_{\text{goal}}: \text{目标判定窗口的纵向总长度} Lgoal:目标判定窗口的纵向总长度
W goal : 目标判定窗口的横向总宽度 W_{\text{goal}}: \text{目标判定窗口的横向总宽度} Wgoal:目标判定窗口的横向总宽度
Θ goal : 目标判定窗口的总角度范围，常以角度制配置 \Theta_{\text{goal}}: \text{目标判定窗口的总角度范围，常以角度制配置} Θgoal:目标判定窗口的总角度范围，常以角度制配置
ϵ x , ϵ y , ϵ θ : 实际用于正负两侧判断的半宽容差 \epsilon_x,\epsilon_y,\epsilon_\theta: \text{实际用于正负两侧判断的半宽容差} ϵx,ϵy,ϵθ:实际用于正负两侧判断的半宽容差
如果配置写的是"纵向目标范围 2.0 m 2.0\,\text{m} 2.0m"，通常表示以目标点为中心，前后合计 2.0 m 2.0\,\text{m} 2.0m。所以真正判断时是：
− 1.0 ≤ x rel ≤ 1.0 -1.0\le x_{\text{rel}}\le1.0 −1.0≤xrel≤1.0
这就是为什么要除以 2 2 2。
换一种更直观的说法：Hybrid A* 的目标不是一个无限精确的点，而是一个允许误差的窗口。搜索时很难要求车辆精确满足：
x = x g , y = y g , θ = θ g x=x_g,\quad y=y_g,\quad \theta=\theta_g x=xg,y=yg,θ=θg
所以只要候选节点落进目标窗口，就可以认为"已经到达目标附近，并且姿态足够接近"。
例如配置为：
L goal = 2.0 m L_{\text{goal}}=2.0\,\text{m} Lgoal=2.0m
它表示目标窗口的纵向总长度是 2.0 m 2.0\,\text{m} 2.0m，不是说单侧允许 2.0 m 2.0\,\text{m} 2.0m。因为窗口以目标点为中心，所以前后各占一半：
ϵ x = 2.0 2 = 1.0 m \epsilon_x= \frac{2.0}{2} =1.0\,\text{m} ϵx=22.0=1.0m
真正判断的是：
∣ x rel ∣ ≤ 1.0 m |x_{\text{rel}}|\le1.0\,\text{m} ∣xrel∣≤1.0m
横向同理。若：
W goal = 0.5 m W_{\text{goal}}=0.5\,\text{m} Wgoal=0.5m
表示左右总宽为 0.5 m 0.5\,\text{m} 0.5m，所以左右各允许：
ϵ y = 0.5 2 = 0.25 m \epsilon_y= \frac{0.5}{2} =0.25\,\text{m} ϵy=20.5=0.25m
判断条件是：
∣ y rel ∣ ≤ 0.25 m |y_{\text{rel}}|\le0.25\,\text{m} ∣yrel∣≤0.25m
角度也一样。若：
Θ goal = 6 ∘ \Theta_{\text{goal}}=6^\circ Θgoal=6∘
表示总角度窗口是 6 ∘ 6^\circ 6∘，所以左右各允许：
ϵ θ = 3 ∘ \epsilon_\theta=3^\circ ϵθ=3∘
程序计算角度时通常使用弧度，因此需要转换：
3 ∘ = π 180 × 3 ≈ 0.0524 rad 3^\circ= \frac{\pi}{180}\times3 \approx0.0524\,\text{rad} 3∘=180π×3≈0.0524rad
所以可以把这一段总结成一句话：
配置给的是整个目标窗口大小，实际判断用目标中心两侧的半宽。 \text{配置给的是整个目标窗口大小，实际判断用目标中心两侧的半宽。} 配置给的是整个目标窗口大小，实际判断用目标中心两侧的半宽。
目标判定为：
∣ x rel ∣ ≤ ϵ x |x_{\text{rel}}|\le\epsilon_x ∣xrel∣≤ϵx
∣ y rel ∣ ≤ ϵ y |y_{\text{rel}}|\le\epsilon_y ∣yrel∣≤ϵy
∣ normalize ( θ rel ) ∣ ≤ ϵ θ |\operatorname{normalize}(\theta_{\text{rel}})|\le\epsilon_\theta ∣normalize(θrel)∣≤ϵθ
这三个条件必须同时满足。也就是说，一个节点要被认为到达目标，需要同时满足：
前后位置差不多 \text{前后位置差不多} 前后位置差不多
左右横向差不多 \text{左右横向差不多} 左右横向差不多
车头方向也差不多 \text{车头方向也差不多} 车头方向也差不多
只满足位置接近还不够。例如车辆到了目标点旁边，但车头横着摆，后续很难自然接上道路中心线。
还可以要求解必须在目标后方。后方定义为：
x rel ≤ 0 x_{\text{rel}}\le0 xrel≤0
这里的"后方"是在目标坐标系里定义的。如果目标车头方向是 x x x 轴正方向，那么：
x rel > 0 x_{\text{rel}}>0 xrel>0
表示候选节点在目标点前方；而：
x rel < 0 x_{\text{rel}}<0 xrel<0
表示候选节点还在目标点后方。
如果启用"只接受目标后方解"，则必须满足：
x rel ≤ 0 x_{\text{rel}}\le0 xrel≤0
这个约束适用于某些希望车辆从目标点后方接近道路中心线的场景，可以减少"冲过目标再折回来"的解。
默认目标范围示例中，若：
L goal = 2.0 m L_{\text{goal}}=2.0\,\text{m} Lgoal=2.0m
W goal = 0.5 m W_{\text{goal}}=0.5\,\text{m} Wgoal=0.5m
Θ goal = 6 ∘ \Theta_{\text{goal}}=6^\circ Θgoal=6∘
则实际容差是：
∣ x rel ∣ ≤ 1.0 m |x_{\text{rel}}|\le1.0\,\text{m} ∣xrel∣≤1.0m
∣ y rel ∣ ≤ 0.25 m |y_{\text{rel}}|\le0.25\,\text{m} ∣yrel∣≤0.25m
∣ θ rel ∣ ≤ 3 ∘ |\theta_{\text{rel}}|\le3^\circ ∣θrel∣≤3∘
这个目标窗口相当严格，尤其是横向和角度，因此最后一段路径会明显受到目标姿态约束。
再看一个判断例子。假设容差为：
ϵ x = 1.0 m , ϵ y = 0.25 m , ϵ θ = 3 ∘ \epsilon_x=1.0\,\text{m},\quad \epsilon_y=0.25\,\text{m},\quad \epsilon_\theta=3^\circ ϵx=1.0m,ϵy=0.25m,ϵθ=3∘
某候选节点在目标坐标系下为：
x rel = − 0.6 m , y rel = 0.18 m , θ rel = 2 ∘ x_{\text{rel}}=-0.6\,\text{m},\quad y_{\text{rel}}=0.18\,\text{m},\quad \theta_{\text{rel}}=2^\circ xrel=−0.6m,yrel=0.18m,θrel=2∘
则：
∣ − 0.6 ∣ ≤ 1.0 , ∣ 0.18 ∣ ≤ 0.25 , ∣ 2 ∘ ∣ ≤ 3 ∘ |-0.6|\le1.0,\quad |0.18|\le0.25,\quad |2^\circ|\le3^\circ ∣−0.6∣≤1.0,∣0.18∣≤0.25,∣2∘∣≤3∘
所以它满足目标条件。如果还启用了"必须在目标后方"：
x rel = − 0.6 ≤ 0 x_{\text{rel}}=-0.6\le0 xrel=−0.6≤0
也满足。
但如果另一个候选节点为：
x rel = 0.3 m , y rel = 0.4 m , θ rel = 1 ∘ x_{\text{rel}}=0.3\,\text{m},\quad y_{\text{rel}}=0.4\,\text{m},\quad \theta_{\text{rel}}=1^\circ xrel=0.3m,yrel=0.4m,θrel=1∘
虽然纵向和角度都可以，但：
∣ 0.4 ∣ > 0.25 |0.4|>0.25 ∣0.4∣>0.25
横向偏差超出范围，所以不能认为到达目标。
21. 目标点附近为什么会出现"横向对齐点"
目标容差允许节点不精确等于目标。如果直接把最后一个搜索节点接到道路中心线，可能出现一个很短但角度不自然的连接段。
这一节的核心是：Hybrid A 找到的终点，只是进入了目标窗口，不一定刚好落在道路中心线上*。
为了看清楚这个问题，先在目标坐标系下讨论。设目标点在道路中心线上：
q g = ( 0 , 0 , 0 ) q_g=(0,\ 0,\ 0) qg=(0, 0, 0)
其中：
x : 沿道路方向 x: \text{沿道路方向} x:沿道路方向
y : 横向偏离道路中心线 y: \text{横向偏离道路中心线} y:横向偏离道路中心线
θ : 相对道路方向的角度 \theta: \text{相对道路方向的角度} θ:相对道路方向的角度
搜索终点可能是：
q end = ( − 0.6 , 0.2 , 2 ∘ ) q_{\text{end}}=(-0.6,\ 0.2,\ 2^\circ) qend=(−0.6, 0.2, 2∘)
它满足上一节的目标窗口，所以可以认为"已经到达目标附近"。但它并不等于真正的目标点：
q g = ( 0 , 0 , 0 ) q_g=(0,\ 0,\ 0) qg=(0, 0, 0)
它仍然有横向偏差：
y rel = 0.2 m y_{\text{rel}}=0.2\,\text{m} yrel=0.2m
如果直接把搜索终点硬接到道路中心线，连接关系可能变成：
( − 0.6 , 0.2 , 2 ∘ ) → ( 0 , 0 , 0 ) (-0.6,\ 0.2,\ 2^\circ) \rightarrow (0,\ 0,\ 0) (−0.6, 0.2, 2∘)→(0, 0, 0)
这段连接很短，却要同时完成三件事：
x : − 0.6 → 0 x:\ -0.6\rightarrow0 x: −0.6→0
y : 0.2 → 0 y:\ 0.2\rightarrow0 y: 0.2→0
θ : 2 ∘ → 0 ∘ \theta:\ 2^\circ\rightarrow0^\circ θ: 2∘→0∘
也就是既要往前走，又要横向贴回中心线，还要调整车头方向。这样容易形成一个短小、不自然的折线，后续控制器跟踪起来也会更别扭。
所以第 21 节引入"横向对齐点"。它解决的不是"能不能到目标"，而是：
到目标附近以后，怎么更顺地接回道路中心线？ \text{到目标附近以后，怎么更顺地接回道路中心线？} 到目标附近以后，怎么更顺地接回道路中心线？
一种处理方式是构造一个与目标朝向一致、但横向位置等于搜索终点横向偏差的辅助点。若搜索终点在目标坐标系中的横向偏差为：
y rel y_{\text{rel}} yrel
则辅助点在目标坐标系中为：
q shift = ( 0 , y rel , 0 ) q_{\text{shift}} = (0,\ y_{\text{rel}},\ 0) qshift=(0, yrel, 0)
这里：
q shift : 目标坐标系下的辅助对齐点 q_{\text{shift}}: \text{目标坐标系下的辅助对齐点} qshift:目标坐标系下的辅助对齐点
它的三个分量分别表示：
0 : 纵向位置放在目标截面上 0: \text{纵向位置放在目标截面上} 0:纵向位置放在目标截面上
y rel : 保留搜索终点相对目标中心线的横向偏差 y_{\text{rel}}: \text{保留搜索终点相对目标中心线的横向偏差} yrel:保留搜索终点相对目标中心线的横向偏差
0 : 朝向与目标朝向一致 0: \text{朝向与目标朝向一致} 0:朝向与目标朝向一致
变回全局坐标：
T shift = T g T ( q shift ) T_{\text{shift}} = T_g T(q_{\text{shift}}) Tshift=TgT(qshift)
这里 T ( q shift ) T(q_{\text{shift}}) T(qshift) 表示辅助点在目标坐标系下的变换，左乘 T g T_g Tg 后，就得到它在全局地图坐标系中的位置和朝向。
举个例子。假设搜索终点已经接近目标，但在目标坐标系里还有：
y rel = 0.2 m y_{\text{rel}}=0.2\ \text{m} yrel=0.2 m
目标朝向是沿道路中心线向前。辅助点：
q shift = ( 0 , 0.2 , 0 ) q_{\text{shift}}=(0,0.2,0) qshift=(0,0.2,0)
表示"位于目标横截面上，向左偏 0.2 m 0.2\,\text{m} 0.2m，但车头方向和道路一致"。
它不是让车辆马上回到中心线，而是先让车辆变成：
位置接近目标截面，车头已经顺着道路方向 \text{位置接近目标截面，车头已经顺着道路方向} 位置接近目标截面，车头已经顺着道路方向
之后再接道路中心线，就会自然很多。这样自由空间路径最后不是斜着硬接道路，而是尽量以道路方向结束。
这个点位于目标的横向截面上，朝向与目标一致。它的作用是让自由空间段末尾更接近"沿道路方向"结束，而不是用一个斜着的末端硬接中心线。
可以把它理解为：
先把车辆带到目标附近同向位置，再交给道路中心线 \text{先把车辆带到目标附近同向位置，再交给道路中心线} 先把车辆带到目标附近同向位置，再交给道路中心线
也可以更短地记成：
先把自由空间路径的末端摆正，再交给道路中心线。 \text{先把自由空间路径的末端摆正，再交给道路中心线。} 先把自由空间路径的末端摆正，再交给道路中心线。
22. 路径回溯与插值
当搜索找到目标节点后，每个节点都保存父节点指针：
n ′ → n n'\rightarrow n n′→n
从目标节点不断回溯到起点，就能得到离散节点序列：
n 0 , n 1 , ... , n K n_0,n_1,\dots,n_K n0,n1,...,nK
这里：
n 0 : 起点节点 n_0: \text{起点节点} n0:起点节点
n K : 满足目标条件的终点节点 n_K: \text{满足目标条件的终点节点} nK:满足目标条件的终点节点
K : 路径中搜索节点的段数规模 K: \text{路径中搜索节点的段数规模} K:路径中搜索节点的段数规模
其中每个节点包含：
( x i , y i , θ i , m i ) (x_i,y_i,\theta_i,m_i) (xi,yi,θi,mi)
这里：
x i , y i : 第 i 个节点的位置 x_i,y_i: \text{第 }i\text{ 个节点的位置} xi,yi:第 i 个节点的位置
θ i : 第 i 个节点的车头朝向 \theta_i: \text{第 }i\text{ 个节点的车头朝向} θi:第 i 个节点的车头朝向
m i : 到达该节点对应的运动方向，前进或倒车 m_i: \text{到达该节点对应的运动方向，前进或倒车} mi:到达该节点对应的运动方向，前进或倒车
m i m_i mi 表示该段属于前进还是倒车。
如果使用自适应扩展，两个相邻搜索节点之间的距离可能大于最小步长。为了让输出路径更细腻，需要在父节点和子节点之间按同一个转角原语插值。
设父节点位姿为 q p q_p qp，子节点由转角 δ u \delta_u δu 和距离 d d d 从父节点积分而来。若两点距离为 D D D，用 N N N 段插值：
N = ⌊ D d min ⌋ N=\left\lfloor\frac{D}{d_{\min}}\right\rfloor N=⌊dminD⌋
这里：
D : 父节点和子节点之间这段运动的实际长度 D: \text{父节点和子节点之间这段运动的实际长度} D:父节点和子节点之间这段运动的实际长度
d min : 希望输出路径点之间不超过的基础间隔 d_{\min}: \text{希望输出路径点之间不超过的基础间隔} dmin:希望输出路径点之间不超过的基础间隔
N : 这段运动要插成多少小段 N: \text{这段运动要插成多少小段} N:这段运动要插成多少小段
⌊ ⋅ ⌋ \lfloor\cdot\rfloor ⌊⋅⌋ 表示向下取整。实际实现里通常还要保证 N N N 至少为 1 1 1，避免两个点太近时无法插值。
第 a a a 个插值点使用距离：
d a = a N D d_a=\frac{a}{N}D da=NaD
其中：
a = 0 , 1 , 2 , ... , N a=0,1,2,\dots,N a=0,1,2,...,N
当 a = 0 a=0 a=0 时：
d a = 0 d_a=0 da=0
得到父节点；当 a = N a=N a=N 时：
d a = D d_a=D da=D
得到子节点；中间的 a a a 就是沿同一条运动原语生成的中间点。
并代入同一个自行车模型：
q a = F ( q p , δ u , d a ) q_a = F(q_p,\delta_u,d_a) qa=F(qp,δu,da)
其中 F F F 就是前面推导的直行或圆弧积分函数。
举个例子。父子节点之间距离为：
D = 1.2 m D=1.2\ \text{m} D=1.2 m
希望路径点间隔约为：
d min = 0.3 m d_{\min}=0.3\ \text{m} dmin=0.3 m
则：
N = ⌊ 1.2 0.3 ⌋ = 4 N=\left\lfloor\frac{1.2}{0.3}\right\rfloor=4 N=⌊0.31.2⌋=4
插值距离分别是：
d 0 = 0 , d 1 = 0.3 , d 2 = 0.6 , d 3 = 0.9 , d 4 = 1.2 d_0=0,\quad d_1=0.3,\quad d_2=0.6,\quad d_3=0.9,\quad d_4=1.2 d0=0,d1=0.3,d2=0.6,d3=0.9,d4=1.2
这些点都使用同一个转角 δ u \delta_u δu 代入自行车模型生成，因此插值出来的是一段连续圆弧或直线，而不是简单用直线把父子节点硬连起来。
最终输出路径按车辆实际执行顺序排列：
q s → ⋯ → q g q_s \rightarrow \cdots \rightarrow q_g qs→⋯→qg
并保留每个点的前进/倒车标记。
23. 按换挡点切分子路径
自由空间路径可能包含前进和倒车。例如：
倒车 → 停车换挡 → 前进左转 → 接回车道 \text{倒车} \rightarrow \text{停车换挡} \rightarrow \text{前进左转} \rightarrow \text{接回车道} 倒车→停车换挡→前进左转→接回车道
控制执行时，方向切换处通常不能当作普通连续路径处理。需要在方向变化处切分：
P = P 1 ∪ P 2 ∪ ⋯ ∪ P K \mathcal{P} = \mathcal{P}_1 \cup \mathcal{P}_2 \cup \cdots \cup \mathcal{P}_K P=P1∪P2∪⋯∪PK
这里：
P : 完整自由空间路径 \mathcal{P}: \text{完整自由空间路径} P:完整自由空间路径
P k : 第 k 个方向一致的子路径 \mathcal{P}_k: \text{第 }k\text{ 个方向一致的子路径} Pk:第 k 个方向一致的子路径
K : 切分后的子路径数量 K: \text{切分后的子路径数量} K:切分后的子路径数量
符号 ∪ \cup ∪ 在这里表达"由这些子路径按顺序组成"，不是集合论里不关心顺序的并集。更直观地说：
P = P 1 → P 2 → ⋯ → P K \mathcal{P}= \mathcal{P}_1 \rightarrow \mathcal{P}_2 \rightarrow \cdots \rightarrow \mathcal{P}_K P=P1→P2→⋯→PK
每个子路径内部方向一致：
∀ q i , q i + 1 ∈ P k , m i = m i + 1 \forall q_i,q_{i+1}\in\mathcal{P}k,\quad m_i=m{i+1} ∀qi,qi+1∈Pk,mi=mi+1
这条公式的意思是：对同一个子路径 P k \mathcal{P}_k Pk 内任意相邻两个点，它们的运动方向标记必须相同。
其中：
∀ : 对所有 \forall: \text{对所有} ∀:对所有
q i , q i + 1 : 路径上的相邻两个位姿点 q_i,q_{i+1}: \text{路径上的相邻两个位姿点} qi,qi+1:路径上的相邻两个位姿点
m i , m i + 1 : 这两个点对应的运动方向 m_i,m_{i+1}: \text{这两个点对应的运动方向} mi,mi+1:这两个点对应的运动方向
例如一条路径的方向序列是：
backward , backward , backward , forward , forward \text{backward},\ \text{backward},\ \text{backward},\ \text{forward},\ \text{forward} backward, backward, backward, forward, forward
那么可以切成：
P 1 : backward , backward , backward \mathcal{P}_1: \text{backward},\text{backward},\text{backward} P1:backward,backward,backward
P 2 : forward , forward \mathcal{P}_2: \text{forward},\text{forward} P2:forward,forward
换挡点就在 P 1 \mathcal{P}_1 P1 和 P 2 \mathcal{P}_2 P2 的连接处。车辆执行到这里时通常要先停车，再从倒挡切到前进挡。
不同子路径之间允许停车、换挡、重新规划或重新检查安全。
速度也要与方向匹配。自由空间驶出的目标速度通常较低，例如：
v free = 1.0 m/s v_{\text{free}}=1.0\,\text{m/s} vfree=1.0m/s
在子路径末端，尤其是换挡点，速度应当被修正为可停车或可切换的形式。这样路径几何和车辆纵向控制才一致。
24. 从自由空间段接回道路中心线
Hybrid A* 只负责从当前位置开到道路回归点附近。到达回归点后，车辆应该继续沿道路中心线行驶。
设自由空间回归目标在道路中心线上的弧长为 s g s_g sg。为了避免自由空间段和中心线段在目标点附近重叠或产生短小折线，可以从目标点前方一点开始拼接：
s join = s g + Δ s offset s_{\text{join}} = s_g+\Delta s_{\text{offset}} sjoin=sg+Δsoffset
这里：
s g : 自由空间回归目标在道路中心线上的弧长坐标 s_g: \text{自由空间回归目标在道路中心线上的弧长坐标} sg:自由空间回归目标在道路中心线上的弧长坐标
Δ s offset : 从回归目标向前错开的拼接距离 \Delta s_{\text{offset}}: \text{从回归目标向前错开的拼接距离} Δsoffset:从回归目标向前错开的拼接距离
s join : 真正开始取道路中心线的弧长位置 s_{\text{join}}: \text{真正开始取道路中心线的弧长位置} sjoin:真正开始取道路中心线的弧长位置
弧长 s s s 可以理解为"沿着道路中心线走了多少米"。如果道路中心线是一条曲线 C ( s ) C(s) C(s)，那么 s = 0 s=0 s=0 是某个起始点， s = 10 s=10 s=10 表示沿中心线向前走 10 m 10\,\text{m} 10m 的位置。
常见偏移量可以是：
Δ s offset = 1.0 m \Delta s_{\text{offset}}=1.0\,\text{m} Δsoffset=1.0m
举个例子。若自由空间回归目标在中心线上的弧长为：
s g = 25.0 m s_g=25.0\ \text{m} sg=25.0 m
并取：
Δ s offset = 1.0 m \Delta s_{\text{offset}}=1.0\ \text{m} Δsoffset=1.0 m
则：
s join = 26.0 m s_{\text{join}}=26.0\ \text{m} sjoin=26.0 m
也就是说，自由空间段负责开到回归目标附近，道路中心线段从目标前方 1 m 1\,\text{m} 1m 左右开始接管。
然后从道路中心线提取：
C ( s ) , s ∈ s join , s terminal C(s),\quad s\ins_{\\text{join}},s_{\\text{terminal}} C(s),s∈sjoin,sterminal
其中：
C ( s ) : 道路中心线在弧长 s 处的位姿或路径点 C(s): \text{道路中心线在弧长 }s\text{ 处的位姿或路径点} C(s):道路中心线在弧长 s 处的位姿或路径点
s terminal : 这次规划需要输出到的道路中心线终点弧长 s_{\text{terminal}}: \text{这次规划需要输出到的道路中心线终点弧长} sterminal:这次规划需要输出到的道路中心线终点弧长
s terminal s_{\text{terminal}} sterminal 由前向路径长度或路线终点决定。
最终路径为：
P final = P free ⊕ P center \mathcal{P}{\text{final}} = \mathcal{P}{\text{free}} \oplus \mathcal{P}_{\text{center}} Pfinal=Pfree⊕Pcenter
其中：
P free : Hybrid A* 搜出来的自由空间脱困路径 \mathcal{P}_{\text{free}}: \text{Hybrid A* 搜出来的自由空间脱困路径} Pfree:Hybrid A* 搜出来的自由空间脱困路径
P center : 从道路中心线提取出来的后续道路跟随路径 \mathcal{P}_{\text{center}}: \text{从道路中心线提取出来的后续道路跟随路径} Pcenter:从道路中心线提取出来的后续道路跟随路径
⊕ : 按顺序拼接 \oplus: \text{按顺序拼接} ⊕:按顺序拼接
所以这条公式读作：
$$
\text{最终路径}
\text{自由空间驶出段}
\rightarrow
\text{道路中心线跟随段}
$$
在拼接前，自由空间段末尾通常会在接近回归目标的位置截断。若自由空间段中某个点 p i p_i pi 已经满足：
∥ p i − p g ∥ < 1.0 m \|p_i-p_g\|<1.0\,\text{m} ∥pi−pg∥<1.0m
这里：
p i : 自由空间路径上的某个点 p_i: \text{自由空间路径上的某个点} pi:自由空间路径上的某个点
p g : 自由空间回归目标点 p_g: \text{自由空间回归目标点} pg:自由空间回归目标点
∥ p i − p g ∥ : 二者之间的二维欧氏距离 \|p_i-p_g\|: \text{二者之间的二维欧氏距离} ∥pi−pg∥:二者之间的二维欧氏距离
则可以删除它之后的自由空间尾巴。这样做是为了避免自由空间搜索末端和道路中心线段在同一区域重复覆盖，减少拼接处的小折线和速度语义冲突。
拼接后通常会用样条或固定间隔重新采样，使点间距更均匀：
P resampled = Resample ( P final , Δ s path ) \mathcal{P}{\text{resampled}} = \operatorname{Resample} \left( \mathcal{P}{\text{final}},\Delta s_{\text{path}} \right) Presampled=Resample(Pfinal,Δspath)
这里：
Resample ( ⋅ ) : 重新采样操作 \operatorname{Resample}(\cdot): \text{重新采样操作} Resample(⋅):重新采样操作
Δ s path : 期望的相邻路径点间隔 \Delta s_{\text{path}}: \text{期望的相邻路径点间隔} Δspath:期望的相邻路径点间隔
例如拼接后的路径点间距可能忽大忽小：
0.1 m , 0.8 m , 0.2 m 0.1\ \text{m},\quad 0.8\ \text{m},\quad 0.2\ \text{m} 0.1 m,0.8 m,0.2 m
重新采样后可以变得更均匀，例如每隔：
Δ s path = 0.5 m \Delta s_{\text{path}}=0.5\ \text{m} Δspath=0.5 m
生成一个路径点。这样后续速度规划和控制器更容易稳定处理。
如果整条路径终点不是全局路线终点，就不应该强制在该末端停车；如果终点就是路线目标，则末端速度应当允许停止。这就是自由空间驶出和普通道路跟随连接时需要处理的纵向语义。
还要注意，自由空间搜索得到的是几何路径，但下游模块通常需要带道路语义的路径。于是路径点会关联到当前道路车道、驶出相关车道或附近可用车道。这个关联不改变 Hybrid A* 的几何搜索结果，却能让后续模块继续理解路径属于哪个道路区域、如何生成可行驶区域、如何处理转向灯和速度。
25. 可行驶区域为什么要放宽
普通车道路径通常可以用车道边界生成可行驶区域。但 Freespace Pull Out 的前半段可能在路肩、停车区或车道边界外侧，不能只依赖车道边界。
因此自由空间驶出时，路径周围的可行驶区域需要按车辆宽度额外放宽。若车辆宽度为 w veh w_{\text{veh}} wveh，可以使用类似下面的裕度：
d drivable = w veh 2 + w veh = 1.5 w veh d_{\text{drivable}} = \frac{w_{\text{veh}}}{2} + w_{\text{veh}} = 1.5w_{\text{veh}} ddrivable=2wveh+wveh=1.5wveh
这里：
w veh : 车辆宽度 w_{\text{veh}}: \text{车辆宽度} wveh:车辆宽度
w veh 2 : 车辆中心线到车身侧边的半宽 \frac{w_{\text{veh}}}{2}: \text{车辆中心线到车身侧边的半宽} 2wveh:车辆中心线到车身侧边的半宽
d drivable : 路径中心线向左右扩展出的可行驶区域裕度 d_{\text{drivable}}: \text{路径中心线向左右扩展出的可行驶区域裕度} ddrivable:路径中心线向左右扩展出的可行驶区域裕度
其中 w veh 2 \frac{w_{\text{veh}}}{2} 2wveh 对应车辆中心到侧边的半宽，额外的 w veh w_{\text{veh}} wveh 提供自由空间轨迹附近的容错范围。
举个例子。若车辆宽度为：
w veh = 2.0 m w_{\text{veh}}=2.0\ \text{m} wveh=2.0 m
则：
d drivable = 1.5 × 2.0 = 3.0 m d_{\text{drivable}}=1.5\times2.0=3.0\ \text{m} ddrivable=1.5×2.0=3.0 m
这表示自由空间路径周围会生成相对宽一些的可行驶区域，避免车辆刚从路肩或停车区驶出时，被过窄的道路车道边界裁掉。
这不意味着车辆可以随便开到不可行驶区域，而是说：自由空间脱困段不能简单使用车道中心线附近的狭窄走廊，否则刚从路肩起步就可能被自己的可行驶区域裁掉。
26. 默认参数如何影响算法行为
下面用一组典型参数解释算法风格。它们不是唯一正确值，但能帮助理解每个旋钮的作用。
回归目标搜索：
d start = 20.0 m d_{\text{start}}=20.0\,\text{m} dstart=20.0m
d end = 30.0 m d_{\text{end}}=30.0\,\text{m} dend=30.0m
Δ s = 2.0 m \Delta s=2.0\,\text{m} Δs=2.0m
这里：
d start : 开始寻找道路回归目标的前向距离 d_{\text{start}}: \text{开始寻找道路回归目标的前向距离} dstart:开始寻找道路回归目标的前向距离
d end : 结束寻找道路回归目标的前向距离 d_{\text{end}}: \text{结束寻找道路回归目标的前向距离} dend:结束寻找道路回归目标的前向距离
Δ s : 相邻候选回归目标之间的弧长间隔 \Delta s: \text{相邻候选回归目标之间的弧长间隔} Δs:相邻候选回归目标之间的弧长间隔
这意味着车辆会尝试回到前方 20 20 20 到 30 30 30 米之间的道路中心线点，每隔 2 2 2 米一个候选。候选弧长大致是：
20 , 22 , 24 , 26 , 28 , 30 m 20,\ 22,\ 24,\ 26,\ 28,\ 30\ \text{m} 20, 22, 24, 26, 28, 30 m
如果目标太近，车辆可能还没来得及摆正；如果目标太远，搜索空间会变大，计算更慢。
低速驶出速度：
v free = 1.0 m/s v_{\text{free}}=1.0\,\text{m/s} vfree=1.0m/s
这里 v free v_{\text{free}} vfree 是自由空间脱困段的参考速度。自由空间脱困不是高速规划，低速能让碰撞风险和控制误差更可控。
例如：
1.0 m/s = 3.6 km/h 1.0\,\text{m/s}=3.6\,\text{km/h} 1.0m/s=3.6km/h
这接近低速挪车速度，适合路边起步、狭窄空间调整和倒车切换。
车辆形状膨胀：
m shape = 1.0 m m_{\text{shape}}=1.0\,\text{m} mshape=1.0m
这里 m shape m_{\text{shape}} mshape 是车辆形状膨胀裕量。车长、车宽会被加上安全裕量，碰撞检测更保守。
例如车辆实际宽度为：
w = 2.0 m w=2.0\,\text{m} w=2.0m
若左右总共按裕量膨胀，碰撞检测使用的等效宽度可能显著大于实际车宽。这样能覆盖定位误差、地图误差和控制误差，但也会让窄通道更容易被判定为不可通行。
搜索时间限制：
T max = 3000 ms T_{\max}=3000\,\text{ms} Tmax=3000ms
这里 T max T_{\max} Tmax 是单次搜索允许消耗的最大时间。自由空间搜索可能很重，必须有时间上限。超过上限就认为本次没有找到可行解。
3000 ms = 3.0 s 3000\,\text{ms}=3.0\,\text{s} 3000ms=3.0s
这表示规划器不会无限搜索下去。对在线自动驾驶系统来说，"及时失败"比"无限等待"更安全，因为上层模块还可以选择停车、换目标或重新规划。
角度离散：
N θ = 120 N_\theta=120 Nθ=120
对应：
Δ θ = 3 ∘ \Delta\theta=3^\circ Δθ=3∘
这里：
N θ : 一圈 360 ∘ 被分成的航向桶数量 N_\theta: \text{一圈 }360^\circ\text{ 被分成的航向桶数量} Nθ:一圈 360∘ 被分成的航向桶数量
Δ θ : 每个航向桶覆盖的角度宽度 \Delta\theta: \text{每个航向桶覆盖的角度宽度} Δθ:每个航向桶覆盖的角度宽度
二者关系是：
Δ θ = 360 ∘ N θ \Delta\theta=\frac{360^\circ}{N_\theta} Δθ=Nθ360∘
所以当 N θ = 120 N_\theta=120 Nθ=120 时：
Δ θ = 360 ∘ 120 = 3 ∘ \Delta\theta=\frac{360^\circ}{120}=3^\circ Δθ=120360∘=3∘
航向桶越多，姿态表示越精细，但状态数量也越多；航向桶越少，搜索更快，但可能错过需要精细角度调整的路径。
转角使用比例与步数：
ρ = 0.7 , N s = 1 \rho=0.7,\quad N_s=1 ρ=0.7,Ns=1
这里：
ρ : 最大物理转角的使用比例 \rho: \text{最大物理转角的使用比例} ρ:最大物理转角的使用比例
N s : 转角离散步数 N_s: \text{转角离散步数} Ns:转角离散步数
实际使用最大转角为车辆物理最大转角的 70 % 70\% 70%，并只使用左、直、右三种转角。
例如车辆最大前轮转角为：
δ vehicle = 35 ∘ \delta_{\text{vehicle}}=35^\circ δvehicle=35∘
则搜索使用的最大转角为：
δ max = 0.7 × 35 ∘ = 24.5 ∘ \delta_{\max}=0.7\times35^\circ=24.5^\circ δmax=0.7×35∘=24.5∘
当 N s = 1 N_s=1 Ns=1 时，动作集就是：
{ − 24.5 ∘ , 0 , + 24.5 ∘ } \{-24.5^\circ,\ 0,\ +24.5^\circ\} {−24.5∘, 0, +24.5∘}
目标容差：
L goal = 2.0 m L_{\text{goal}}=2.0\,\text{m} Lgoal=2.0m
W goal = 0.5 m W_{\text{goal}}=0.5\,\text{m} Wgoal=0.5m
Θ goal = 6 ∘ \Theta_{\text{goal}}=6^\circ Θgoal=6∘
实际判定半宽为 1.0 m 1.0\,\text{m} 1.0m、 0.25 m 0.25\,\text{m} 0.25m、 3 ∘ 3^\circ 3∘。
这组参数表达的是：位置可以有一点纵向误差，但横向和角度必须比较准。因为车辆接回道路中心线时，横向偏差和车头角度会直接影响拼接是否自然。
代价权重：
w c = 0.5 w_c=0.5 wc=0.5
w r = 1.0 w_r=1.0 wr=1.0
w sw = 1.5 w_{\text{sw}}=1.5 wsw=1.5
w h = 2.0 w_h=2.0 wh=2.0
w s = 0.5 w_s=0.5 ws=0.5
w o = 1.75 w_o=1.75 wo=1.75
w lat = 5.0 w_{\text{lat}}=5.0 wlat=5.0
这些值表达了明确偏好：可以倒车，但倒车更贵；可以转弯，但急转更贵；可以换挡，但频繁换挡更贵；接近障碍物有惩罚；接近目标时横向偏差惩罚较强。
可以把这些权重理解成搜索的"性格"。例如：
w r = 1.0 w_r=1.0 wr=1.0
表示倒车同等距离约翻倍；而：
w lat = 5.0 w_{\text{lat}}=5.0 wlat=5.0
表示目标附近横向偏差 0.2 m 0.2\,\text{m} 0.2m 就会带来：
5.0 × 0.2 = 1.0 5.0\times0.2=1.0 5.0×0.2=1.0
的额外代价，因此最后接回道路时会明显追求横向对齐。
障碍物阈值：
τ obs = 30 \tau_{\text{obs}}=30 τobs=30
这里 τ obs \tau_{\text{obs}} τobs 是占据判断阈值。代价地图中大于等于该值的格子会被视为障碍物，未知格子也会被视为不可通行。
例如某格子的代价值为：
M i j = 45 M_{ij}=45 Mij=45
因为：
45 ≥ 30 45\ge30 45≥30
所以它会被当作障碍物。若另一个格子：
M i j = 10 M_{ij}=10 Mij=10
则它不会因为这个阈值被直接判为障碍物，但仍可能因为离障碍物太近而在代价函数里受到惩罚。
27. 怎样调参才有方向感
如果搜索经常找不到路径，可以先判断是哪类失败。
若空间其实足够，但搜索超时，常见原因是搜索分辨率太细或启发不够强。可以考虑增大基础扩展距离、减小角度桶数量、提高启发权重，或缩短目标候选距离范围。
若路径太贴近障碍物，可以增大车辆形状裕量：
m shape ↑ m_{\text{shape}}\uparrow mshape↑
或增大障碍物距离权重：
w o ↑ w_o\uparrow wo↑
也可以降低障碍物阈值，让更多高代价区域被视为不可通行：
τ obs ↓ \tau_{\text{obs}}\downarrow τobs↓
这三种调法的区别是：
m shape ↑ : 让碰撞检测里的车变大，更保守 m_{\text{shape}}\uparrow: \text{让碰撞检测里的车变大，更保守} mshape↑:让碰撞检测里的车变大，更保守
w o ↑ : 不一定禁止贴近障碍物，但让贴近障碍物更贵 w_o\uparrow: \text{不一定禁止贴近障碍物，但让贴近障碍物更贵} wo↑:不一定禁止贴近障碍物，但让贴近障碍物更贵
τ obs ↓ : 把更多代价地图格子直接变成不可通行障碍 \tau_{\text{obs}}\downarrow: \text{把更多代价地图格子直接变成不可通行障碍} τobs↓:把更多代价地图格子直接变成不可通行障碍
例如路径只是略微贴近路沿，但仍有空间，可以优先增大 w o w_o wo；如果路径已经危险地穿过高代价区域，可以考虑降低 τ obs \tau_{\text{obs}} τobs 或增加形状裕量。
若路径频繁前后挪车，可以增大换挡权重或倒车权重：
w sw ↑ , w r ↑ w_{\text{sw}}\uparrow,\quad w_r\uparrow wsw↑,wr↑
二者也有区别：
w sw ↑ : 主要惩罚前进和倒车之间的切换次数 w_{\text{sw}}\uparrow: \text{主要惩罚前进和倒车之间的切换次数} wsw↑:主要惩罚前进和倒车之间的切换次数
w r ↑ : 整体提高倒车动作的代价 w_r\uparrow: \text{整体提高倒车动作的代价} wr↑:整体提高倒车动作的代价
如果路径是"倒一下再顺利开走"，但你不希望来回反复挪，优先调大 w sw w_{\text{sw}} wsw。如果你希望只在必要时才倒车，调大 w r w_r wr 更直接。
若路径过于僵硬，狭窄区域里转不过去，可以增加转角步数：
N s ↑ N_s\uparrow Ns↑
这样动作集更丰富，但搜索分支数也会增加：
B = 2 ( 2 N s + 1 ) B=2(2N_s+1) B=2(2Ns+1)
例如允许倒车时：
N s = 1 ⇒ B = 2 ( 2 × 1 + 1 ) = 6 N_s=1\Rightarrow B=2(2\times1+1)=6 Ns=1⇒B=2(2×1+1)=6
如果增加到：
N s = 2 N_s=2 Ns=2
则：
B = 2 ( 2 × 2 + 1 ) = 10 B=2(2\times2+1)=10 B=2(2×2+1)=10
每个节点最多尝试的动作从 6 6 6 个增加到 10 10 10 个，路径选择更丰富，但搜索量也会明显增加。
若目标附近接不顺，可以增大目标横向代价：
w lat ↑ w_{\text{lat}}\uparrow wlat↑
或适当调整目标搜索距离，让回归点落在更开阔、更顺直的位置。
这里也要区分两个方向：
w lat ↑ : 让车辆在目标附近更努力贴近目标中心线 w_{\text{lat}}\uparrow: \text{让车辆在目标附近更努力贴近目标中心线} wlat↑:让车辆在目标附近更努力贴近目标中心线
d start , d end 调整 : 改变候选回归目标的位置 d_{\text{start}},d_{\text{end}}\text{ 调整}: \text{改变候选回归目标的位置} dstart,dend 调整:改变候选回归目标的位置
如果目标本身选在弯道、障碍物旁边或空间很挤的位置，只增大 w lat w_{\text{lat}} wlat 不一定有效，因为目标点就不适合接入。此时应该调整目标候选范围。
若路径太保守，经常认为起点或目标碰撞，需要检查车辆形状裕量和代价地图障碍物膨胀是否叠加过度。因为碰撞检测中的车辆形状已经带裕量，代价地图本身如果也做了强膨胀，二者叠加会让可行空间显著变窄。
28. Hybrid A* 与 RRT* 的边界
自由空间驶出层可以接受不同的自由空间规划器。Hybrid A* 和 RRT* 都能解决从起点到目标的可行路径问题，但它们风格不同。
Hybrid A* 更像确定性的图搜索：
栅格状态 + 车辆运动原语 + A* 代价 \text{栅格状态} + \text{车辆运动原语} + \text{A* 代价} 栅格状态+车辆运动原语+A* 代价
它的优点是行为稳定、可解释、容易通过代价函数塑造驾驶偏好。缺点是栅格和角度离散会带来状态爆炸。
RRT* 更像采样式搜索：
随机采样 + 树扩展 + 渐近优化 \text{随机采样} + \text{树扩展} + \text{渐近优化} 随机采样+树扩展+渐近优化
它在高维空间和复杂几何中很灵活，但输出稳定性、可重复性和代价塑形通常不如 Hybrid A* 直观。
在倒车能力上，两者的配置习惯也不同：Hybrid A* 可以显式关闭倒车扩展，从而只搜索前进可行路径；RRT* 类自由空间脱困通常默认允许倒车，因为采样树需要更强的机动性来穿过狭窄空间。
在路边起步脱困中，Hybrid A* 的优势是：车辆低速、目标明确、地图是占据栅格、路径需要可解释且稳定。因此它非常适合做默认自由空间驶出算法。
29. 算法的能力边界
Hybrid A* Freespace Pull Out 很强，但不是万能的。
第一，它依赖代价地图质量。如果障碍物漏检，搜索可能穿过真实障碍；如果误检太多，搜索可能认为没有可行空间。
第二，它是离散搜索。角度桶、栅格分辨率、扩展步长都会影响可达性。连续世界中存在的窄通道，在离散搜索里可能被错过。
第三，它主要解决几何和静态障碍物问题。动态来车、行人预测、交互博弈通常需要外层行为安全模块继续判断。
第四，代价函数不等于车辆控制器。Hybrid A* 给出的是几何可行路径，但路径是否足够平滑、速度是否合适、控制器能否稳定跟踪，还需要后续轨迹平滑和控制闭环保证。
第五，允许倒车会增加可行性，也会增加执行复杂度。每个换挡点都意味着车辆可能需要停车、重新确认安全，再继续下一段。
理解这些边界很重要。自由空间规划不是替代整套行为规划，而是在普通车道语义方法失败时，为车辆提供一个低速、保守、可执行的脱困方案。
30. 从零复现这个算法的最小步骤
如果要自己实现一个同类 Hybrid A* Freespace Pull Out，可以按下面的自然顺序搭建。
第一步，准备占据栅格。把未知区域和高代价区域视为障碍物：
O ( i , j ) = 1 ⇔ M i j < 0 或 M i j ≥ τ obs O(i,j)=1 \Leftrightarrow M_{ij}<0 \text{ 或 } M_{ij}\ge\tau_{\text{obs}} O(i,j)=1⇔Mij<0 或 Mij≥τobs
这里：
M i j : 代价地图中格子 ( i , j ) 的原始代价值 M_{ij}: \text{代价地图中格子 }(i,j)\text{ 的原始代价值} Mij:代价地图中格子 (i,j) 的原始代价值
τ obs : 把代价值判为障碍物的阈值 \tau_{\text{obs}}: \text{把代价值判为障碍物的阈值} τobs:把代价值判为障碍物的阈值
M i j < 0 : 未知区域，通常保守地当作不可通行 M_{ij}<0: \text{未知区域，通常保守地当作不可通行} Mij<0:未知区域，通常保守地当作不可通行
M i j ≥ τ obs : 代价太高，视为障碍物 M_{ij}\ge\tau_{\text{obs}}: \text{代价太高，视为障碍物} Mij≥τobs:代价太高，视为障碍物
这一步的目标很简单：先把地图变成"能不能走"的二值世界。比如某格子代价为 45 45 45，阈值是 30 30 30，那它就直接算障碍物；如果代价是 10 10 10，则暂时认为可通行，但后面还会被 EDT、车身碰撞和代价函数进一步检查。
第二步，把起点和道路回归目标转成地图局部坐标，并离散为：
( i , j , k ) (i,j,k) (i,j,k)
这里：
i , j : 车辆基准点所在的栅格索引 i,j: \text{车辆基准点所在的栅格索引} i,j:车辆基准点所在的栅格索引
k : 航向桶索引 k: \text{航向桶索引} k:航向桶索引
这一步的意思是把连续世界里的一点，变成搜索图里的一个状态编号。比如车辆当前位置是 ( x , y , θ ) (x,y,\theta) (x,y,θ)，地图分辨率是 0.2 m 0.2\,\text{m} 0.2m，航向桶宽度是 3 ∘ 3^\circ 3∘，那么同一个连续位姿会落到某个固定的 ( i , j , k ) (i,j,k) (i,j,k) 上，A* 就能在这个离散状态空间里展开。
第三步，构建矩形车身碰撞检测。车辆区域为：
B ( q ) = p + R ( θ ) B \mathcal{B}(q) = p+R(\theta)\mathcal{B} B(q)=p+R(θ)B
这里：
q : 车辆当前位姿 q: \text{车辆当前位姿} q:车辆当前位姿
p : 车辆基准点在地图中的位置向量 p: \text{车辆基准点在地图中的位置向量} p:车辆基准点在地图中的位置向量
R ( θ ) : 由车头朝向 θ 生成的旋转矩阵 R(\theta): \text{由车头朝向 }\theta\text{ 生成的旋转矩阵} R(θ):由车头朝向 θ 生成的旋转矩阵
B : 车辆在自身坐标系下的矩形轮廓区域 \mathcal{B}: \text{车辆在自身坐标系下的矩形轮廓区域} B:车辆在自身坐标系下的矩形轮廓区域
这条式子表示：先把车辆在自身坐标系里的矩形车身旋转到当前朝向，再平移到地图中的基准点位置。比如车辆本体是一个长方形，朝向为 30 ∘ 30^\circ 30∘，那么它在地图上覆盖的区域就是"旋转后的长方形"。
并预计算每个航向桶的车身覆盖格子。
例如每个航向桶预先存一份"车身会碰到哪些格子"的列表，搜索时就不用每次都重新旋转和采样整辆车，速度会快很多。
第四步，计算 EDT：
D obs ( i , j ) D_{\text{obs}}(i,j) Dobs(i,j)
这里 D obs ( i , j ) D_{\text{obs}}(i,j) Dobs(i,j) 是格子 ( i , j ) (i,j) (i,j) 到最近障碍物的欧氏距离。它至少有三个用途：
加速碰撞判断 调整扩展距离 构造障碍物距离代价 \text{加速碰撞判断} \quad \text{调整扩展距离} \quad \text{构造障碍物距离代价} 加速碰撞判断调整扩展距离构造障碍物距离代价
比如某格子离最近障碍物只有 0.3 m 0.3\,\text{m} 0.3m，那它显然不适合做大步长扩展；如果离障碍物有 2.0 m 2.0\,\text{m} 2.0m，则可以更放心地把它视为较宽松的区域。
第五步，定义运动原语。对每个转角：
δ u = u δ max N s \delta_u=u\frac{\delta_{\max}}{N_s} δu=uNsδmax
这里：
δ u : 第 u 个离散转角对应的真实前轮转角 \delta_u: \text{第 }u\text{ 个离散转角对应的真实前轮转角} δu:第 u 个离散转角对应的真实前轮转角
δ max : 允许搜索使用的最大转角 \delta_{\max}: \text{允许搜索使用的最大转角} δmax:允许搜索使用的最大转角
N s : 从 0 到最大转角分成的步数 N_s: \text{从 }0\text{ 到最大转角分成的步数} Ns:从 0 到最大转角分成的步数
例如当 N s = 1 N_s=1 Ns=1 时，只有 − δ max , 0 , + δ max -\delta_{\max},0,+\delta_{\max} −δmax,0,+δmax 三种动作；当 N s = 2 N_s=2 Ns=2 时，就会有更细的转角档位。
对前进和倒车距离 d d d，用自行车模型生成：
q ′ = F ( q , δ u , d ) q'=F(q,\delta_u,d) q′=F(q,δu,d)
这里：
F ( ⋅ ) : 自行车模型的积分映射 F(\cdot): \text{自行车模型的积分映射} F(⋅):自行车模型的积分映射
q ′ : 根据当前状态和动作计算出的下一状态 q': \text{根据当前状态和动作计算出的下一状态} q′:根据当前状态和动作计算出的下一状态
它回答的是"我现在朝这个方向打这个角，走这么远之后会到哪里"。
第六步，设计代价函数：
g ′ = g + C motion + C smooth + C switch + C obs + C lat g'=g + C_{\text{motion}} + C_{\text{smooth}} + C_{\text{switch}} + C_{\text{obs}} + C_{\text{lat}} g′=g+Cmotion+Csmooth+Cswitch+Cobs+Clat
这里的意思是：每生成一个新节点，就把本次动作造成的代价加到父节点代价上，得到新的累计代价。它不像单纯的最短路，只关心距离；而是把"走得稳不稳、离障碍物远不远、换挡多不多"都放进去。
第七步，设计启发函数：
h = w h max ( D free , D RS ) h=w_h\max(D_{\text{free}},D_{\text{RS}}) h=whmax(Dfree,DRS)
这里：
h : 从当前节点到目标的估计剩余代价 h: \text{从当前节点到目标的估计剩余代价} h:从当前节点到目标的估计剩余代价
w h : 启发权重 w_h: \text{启发权重} wh:启发权重
D free , D RS : 两个互补的距离估计 D_{\text{free}},D_{\text{RS}}: \text{两个互补的距离估计} Dfree,DRS:两个互补的距离估计
它的作用是告诉 A*：别只盯着已经走过的代价 g g g，还要看"离目标还差多少"。例如当前节点已经绕开障碍物，但车头还没对准目标， h h h 仍然会提醒它继续朝合理方向搜索。
第八步，用 A* 优先队列搜索：
f = g + h f=g+h f=g+h
这里 f f f 是排序键。A* 每次都从 Open 集合里取 f f f 最小的节点。你可以把它理解成"当前已花费 + 未来预计还要花费"的总账。总账最小的节点，先拿出来继续扩展。
直到满足目标窗口：
∣ x rel ∣ ≤ ϵ x , ∣ y rel ∣ ≤ ϵ y , ∣ θ rel ∣ ≤ ϵ θ |x_{\text{rel}}|\le\epsilon_x,\quad |y_{\text{rel}}|\le\epsilon_y,\quad |\theta_{\text{rel}}|\le\epsilon_\theta ∣xrel∣≤ϵx,∣yrel∣≤ϵy,∣θrel∣≤ϵθ
这里的三个条件分别对应前后、左右和朝向。一个节点必须三项都过线，才能算到达目标。这样做的好处是，最后得到的路径不仅"到了"，而且"姿态也像样"。
第九步，沿父节点回溯路径，并根据运动方向切分前进/倒车子路径。
这里的父节点回溯，就是从目标节点一路沿着 parent 指针回走到起点；方向切分则是把"倒车段"和"前进段"拆开，方便车辆执行时在换挡点停车。
第十步，把自由空间段接回道路中心线，并重新采样、修正速度、生成可执行路径。
这里的关键不是"几何上把两段拼上"，而是拼接后仍然要保留车辆能执行的速度语义、方向语义和道路语义。否则几何曲线看起来连上了，实际控制却可能很别扭。
完成这十步，就已经具备一个完整的自由空间起步规划器雏形。
31. 总结
Hybrid A* Freespace Pull Out 的本质，是把"路边起步失败后的脱困"建模成一个带车辆运动学约束的占据栅格搜索问题。
它首先在道路中心线前方选择回归目标：
q g = C ( s m ) q_g=C(s_m) qg=C(sm)
然后在三维状态空间中搜索：
q = ( x , y , θ ) q=(x,y,\theta) q=(x,y,θ)
用自行车模型生成可执行运动：
q ′ = F ( q , δ , d ) q'=F(q,\delta,d) q′=F(q,δ,d)
用矩形车身和 EDT 判断安全：
collision ( q ′ ) = 0 \operatorname{collision}(q')=0 collision(q′)=0
用综合代价塑造驾驶偏好：
f = g + h f=g+h f=g+h
其中：
g = 距离 + 转弯 + 倒车 + 换挡 + 障碍物距离 + 目标横向对齐 g= \text{距离} + \text{转弯} + \text{倒车} + \text{换挡} + \text{障碍物距离} + \text{目标横向对齐} g=距离+转弯+倒车+换挡+障碍物距离+目标横向对齐
h = 自由空间距离 ∨ Reeds-Shepp 车辆距离 h= \text{自由空间距离} \vee \text{Reeds-Shepp 车辆距离} h=自由空间距离∨Reeds-Shepp 车辆距离
最后把自由空间轨迹切分为可执行子路径，并接回道路中心线。
从学习角度看，可以把这套算法记成一句话：
用 A* 决定往哪里走，用车辆模型决定能不能这么走，用代价函数决定这样走好不好。 \text{用 A* 决定往哪里走，用车辆模型决定能不能这么走，用代价函数决定这样走好不好。} 用 A* 决定往哪里走，用车辆模型决定能不能这么走，用代价函数决定这样走好不好。
这就是 Hybrid A* 在 Freespace Pull Out 中的核心价值。