openpilot 横向机动测试模式：整车横向控制调参实测与自动化报告生成

本文基于 openpilot 仓库中 lateral_maneuvers 工具，讲解横向机动测试模式（Lateral Maneuver Mode）的完整使用流程：如何在开发版设备上启用该模式、让车辆自动执行 step / sine / jitter 等横向机动动作，以及如何用报告生成脚本从行车日志（route）中量化提取横向加速度响应、交叉时间与转向执行量等指标，从而评估车辆横向控制的调参质量。

安全警告：该模式会让 openpilot 主动指令横向加速度台阶，测试时必须确保前方区域空旷并随时准备接管。

1. 工具定位：为什么需要横向机动测试

车辆横向控制的调参（如转向扭矩 PID、MPC 参数、车辆延迟估计）在不同车型上的表现差异很大。openpilot 为此提供了 tools/lateral_maneuvers 工具：它接管正常模型输出的期望曲率，向车辆注入一组标准化的横向机动（横向加速度台阶、正弦扫频、高频抖动），覆盖 20 mph 与 30 mph 两档速度，每个机动重复执行多次；测试全程的日志随 route 上传后，可用脚本离线生成 HTML 报告，量化比较各次运行的响应时间。

工具由两个部分组成：

• 车载进程 lateral_maneuversd.py：运行在 comma 设备上，实时生成机动指令并显示状态；
• 离线脚本 generate_report.py：运行在开发机上，从 route 日志中生成可视化报告。

2. 启用机制：LateralManeuverMode 参数与进程门控

2.1 参数定义与生命周期

该模式由持久化参数 LateralManeuverMode 控制。从参数键定义看（params_keys.h）：

