当前位置： 首页 > news >正文

[pilot智驾系统] 纵向规划器(LongitudinalPlanner) | 模型预测控制(MPC)

news 2025/8/27 9:27:55

第6章：纵向规划器(LongitudinalPlanner)

在上一章中，我们了解到模型守护进程(modeld)是sunnypilot的"视觉先知"，能够预测前方道路、车道线和其他车辆。

modeld告诉我们即将发生的情况，并给出建议的期望路径和加速度。但这些仅仅是预测。

sunnypilot如何将这些原始预测转化为车辆速度与加速度的平稳、舒适且最重要的是安全的规划？

它如何精确决定何时踩油门或刹车以跟随前车、在信号灯前停下或达到巡航速度？

这就是**纵向规划器(LongitudinalPlanner)**的职责所在

将纵向规划器视为sunnypilot的战略速度管理者。它就像大脑中持续计算最佳加速和刹车策略的部分，综合当前车速、期望速度（来自巡航控制或导航）、其他车辆距离以及modeld对道路的预测等信息，为未来几秒制定详细的加速度"计划"。

其主要职责是生成平稳安全的加速度曲线，确保保持舒适跟车距离、平顺停车并高效达到目标速度，同时遵守交通规则。

为什么需要纵向规划器？

没有纵向规划器，sunnypilot虽然能接收modeld的预测（如"此处需要轻微加速"），但无法智能地将其转化为随时间推移的系列油门刹车动作。可能导致刹车过猛、加速突兀或跟车过近。

纵向规划器采用**模型预测控制(MPC)**技术解决这个问题。MPC如同拥有一个超级智能助手，持续展望未来（约10秒），尝试不同油门刹车组合，最终选择最优动作序列。

这个"最优"序列需满足：保证安全（避免碰撞）、确保舒适（无突然颠簸）并实现目标（达到期望速度/距离）。

它确保sunnypilot不仅对当前时刻做出反应，更能提前规划以实现平顺最优的行驶体验。

用例：保持安全跟车距离

假设我们在高速公路上开启sunnypilot跟随前车，前车突然开始减速。

纵向规划器如何确保我们的车辆也平稳减速，保持安全舒适的跟车距离，避免刹车过猛或距离过近？

它通过持续执行以下步骤实现：

  1. 观察：通过雷达获取当前车速及前车距离和速度
  2. 预测：使用modeld的输出确认前车行为和道路曲率
  3. 规划：运行优化算法，找到未来几秒最平顺安全的加速度（或减速度）曲线，完美匹配前车减速，同时遵循设定的跟车距离和舒适性参数
  4. 输出：向控制守护进程(controlsd)提供具体的期望加速度(aTarget)和是否完全停车(shouldStop)指令

纵向规划器工作原理

纵向规划器持续运行，每秒多次（20Hz频率，即每0.05秒与modeld输出同步）。以下是其核心任务的简化流程：

  1. 收集信息：获取所有必要输入：
    • 当前车辆状态：通过车辆接口(CarInterface)获取车速和加速度
    • 雷达信息：雷达系统提供前车距离和速度
    • 模型预测：模型守护进程(modeld)提供的未来路径、速度和加速度预测
    • 巡航速度：设定的巡航控制速度
    • 系统状态：自动驾驶守护进程(selfdrived)提供的sunnypilot激活状态和驾驶"个性"
  2. 运行优化：将所有信息输入核心组件LongitudinalMpc（模型预测控制求解器），计算未来约10秒车辆的最优加速度和速度轨迹（速度随时间变化的计划）
  3. 提取关键指令：从详细轨迹中提取最重要的即时指令：下一时刻的目标加速度(aTarget)和停车标志(shouldStop)
  4. 发布输出：将这些指令与完整预测的速度/加速度轨迹打包成longitudinalPlan消息，发送给控制守护进程(controlsd)执行

流程可视化：
在这里插入图片描述

如何使用纵向规划器（与其输出交互）

作为初学者，我们不会直接"调用"纵向规划器。

其他sunnypilot组件（特别是控制守护进程(controlsd)）会监听其发布的longitudinalPlan消息。

该消息包含controlsd用于控制车辆油门刹车的精确aTarget（期望加速度）和shouldStop标志。

以下是控制守护进程(controlsd)读取该信息的简化示例：

import cereal.messaging as messaging
from cereal import log # 访问LongitudinalPlan消息结构# 创建SubMaster监听消息
sm = messaging.SubMaster(["longitudinalPlan"])# 在控制守护进程主循环中：
while True:sm.update(0) # 检查新消息if sm.updated["longitudinalPlan"]:plan = sm["longitudinalPlan"]# 从规划器获取期望加速度desired_accel = plan.aTargetprint(f"纵向规划器期望加速度: {desired_accel:.2f} m/s^2")# 检查规划器是否认为应该停车if plan.shouldStop:print("纵向规划器指令: 准备停车!")else:print("纵向规划器指令: 继续行驶")# 也可查看预测的未来速度轨迹:# print(f"未来速度(前5个值): {plan.speeds[:5]}")# ... controlsd其余逻辑 ...

