粉丝问:
在apollo中由于在s-t图中没有体现横向,将对向车道的车辆轨迹和速度投射到超车速度规划的 s-t 图中,自车和对象车道一定会相交,怎么解决?也就是说自车在s-t图中是单调递增的,而预测车辆时单调递减的,会有相交,怎么解决?
有趣的问题:
这个问题是Frenet+s-t图做跨对向车道超车规划的核心坐标映射痛点 :s-t图本身只有纵向路径长度s 和时间t ,丢失了横向维度信息,直接将对向车辆投射到自车的超车s轴上,会出现自车s随t单调递增、对向车s随t单调递减的必然相交,误判为时空冲突。
Apollo针对这个问题的解决核心,不是简单把对向车投射到自车的s轴,而是通过Frenet横向分层+超车道时空窗口限定+对向车s轴反向映射+碰撞安全区判定 四层逻辑,在保留s-t图速度规划优势的同时,还原横向维度的安全约束,让对向车的投射仅在自车借道超车的超车道段 生效,且通过横向安全距离过滤无效相交,最终实现**"时空无冲突"的正确判定**。
结合Apollo EM Planner的工程落地实现,下面从问题根源、核心解决思路、四层具体实现逻辑、工程落地细节四个方面拆解,全程贴合Apollo的实际做法,解释清楚如何避免虚假相交、实现对向车的有效时空约束。
先明确:问题的核心根源(3个关键)
之所以会出现"必然相交",本质是对向车投射时的3个坐标映射误区,也是你疑惑的核心:
- s轴定义单一 :自车的s轴是沿自身超车参考路径的纵向长度(本车道→超车道→本车道),而对向车的行驶路径与自车完全反向,直接投影会导致s轴变化趋势相反(自车↑,对向↓);
- 丢失横向维度 :s-t图无横向l维度,无法区分"自车在超车道""对向车在对向车道"的空间差异,把物理上不重叠的横向位置 误判为同一s轴的空间重叠;
- 投射范围无限制 :若将对向车的全轨迹都投射到自车s轴,会导致自车整个超车过程都被判定为与对向车冲突,而实际冲突仅存在于自车借道超车道的短路段。
核心结论 :Apollo的解决思路不是"修改s-t图的单调特性",而是**"在投射前做横向过滤,在投射中做s轴反向映射,在投射后做超车道时空窗口限定",让对向车的时空约束仅在自车物理上可能与对向车碰撞的超车道段生效**,且通过横向安全距离排除物理上不重叠的虚假相交。
一、Apollo的核心解决框架:Frenet横向分层+超车道时空窗口
在做任何s-t投射前,Apollo会先通过Frenet坐标系的横向分层 ,把道路划分为本车道、超车道、对向车道 等独立的横向区域 ,并为超车规划定义严格的超车道时空窗口 ------这是解决问题的前提,也是所有投射的约束边界:
1. Frenet横向分层:道路横向区域的量化定义
基于高精地图+感知车道线 ,在Frenet坐标系中为每一条车道定义横向有效范围 (l∈[l1,l2]l \in [l_1, l_2]l∈[l1,l2]),其中lll是Frenet的横向坐标(垂直于s轴):
- 自车本车道 :l∈[lm1,lm2]l \in [l_m1, l_m2]l∈[lm1,lm2](如l∈[0,3.5m]l \in [0, 3.5m]l∈[0,3.5m],3.5m为标准车道宽度);
- 自车超车道 (同向相邻车道):l∈[lo1,lo2]l \in [l_o1, l_o2]l∈[lo1,lo2](如l∈[3.5m,7.0m]l \in [3.5m, 7.0m]l∈[3.5m,7.0m]);
- 对向车道 :l∈[lp1,lp2]l \in [l_p1, l_p2]l∈[lp1,lp2](如l∈[−7.0m,−3.5m]l \in [-7.0m, -3.5m]l∈[−7.0m,−3.5m],负号表示在自车参考路径左侧);
- 横向安全冗余 :为每个车道增加Δl=0.5m\Delta l=0.5mΔl=0.5m的安全距离,避免车道线附近的虚假碰撞判定。
2. 超车道时空窗口:自车借道超车的唯一冲突区间
为自车的超车规划定义超车道时空窗口 [Sstart,Send]×[Tstart,Tend][S_{start}, S_{end}] \times [T_{start}, T_{end}][Sstart,Send]×[Tstart,Tend],这是自车唯一可能与对向车发生物理碰撞的区间:
- 空间窗口 [Sstart,Send][S_{start}, S_{end}][Sstart,Send]:自车超车参考路径中,借道超车道的s轴段(如s从20m到60m,仅40m的长度),自车在s<20m(本车道)和s>60m(回正本车道)时,横向位置与对向车完全分离,无碰撞可能;
- 时间窗口 [Tstart,Tend][T_{start}, T_{end}][Tstart,Tend]:自车行驶过[Sstart,Send][S_{start}, S_{end}][Sstart,Send]的时间区间(由超车速度规划的初始曲线预估,如t从1s到5s,仅4s的时间);
- 核心作用 :对向车的时空约束仅在这个窗口内生效,窗口外的对向车轨迹直接忽略,避免全轨迹投射的虚假冲突。
二、四层具体实现:解决s-t图投射相交的核心逻辑
Apollo将对向车的轨迹/速度投射到自车超车s-t图的过程,分为四层严格的处理步骤,每一步都针对一个问题根源做解决,最终实现**"仅保留真实碰撞风险的时空约束"**,彻底避免虚假相交。
步骤1:对向车的横向位置过滤------先排除物理上无碰撞可能的对向车
这是第一层过滤 ,也是最基础的一步:仅对"横向位置进入超车道碰撞区"的对向车做后续投射,直接过滤掉横向远离的对向车。
- 计算对向车的实时Frenet坐标:将对向车的笛卡尔坐标(xopp,yopp)(x_{opp}, y_{opp})(xopp,yopp)转化为自车超车参考路径的Frenet坐标(sopp,lopp)(s_{opp}, l_{opp})(sopp,lopp) ,其中loppl_{opp}lopp是对向车的横向位置;
- 定义超车道横向碰撞区 :自车超车道的横向范围向对向车道扩展Δl=0.5 1.0m\Delta l=0.5~1.0mΔl=0.5 1.0m,即l∈[lo1−Δl,lo2+Δl]l \in [l_o1 - \Delta l, l_o2 + \Delta l]l∈[lo1−Δl,lo2+Δl](如l∈[3.0m,7.5m]l \in [3.0m, 7.5m]l∈[3.0m,7.5m]);
- 横向过滤逻辑:
- 若对向车的loppl_{opp}lopp落在超车道横向碰撞区内→存在物理碰撞风险,继续后续投射;
- 若对向车的loppl_{opp}lopp远离该区域→无物理碰撞风险,直接跳过该对向车的所有后续处理,不投射到s-t图;
- 工程细节 :对向车的横向位置是随时间变化的 ,因此会对预测的未来5~8s轨迹的每一个时间步都做横向过滤,仅保留轨迹中进入碰撞区的时间段。
作用:解决"丢失横向维度"的问题,先把物理上不重叠的对向车排除,避免无意义的投射。
步骤2:对向车的s轴反向映射------让对向车的s轴变化与自车保持一致
这是解决**"自车s递增、对向车s递减导致必然相交"的 核心步骤**:Apollo会将对向车的反向s轴 映射为与自车超车道一致的正向s轴,让对向车的s-t变化趋势与自车相同,避免因方向相反导致的虚假相交。
具体映射逻辑(超车道段专属)
针对自车超车道的空间窗口[Sstart,Send][S_{start}, S_{end}][Sstart,Send],构建对向车的反向s轴到正向s轴的映射关系:
- 定义对向车的本地反向s轴 sopp_locals_{opp\local}sopp_local:沿对向车道的行驶方向,从SstartS{start}Sstart的对向投影点到SendS_{end}Send的对向投影点,sopp_local∈[0,Send−Sstart]s_{opp\local} \in [0, S{end}-S_{start}]sopp_local∈[0,Send−Sstart](随对向车行驶单调递增);
- 映射为自车超车道的正向s轴 sopp_maps_{opp\map}sopp_map:
sopp_map=Send−sopp_locals{opp\map} = S{end} - s_{opp\_local}sopp_map=Send−sopp_local - 映射后效果:对向车在自车超车道的s-t图中,s轴变化趋势与自车完全一致(单调递增) ,自车从Sstart→SendS_{start}→S_{end}Sstart→Send,对向车也从Sstart→SendS_{start}→S_{end}Sstart→Send,避免因方向相反导致的必然相交。
举例 :自车超车道s窗口为[20m,60m](长度40m),对向车在本地s轴从0→40m,映射后为60→20m?注意 :此处是超车道段的局部映射 ,Apollo会将对向车的轨迹裁剪到超车道的空间窗口 ,仅保留对向车行驶过该窗口的轨迹段,再做反向映射,最终映射后的对向车s轴与自车超车道s轴范围一致、趋势一致。
步骤3:超车道时空窗口限定------仅在冲突区间内做s-t投射
这是第二层过滤 ,将对向车的投射范围严格限定在**自车超车道的时空窗口[Sstart,Send]×[Tstart,Tend][S_{start}, S_{end}] \times [T_{start}, T_{end}][Sstart,Send]×[Tstart,Tend]**内:
- 轨迹裁剪:将对向车映射后的正向s轨迹 裁剪到s∈[Sstart,Send]s \in [S_{start}, S_{end}]s∈[Sstart,Send],仅保留该区间内的轨迹段,超出部分直接删除;
- 时间裁剪:根据对向车的预测速度,计算其行驶过裁剪后s段的时间区间[topp_s,topp_e][t_{opp\s}, t{opp\_e}][topp_s,topp_e],仅保留该时间区间内的轨迹点;
- 窗口匹配:将裁剪后的对向车轨迹,与自车超车道的时间窗口[Tstart,Tend][T_{start}, T_{end}][Tstart,Tend]做匹配,仅当两个时间区间有重叠时,才将该轨迹段投射到自车的s-t图中,无重叠则直接忽略。
核心作用:解决"投射范围无限制"的问题,让对向车的约束仅在自车借道超车道的短时间/短路径内生效,避免全轨迹投射的虚假冲突。
步骤4:s-t图中构建"时空碰撞安全区"------排除虚假相交,仅保留真实冲突
经过前3步处理后,对向车的轨迹已实现**"横向过滤+正向s映射+时空窗口裁剪",此时投射到自车s-t图中,不会再出现无意义的相交,但Apollo还会在s-t图中为对向车构建 "时空碰撞安全区",这是最后一层安全保障**,彻底解决剩余的虚假相交问题。
4.1 时空碰撞安全区的定义
在裁剪后的超车道s-t窗口内,以对向车映射后的s-t轨迹点(sopp(t),t)(s_{opp}(t), t)(sopp(t),t)为中心,结合车辆物理尺寸、横向安全距离、纵向安全距离 ,构建不可穿越的矩形时空安全区,这是自车速度曲线必须绕开的区域,也是真实碰撞风险的量化表示。
安全区的s范围 和t范围 计算公式:
{s∈[sopp(t)−Lself/2−Δs,sopp(t)+Lopp/2+Δs]t∈[t−Δt,t+Δt] \begin{cases} s \in [s_{opp}(t) - L_{self}/2 - \Delta s, s_{opp}(t) + L_{opp}/2 + \Delta s] \\ t \in [t - \Delta t, t + \Delta t] \end{cases} {s∈[sopp(t)−Lself/2−Δs,sopp(t)+Lopp/2+Δs]t∈[t−Δt,t+Δt]
Δs=max(vself×treact,vopp×treact,10m) \Delta s = \max(v_{self} \times t_{react}, v_{opp} \times t_{react}, 10m) Δs=max(vself×treact,vopp×treact,10m)
其中:
- LselfL_{self}Lself:自车长度,LoppL_{opp}Lopp:对向车长度(感知模块输出);
- Δs\Delta sΔs:纵向安全距离 (取自车/对向车的反应距离和10m的最大值,treact=0.5 1st_{react}=0.5~1streact=0.5 1s为车辆反应时间);
- Δt\Delta tΔt:时间安全冗余(0.1~0.2s,匹配规划时间步长);
- 关键补充 :Δs\Delta sΔs中已融入横向安全距离的等效转化 ------将对向车与自车的横向安全距离Δl\Delta lΔl,通过道路曲率、车辆转向角等效为纵向的安全距离补偿,让s-t图的一维约束覆盖二维的物理碰撞风险。
4.2 真实冲突的判定逻辑
自车速度曲线在s-t图中为S(t)S(t)S(t)(单调递增),对向车的时空碰撞安全区为[Slow(t),Shigh(t)]×[tlow,thigh][S_{low}(t), S_{high}(t)] \times [t_{low}, t_{high}][Slow(t),Shigh(t)]×[tlow,thigh],仅当自车的s-t点(S(t),t)(S(t), t)(S(t),t)落在安全区内时,才判定为真实时空冲突,否则为虚假相交,直接忽略。
判定公式 :
{t∈[tlow,thigh]S(t)∈[Slow(t),Shigh(t)] \begin{cases} t \in [t_{low}, t_{high}] \\ S(t) \in [S_{low}(t), S_{high}(t)] \end{cases} {t∈[tlow,thigh]S(t)∈[Slow(t),Shigh(t)]
只有同时满足以上两个条件,才判定为冲突,自车速度规划必须绕开该区域;否则即使s-t曲线有交点,也判定为无冲突。
三、Apollo的完整执行流程:从对向车感知到s-t图约束落地
结合以上四层逻辑,Apollo将对向车轨迹/速度投射到超车s-t图并做速度规划约束的完整工程流程如下,每一步都有明确的输入、处理、输出,确保无虚假相交、无漏判冲突:
输入
- 自车超车参考路径(Frenet坐标系,含s轴和各车道横向l范围);
- 感知模块输出:对向车的实时位姿、速度、尺寸,多帧轨迹跟踪结果;
- 预测模块输出:对向车未来5~8s的多轨迹预测结果(含笛卡尔坐标和Frenet坐标,带置信度);
- 自车超车道时空窗口:[Sstart,Send]×[Tstart,Tend][S_{start}, S_{end}] \times [T_{start}, T_{end}][Sstart,Send]×[Tstart,Tend](由路径规划模块预估)。
处理步骤
- 对向车轨迹筛选 :取预测模块输出的最危险高置信度轨迹(工程保守设计);
- 横向过滤:将对向车轨迹转化为Frenet坐标,过滤掉横向位置远离超车道碰撞区的轨迹段;
- s轴反向映射 :将过滤后的对向车轨迹段,映射为自车超车道的正向s轴,保证s-t变化趋势与自车一致;
- 时空窗口裁剪 :将映射后的轨迹段,裁剪到自车超车道的s∈[Sstart,Send]s \in [S_{start}, S_{end}]s∈[Sstart,Send]和预估的t∈[Tstart,Tend]t \in [T_{start}, T_{end}]t∈[Tstart,Tend]内;
- 构建时空碰撞安全区:在裁剪后的s-t窗口内,为对向车构建带安全冗余的时空安全区;
- s-t图约束加载 :将时空碰撞安全区作为最高优先级硬约束,加载到自车超车速度规划的s-t图中,标记为不可穿越区域。
输出
带对向车真实时空冲突约束的s-t图,作为自车超车速度规划(DP+QP)的输入,自车速度曲线必须绕开所有时空碰撞安全区。
四、工程落地的关键细节(Apollo的额外优化)
为了保证该逻辑在车载实时环境中高效、鲁棒执行,Apollo还做了以下工程优化,避免理论逻辑与实际落地的偏差:
1. 对向车预测的"保守性选择"
预测模块会输出对向车的多轨迹预测结果 (如3条轨迹,置信度90%/70%/50%),Apollo不会取平均轨迹,而是选择对超车最危险的轨迹 (即最早进入超车道时空窗口、速度最快的轨迹)做投射,避免因预测乐观导致的漏判冲突。
2. 实时重规划与窗口动态更新
超车过程中,感知模块每100ms更新一次对向车的状态,Apollo会:
- 重新执行上述四层投射逻辑,动态更新s-t图中的时空碰撞安全区;
- 若对向车的轨迹发生突变(如突然加速/变道),立即触发快速QP重规划(无需重新做DP全局搜索),调整自车速度曲线,绕开新的安全区;
- 若新的安全区让自车无可行速度曲线,立即终止超车,执行紧急回正+制动,拉回本车道。
3. 横向-纵向安全距离的动态补偿
当超车道存在弯道 时,车辆的横向加速度会导致横向安全距离需求增加,Apollo会根据**道路曲率κ\kappaκ**动态补偿纵向安全距离Δs\Delta sΔs:
Δs=Δs0+v2×κg×Δl \Delta s = \Delta s_0 + \frac{v^2 \times \kappa}{g} \times \Delta l Δs=Δs0+gv2×κ×Δl
其中Δs0\Delta s_0Δs0为直道基础纵向安全距离,ggg为重力加速度,确保弯道时的安全区能覆盖横向侧倾带来的碰撞风险。
4. 硬件延迟的时间补偿
车载感知/通信/计算存在固定延迟 (约50~100ms),Apollo会在对向车的预测时间中增加Δtdelay=0.1s\Delta t_delay=0.1sΔtdelay=0.1s的延迟补偿,将对向车的轨迹在时间轴上向前偏移,避免因硬件延迟导致的时空安全区滞后。
五、直观举例:解决后的s-t图效果
用一个具体的例子,看经过Apollo处理后,对向车投射到s-t图中的效果,对比处理前 和处理后的差异:
处理前(直接投射)
- 自车s-t曲线:S(t)=10tS(t)=10tS(t)=10t(从20m→60m,t=2s→6s);
- 对向车直接投射s-t曲线:Sopp(t)=80−10tS_{opp}(t)=80-10tSopp(t)=80−10t(从60m→20m,t=2s→6s);
- 结果:两条曲线在t=4s、s=40m处必然相交,误判为冲突,超车无法执行。
处理后(Apollo四层逻辑)
- 横向过滤:对向车横向位置进入超车道碰撞区,保留轨迹;
- s轴反向映射:对向车曲线从Sopp(t)=80−10tS_{opp}(t)=80-10tSopp(t)=80−10t映射为Sopp_map(t)=10tS_{opp\_map}(t)=10tSopp_map(t)=10t(与自车趋势一致);
- 时空窗口裁剪:仅保留t=2s→6s、s=20m→60m的轨迹段;
- 构建时空碰撞安全区:对向车安全区s范围为[10t−15,10t+15][10t-15, 10t+15][10t−15,10t+15](含车辆尺寸+10m纵向安全距离);
- 冲突判定:自车曲线S(t)=10tS(t)=10tS(t)=10t落在对向车安全区内 ,判定为真实冲突,速度规划需调整为S(t)=15tS(t)=15tS(t)=15t(加速),让自车在t=3.3s时到达s=50m,提前驶出超车道时空窗口,绕开对向车的安全区。
最终效果:自车通过加速,在对向车到达超车道核心冲突区前完成超车,s-t曲线无重叠,物理上无碰撞,速度规划合理。
总结
Apollo解决"对向车投射到s-t图必然相交"的核心,不是修改s-t图的单调特性,而是通过四层逻辑还原横向维度的空间约束,让对向车的时空约束仅在真实碰撞风险的区间生效,核心要点可概括为3点:
- 前提 :通过Frenet横向分层 划分车道,定义自车超车道时空窗口,明确唯一的冲突区间,避免全轨迹投射;
- 核心 :对向车轨迹先做横向过滤 排除无碰撞可能的车辆,再做s轴反向映射让s-t变化趋势与自车一致,解决方向相反导致的必然相交;
- 保障 :在s-t图中为对向车构建带安全冗余的时空碰撞安全区 ,将二维物理碰撞风险 量化为一维s-t图的硬约束,仅当自车曲线落在安全区内时才判定为真实冲突,彻底排除虚假相交。
这套逻辑的本质是**"在s-t图的高效性和横向维度的完整性之间做平衡",既保留了s-t图对速度规划的全局优化优势,又通过坐标映射和窗口约束,还原了自动驾驶所需的二维空间安全,也是Apollo借道超车速度规划能保证对向车安全的核心工程实现**。
追问:
步骤 2:对向车的 s 轴反向映射 ------ 让对向车的 s 轴变化与自车保持一致,这里还是没有理解,通俗易懂的说明一下
回答:
核心是**"为啥反向映射能让对向车和自车的s变化趋势一致",其实这个操作本质是 给对向车做一次"局部坐标反转"------只针对自车要借道的超车道这段路,把对向车的"反向行驶距离"转换成和自车一样的"正向行驶距离",相当于咱俩走同一段路,你从东往西走,我把你的路程数改成从西往东算,这样咱俩的路程数就都是越走越大,不会一增一减必然相交了**。
不用看公式,用生活中的马路场景+通俗数字举例,一步讲清这个反向映射的逻辑,保证秒懂。
先搭一个最基础的物理场景(所有车都走直线,无弯道,方便理解)
假设你开的自车要在双向四车道 借道超车,我们先定好「超车道的物理范围」------这是映射的唯一有效范围(只处理这段路,外面的不管):
- 自车的超车道 (同向左侧车道):物理上是从A点到B点的一段路 ,这段路的实际长度40米(A是起点,B是终点);
- 自车的行驶方向:从A→B(向前开,越走离A越远);
- 对向车的行驶方向:从B→A(迎面开,越走离B越远);
- 核心问题:这段40米的超车道,自车数路程是「A=0米→B=40米」(递增),对向车数路程是「B=0米→A=40米」(对应自车的40米→0米,递减),直接投s-t图必相交。
反向映射的核心操作:给对向车"换个路程起点"
映射只做一件事 :把对向车的路程起点,从B点改成和自车一样的A点,让对向车在这段40米超车道里,路程数也和自车一样「越走越大」。
具体数字举例(超车道物理长度40米,A=自车起点,B=自车终点)
| 车辆 | 行驶方向 | 映射前(自身原路程数) | 反向映射后(统一成自车的路程数) | 路程变化趋势 |
|---|---|---|---|---|
| 自车 | A→B | A=20m → 中点=40m → B=60m(注:这是自车全局s轴的数,核心看区间差40m) | 不变:A=20m → 中点=40m → B=60m | 单调递增(20→40→60) |
| 对向车 | B→A | B=0m → 中点=20m → A=40m(对向车自己从B开始数) | 换算:B=60m → 中点=40m → A=20m → 再反转成60→40→20的反向:20→40→60 | 单调递增(20→40→60) |
通俗翻译这个映射:
对向车从B点(自车的60m)开到A点(自车的20m),原本它的路程是「0→40m」,我们把它倒过来算 ------从A点(20m)开始数,对向车开到中点,就是20→40m,开到B点,就是40→60m。
这样一来,自车和对向车在这段超车道里的路程数,都是从20m涨到60m,趋势完全一致,再也不会出现"自车涨、对向车跌"的必然相交了。
关键补充:映射只针对"超车道这段路",不是全局改对向车轨迹
这个反向映射是**"局部操作",只处理对向车经过超车道A→B这段40米路**的轨迹,超出这段路的部分:
- 对向车还没到B点(没进入超车道):不映射,直接过滤;
- 对向车已经过了A点(驶出超车道):不映射,直接过滤;
简单说:只把对向车"闯进"自车超车道的这一段轨迹,做路程数的反转,让它和自车的路程趋势一致,其他部分一概不管。
再用s-t图的极简曲线,看映射前后的差异(一眼看懂效果)
还是用上面的40米超车道(自车A=20m→B=60m,预计行驶时间t=2s→6s,速度10m/s),对向车速度也是10m/s:
映射前(直接投s-t图):
- 自车s-t曲线:t=2s→20m,t=4s→40m,t=6s→60m(↑);
- 对向车s-t曲线:t=2s→60m,t=4s→40m,t=6s→20m(↓);
- 结果:t=4s时,两车都在40m处必然相交,误判冲突。
映射后(反向换算路程数):
- 自车s-t曲线:不变(20→40→60,↑);
- 对向车s-t曲线:重新数路程后,变成t=2s→20m,t=4s→40m,t=6s→60m(↑);
- 结果:两车趋势完全一致,不会因为"方向相反"必然相交 ,此时的相交才是「同一时间到同一位置」的真实冲突(比如都t=4s到40m,才是真的会撞)。
最后一句话总结反向映射的本质
把对向车在超车道里的"反向路程",倒过来变成和自车一样的"正向路程",让两车在这段路的路程数都是"越走越大"------消除了"方向相反导致的虚假相交",只保留"同一时间到同一位置"的真实相交,这样s-t图的冲突判定才准。
其实你可以把这个操作理解成:两个人在同一段40米的跑道上,一个从东往西跑,一个从西往东跑,原本一个数0→40,一个数40→0,现在把往西跑的人的数字倒过来,改成0→40,这样两个人的数字都是越跑越大,只有他俩在同一秒跑到同一个数字时,才是真的撞上了。