{"LateralManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},
{"LongitudinalManeuverMode", {CLEAR_ON_MANAGER_START | CLEAR_ON_OFFROAD_TRANSITION, BOOL}},

两个特性值得注意：CLEAR_ON_MANAGER_START 表示每次 manager 重启时清空（回到 0），CLEAR_ON_OFFROAD_TRANSITION 表示车辆转回 offroad 时清空。也就是说这是一个一次性的开关：设置为 1 后，车辆上电进入 onroad 即进入机动模式，一次测试结束后参数自动归零，不会意外复测。

2.2 UI 开关与互斥逻辑

在设备的 Settings > Developer 页面中（developer.py）有 “Lateral Maneuver Mode” 开关。开启时源码会显式关闭若干互斥模式（developer.py）：

def _on_lat_maneuver_mode(self, state: bool):
    self._params.put_bool("LateralManeuverMode", state, block=True)
    self._params.put_bool("ExperimentalMode", False, block=True)
    self._params.put_bool("JoystickDebugMode", False, block=True)
    self._joystick_toggle.action_item.set_state(False)

即横向机动模式与实验模式、方向盘调试模式互斥——测试期间必须使用确定性的控制输入。也可以直接通过命令行设置参数（offroad 时执行）：

echo -n 1 > /data/params/d/LateralManeuverMode

2.3 进程启动门控

manager 根据参数决定是否启动该进程（process_config.py）：

def lat_maneuver(started: bool, params: Params, CP: car.CarParams) -> bool:
  return started and params.get_bool("LateralManeuverMode")
...
PythonProcess("lateral_maneuversd", "openpilot.tools.lateral_maneuvers.lateral_maneuversd", lat_maneuver),

只有车辆 onroad（started）且参数为 1 时 lateral_maneuversd 才会运行。同时它替换了模型管线的一部分：plannerd 仅在非机动模式运行（not_long_maneuver 门控，process_config.py），而 maneuversd（纵向机动）与 lateral_maneuversd（横向机动）各自按参数启动。

3. 完整操作步骤

以下流程继承自 README，并补充了源码层面的行为说明：

1. 检出开发分支：在 comma 设备上检出 master 等开发分支（该工具随开发版代码提供）。

2. 了解套件范围：完整机动套件在 20 mph 与 30 mph 两档速度下执行。

3. 启用模式：offroad 时通过 Settings > Developer 打开 “Lateral Maneuver Mode”，或手动写参数（见 2.2 节命令）。

4. 重新上电：车辆再次上电后，屏幕会显示 “Lateral Maneuver Mode” 永久提示。该提示来自 selfdrived：当收到 lateralManeuverPlan 消息时加入 lateralManeuver 事件（selfdrived.py），事件文案定义为 NormalPermanentAlert("Lateral Maneuver Mode")（events.py）。

5. 执行测试：

   • 确保前方区域空旷——该模式下 openpilot 会指令横向加速度台阶；
   • 手动把 ACC 设定到屏幕提示的目标速度（20 或 30 mph），等待横向控制稳定；
   • 当车速、期望曲率、车身 roll 均满足就绪条件并稳定持续 1~2 秒后，机动自动开始；机动之间 openpilot 横向控制保持激活，等待下一动动作的就绪条件；
   • 若车速超出容差、方向盘/油门被触碰、或 openpilot 解除激活，当前机动会被中止并从头重复。这些中止逻辑对应 lateral_maneuversd.py 中的检查（见第 4 节）。

6. 结束测试：全部机动完成后屏幕显示 “Maneuvers Finished”。靠边停车并关闭车辆以完成该 route。

7. 上传日志：在 connect.comma.ai 上找到对应 route，其时间轴上会有大量橙色（异常/告警）区间，这是机动注入造成的预期现象；确认 “All logs” 显示为 “uploaded”。

8. 生成报告：拿到 route ID 后运行报告生成器，报告输出到工具目录下的 lateral_reports/：

   $ python openpilot/tools/lateral_maneuvers/generate_report.py 98395b7c5b27882e/000001cc--5a73bde686
   
   processing report for KIA_EV6
   plotting maneuver: step right 20mph, runs: 3
   plotting maneuver: step left 20mph, runs: 3
   plotting maneuver: sine 0.5Hz 20mph, runs: 3
   plotting maneuver: step right 30mph, runs: 3
   
   Opening report: /home/batman/openpilot/tools/lateral_maneuvers/lateral_reports/KIA_EV6_98395b7c5b27882e_000001cc--5a73bde686.html
   

   注意 README 中的示例输出显示每个机动 runs: 3，而当前源码中每个机动 repeat=2 且首跑一次共 3 次运行——二者一致（首次执行 + 2 次重复）。

4. 机动套件与就绪判定（源码级解析）

4.1 机动定义

机动列表定义在 lateral_maneuversd.py，每档速度 4 个动作，共 8 个机动：

机动名称	动作序列（横向加速度 m/s² / 持续时间 s）	重复次数	目标速度
step right 20mph	+0.5 持续 1.0s，随后 −0.5 持续 1.5s	2	20 mph
step left 20mph	−0.5 持续 1.0s，随后 +0.5 持续 1.5s	2	20 mph
sine 0.5Hz 20mph	幅值 1.0 m/s²、周期 2.0s 的正弦，持续 2.0s，末尾 0.5s 归零	2	20 mph
jitter 20mph	±0.5 m/s² 交替、每段 0.1s，共 10 段	2	20 mph
step right 30mph	同 step right，速度 30 mph	2	30 mph
step left 30mph	同 step left，速度 30 mph	2	30 mph
sine 0.5Hz 30mph	同 sine 0.5Hz，速度 30 mph	2	30 mph
jitter 30mph	同 jitter，速度 30 mph	2	30 mph

其中正弦动作由辅助函数生成（lateral_maneuversd.py）：

def _sine_action(amplitude, period, duration):
  t = np.linspace(0, duration, int(duration / DT_MDL) + 1)
  a = amplitude * np.sin(2 * np.pi * t / period)
  return Action(a.tolist(), t.tolist())

Maneuver 与 Action 基类与纵向机动工具共享（maneuversd.py）：Action 用 accel_bp（加速度断点）与 time_bp（时间断点）两个数组描述一段动作，执行时以 np.interp 按模型帧长 DT_MDL 插值取当前目标加速度；Maneuver._step() 按帧推进动作索引，单个动作时长耗尽后进入下一动作、重复执行直至 repeat 次数用尽，最终置位 _finished。横向版在此基础上扩展了一个 _baseline_curvature 字段（记录机动开始时的路面曲率基线），并重写了 get_accel() 的就绪判定。

4.2 就绪条件：只在平直路面上启动

横向版机动只在直线、平路、车速匹配时启动，阈值定义在 lateral_maneuversd.py：

# thresholds for starting maneuvers
MAX_SPEED_DEV = 0.7 # deviation in m/s
MAX_CURV = 0.004 # 250 m radius
MAX_ROLL = 0.12 # 6.8°
TIMER = 2.0 # sec stable conditions before starting maneuver

即：车速与目标速度偏差小于 0.7 m/s、横向控制激活、期望曲率小于 0.004 1/m（约 250 m 转弯半径）、车身 roll 角小于 0.12 rad（约 6.8°）。就绪判定采用带迟滞的计数器（lateral_maneuversd.py）：

def get_accel(self, v_ego: float, lat_active: bool, curvature: float, roll: float) -> float:
    self._run_completed = False
    # only start maneuver on straight, flat roads
    ready = abs(v_ego - self.initial_speed) < MAX_SPEED_DEV and lat_active and abs(curvature) < MAX_CURV and abs(roll) < MAX_ROLL
    self._ready_cnt = (self._ready_cnt + 1) if ready else max(self._ready_cnt - 1, 0)

    if self._ready_cnt > (TIMER / DT_MDL):
      if not self._active:
        self._baseline_curvature = curvature
      self._active = True

条件满足时计数 +1，不满足时 −1（钳到 0），连续稳定超过 TIMER（2.0s，约 100 个 20Hz 模型帧）才置位 _active；激活瞬间记录当前曲率作为 baseline_curvature，用于后续把“路面固有曲率”从目标曲率中扣除。UI 上会显示 “Waiting: road not straight / road not flat” 或 “Starting: N” 倒计时（lateral_maneuversd.py）。

4.3 指令注入：desiredCurvature 的构造与消费

机动激活期间，lateral_maneuversd 以 20 Hz 发布 lateralManeuverPlan 消息（服务定义见 services.py，消息结构见 log.capnp：仅含一个 desiredCurvature: Float32 字段）。目标曲率由横向加速度换算而来（lateral_maneuversd.py）：

plan_send.valid = maneuver is not None and maneuver.active and complete_cnt == 0
if plan_send.valid:
    plan_send.lateralManeuverPlan.desiredCurvature = maneuver._baseline_curvature + accel / max(v_ego, MIN_SPEED) ** 2

即 desiredCurvature = 基线曲率 + a_lat / v²，其中 MIN_SPEED = 1.0 m/s（drive_helpers.py）防止低速除零。控制端在 controlsd.py 中消费该消息：当 lateralManeuverPlan 有效时，它直接替代模型 V2 的期望曲率，随后仍经过 clip_curvature 限速限幅并交给横向控制器（torque MPC 等）执行——这意味着测试中车辆的横向闭环控制器与真实驾驶完全一致，只是目标由标准动作序列驱动：

if self.sm.valid['lateralManeuverPlan']:
  new_desired_curvature = self.sm['lateralManeuverPlan'].desiredCurvature if CC.latActive else self.curvature
else:
  new_desired_curvature = model_v2.action.desiredCurvature if CC.latActive else self.curvature
self.desired_curvature, curvature_limited = clip_curvature(CS.vEgo, self.desired_curvature, new_desired_curvature, lp.roll)

4.4 中止条件与状态显示

主循环中任一司机输入都会中止当前机动（lateral_maneuversd.py）：steeringPressed 或 gasPressed 置 aborted_cnt 为 1 秒，车速偏离目标超过 MAX_SPEED_DEV 也会触发 maneuver.reset()，随后同一机动从头重试。屏幕文本按状态机切换（lateral_maneuversd.py）：

• 未就绪：“Waiting: road not straight / road not flat”；
• 车速/激活不满足：“Set speed to 20/30 mph”；
• 就绪倒计时：“Starting: N”（N 为剩余秒数）；
• 激活中：“Active ±X.X m/s² 剩余秒数”（正弦显示 “Active sine 0.5Hz …”）；
• 中止原因：“steering pressed” / “gas pressed”；
• 单次完成：“Complete {机动名}”；全部完成：“Maneuvers Finished”。

源码还包含一段防闪烁逻辑（lateral_maneuversd.py）：Waiting/Starting/Set speed 类文本切换后保持旧文本 0.5 秒，避免数字跳动导致 UI 抖动。

5. 报告生成：从 route 到量化指标

generate_report.py 的命令行接口（generate_report.py）：

python openpilot/tools/lateral_maneuvers/generate_report.py <route> [description]

• route 支持带 | 或 / 的标准 route 名（走云端/本地 LogReader），也支持仅给末段名，此时会在本地 Paths.log_root() 下按目录名匹配所有包含该字符串的分段并读取各段的 rlog.zst；
• description 为可选的自定义描述，会写入报告头部。

5.1 运行切分与有效性过滤

脚本遍历日志，以 alertDebug 消息中 “Active”/“Complete” 文本的出现为边界，把日志切成 (机动名, [运行, 运行, …]) 的嵌套结构（generate_report.py）。每次运行（run）再做有效性校验（generate_report.py）：全程横向控制保持激活且期间没有任何 steeringPressed，否则该运行标记为 “(invalid maneuver!)” 并排除出统计，但图表仍保留供人工检查。未完成（未出现 “Complete”）的整组运行也会被跳过。

5.2 核心指标：交叉时间（cross time）

所有指标基于横向加速度 a_lat = curvature × v²，并减去运行开始时的基线值（路面曲率贡献）：

• step 类机动：检测 lateralManeuverPlan.desiredCurvature 变化超过 0.001 1/m 的时刻划分 action 段，统计实际加速度第一次跨越目标台阶值的时刻，记为 “aTarget: X m/s², crossed in T s”（generate_report.py）；
• sine / jitter 类机动：以本次运行实际峰值的 50% 为阈值，统计实际加速度第一次超过该阈值的时间，记为 “50% peak, crossed in T s”（generate_report.py）。

报告头部生成一张汇总表（maneuver / crossed 次数 / mean / min / max 交叉时间），这是横向控制调参对比中最直接的响应速度指标：交叉时间越短、方差越小，说明横向控制器跟踪目标曲率越迅速稳定。

5.3 报告图表

每个有效 run 生成一张五联图（webp 内嵌 base64 到 HTML）：

1. 横向加速度：橙线为 lateralManeuverPlan.desiredCurvature × vEgo²（目标），绿线为 controlsState.curvature × vEgo²（实际），黑圈标注交叉点；
2. 转向执行量：按车型 steerControlType 自适应绘制 carControl.actuators 与 carOutput.actuatorsOutput 的转向扭矩、转角或曲率（generate_report.py）；
3. 车速（mph）；
4. 横向加加速度（jerk）：对实际横向加速度求导后过一阶低通，截止频率复用 latcontrol_torque.py 中的 LP_FILTER_CUTOFF_HZ = 1.2 Hz；
5. 车身 roll 角（carControl.orientationNED[0]）。

报告同时内嵌 CarParams 折叠面板与 git commit/branch/remote 信息，输出路径为 generate_report.py 所示的 tools/lateral_maneuvers/lateral_reports/{platform}_{route}.html，生成后自动用默认浏览器打开。

6. 关键文件索引

文件	作用
openpilot/tools/lateral_maneuvers/README.md	使用流程（本文主体来源）
openpilot/tools/lateral_maneuvers/lateral_maneuversd.py	车载机动进程：阈值、机动定义、就绪/中止状态机、计划消息发布
openpilot/tools/lateral_maneuvers/generate_report.py	离线报告生成：run 切分、交叉时间统计、五联图与汇总表
openpilot/tools/longitudinal_maneuvers/maneuversd.py	Action / Maneuver 共享基类（动作断点插值与重复执行逻辑）
openpilot/selfdrive/controls/controlsd.py	机动计划替代模型曲率的注入点
openpilot/cereal/log.capnp	LateralManeuverPlan 消息结构定义
openpilot/system/manager/process_config.py	进程启动门控（lat_maneuver）
openpilot/selfdrive/selfdrived/events.py	“Lateral Maneuver Mode” 屏幕提示事件
openpilot/common/params_keys.h	LateralManeuverMode 参数键与清除策略

7. 适用前提与限制

• 该工具面向开发分支（如 master）设备与车型调参场景，参数在 offroad 或 manager 重启时自动清零，不会常驻生效；
• 测试需要长直、平坦、空旷的路段：曲率超过 0.004 1/m 或 roll 超过 6.8° 时机动一直等待；
• 车速偏离目标 0.7 m/s（约 1.6 mph）以上即中止当前机动，需将 ACC 稳定设在 20 或 30 mph；
• 报告统计仅纳入“全程 latActive 且无转向输入”的有效 run，人工碰过方向盘的运行只展示、不计入均值；
• 报告依赖 route 日志已完整上传（或本地存在对应 rlog.zst 分段），生成过程使用 LogReader 与 matplotlib，需在具备 Python 环境的开发机上运行。