这段代码展示controlsd如何将aTarget作为油门/刹车的主要指令，shouldStop用于特定停车操作。

纵向规划器的详细速度计划正是通过这些指令直接影响车辆加速度。

底层原理：LongitudinalPlanner核心循环

在这里插入图片描述
LongitudinalPlanner主要在selfdrive/controls/lib/longitudinal_planner.py实现，其update方法被重复调用以生成新计划。

以下是update方法中关键步骤的简化版本：

# 摘自selfdrive/controls/lib/longitudinal_planner.py（简化版）
import numpy as np
from openpilot.selfdrive.controls.lib.longitudinal_mpc_lib.long_mpc import LongitudinalMpcclass LongitudinalPlanner:def __init__(self, CP, init_v=0.0, init_a=0.0, dt=0.05):self.mpc = LongitudinalMpc(dt=dt) # 核心优化求解器self.output_a_target = 0.0self.output_should_stop = False# ... 其他初始化 ...def update(self, sm):v_ego = sm['carState'].vEgo # 当前速度v_cruise = sm['carState'].vCruise * CV.KPH_TO_MS # 设定巡航速度# 1. 解析模型守护进程的预测x, v, a, j, throttle_prob = self.parse_model(sm['modelV2'])# x, v, a, j是自车的预测轨迹# 2. 设置MPC求解器当前状态和参数#    (如根据激活状态启用/禁用先前加速度约束)prev_accel_constraint = not sm['carState'].standstill # 简化检查self.mpc.set_weights(prev_accel_constraint, personality=sm['selfdriveState'].personality)self.mpc.set_cur_state(self.v_desired_filter.x, self.a_desired) # 设置MPC初始速度和加速度# 3. 运行核心MPC优化!self.mpc.update(sm['radarState'], v_cruise, x, v, a, j, personality=sm['selfdriveState'].personality)# 4. 从MPC解中提取期望轨迹self.v_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.v_solution)self.a_desired_trajectory = np.interp(CONTROL_N_T_IDX, T_IDXS_MPC, self.mpc.a_solution)# 5. 确定即时输出指令(aTarget和shouldStop)action_t = self.CP.longitudinalActuatorDelay + DT_MDL # 考虑延迟self.output_a_target, self.output_should_stop = get_accel_from_plan(self.v_desired_trajectory, self.a_desired_trajectory, CONTROL_N_T_IDX, action_t=action_t)# 6. 对输出加速度应用最终裁剪/平滑self.output_a_target = np.clip(self.output_a_target, ACCEL_MIN, ACCEL_MAX) # 简化裁剪def publish(self, sm, pm):plan_send = messaging.new_message('longitudinalPlan')longitudinalPlan = plan_send.longitudinalPlanlongitudinalPlan.aTarget = float(self.output_a_target)longitudinalPlan.shouldStop = bool(self.output_should_stop)# ... 填充其他轨迹数据 ...pm.send('longitudinalPlan', plan_send)

这个简化的update方法展示了self.mpc.update()的核心作用，正是这里完成了计算最优加速度曲线的繁重工作。

规划器核心：LongitudinalMpc

在这里插入图片描述

LongitudinalMpc类（位于selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py）是驱动规划过程的引擎，实现了**模型预测控制(MPC)**求解器。

在此上下文中MPC的含义：

  • 模型：具有车辆速度加速度(x_ego, v_ego, a_ego)随加加速度(j_ego，即加速度变化率)变化的简化数学模型
  • 预测：使用该模型预测未来"视野"（约10秒，分为N=12个时间步长）内车辆速度加速度的变化
  • 控制：计算该时间范围内最佳的加加速度指令序列以实现特定目标：
    • 跟车距离：保持与前车的安全距离
    • 巡航速度：达到并维持巡航控制设定速度
    • 舒适性：最小化加速度突变（加加速度）
    • 安全性：避免碰撞障碍物或前车
    • 限制：遵守车辆物理加速度/减速度极限

LongitudinalMpc通过gen_long_model()和gen_long_ocp()（脱机构建求解器代码）设置。这些函数定义了车辆物理模型（“模型”）、“成本”（需最小化或最大化的目标，如舒适性、与障碍物距离）和"约束"（必须遵守的规则，如不超过最大加速度或避免碰撞）。

LongitudinalMpc的update方法运行实际求解器：

