参考来源：ChatGPT、autoware
从起点到目标点：自动驾驶车道级路线规划教程
自动驾驶里的"路线规划"很容易被误解成"画一条能开的曲线"。实际上，在完整的规划栈里，它通常做的是更高一层的事情：根据车辆当前位置、目标点和静态高精地图，找出车辆应该经过哪些车道、允许在哪些相邻车道内行驶，并把这个车道级结果交给后续的行为规划、路径规划和控制模块。
这篇教程从零开始讲清楚这种车道级路线规划的完整算法。读完之后，即使不了解 Autoware，也应该能够理解：为什么要把道路建模成图、起点和终点怎样投影到车道上、最短路线怎样产生、为什么路线输出不是单条曲线、目标点为什么需要合法性检查，以及行驶中重规划为什么必须先做安全距离判断。
1. 先分清"路线""路径"和"轨迹"
自动驾驶规划通常是分层的。
路线是最上层的导航结果，回答的问题是：从当前位置到目的地，应该经过哪些道路和车道。它的粒度是车道段，不直接关心障碍物、不输出每一时刻的速度，也不生成车辆可直接跟踪的连续轨迹。
路径是在路线约束下生成的几何曲线，回答的问题是：车辆在这些车道范围内具体沿哪条线走。它会考虑车道中心线、避障、靠边停车、换道等局部几何约束。
轨迹是在路径基础上加入时间和速度，回答的问题是：车辆在每个时刻应该在哪里、以多快速度运动。
可以用一个简单关系表示：
Route→Path→Trajectory \text{Route} \rightarrow \text{Path} \rightarrow \text{Trajectory} Route→Path→Trajectory
路线规划只做最左边这一层。因此它主要依赖静态地图、车辆当前位姿、目标位姿和必要的途经点，而不直接处理行人、车辆、施工封路等动态因素。动态环境一般留给下游行为规划和局部规划处理。
2. 把道路变成可搜索的车道图
要从起点到终点找路线，第一步是把真实道路抽象成图。
高精地图里的基础单元可以理解为一个个"车道片段"。每个车道片段由左边界、右边界和中心线组成。左、右边界围成一个多边形，中心线表示该车道片段的参考行驶方向。
对第 iii 个车道片段，可以记为：
ℓi=(BiL,BiR,Ci,Ai) \ell_i = \left(B_i^L, B_i^R, C_i, A_i\right) ℓi=(BiL,BiR,Ci,Ai)
其中：
BiLB_i^LBiL 是左边界折线，BiRB_i^RBiR 是右边界折线，CiC_iCi 是中心线折线，AiA_iAi 是属性集合，例如道路类型、是否可通行、是否是路肩、是否属于停车区域等。
中心线通常是一串三维点：
Ci={ci,0,ci,1,...,ci,n} C_i = \{c_{i,0}, c_{i,1}, \dots, c_{i,n}\} Ci={ci,0,ci,1,...,ci,n}
车道长度可以由中心线分段累加得到：
L(ℓi)=∑k=0n−1∥ci,k+1−ci,k∥2 L(\ell_i)=\sum_{k=0}^{n-1}\|c_{i,k+1}-c_{i,k}\|_2 L(ℓi)=k=0∑n−1∥ci,k+1−ci,k∥2
有了车道片段之后，地图还需要描述车道之间的拓扑关系。常见关系包括：
- 后继关系：车道 ℓa\ell_aℓa 的末端连接到车道 ℓb\ell_bℓb 的起点，车辆可以从 ℓa\ell_aℓa 向前进入 ℓb\ell_bℓb。
- 前驱关系：后继关系的反方向。
- 可换道左邻或右邻：车辆允许从当前车道换到相邻车道。
- 几何相邻但不可直接换道：两个车道挨着，但中间可能有实线、隔离带或规则限制。
于是，道路地图可以抽象为有向图：
G=(V,E) G=(V,E) G=(V,E)
其中 VVV 是所有车道片段集合，EEE 是车道之间允许通行或换道的关系集合。路线规划本质上就是在这个图上做搜索。
从 OSM 地图生成车道图
现在把这个抽象过程落到一张实际的 Lanelet2 OSM 地图上。OSM 文件里有三层元素：node、way、relation。它们和路线图之间的关系是：
OSM node→几何点 \text{OSM node} \rightarrow \text{几何点} OSM node→几何点
OSM way→有序折线 \text{OSM way} \rightarrow \text{有序折线} OSM way→有序折线
OSM relation(type=lanelet)→车道片段 \text{OSM relation(type=lanelet)} \rightarrow \text{车道片段} OSM relation(type=lanelet)→车道片段
注意，OSM 里的 <node> 不是路线图里的节点。OSM node 只是一个经纬高程点；路线图里的一个节点 v∈Vv\in Vv∈V 通常对应一个完整的 lanelet，也就是一个由左右边界围成的车道片段。
以 lanelet2_map.osm 中的 lanelet 9102 为例，它可以写成：
xml
<relation id='9102'>
<member type='way' ref='9536' role='left' />
<member type='way' ref='9462' role='right' />
<tag k='one_way' v='yes' />
<tag k='subtype' v='road' />
<tag k='type' v='lanelet' />
</relation>这表示 lanelet 9102 的左边界是 way 9536，右边界是 way 9462。继续展开：
xml
<way id='9536'>
<nd ref='1718' />
<nd ref='1719' />
<nd ref='9535' />
</way>
<way id='9462'>
<nd ref='1097' />
<nd ref='1095' />
<nd ref='2193' />
</way>每个 nd ref 指向一个 OSM node。一个 OSM node 保存的是经纬度和高程：
gk=(latk,lonk,elek) g_k=(\operatorname{lat}_k,\operatorname{lon}_k,\operatorname{ele}_k) gk=(latk,lonk,elek)
路线规划不能直接在经纬度上做欧氏距离和多边形计算，因此加载地图时会把经纬度投影到局部地图坐标系：
pk=Π(gk)=(xk,yk,zk) p_k=\Pi(g_k)=(x_k,y_k,z_k) pk=Π(gk)=(xk,yk,zk)
其中 Π\PiΠ 是地图投影函数，例如把 WGS84 经纬度转换到局部平面坐标。这样，一个 way 就从 OSM node 序列变成了三维折线：
Wj=[pj,0,pj,1,...,pj,nj] W_j=\left[p_{j,0},p_{j,1},\dots,p_{j,n_j}\right] Wj=[pj,0,pj,1,...,pj,nj]
因此 lanelet 9102 可以表示为：
ℓ9102=(W9536,W9462,A9102) \ell_{9102}=\left(W_{9536},W_{9462},A_{9102}\right) ℓ9102=(W9536,W9462,A9102)
其中：
A9102={one_way=yes,subtype=road,speed_limit=30,... } A_{9102}=\{\text{one\_way=yes},\text{subtype=road},\text{speed\_limit=30},\dots\} A9102={one_way=yes,subtype=road,speed_limit=30,...}
左、右边界围成车道多边形。若左边界为：
BL=[l0,l1,...,lm] B^L=[l_0,l_1,\dots,l_m] BL=[l0,l1,...,lm]
右边界为：
BR=[r0,r1,...,rn] B^R=[r_0,r_1,\dots,r_n] BR=[r0,r1,...,rn]
则车道多边形可以理解为：
poly(ℓ)=BL⊕reverse(BR) \operatorname{poly}(\ell)=B^L\oplus \operatorname{reverse}(B^R) poly(ℓ)=BL⊕reverse(BR)
其中 ⊕\oplus⊕ 表示首尾拼接。这个多边形用于判断"点是否在车道内"，例如起点、终点或目标车辆轮廓是否合法。
中心线通常不是 OSM relation 直接给出的，而是由左右边界生成或细化出来。一个常见近似是先按归一化弧长参数 u∈[0,1]u\in[0,1]u∈[0,1] 分别在左右边界上采样：
bL(u),bR(u) b^L(u),\quad b^R(u) bL(u),bR(u)
然后取中点作为中心线点：
c(u)=bL(u)+bR(u)2 c(u)=\frac{b^L(u)+b^R(u)}{2} c(u)=2bL(u)+bR(u)
离散采样后得到：
C={c(u0),c(u1),...,c(uN)} C=\{c(u_0),c(u_1),\dots,c(u_N)\} C={c(u0),c(u1),...,c(uN)}
这条中心线就是后面计算车道长度、车道方向、弧长坐标和目标高度投影的基础。
完成所有 lanelet 的解析后，可以生成路线图的顶点集合。对车辆路线规划来说，通常只把车辆可通行的道路 lanelet 放进车辆图：
Vvehicle={ℓi∣type(ℓi)=lanelet,subtype(ℓi)=road,vehicleAllowed(ℓi)=true} V_{\text{vehicle}}=\{\ell_i \mid \operatorname{type}(\ell_i)=\text{lanelet},\operatorname{subtype}(\ell_i)=\text{road},\operatorname{vehicleAllowed}(\ell_i)=\text{true}\} Vvehicle={ℓi∣type(ℓi)=lanelet,subtype(ℓi)=road,vehicleAllowed(ℓi)=true}
接下来生成边集合 EEE。第一类边是前后连接边，也就是"从一个 lanelet 能不能继续开到下一个 lanelet"。设 End(ℓi)\operatorname{End}(\ell_i)End(ℓi) 表示 lanelet iii 末端的左右边界端点集合，Start(ℓj)\operatorname{Start}(\ell_j)Start(ℓj) 表示 lanelet jjj 起点的左右边界端点集合。如果两者在几何上重合或足够接近：
d(End(ℓi),Start(ℓj))<ε d\left(\operatorname{End}(\ell_i),\operatorname{Start}(\ell_j)\right)<\varepsilon d(End(ℓi),Start(ℓj))<ε
并且车道方向连续：
∣norm(ψiend−ψjstart)∣<θconnect \left|\operatorname{norm}\left(\psi_i^{\text{end}}-\psi_j^{\text{start}}\right)\right|<\theta_{\text{connect}} norm(ψiend−ψjstart) <θconnect
同时交通规则允许从 ℓi\ell_iℓi 进入 ℓj\ell_jℓj，则加入一条有向后继边：
(ℓi,ℓj,succeeding)∈E (\ell_i,\ell_j,\text{succeeding})\in E (ℓi,ℓj,succeeding)∈E
由于 lanelet 通常带有 one_way=yes，这个边是有方向的。也就是说，即使几何上 ℓj\ell_jℓj 和 ℓi\ell_iℓi 首尾相接，也不代表能反向从 ℓj\ell_jℓj 开回 ℓi\ell_iℓi。
第二类边是横向关系，也就是左右相邻或可换道关系。设两个 lanelet 共享一条边界，例如：
BiR≈BjL B_i^R \approx B_j^L BiR≈BjL
这表示 ℓj\ell_jℓj 在 ℓi\ell_iℓi 的右侧。是否能从 ℓi\ell_iℓi 换到 ℓj\ell_jℓj，还要看共享边界的线型和交通规则。若共享边界允许跨越，例如虚线或规则允许换道，则加入可换道边：
(ℓi,ℓj,right)∈E (\ell_i,\ell_j,\text{right})\in E (ℓi,ℓj,right)∈E
若它们只是几何相邻，但边界不允许跨越，例如实线，则只记录相邻关系：
(ℓi,ℓj,adjacent_right) (\ell_i,\ell_j,\text{adjacent\_right}) (ℓi,ℓj,adjacent_right)
相邻关系不等价于可换道关系。路线走廊生成时会谨慎使用 adjacent lanelet：只有当前后都能接回主路线时，才可能把它纳入路线范围。
第三类信息来自 regulatory element，例如交通灯、停止线、让行、优先权、道路标记等。它们在 OSM 中通常也是 relation，并被 lanelet 作为 regulatory_element 引用。这些元素不一定都会改变路线图拓扑，但会影响交通规则判断、行为规划和后续决策。例如 right_of_way 可以描述路口优先关系，traffic_light 可以把车道和信号灯关联起来。
综上，从 OSM 到路线图的生成可以写成：
Nosm→ΠP \mathcal{N}_{osm}\xrightarrow{\Pi}\mathcal{P} NosmΠ P
Wosm→按 nd 顺序取点B \mathcal{W}_{osm}\xrightarrow{\text{按 nd 顺序取点}}\mathcal{B} Wosm按 nd 顺序取点 B
Rlanelet→left/right wayL \mathcal{R}_{lanelet}\xrightarrow{\text{left/right way}}\mathcal{L} Rlaneletleft/right way L
L→几何连接 + 交通规则G=(V,E) \mathcal{L}\xrightarrow{\text{几何连接 + 交通规则}}G=(V,E) L几何连接 + 交通规则 G=(V,E)
这里 Nosm\mathcal{N}{osm}Nosm 是 OSM 点集合，Wosm\mathcal{W}{osm}Wosm 是 OSM 折线集合，Rlanelet\mathcal{R}_{lanelet}Rlanelet 是 lanelet relation 集合，L\mathcal{L}L 是解析出的车道片段集合。路线规划算法真正搜索的是最后生成的 G=(V,E)G=(V,E)G=(V,E)，而不是原始 XML 文本。
成熟系统通常还会把"外层路线管理"和"具体地图算法"分开。外层只关心输入输出契约：给定起点、途经点、目标点和地图，返回车道级路线；内层算法可以基于 Lanelet2、其他高精地图格式，甚至完全不同的图搜索策略。这样做的好处是，状态管理、重规划安全、到达判定和结果发布不必和某一种地图格式绑死。
3. 位姿必须先统一到地图坐标系
车辆当前状态、用户给出的目标点、途经点可能来自不同坐标系。路线规划必须先把所有点统一到地图坐标系中，否则同一个数值坐标可能表示完全不同的位置。
一个位姿包含位置和朝向：
x=(p,q) \mathbf{x}=(p,q) x=(p,q)
其中 p=(x,y,z)Tp=(x,y,z)^Tp=(x,y,z)T 是位置，qqq 是四元数朝向。若某个位姿定义在坐标系 FaF_aFa 中，需要转换到地图坐标系 FmF_mFm，可写为：
pm=Rmapa+tma p_m=R_{ma}p_a+t_{ma} pm=Rmapa+tma
qm=qma⊗qa q_m=q_{ma}\otimes q_a qm=qma⊗qa
这里 RmaR_{ma}Rma 和 tmat_{ma}tma 是从 FaF_aFa 到 FmF_mFm 的旋转和平移，⊗\otimes⊗ 表示四元数乘法。
由于车道级路线主要关心平面行驶方向，通常会从四元数中取偏航角 yawyawyaw：
ψ=atan2(2(wz+xy),1−2(y2+z2)) \psi=\operatorname{atan2}\left(2(wz+xy),1-2(y^2+z^2)\right) ψ=atan2(2(wz+xy),1−2(y2+z2))
角度差必须归一化到 [−π,π][-\pi,\pi][−π,π]，否则接近 π\piπ 和 −π-\pi−π 的两个方向会被误认为相差很大：
norm(θ)=atan2(sinθ,cosθ) \operatorname{norm}(\theta)=\operatorname{atan2}(\sin\theta,\cos\theta) norm(θ)=atan2(sinθ,cosθ)
后续所有几何判断，比如"车辆朝向是否和车道方向一致""目标朝向是否合法""是否已经到达目标"，都依赖这个归一化角度差。
4. 从连续点找到所在车道
起点和终点是连续空间中的位姿，而路线搜索需要图上的节点。于是需要完成一个映射：
Pose→Lanelet \text{Pose} \rightarrow \text{Lanelet} Pose→Lanelet
对起点来说，理想情况是车辆位置刚好落在一个或多个道路车道多边形内。记起点位置为 psp_sps，候选起点车道集合为：
S={ℓ∈V∣ps∈poly(ℓ),ℓ 是道路车道} S=\{\ell \in V \mid p_s \in \operatorname{poly}(\ell), \ell \text{ 是道路车道}\} S={ℓ∈V∣ps∈poly(ℓ),ℓ 是道路车道}
在路口、并行车道边界或地图边界附近，一个点可能同时属于多个候选车道。此时不能随便选一个，因为选错车道会导致路线突然反向或绕远。
如果起点不在任何道路车道内，系统会在一定搜索半径内寻找最近的道路车道。这个兜底逻辑很重要，因为定位误差、地图边界误差、车辆停在路肩附近时，都可能让当前位置稍微偏出车道多边形。
终点也需要映射到车道。终点的处理和起点相似，但有一个额外偏好：如果正在重规划，且终点附近存在当前路线上的车道，那么优先选当前路线或当前首选车道中的候选车道。这样可以减少重规划时的跳变，避免路线在相邻车道之间不必要地摆动。
5. 用中心线计算弧长、方向和投影
车道中心线是很多几何量的基础。给定一个点 ppp 和一条中心线折线 C={c0,c1,...,cn}C=\{c_0,c_1,\dots,c_n\}C={c0,c1,...,cn}，可以把 ppp 投影到中心线上，得到它沿车道方向的弧长坐标。
对第 kkk 段中心线，令：
a=ck,b=ck+1,d=b−a a=c_k,\quad b=c_{k+1},\quad d=b-a a=ck,b=ck+1,d=b−a
点 ppp 在该线段上的投影参数为：
uk=clip((p−a)Td∥d∥2,0,1) u_k=\operatorname{clip}\left(\frac{(p-a)^Td}{\|d\|^2},0,1\right) uk=clip(∥d∥2(p−a)Td,0,1)
投影点为：
rk=a+ukd r_k=a+u_kd rk=a+ukd
这里要特别注意：这个投影只应该发生在"已经选定某个 lanelet"之后，用来求该 lanelet 中心线上的纵向弧长。它不能单独用来完成地图匹配。因为在宽车道、路口、弯道或多车道并行区域，点 ppp 到某条中心线可能不算最近，但只要 ppp 位于该车道多边形内，并且车辆朝向与该车道方向一致，这个 lanelet 仍然可能是正确候选。
第一层是选择候选 lanelet。对起点，先找所有包含起点位置的道路 lanelet：
S={ℓ∣p∈poly(ℓ),subtype(ℓ)=road} S=\{\ell\mid p\in\operatorname{poly}(\ell),\operatorname{subtype}(\ell)=\text{road}\} S={ℓ∣p∈poly(ℓ),subtype(ℓ)=road}
如果 SSS 为空，则在起点附近的有限搜索范围内找道路 lanelet 候选，并选择几何上最近的 lanelet 作为兜底：
ℓfallback=argminℓ∈B(p,R)d(p,poly(ℓ)) \ell_{\text{fallback}}=\arg\min_{\ell\in\mathcal{B}(p,R)}d(p,\operatorname{poly}(\ell)) ℓfallback=argℓ∈B(p,R)mind(p,poly(ℓ))
如果 SSS 不为空，系统不会立即用"最近中心线段"选车道，而是对每个候选起点 lanelet 计算车道方向和车辆朝向差：
Δψ(ℓ)=∣norm(ψℓ(p)−ψ)∣ \Delta\psi(\ell)=\left|\operatorname{norm}\left(\psi_\ell(p)-\psi\right)\right| Δψ(ℓ)=∣norm(ψℓ(p)−ψ)∣
只有满足朝向阈值的候选才参与路线搜索：
Δψ(ℓ)≤π2 \Delta\psi(\ell)\le\frac{\pi}{2} Δψ(ℓ)≤2π
对能到达目标 lanelet 的候选起点，当前实现使用"路线长度 + 朝向惩罚"的代价选择起点 lanelet：
J(ℓ)=Lroute(ℓ,ℓg)+1000⋅Δψ(ℓ) J(\ell)=L_{\text{route}}(\ell,\ell_g)+1000\cdot\Delta\psi(\ell) J(ℓ)=Lroute(ℓ,ℓg)+1000⋅Δψ(ℓ)
正确 lanelet 的选择主要发生在"车道多边形候选 + 朝向约束 + 是否能路由到目标 + 路线代价"这一层，而不是只看中心线投影距离。
第二层才是在已确定的 lanelet 中计算弧长坐标。此时可以在该 lanelet 的中心线上找距离 ppp 最近的投影段：
k∗=argmink∥p−rk∥ k^{*}=\arg\min_k \|p-r_k\| k∗=argkmin∥p−rk∥
那么点 ppp 在中心线上的弧长为：
s(p)=∑j=0k∗−1∥cj+1−cj∥+uk∗∥ck∗+1−ck∗∥ s(p)=\sum_{j=0}^{k^{*}-1}\|c_{j+1}-c_j\| + u_{k^{*}}\|c_{k^{*}+1}-c_{k^{*}}\| s(p)=j=0∑k∗−1∥cj+1−cj∥+uk∗∥ck∗+1−ck∗∥
该位置的车道方向可以由相邻中心线点的切线得到：
ψℓ(p)=atan2(dy,dx) \psi_\ell(p)=\operatorname{atan2}(d_y,d_x) ψℓ(p)=atan2(dy,dx)
这个弧长坐标后面会反复出现：判断当前位置离车道末端还有多远、判断目标点在终点车道中的位置、给目标点补齐地图高程，都要用它。
6. 起点候选不只看距离，还要看朝向
如果起点附近有多个候选车道，仅靠距离选择不够。车辆可能位于双向道路中间，离相反方向车道也很近；如果误选相反方向车道，路线会从一开始就错。
因此要比较车辆朝向和候选车道方向。记车辆偏航角为 ψs\psi_sψs，候选车道在起点附近的方向为 ψℓ(ps)\psi_\ell(p_s)ψℓ(ps)，角度差为：
Δψs=∣norm(ψℓ(ps)−ψs)∣ \Delta\psi_s=\left|\operatorname{norm}\left(\psi_\ell(p_s)-\psi_s\right)\right| Δψs=∣norm(ψℓ(ps)−ψs)∣
候选车道至少要满足：
Δψs≤π2 \Delta\psi_s \leq \frac{\pi}{2} Δψs≤2π
这表示车辆朝向与车道方向最多相差 90∘90^\circ90∘。超过这个阈值，通常意味着车辆面对的是反方向车道，不适合作为路线起点。
当多个候选起点都能到达目标车道时，可以用路线长度和起点朝向误差构造综合代价：
Jstart(ℓs)=Lroute(ℓs,ℓg)+αΔψs J_{\text{start}}(\ell_s)=L_{\text{route}}(\ell_s,\ell_g)+\alpha\Delta\psi_s Jstart(ℓs)=Lroute(ℓs,ℓg)+αΔψs
其中 ℓg\ell_gℓg 是目标车道，LrouteL_{\text{route}}Lroute 是从候选起点车道到目标车道的路线长度，α\alphaα 是角度惩罚权重。在实际系统中，α\alphaα 会设置得较大，使朝向错误的车道即使路径稍短也不容易被选中。
如果当前路线中已经有"首选车道"，并且候选起点正好落在这个首选车道上，则优先沿用它。这是重规划稳定性的一个关键细节：同样能到达目标时，路线规划应尽量保持连续，而不是频繁切换到邻车道。
7. 在车道图上求最短路线
当起点车道 ℓs\ell_sℓs 和目标车道 ℓg\ell_gℓg 确定后，问题就变成图搜索：
ℓs⇝ℓg \ell_s \leadsto \ell_g ℓs⇝ℓg
对图中每条边 e=(u,v)e=(u,v)e=(u,v)，定义通行代价 w(e)w(e)w(e)。最基本的代价可以是车道长度，也可以在换道边上增加额外惩罚：
w(e)={L(v),e 是前进边L(v)+λlc,e 是换道边 w(e)= \begin{cases} L(v), & e \text{ 是前进边} \\ L(v)+\lambda_{\text{lc}}, & e \text{ 是换道边} \end{cases} w(e)={L(v),L(v)+λlc,e 是前进边e 是换道边
其中 λlc\lambda_{\text{lc}}λlc 是换道惩罚。它体现了一个工程偏好：如果两条路线长度差不多，少换道的路线通常更稳定、更舒适。
最短路问题可以写成：
P∗=argminP:ℓs⇝ℓg∑e∈Pw(e) P^{*}=\arg\min_{P:\ell_s\leadsto\ell_g}\sum_{e\in P}w(e) P∗=argP:ℓs⇝ℓgmine∈P∑w(e)
常见求解方式是 Dijkstra 或 A*。如果没有使用启发函数，Dijkstra 的动态规划递推形式是：
D(v)=minu:(u,v)∈E(D(u)+w(u,v)) D(v)=\min_{u:(u,v)\in E}\left(D(u)+w(u,v)\right) D(v)=u:(u,v)∈Emin(D(u)+w(u,v))
初始化为：
D(ℓs)=0,D(v)=+∞(v≠ℓs) D(\ell_s)=0,\quad D(v)=+\infty \quad (v\neq \ell_s) D(ℓs)=0,D(v)=+∞(v=ℓs)
搜索结束后，从 ℓg\ell_gℓg 回溯前驱节点即可得到车道序列：
P∗=(ℓ0,ℓ1,...,ℓn) P^{*}=(\ell_0,\ell_1,\dots,\ell_n) P∗=(ℓ0,ℓ1,...,ℓn)
其中 ℓ0=ℓs\ell_0=\ell_sℓ0=ℓs，ℓn=ℓg\ell_n=\ell_gℓn=ℓg。
8. 途经点让路线变成分段最短路
用户可能不只给一个目标点，还可能给多个途经点。途经点的语义是：路线必须按顺序经过这些检查点。
设统一坐标系后的检查点序列为：
Q=(q0,q1,...,qm) Q=(q_0,q_1,\dots,q_m) Q=(q0,q1,...,qm)
其中 q0q_0q0 是起点，qmq_mqm 是终点，中间是途经点。路线规划会对每一对相邻检查点分别求最短车道序列：
Pi∗=ShortestPath(qi−1,qi),i=1,...,m P_i^{*}=\operatorname{ShortestPath}(q_{i-1},q_i),\quad i=1,\dots,m Pi∗=ShortestPath(qi−1,qi),i=1,...,m
最终路线是这些分段结果的拼接：
P∗=P1∗⊕P2∗⊕⋯⊕Pm∗ P^{*}=P_1^{*}\oplus P_2^{*}\oplus\dots\oplus P_m^{*} P∗=P1∗⊕P2∗⊕⋯⊕Pm∗
如果相邻两段的连接处出现同一个车道，需要去掉重复项。否则同一个车道会在拼接边界被记录两次，后续的路线段生成和环路检查都会受到影响。
这种"分段最短路"并不一定等价于全局最短路，但它满足途经点约束。对导航任务来说，途经点本身就是用户或上游模块指定的硬约束，因此按顺序分段求解是合理的。
9. 不可通行车道的代价处理
有些地图会给车道打上"不可通行"或"不建议通行"的属性。路线规划可以选择是否考虑这些属性。
最简单的处理方式是把经过不可通行车道的边代价设为无穷大：
w′(u,v)={+∞,u 或 v 是不可通行车道w(u,v),其他情况 w'(u,v)= \begin{cases} +\infty, & u \text{ 或 } v \text{ 是不可通行车道} \\ w(u,v), & \text{其他情况} \end{cases} w′(u,v)={+∞,w(u,v),u 或 v 是不可通行车道其他情况
这样最短路搜索会自动绕开不可通行车道。如果完全没有替代路线，搜索就会失败。
这个设计比事先删除节点更灵活，因为同一张地图可以根据不同任务切换不同代价模型。例如，普通导航可以避开不可通行车道，而某些特殊任务可以允许它们参与候选路线。
10. 为什么输出不是一条车道，而是一串"路线段"
最短路得到的是一条主车道序列：
P∗=(ℓ0,ℓ1,...,ℓn) P^{*}=(\ell_0,\ell_1,\dots,\ell_n) P∗=(ℓ0,ℓ1,...,ℓn)
但自动驾驶下游模块不能只知道"首选车道"。它还需要知道在当前位置附近有哪些相邻车道属于同一条路线走廊，因为换道、避障、并线、分流都需要在这些邻接车道中选择局部行为。
因此，路线输出通常会把主车道序列扩展成"路线走廊"。对主序列中的每个首选车道 ℓi\ell_iℓi，构造一个路线段：
Ri=(ℓipref,{ℓi,1,ℓi,2,...,ℓi,k}) R_i=(\ell_i^{\text{pref}}, \{\ell_{i,1},\ell_{i,2},\dots,\ell_{i,k}\}) Ri=(ℓipref,{ℓi,1,ℓi,2,...,ℓi,k})
其中 ℓipref\ell_i^{\text{pref}}ℓipref 是首选车道，集合中的车道是同一横向切片内允许纳入路线的相邻车道。
路线段有两个层次：
- 首选车道：沿着它可以一路朝目标前进，是路线的主干。
- 车道集合：包含首选车道以及同方向、与路线相关的相邻车道，供下游模块进行局部车道选择。
这样，下游模块看到的不是一条没有宽度的线，而是一条带宽度的可行驶走廊。
11. 从主路径扩展到可行驶走廊
路线走廊不是简单地把主路径左右各扩一条车道，而是按 Lanelet2 routing graph 的关系类型做集合构造。这里的输入是最短路得到的主路径：
P∗=(ℓ0,ℓ1,...,ℓn) P^{*}=(\ell_0,\ell_1,\dots,\ell_n) P∗=(ℓ0,ℓ1,...,ℓn)
同时记录起点段和终点段附近的 lanelet：
S0=AllNeighbors(ℓ0) S_0=\operatorname{AllNeighbors}(\ell_0) S0=AllNeighbors(ℓ0)
G0=AllNeighbors(ℓn) G_0=\operatorname{AllNeighbors}(\ell_n) G0=AllNeighbors(ℓn)
其中 S0S_0S0 用来判断向前追溯是否已经到达路线起点附近，G0G_0G0 用来判断向后搜索是否已经到达路线终点附近。
第一步，把最短路主路径中的车道全部加入路线走廊：
R←{id(ℓ)∣ℓ∈P∗} \mathcal{R} \leftarrow \{id(\ell)\mid \ell \in P^{*}\} R←{id(ℓ)∣ℓ∈P∗}
第二步，遍历主路径中每个 lanelet 的左右关系。Lanelet2 对左右关系会区分"可换道"和"仅相邻"。
对每个 ℓ∈P∗\ell\in P^{*}ℓ∈P∗，若右侧关系类型为 Right，说明它是可向右换道的 lanelet，直接加入路线走廊：
type(ℓ,r)=Right⇒R←R∪{id(r)} \operatorname{type}(\ell,r)=\text{Right} \Rightarrow \mathcal{R}\leftarrow\mathcal{R}\cup\{id(r)\} type(ℓ,r)=Right⇒R←R∪{id(r)}
左侧同理：
type(ℓ,l)=Left⇒R←R∪{id(l)} \operatorname{type}(\ell,l)=\text{Left} \Rightarrow \mathcal{R}\leftarrow\mathcal{R}\cup\{id(l)\} type(ℓ,l)=Left⇒R←R∪{id(l)}
如果关系类型是 AdjacentRight 或 AdjacentLeft，说明它只是几何相邻，不一定能在当前位置合法换道。代码不会立刻把它加入路线走廊，而是先放入候选集合：
type(ℓ,a)∈{AdjacentRight,AdjacentLeft}⇒C←C∪{id(a)} \operatorname{type}(\ell,a)\in\{\text{AdjacentRight},\text{AdjacentLeft}\} \Rightarrow \mathcal{C}\leftarrow\mathcal{C}\cup\{id(a)\} type(ℓ,a)∈{AdjacentRight,AdjacentLeft}⇒C←C∪{id(a)}
因此，遍历完主路径后，有两个集合：
R=主路径 lanelet + 可换道邻居 \mathcal{R}=\text{主路径 lanelet + 可换道邻居} R=主路径 lanelet + 可换道邻居
C=几何相邻但不可直接换道的候选 lanelet \mathcal{C}=\text{几何相邻但不可直接换道的候选 lanelet} C=几何相邻但不可直接换道的候选 lanelet
第三步，检查每个候选 lanelet c∈Cc\in\mathcal{C}c∈C 是否真的属于路线走廊。代码要求它在前向和后向都能接回 R\mathcal{R}R。
先看向前追溯，也就是不断查询 previous(c)。定义：
Prev(c)={ℓ∣ℓ→c} \operatorname{Prev}(c)=\{\ell\mid \ell\rightarrow c\} Prev(c)={ℓ∣ℓ→c}
代码中的向前追溯逻辑可以写成下面的递推过程。初始化：
Qprev←Prev(c) Q_{\text{prev}}\leftarrow \operatorname{Prev}(c) Qprev←Prev(c)
Bprev←false B_{\text{prev}}\leftarrow \text{false} Bprev←false
如果 c∈S0c\in S_0c∈S0，则不继续向前追溯；否则循环检查 QprevQ_{\text{prev}}Qprev。在每一轮中，对 ℓ∈Qprev\ell\in Q_{\text{prev}}ℓ∈Qprev：
id(ℓ)∈R⇒Bprev←true id(\ell)\in\mathcal{R} \Rightarrow B_{\text{prev}}\leftarrow \text{true} id(ℓ)∈R⇒Bprev←true
这表示候选车道的上游已经接回路线走廊。
如果遇到 ℓ∈S0\ell\in S_0ℓ∈S0，说明已经追到起点附近但还没有接回 R\mathcal{R}R，这一支搜索停止：
ℓ∈S0⇒stop previous search \ell\in S_0 \Rightarrow \text{stop previous search} ℓ∈S0⇒stop previous search
如果遇到另一个候选 lanelet ℓ∈C\ell\in\mathcal{C}ℓ∈C，则沿候选链继续向前追溯：
id(ℓ)∈C⇒Qprev←Prev(ℓ) id(\ell)\in\mathcal{C} \Rightarrow Q_{\text{prev}}\leftarrow \operatorname{Prev}(\ell) id(ℓ)∈C⇒Qprev←Prev(ℓ)
实现上，每一轮遇到第一个可继续追踪的候选 lanelet 就进入下一轮，因此它不是对所有候选分支做完整 BFS，而是沿 routing graph 返回的候选链做有限追踪。
向后搜索完全对称，只是把 previous 换成 following。定义：
Next(c)={ℓ∣c→ℓ} \operatorname{Next}(c)=\{\ell\mid c\rightarrow \ell\} Next(c)={ℓ∣c→ℓ}
初始化：
Qnext←Next(c) Q_{\text{next}}\leftarrow \operatorname{Next}(c) Qnext←Next(c)
Bnext←false B_{\text{next}}\leftarrow \text{false} Bnext←false
如果 c∈G0c\in G_0c∈G0，则不继续向后搜索；否则循环检查 QnextQ_{\text{next}}Qnext。在每一轮中：
id(ℓ)∈R⇒Bnext←true id(\ell)\in\mathcal{R} \Rightarrow B_{\text{next}}\leftarrow \text{true} id(ℓ)∈R⇒Bnext←true
如果遇到 ℓ∈G0\ell\in G_0ℓ∈G0，停止这一支搜索：
ℓ∈G0⇒stop next search \ell\in G_0 \Rightarrow \text{stop next search} ℓ∈G0⇒stop next search
如果遇到另一个候选 lanelet，则继续向后：
id(ℓ)∈C⇒Qnext←Next(ℓ) id(\ell)\in\mathcal{C} \Rightarrow Q_{\text{next}}\leftarrow \operatorname{Next}(\ell) id(ℓ)∈C⇒Qnext←Next(ℓ)
最终加入路线走廊的条件是：
Bprev=true∧Bnext=true⇒R←R∪{id(c)} B_{\text{prev}}=\text{true} \land B_{\text{next}}=\text{true} \Rightarrow \mathcal{R}\leftarrow\mathcal{R}\cup\{id(c)\} Bprev=true∧Bnext=true⇒R←R∪{id(c)}
这就是代码里"候选车道既向前连接到主线，又向后连接到主线"才纳入路线的精确定义。它避免把只是贴在旁边、但无法合法进入或离开路线主干的相邻 lanelet 加进来。
最后，路线段并不是直接按 R\mathcal{R}R 平铺输出。系统会重新得到一条主 lanelet 序列，然后对每个主 lanelet ℓi\ell_iℓi 取其所有同向邻居，再只保留属于 R\mathcal{R}R 的那些：
Section(ℓi)={ℓ∣ℓ∈AllNeighbors(ℓi), id(ℓ)∈R} \operatorname{Section}(\ell_i)= \{\ell\mid \ell\in\operatorname{AllNeighbors}(\ell_i),\ id(\ell)\in\mathcal{R}\} Section(ℓi)={ℓ∣ℓ∈AllNeighbors(ℓi), id(ℓ)∈R}
输出的 route section 为：
Ri=(preferred=id(ℓi),primitives={id(ℓ)∣ℓ∈Section(ℓi)}) R_i= \left( \operatorname{preferred}=id(\ell_i), \operatorname{primitives}=\{id(\ell)\mid \ell\in\operatorname{Section}(\ell_i)\} \right) Ri=(preferred=id(ℓi),primitives={id(ℓ)∣ℓ∈Section(ℓi)})
也就是说，preferred_primitive 是主路径上的 lanelet，而 primitives 是该横向切片中被路线走廊允许使用的 lanelet 集合。
12. 用空间索引加速车道查询
路线走廊生成后，系统经常需要回答这样的问题：某个位置附近有哪些路线车道？某个车道是否属于当前路线？目标点附近的候选车道有哪些？
如果每次都遍历全部车道，代价会随着地图规模线性增长。工程上通常会为路线车道建立空间索引，例如 R-tree。
每个车道多边形可以先取轴对齐包围盒：
AABB(ℓ)=[xmin,xmax]×[ymin,ymax] \operatorname{AABB}(\ell)= [x_{\min},x_{\max}]\times[y_{\min},y_{\max}] AABB(ℓ)=[xmin,xmax]×[ymin,ymax]
R-tree 通过层级包围盒组织这些车道，使窗口查询和邻近查询更快。它不改变路线算法本身，但对实时系统很重要。
13. 目标点不能只看位置，还要看姿态和车辆轮廓
找到从起点到目标附近的车道序列后，还不能马上认为路线有效。目标点必须满足几何和语义约束。
首先看目标朝向。若目标点在某条道路车道 ℓg\ell_gℓg 内，目标偏航角为 ψg\psi_gψg，车道方向为 ψℓg(pg)\psi_{\ell_g}(p_g)ψℓg(pg)，则需要：
∣norm(ψℓg(pg)−ψg)∣<θgoal \left|\operatorname{norm}\left(\psi_{\ell_g}(p_g)-\psi_g\right)\right|<\theta_{\text{goal}} norm(ψℓg(pg)−ψg) <θgoal
其中 θgoal\theta_{\text{goal}}θgoal 是目标角度阈值。默认思路是：目标姿态必须大致顺着车道方向。若目标朝反了，即使位置在车道内，也不应该接受。
其次看车辆轮廓。目标点一般表示车辆基准点，而车辆有长度和宽度。即使基准点在车道内，车辆外轮廓也可能压出车道边界。
设车辆在自身坐标系下的轮廓点为：
Fb={f1,f2,...,fN} \mathcal{F}_b=\{f_1,f_2,\dots,f_N\} Fb={f1,f2,...,fN}
目标位姿为 (Rg,tg)(R_g,t_g)(Rg,tg)，则车辆目标轮廓在地图坐标系下为：
fim=Rgfi+tg f_i^m=R_g f_i+t_g fim=Rgfi+tg
组成目标轮廓多边形：
Fm=poly(f1m,...,fNm) \mathcal{F}_m=\operatorname{poly}(f_1^m,\dots,f_N^m) Fm=poly(f1m,...,fNm)
目标附近的车道多边形合并为：
Lg=⋃ℓ∈Near(pg)poly(ℓ) \mathcal{L}g=\bigcup{\ell\in \operatorname{Near}(p_g)}\operatorname{poly}(\ell) Lg=ℓ∈Near(pg)⋃poly(ℓ)
目标轮廓需要被车道区域覆盖：
Fm⊆Lg \mathcal{F}_m\subseteq \mathcal{L}_g Fm⊆Lg
这里目标附近不只取当前车道，还会把前后相邻车道纳入检查。原因是车辆基准点可能位于车道片段的起点或终点附近，如果只看单个车道片段，车尾或车头可能因为跨越片段边界而被误判为越界。
停车区域是特殊情况。如果目标点位于合法停车位或停车场区域，可以不完全套用普通道路车道的朝向约束，因为停车姿态可能与道路中心线方向不同。
14. 可选的目标点修正：投到最近中心线
有些系统允许自动修正目标点，使它贴近最近车道中心线。这个操作不是必须的，但对用户在界面上随手点击目标点很有帮助。
给定目标点 pgp_gpg，先找到最近道路车道，再在其精细化中心线上找最近点 c∗c^{*}c∗。目标位置可以替换为 c∗c^{*}c∗，朝向替换为中心线切线方向：
ψg′=ψℓ(c∗) \psi_g'=\psi_\ell(c^{*}) ψg′=ψℓ(c∗)
如果车辆左右悬垂不完全对称，还可以沿局部横向做一个微小偏移。设右悬垂为 rrr，左悬垂为 lll，偏移量为：
δ=r−l2 \delta=\frac{r-l}{2} δ=2r−l
局部横向单位向量为：
n=(−sinψg′,cosψg′) n=(-\sin\psi_g',\cos\psi_g') n=(−sinψg′,cosψg′)
修正后位置为：
pg′=c∗+δn p_g'=c^{*}+\delta n pg′=c∗+δn
也就是：
xg′=xc∗−δsinψg′ x_g'=x_{c^{*}}-\delta\sin\psi_g' xg′=xc∗−δsinψg′
yg′=yc∗+δcosψg′ y_g'=y_{c^{*}}+\delta\cos\psi_g' yg′=yc∗+δcosψg′
这能让车辆轮廓相对车道边界更居中，减少因为车辆基准点不在几何中心而导致的边界误判。
15. 目标高度也要贴合地图
路线规划主要在二维平面上搜索，但输出目标位姿仍然需要合理的 zzz 值。目标点可能来自用户点击或外部接口，其高度不一定精确。更稳妥的做法是把目标点投影到终点首选车道的三维中心线上，用地图中心线高度修正目标高度。
若目标点投影在线段 [a,b][a,b][a,b] 上，投影参数为 uuu，则高度可以插值得到：
zg′=za+u(zb−za) z_g'=z_a+u(z_b-z_a) zg′=za+u(zb−za)
最终目标位姿使用原目标的平面语义，但高度贴合地图：
pg′=(xg,yg,zg′) p_g'=(x_g,y_g,z_g') pg′=(xg,yg,zg′)
这对坡道、桥梁、立交等三维道路尤其重要。
16. 环路为什么会被拒绝
有些地图或目标组合可能产生包含重复车道的路线。如果某个车道片段在路线段集合里出现多次，就形成了环路或至少形成了重复拓扑。
可以用车道 ID 集合检测：
∃i≠j,id(ℓi)=id(ℓj) \exists i\neq j,\quad id(\ell_i)=id(\ell_j) ∃i=j,id(ℓi)=id(ℓj)
若存在重复 ID，则认为路线存在环路风险。
许多车道级规划系统会拒绝环路路线，因为后续模块常默认路线是从起点到终点的有序无环走廊。一旦允许环路，"当前车道的下一个路线车道""离目标还有多远""是否已经过了某个路段"等问题都会变得歧义。
17. 路线请求有两种来源
车道级路线规划通常支持两类输入。
第一类是位姿点输入：给定起点、途经点和目标点，让系统从地图上搜索车道路线。这是最常见的导航方式。
第二类是外部已经给出的车道段路线：上游模块直接指定路线段集合。此时系统不需要再做图搜索，只需要统一坐标系、记录起点和目标、发布路线结果。这适合路线已经由其他模块算好，或者需要恢复某条预定义路线的场景。
无论哪种来源，最终都会形成同一种抽象结果：
Lane-level Route=(start pose,goal pose,{Ri},route id) \text{Lane-level Route}=(\text{start pose},\text{goal pose},\{R_i\},\text{route id}) Lane-level Route=(start pose,goal pose,{Ri},route id)
这里的 route id 很重要。行驶中目标可能被局部模块微调，必须确认这个修改属于当前路线，而不是属于已经过期的旧路线。
18. 路线状态机让接口行为可控
路线规划不是一个孤立函数，而是一个长期运行的系统。它需要知道自己处于什么状态，才能决定是否接受新请求。
常见状态可以理解为：
| 状态 | 含义 |
|---|---|
| INITIALIZING | 地图、定位等必要数据尚未准备好 |
| UNSET | 系统已准备好，但当前没有路线 |
| ROUTING | 正在首次规划路线 |
| SET | 已经有一条有效路线 |
| REROUTING | 正在已有路线基础上重规划 |
| ARRIVED | 已到达目标 |
| ABORTED / INTERRUPTED | 路线被异常终止或中断 |
状态机的核心原则是：没有地图和定位时不能规划；已有路线时重新设置目标属于重规划；规划失败时不能把系统留在半更新状态。
首次规划的典型状态变化是：
UNSET→ROUTING→SET \text{UNSET}\rightarrow\text{ROUTING}\rightarrow\text{SET} UNSET→ROUTING→SET
行驶中重规划的典型状态变化是：
SET→REROUTING→SET \text{SET}\rightarrow\text{REROUTING}\rightarrow\text{SET} SET→REROUTING→SET
清除路线后，状态回到：
UNSET \text{UNSET} UNSET
如果重规划失败，系统应恢复原路线，而不是发布一条空路线或半成品路线。
19. 行驶中重规划为什么需要额外约束
车辆静止时换一条路线很简单；车辆正在自动驾驶时换路线就危险得多。因为下游模块正在沿当前路线生成路径和控制指令，如果新路线在车辆附近立即分叉，车辆可能没有足够距离完成行为切换。
因此，重规划通常需要满足三个条件：
- 当前驾驶模式允许重规划。
- 下游规划器处于适合接收重规划的状态，例如正在车道跟随，而不是正在复杂避障或换道中。
- 新旧路线在车辆前方必须有足够长的共同路段。
前两个条件是系统状态约束，第三个条件是几何安全约束。
20. 新旧路线的共同路段长度
设原路线段序列为：
O=(O0,O1,...,Om) O=(O_0,O_1,\dots,O_m) O=(O0,O1,...,Om)
新路线段序列为：
T=(T0,T1,...,Tn) T=(T_0,T_1,\dots,T_n) T=(T0,T1,...,Tn)
每个路线段不是单个车道，而是一组车道 primitive。两个路线段相同，要求它们包含相同的车道 ID 集合：
IDs(Oi)=IDs(Tj) \operatorname{IDs}(O_i)=\operatorname{IDs}(T_j) IDs(Oi)=IDs(Tj)
先找到原路线和新路线之间的第一个共同路线段：
(i0,j0)=min{(i,j)∣IDs(Oi)=IDs(Tj)} (i_0,j_0)=\min\{(i,j)\mid \operatorname{IDs}(O_i)=\operatorname{IDs}(T_j)\} (i0,j0)=min{(i,j)∣IDs(Oi)=IDs(Tj)}
然后从这个位置开始，向后寻找连续相同的区间：
IDs(Oi0+k)=IDs(Tj0+k) \operatorname{IDs}(O_{i_0+k})=\operatorname{IDs}(T_{j_0+k}) IDs(Oi0+k)=IDs(Tj0+k)
直到第一次不相同为止。这样得到新旧路线在前方保持一致的共同区间：
i0,i1\] \[i_0,i_1\] \[i0,i1
车辆必须位于新路线的第一个路线段内。否则说明新路线并没有从当前车辆所在车道自然开始，重规划会产生跳变。
21. 重规划安全距离的数学推导
共同路段长度代表车辆在真正遇到新旧路线分歧之前，还有多少距离可以继续按原有路线行驶。记这个长度为 LcommonL_{\text{common}}Lcommon。
如果车辆当前速度为 vvv，希望系统至少有 TrerouteT_{\text{reroute}}Treroute 秒用于感知、规划、行为切换和控制稳定，那么车辆在这段时间内行驶距离约为：
LT=vTreroute L_T=vT_{\text{reroute}} LT=vTreroute
此外，还需要一个最小绝对距离 LminL_{\min}Lmin，用于低速但仍需留出空间的情况。于是安全距离阈值为：
Lsafe=max(vTreroute,Lmin) L_{\text{safe}}=\max(vT_{\text{reroute}},L_{\min}) Lsafe=max(vTreroute,Lmin)
重规划允许条件为：
Lcommon>Lsafe L_{\text{common}}>L_{\text{safe}} Lcommon>Lsafe
现在看 LcommonL_{\text{common}}Lcommon 如何计算。
车辆当前所在车道记为 ℓc\ell_cℓc，当前位置在该车道中心线上的弧长为 scs_csc，车道长度为 L(ℓc)L(\ell_c)L(ℓc)，则当前车道剩余长度为：
Lremain=L(ℓc)−sc L_{\text{remain}}=L(\ell_c)-s_c Lremain=L(ℓc)−sc
对共同区间后续的每个路线段 OiO_iOi，由于一个路线段可能包含多个相邻车道，为了保守估计，可以取该段内车道长度的最小值：
Li=minℓ∈OiL(ℓ) L_i=\min_{\ell\in O_i}L(\ell) Li=ℓ∈OiminL(ℓ)
于是：
Lcommon=Lremain+∑i=i0+1i1Li L_{\text{common}}=L_{\text{remain}}+\sum_{i=i_0+1}^{i_1}L_i Lcommon=Lremain+i=i0+1∑i1Li
如果目标点位于共同区间最后一个路线段内部，那么不能把目标点之后的车道长度也算入可用共同路段。设目标点在终点车道上的弧长为 sgs_gsg，终点车道长度为 L(ℓg)L(\ell_g)L(ℓg)，需要减去目标点之后的剩余长度：
Lcommon←max(Lcommon−(L(ℓg)−sg),0) L_{\text{common}}\leftarrow \max\left(L_{\text{common}}-\left(L(\ell_g)-s_g\right),0\right) Lcommon←max(Lcommon−(L(ℓg)−sg),0)
车辆速度接近零时，可以跳过这个距离安全检查。因为静止车辆没有立即冲入分歧点的风险，系统有时间重新建立路线。
22. 目标点微调也是一种重规划
行驶接近终点时，局部规划可能会根据路边停车车辆、可停靠空间或局部障碍物，把最终目标点稍微移动。这个操作看似只是"改终点"，本质上仍然会改变路线。
为了让这种修改稳定，通常会遵守几个原则：
- 只允许修改当前有效路线的目标点，因此要检查 route id。
- 修改应发生在原路线终点附近，而不是跳到完全无关的位置。
- 重规划时尽量保留原路线起点，避免因为车辆已经向前行驶而让整条路线突然变短。
- 可以把车辆当前位姿作为额外途经点，促使新路线从当前位置自然接回原路线走廊。
这样做的目的不是追求最短，而是追求连续性。行驶中的路线规划，稳定往往比几米的长度差更重要。
23. 到达目标的判定
路线发布后，系统还需要判断车辆是否已经到达目标。一个可靠的到达判定不能只看距离，因为车辆可能从目标旁边经过，或者停在目标附近但朝向不对。
通常需要同时满足三个条件。
第一，车辆和目标在同一坐标系下，且平面距离足够近：
d=(x−xg)2+(y−yg)2 d=\sqrt{(x-x_g)^2+(y-y_g)^2} d=(x−xg)2+(y−yg)2
d<darrive d<d_{\text{arrive}} d<darrive
第二，车辆朝向和目标朝向足够接近：
∣norm(ψ−ψg)∣<θarrive \left|\operatorname{norm}(\psi-\psi_g)\right|<\theta_{\text{arrive}} ∣norm(ψ−ψg)∣<θarrive
第三，车辆已经保持停止一段时间：
v(t)≈0,t∈[t0,t0+Tstop] v(t)\approx 0,\quad t\in[t_0,t_0+T_{\text{stop}}] v(t)≈0,t∈[t0,t0+Tstop]
只有距离、朝向和停止时间都满足，才认为真正到达。这样可以避免车辆低速经过目标附近时误触发到达状态。
24. 发布、保持与诊断
车道级路线不是高频轨迹。它通常只在收到新目标、途经点或清除请求时更新；一旦生成，会保持为当前有效路线，直到被新路线替换或被清除。
这种"保持"语义很重要。下游模块可能比路线规划启动得晚，也可能在运行中重启。如果路线结果只发布一次且不保持，新加入的模块就可能拿不到当前路线。因此成熟系统通常会让路线、状态和调试标记具备可持久读取的语义。
路线发布通常包含三类信息：
- 路线本体：起点、目标、路线段集合、唯一标识和是否允许目标修改。
- 路线状态：当前是未设置、规划中、已设置、重规划中还是已到达。
- 调试信息：路线走廊可视化、目标车辆轮廓可视化、规划耗时等。
路线走廊可视化可以帮助确认首选车道和相邻候选车道是否符合预期。目标轮廓可视化可以帮助排查"目标点在车道内但车身越界"的问题。处理耗时则用于判断路线搜索和目标检查是否满足系统实时性要求。
这些诊断信息不是路线算法的核心，但在调试地图、目标点、重规划安全距离和目标合法性时非常关键。
25. 关键参数如何理解
下面这些参数控制路线规划的安全性和宽容度。
| 参数 | 作用 | 直观理解 |
|---|---|---|
| map frame | 地图坐标系名称 | 所有输入点最终都要变到这个坐标系 |
| arrival distance | 到达距离阈值 | 车离目标多近才算到达 |
| arrival angle | 到达角度阈值 | 车头方向和目标方向差多少以内才算到达 |
| arrival duration | 停止持续时间 | 需要停稳多久才算真正到达 |
| goal angle threshold | 目标朝向阈值 | 用户给的目标方向必须与车道方向大致一致 |
| enable correct goal pose | 是否修正目标到中心线 | 适合处理用户点击不精确的目标 |
| check footprint inside lanes | 是否检查车辆轮廓在车道内 | 防止目标点虽然在车道内但车身越界 |
| consider no drivable lanes | 是否避开不可通行车道 | 开启后会给不可通行车道无穷大代价 |
| reroute time threshold | 重规划时间安全阈值 | 速度越快，需要越长共同路段 |
| minimum reroute length | 重规划最小距离 | 低速时也要保留的最低安全距离 |
| allow reroute in autonomous mode | 自动驾驶中是否允许重规划 | 允许时仍需通过安全检查 |
这些参数没有绝对正确值。它们取决于车辆速度范围、控制稳定性、地图质量、下游规划模块响应时间和测试安全策略。
26. 一条完整路线是怎样自然生成的
现在把前面的内容串起来。
首先，系统等待地图和车辆定位准备好。地图被转换成车道图，车辆当前位姿用于确定起点。
然后，目标点和途经点被统一到地图坐标系。每个连续空间中的检查点都会被映射到附近的道路车道。起点候选会额外检查车辆朝向，目标候选会在重规划时优先沿用当前路线附近的车道。
接着，在车道图上对相邻检查点做最短路搜索。若需要避开不可通行车道，就把这些车道的通行代价设为无穷大后重新搜索。多段结果拼接后，得到从起点到目标的主车道序列。
随后，主车道序列被扩展为路线走廊。可换道邻居会被纳入路线，与主路线前后连通的几何相邻车道也会被谨慎纳入。每个主车道对应一个路线段，路线段中既有首选车道，也有同一横向切片内的可用车道集合。
之后，系统检查目标点是否合法：位置是否在道路、路肩、停车位或停车场等允许区域内；目标朝向是否与车道方向一致；车辆目标轮廓是否落在车道区域内；目标高度是否需要贴合地图中心线。
最后，系统拒绝环路路线，发布有效路线，并进入已设置状态。行驶过程中，如果目标被修改或收到新的路线请求，会进入重规划流程。重规划只有在模式允许、下游状态允许、新旧路线共同路段足够长时才会被接受。
这就是车道级路线规划的完整闭环。
27. 这个算法做什么，也不做什么
它做的事情是：
- 把起点、途经点和目标点转成地图车道上的搜索问题。
- 在车道拓扑图上找到满足方向和规则约束的路线。
- 输出带有首选车道和相邻可用车道的路线走廊。
- 检查目标点的位置、朝向、车身轮廓和地图高度。
- 管理路线状态、清除路线、到达判断和行驶中安全重规划。
它不做的事情是：
- 不根据动态障碍物实时绕行。
- 不根据交通灯状态生成行为决策。
- 不输出可直接给控制器跟踪的时序轨迹。
- 不支持包含重复车道的环路路线。
- 不保证重规划本身达到轨迹级或控制级安全，只提供路线层的几何安全过滤。
理解这条边界非常重要。车道级路线规划解决的是"该走哪些车道"的问题，而不是"下一秒方向盘打多少、速度是多少"的问题。
28. 一个最小数学总结
整套路线规划可以压缩成下面几个核心公式。
连续位姿统一到地图坐标系：
pm=Rmapa+tma,qm=qma⊗qa p_m=R_{ma}p_a+t_{ma},\quad q_m=q_{ma}\otimes q_a pm=Rmapa+tma,qm=qma⊗qa
起点候选角度过滤：
∣norm(ψℓ(ps)−ψs)∣≤π2 \left|\operatorname{norm}\left(\psi_\ell(p_s)-\psi_s\right)\right|\leq \frac{\pi}{2} ∣norm(ψℓ(ps)−ψs)∣≤2π
候选起点总代价：
Jstart=Lroute+αΔψs J_{\text{start}}=L_{\text{route}}+\alpha\Delta\psi_s Jstart=Lroute+αΔψs
图搜索目标：
P∗=argminP:ℓs⇝ℓg∑e∈Pw(e) P^{*}=\arg\min_{P:\ell_s\leadsto\ell_g}\sum_{e\in P}w(e) P∗=argP:ℓs⇝ℓgmine∈P∑w(e)
途经点分段拼接：
P∗=P1∗⊕P2∗⊕⋯⊕Pm∗ P^{*}=P_1^{*}\oplus P_2^{*}\oplus\dots\oplus P_m^{*} P∗=P1∗⊕P2∗⊕⋯⊕Pm∗
目标朝向合法性：
∣norm(ψℓg(pg)−ψg)∣<θgoal \left|\operatorname{norm}\left(\psi_{\ell_g}(p_g)-\psi_g\right)\right|<\theta_{\text{goal}} norm(ψℓg(pg)−ψg) <θgoal
车辆轮廓合法性：
Fm⊆Lg \mathcal{F}_m\subseteq\mathcal{L}_g Fm⊆Lg
重规划安全距离：
Lcommon>max(vTreroute,Lmin) L_{\text{common}}>\max(vT_{\text{reroute}},L_{\min}) Lcommon>max(vTreroute,Lmin)
到达判定：
d<darrive,∣norm(ψ−ψg)∣<θarrive,v≈0 持续 Tstop d<d_{\text{arrive}},\quad \left|\operatorname{norm}(\psi-\psi_g)\right|<\theta_{\text{arrive}},\quad v\approx0 \text{ 持续 } T_{\text{stop}} d<darrive,∣norm(ψ−ψg)∣<θarrive,v≈0 持续 Tstop
如果只记住一句话：这类路线规划不是在连续空间里直接画曲线，而是把道路高精地图抽象成有向车道图，在图上求受约束的最短路线，再把主路线扩展成可供下游规划使用的车道走廊。