4、参考速度规划（二）：基于交通规则的速度规划（限速牌、红绿灯）、基于道路类型的速度规划
各位同学，咱们接着聊速度规划。上一节讲了基于道路几何的规划，说白了就是根据弯道曲率来定速。但光有那个还不够——你想想看，如果前面有个限速牌，或者路口亮着红灯，你光靠弯道曲率能刹得住吗？显然不能。
所以这一节，我们来解决「规则约束」的问题。我个人习惯把这类规划叫做「社会性速度规划」，因为它不是物理极限决定的，而是交通规则和道路属性决定的。
4.1 限速牌检测与速度映射
限速牌是最常见的速度约束源。我最早做这个功能时，以为只要把视觉检测到的限速值直接丢给规划模块就行了。结果第一次路测就出了洋相——车在高速上突然从120急刹到60，后排的同事差点把咖啡泼我头上。
为什么会这样？因为限速牌不是孤立的。你得考虑几个问题：
- 置信度问题：视觉检测可能误检，比如把「80」看成「60」
- 生效时机：看到牌子就减速？还是过了牌子再减速？
- 多牌冲突：同时看到两个限速牌怎么办？
我一般这样处理：
限速牌处理流程
- 视觉模块输出限速值 + 置信度（0~1）
- 置信度低于0.6的，直接丢弃，不参与规划
- 置信度0.6~0.8的，作为参考值，但不强制
- 置信度高于0.8的，作为硬约束
- 多牌冲突时，取最小值（安全优先）
代码实现其实不复杂，核心逻辑就这几行：
def process_speed_limit(signs):
"""
处理限速牌列表，返回最终限速值
signs: [(value, confidence), ...]
"""
valid_signs = []
for val, conf in signs:
if conf < 0.6:
continue # 置信度太低，忽略
elif conf < 0.8:
val *= 1.1 # 低置信度时，给个裕量
valid_signs.append(val)
if not valid_signs:
return None # 没有有效限速牌
return min(valid_signs) # 安全优先，取最小值嗯，这里要注意：低置信度时我习惯给个10%的裕量。比如检测到「80」但置信度只有0.7，我就按72来规划。这样即使误检，也不会出大问题。
4.2 红绿灯速度规划
红绿灯的规划比限速牌复杂得多。限速牌只是告诉你「不能超过多少」，红绿灯则要求你「在某个时间点前通过」或「在某个位置停下」。
我遇到过最头疼的情况是：车离路口还有200米，绿灯还剩5秒。这时候该加速冲过去，还是减速等红灯？
我的做法是分两步：
红绿灯规划两步法
- 第一步：判断能否通过——计算以当前速度能否在绿灯结束前通过停止线
- 第二步：规划速度曲线——如果能通过，保持或微调速度；如果不能，提前减速
判断能否通过的公式很简单：
def can_pass_traffic_light(dist_to_stop, current_speed, green_remaining):
"""
判断能否在绿灯结束前通过
dist_to_stop: 到停止线的距离（米）
current_speed: 当前速度（m/s）
green_remaining: 绿灯剩余时间（秒）
"""
time_needed = dist_to_stop / max(current_speed, 0.1)
return time_needed < green_remaining - 1.0 # 留1秒安全余量这里我留了1秒的安全余量。为什么？因为实际执行时有延迟，油门响应、刹车响应都不是瞬时的。我曾经因为没留余量，结果车头刚过停止线就变红灯了——虽然不算闯红灯，但体验很差。
如果判断不能通过，那就需要规划一个「优雅的减速」：
def plan_stop_profile(current_speed, dist_to_stop):
"""
规划到停止线的减速曲线
返回: 目标速度序列 (时间, 速度)
"""
# 使用恒定减速度规划
decel = 2.0 # m/s²，舒适减速度
stop_time = current_speed / decel
stop_dist = 0.5 * current_speed * stop_time
if stop_dist > dist_to_stop:
# 距离不够，需要更猛的刹车
decel = current_speed**2 / (2 * dist_to_stop)
decel = min(decel, 4.0) # 最大减速度限制
# 生成速度曲线
profile = []
for t in np.arange(0, stop_time, 0.1):
v = max(0, current_speed - decel * t)
profile.append((t, v))
return profile注意：减速度不要超过4.0 m/s²，否则后排乘客会骂娘。紧急情况可以到5.0，但日常规划建议控制在2.0~3.0之间。
4.3 基于道路类型的速度规划
道路类型对速度的影响，说白了就是「不同路，不同速」。高速公路、城市道路、乡村小路，限速逻辑完全不同。
我一般把道路类型分为这几类：
| 道路类型 | 默认限速（km/h） | 规划策略 |
|---|---|---|
| 高速公路 | 120 | 巡航为主，关注出口匝道 |
| 城市快速路 | 80 | 关注出入口，注意汇入车辆 |
| 城市主干道 | 60 | 频繁加减速，关注路口 |
| 次干道/支路 | 40 | 低速行驶，注意行人 |
| 乡村道路 | 30 | 极低速，随时准备停车 |
这里有个坑：道路类型不是一成不变的。比如你从高速下来，进入匝道，再进入城市道路——这个过程中道路类型会变化，限速也要跟着变。
我习惯的做法是：
- 提前200米开始关注下一段路的类型
- 如果下一段限速更低，提前减速，不要到了跟前才急刹
- 如果下一段限速更高，可以保持当前速度，等进入后再加速
说白了就是「平滑过渡」。你想想看，如果限速从120突然降到60，你一脚急刹，后面的车追尾怎么办？
4.4 多约束融合
实际场景中，限速牌、红绿灯、道路类型会同时存在。比如：你在城市主干道上（限速60），前面有个限速40的牌子，再往前200米有个红绿灯。
这时候怎么规划？
我的做法是「取最小值」：
def fuse_speed_constraints(sign_limit, road_limit, traffic_light_limit):
"""
融合多个速度约束
返回: 最终目标速度
"""
constraints = []
if sign_limit is not None:
constraints.append(('限速牌', sign_limit))
if road_limit is not None:
constraints.append(('道路类型', road_limit))
if traffic_light_limit is not None:
constraints.append(('红绿灯', traffic_light_limit))
if not constraints:
return None # 没有约束，自由行驶
# 取最小值
min_limit = min(c[1] for c in constraints)
print(f"融合约束: {constraints}, 最终限速: {min_limit}")
return min_limit但这里有个细节：红绿灯的约束不是固定值，而是随时间变化的。比如你离路口还有100米，绿灯还剩3秒，这时候红绿灯约束可能是「必须在3秒内通过，否则停车」——这其实是一个动态约束，不能简单取最小值。
我一般这样处理：
- 静态约束（限速牌、道路类型）直接取最小值
- 动态约束（红绿灯）单独处理，生成一个「时间-速度」曲线
- 最后把静态约束作为上限，叠加到动态曲线上
嗯，这里要强调一下：多约束融合是速度规划中最容易出bug的地方。我见过不少团队，每个约束单独看都没问题，一融合就出各种奇怪的问题——比如车在路口前反复加减速，或者该停的时候没停。
4.5 知识体系总览
为了让大家更直观地理解这一节的内容，我画了一张流程图：
这张图把整个流程串起来了。从三个输入源开始，经过融合处理，再分静态和动态两条路径，最后输出目标速度。我个人觉得这个结构比较清晰，实际项目中我也是这么搭的。
好了，这一节的内容就到这里。核心就三件事：限速牌怎么处理、红绿灯怎么规划、道路类型怎么用。再加上多约束融合的思路，基本覆盖了日常开发中90%的场景。
记住一点：速度规划不是数学题，而是工程题。理论公式只是基础，真正让车跑得稳、跑得安全，靠的是对各种边界情况的处理。我踩过的坑，希望你们能绕过去。