# 摘自selfdrive/controls/lib/longitudinal_mpc_lib/long_mpc.py（简化版）
class LongitudinalMpc:# ... (初始化方法) ...def update(self, radarstate, v_cruise, x, v, a, j, personality=log.LongitudinalPersonality.standard):# 1. 根据驾驶个性计算期望跟车距离t_follow = get_T_FOLLOW(personality) # 如标准个性为1.45秒# 2. 处理前车数据（推算其未来位置）lead_xv_0 = self.process_lead(radarstate.leadOne) # 前车0的未来x, vlead_xv_1 = self.process_lead(radarstate.leadTwo) # 前车1的未来x, v# 3. 为MPC创建"障碍物"：#    - 实际前车（推算位置+停车等效距离）#    - 代表巡航设定速度的"虚拟"障碍物lead_0_obstacle = lead_xv_0[:,0] + get_stopped_equivalence_factor(lead_xv_0[:,1])# ... 计算其他障碍物 ...cruise_obstacle = np.cumsum(T_DIFFS * v_cruise_clipped) + get_safe_obstacle_distance(v_cruise_clipped, t_follow)# 4. 确定未来每个时间步最近的障碍物self.params[:,2] = np.min(x_obstacles, axis=1) # MPC的'x_obstacle'参数# 5. 设置其他MPC参数（加速度最小/最大值、先前加速度等）self.params[:,0] = ACCEL_MINself.params[:,1] = ACCEL_MAXself.params[:,3] = np.copy(self.prev_a) # 用于平滑self.params[:,4] = t_followself.params[:,5] = LEAD_DANGER_FACTOR # 使跟车成为软约束# 6. 设置MPC参考轨迹（期望实现的目标）#    这些是模型的期望路径/速度或调整后的巡航目标self.yref[:,1] = x # 期望位置self.yref[:,2] = v # 期望速度self.yref[:,3] = a # 期望加速度self.yref[:,5] = j # 期望加加速度# 7. 运行Acados求解器（实际优化过程）self.run()# 8. 存储最优解（预测速度、加速度、加加速度）self.v_solution = self.x_sol[:,1] # 最优速度轨迹self.a_solution = self.x_sol[:,2] # 最优加速度轨迹self.j_solution = self.u_sol[:,0] # 最优加加速度轨迹

LongitudinalMpc.update方法通过定义当前状态、未来障碍物（前车、巡航速度）和参考轨迹来构建优化问题

随后调用self.run()触发高效的C语言生成ACADOS求解器，找到未来10秒的最优v_solution、a_solution和j_solution。

个性化与跟车距离

在这里插入图片描述

sunnypilot允许选择不同的"纵向驾驶个性"（轻松、标准、激进）。这些个性直接影响纵向规划器的参数，特别是t_follow（跟车时距）和jerk_factor（加加速度系数）。

  • t_follow：定义跟随前车的秒数。数值越大跟车距离越远，越小则越近
  • jerk_factor：影响MPC对加速度突变的惩罚程度。数值越高驾驶越平顺（减少颠簸）

这让我们可以自定义sunnypilot的加速刹车行为以适应个人偏好。

总结

**纵向规划器(LongitudinalPlanner)**是sunnypilot精密的速度与加速度管理者。

它综合车辆状态、前车信息、道路条件和用户偏好，采用高级优化算法（模型预测控制）生成未来几秒平稳安全的加速度计划。

通过输出精确的aTarget和shouldStop标志，直接控制车辆油门刹车，提供精致智能的驾驶体验。

接下来我们将聚焦驾驶安全，探索监控驾驶员注意力的驾驶员监控守护进程(dmonitoringd)

下一章：驾驶员监控守护进程(dmonitoringd)

http://www.dtcms.com/a/352066.html

相关文章：

  • jdk9安装步骤及下载（附小白详细教程）
  • 在Linux系统文件上次及下载
  • 《2025年最新IDE激活码永久破解教程 – 支持JetBrain全家桶2099年授权》
  • UE5安全架构审视：创造者的伊甸园与黑客的游乐场
  • pytorch入门4：cnn卷积神经网络
  • 《UE5_C++多人TPS完整教程》学习笔记44 ——《P45 倾斜与侧向移动（Leaning And Strafing）》
  • MoonBit Pearls Vol.06: MoonBit C-FFI 开发指南
  • 【新启航】现场逆向抄数实战：手持 3D 扫描仪 + 移动建模 APP 的轻量化工具组合与快速响应能力
  • 三款音乐生成工具，你更喜欢哪一个？
  • 如何在pixel上验证webview的功能
  • 服务初始化
  • 基于单张图像的深度估计方法研究：利用 Hugging Face 与 FiftyOne 实现单目深度估计模型的运行与评估
  • 从零开始学MCP(7) | 实战：用 MCP 构建论文分析智能体
  • 零基础从头教学Linux（Day 20）
  • javascript 基础知识- 字面量/内置对象
  • LVGL学习
  • 【设计模式】 面向对象基础
  • K8S-Service资源对象
  • 虚拟机中kubeadim部署的k8s集群,虚拟机关机了，重新开机后集群状态能否正常恢复的两种可能（详解）
  • 114、【OS】【Nuttx】【周边】效果呈现方案解析：-print0 补充（下）
  • WeakAuras Lua Script ICC （BarneyICC） Simplified Chinese [Mini]
  • WeakAuras Lua Script （My Version)
  • 【数据分享】各地级市当年实际使用外商外资金额（2003-2021）-有缺失值
  • 【AI Agent三】工具使用设计模式
  • 系统设计（数据库/微服务）
  • 基于Python+AlphaBot 实现红外遥控且自动避障的嵌入式智能小车系统
  • Cursor 中文输出设置：繁体改为简体的方法
  • uniapp 页面favicon.ico文件不存在提示404问题解决
  • uniapp 自动升级-uni-upgrade-center
  • 家庭事务管理系统|基于java和vue的家庭事务管理系统设计与实现(源码+数据库+文档)