战斗包子
控制算法

控制算法

2025年10月30日 7.4k 字 大概 31 分钟

本文由AI根据2024年7月版本代码梳理

控制系统架构文档

1. 概述

本文档详细描述了自动驾驶控制系统的软件架构。

该系统的核心使命是扮演“数字驾驶员”的角色。它承接上游规划 (Planning) 模块输出的期望轨迹，融合定位 (Localization) 模块和底盘 (HxCan) 的实时状态信息，通过一套可配置的控制算法，计算出精确、平顺且安全的转向、油门和刹车指令，最终发布给车辆硬件执行。

该框架的核心设计思想是高度模块化和算法可插拔，它支持两种不同的顶层执行架构和至少三种主流的控制策略。

2. 顶层执行架构

系统支持两种截然不同的执行架构，通过 FLAGS_use_control_submodules 标志在启动时进行切换。

架构 A：整体式 (Monolithic) 架构

当 FLAGS_use_control_submodules 为 false 时，系统运行在“整体式”架构下。

  • 核心文件: control_component.cc
  • 执行流程:
    1. ControlComponent 模块独自完成所有工作。
    2. 它订阅所有上游消息（规划、定位、底盘、Pad等）。
    3. 在 Proc() 函数中，它首先进行数据同步和有效性检查（CheckInput, CheckTimestamp）。
    4. 处理驾驶员的自动驾驶启用逻辑（ctrl_en）。
    5. 调用 controller_agent_ 来执行核心算法（详见 3. 核心算法策略）。
    6. 执行复杂的指令后处理，包括平滑滤波、变化率限制（Rate Limit）和加速度到扭矩的转换。
    7. 最后将 ControlSignal 消息发布。

架构 B：子模块流水线 (Submodule Pipeline) 架构

当 FLAGS_use_control_submodules 为 true 时，系统切换到“子模块流水线”架构。原先 ControlComponent 的职责被拆分到三个独立的模块中，形成一个清晰的数据处理流。

  • 第 1 站：preprocessor_submodule.cc (预处理器)

    • 职责：数据验证与分发。
    • 流程：订阅 LocalView，执行所有的数据验证工作（CheckInput, CheckTimestamp），并检查来自规划的 estop 信号。
    • 输出：一个“干净且可信”的 Preprocessor 消息，供下游使用。

  • 第 2 站：[Controller_Submodule] (控制器核心)

    • 职责：核心算法计算。
    • 流程：订阅 Preprocessor 消息。根据配置，系统会加载两种实现中的一种：
      1. lat_lon_controller_submodule.cc：用于横纵解耦控制，它内部会依次调用 lateral_controller_ 和 longitudinal_controller_。
      2. mpc_controller_submodule.cc：用于横纵统一控制，它内部只调用 mpc_controller_。
    • 输出：一个包含原始算法结果的 ControlCommand 消息。

  • 第 3 站：postprocessor_submodule.cc (后处理器)

    • 职责：安全冗余与发布。
    • 流程：订阅来自第 2 站的 ControlCommand。它的主要职责是做最后一道安全检查。如果检测到 error_code，它将覆盖算法结果，执行紧急刹车（soft_estop_brake）。
    • 输出：最终发送给硬件的 ControlCommand 消息。

3. 架构流程图

以下流程图展示了上述两种顶层架构和三种核心算法策略的组合关系。

graph TD
    subgraph InputSection[输入处理]
        A[输入: LocalView 规划/定位/底盘] --> B[ControlComponent]
    end

    B --> Condition1{FLAGS_use_control_submodules}
    
    Condition1 -->|true| C
    Condition1 -->|false| D

    subgraph Pipeline[流水线架构 Submodule Pipeline]
        C[架构B: 子模块流水线] --> E[1. Preprocessor 验证/Estop]
        E --> F[2. Controller Submodule 算法]
        F --> G[3. Postprocessor 安全/发布]
        G --> Z[输出: ControlCommand]
    end
    
    subgraph Monolithic[整体式架构 Monolithic]
        D[架构A: 整体式] --> H[ControllerAgent 算法分发]
        H --> I[指令后处理与力矩转换]
        I --> Z2[输出: ControlCommand]
    end

    subgraph AlgorithmLib[核心算法库]
        J{算法策略 根据Config配置选择}
        
        F --> J
        H --> J
        
        J -->|策略1: 统一控制| K[MPCController 横纵一体]
        
        J -->|策略2: 解耦控制| L[LonController 纵向PID]
        J -->|策略2: 解耦控制| M[LatController 横向LQR]
        
        J -->|策略3: 解耦控制| L
        J -->|策略3: 解耦控制| N[LatController 横向Simulink PID]
    end

    Z --> Output[最终控制指令]
    Z2 --> Output

4. 核心算法详解

本框架的 ControllerAgent (整体式) 和 Controller_Submodule (流水线式) 充当了“算法容器”，可根据配置文件加载并执行以下三种核心控制策略之一。

4.1 策略 A：MPC 统一控制 (mpc_controller.cc)

这是一种基于优化的横纵一体控制策略，它在一个模型中同时求解转向和加减速。

  • 算法: MPC (Model Predictive Control，模型预测控制)。
  • 状态向量: 其状态向量 matrix_state_ 包含了横向和纵向的全部误差信息：
    • [横向误差, 横向误差率, 朝向误差, 朝向误差率, 纵向位置误差, 纵向速度误差]
  • 核心逻辑:
    1. 预测模型: 使用一个二自由度单车模型（“自行车模型”）来预测车辆在未来 horizon_ 时间步（例如 20 步）内的动态。
    2. 优化求解: 在每个控制周期，它将控制问题构建为一个二次规划 (QP) 问题。
    3. 求解器: 调用 rina::common::math::MpcOsqp 优化求解器。
    4. 目标: 找到一个最优控制序列（未来 N 步的转向和加速度指令），该序列能在满足车辆物理约束（如最大转向角、最大加/减速度）的同时，最小化一个代价函数（由 matrix_q_ 惩罚状态误差和 matrix_r_ 惩罚控制量定义）。
    5. 执行: 算法只执行该序列中的第一个指令 (control[0])，并在下一个周期（例如 20ms 后）基于新的车辆状态重新进行整个预测和优化过程。

4.2 策略 B/C：纵向解耦控制 (lon_controller.cc)

这是一个经典的横纵解耦控制器，专门负责纵向（油门/刹车）控制。它被策略 B 和 C 共同使用。

  • 算法: 级联 PID (Cascade PID)。
  • 核心逻辑:
    1. 外环 (位置环): station_pid_controller_
      • 输入: station_error (车辆在规划轨迹上的位置误差，即落后或超前了多少米)。
      • 输出: speed_offset (一个用于追赶或等待目标点的“补偿速度”)。
    2. 内环 (速度环): speed_pid_controller_
      • 输入: 总速度误差 (即 规划目标速度 + speed_offset - 当前速度)。
      • 输出: 期望的加速度 acceleration_cmd_closeloop。
    3. 前馈与补偿: 最终的加速度指令 acceleration_cmd 是三者之和：
      • acceleration_cmd_closeloop (PID 闭环反馈)
      • preview_acceleration_reference (规划轨迹的前馈加速度)
      • slope_offset_compensation (基于车辆 Pitch 角估算的坡度补偿)
    4. 指令映射: 最后，通过一个 2D 标定查找表 control_interpolation_，将（期望加速度, 当前车速）键值对映射为最终的 throttle_cmd (油门开度) 和 brake_cmd (刹车压力) 百分比。

4.3 策略 B：横向解耦控制 - LQR (lat_controller_lqr.cc)

这是策略 B 使用的横向（转向）控制器，基于现代控制理论。

  • 算法: LQR (Linear-Quadratic Regulator，线性二次调节器)。
  • 状态向量: matrix_state_ 只包含横向状态：
    • [横向误差, 横向误差率, 朝向误差, 朝向误差率]
  • 核心逻辑:
    1. 车辆模型: 使用一个二自由度单车模型（matrix_a_, matrix_b_），该模型在 UpdateMatrix() 中根据当前车速实时更新。
    2. LQR 求解: 在每个周期，算法调用 rina::common::math::SolveLQRProblem 来实时求解代数黎卡提方程 (Riccati equation)。
    3. 最优增益: 求解器根据 matrix_q_ (状态误差代价) 和 matrix_r_ (控制量代价) 计算出一个最优反馈增益矩阵 matrix_k_。
    4. 最终指令: 转向角 = 反馈 + 前馈
      • 反馈 (steer_angle_feedback) = -K * X (即 -(matrix_k_ * matrix_state_))。这是 LQR 的核心反馈。
      • 前馈 (steer_angle_feedforward) (基于规划路径的曲率和车速计算的前馈量)。

4.4 策略 C：横向解耦控制 - Simulink PID (lat_controller.cc)

这是策略 C 使用的横向（转向）控制器，它是一种基于 PID 的混合实现。

  • 算法: PID (通过 Simulink Coder 实现) + 传递函数。
  • 核心逻辑: 这是一个复杂的混合实现：
    1. Simulink 代码生成: 代码的核心逻辑 ads_control_ 来自于 control/controller/matlab_code/ads_control.h。这表明其 PID 算法（包括复杂的积分器启动、重置和抗饱和逻辑）是在 MATLAB/Simulink 中设计和仿真，然后通过 Simulink Coder 自动生成为 C++ 代码的。
    2. 传递函数 (TF) 补偿: transfer_function.cc 文件提供了一阶和二阶传递函数的 C++ 实现。lat_controller.cc 使用这些 TF (如 lateral_error_transfer, heading_error_transfer) 对误差信号进行滤波和动态补偿，作为 ads_control_ 的辅助或补充。
    3. 最终指令: 最终的转向指令 steer_angle_req_raw_ 是由 Simulink 生成的 PID 输出、TF 补偿输出以及前馈量 (turn_ang_req_ff_) 组合而成的。

5. 关键辅助组件与设计模式

5.1 状态注入器 (DependencyInjector)

  • 文件: control_component.cc, controller_agent.cc, lat_lon_controller_submodule.cc 等。
  • 设计模式: 依赖注入 (Dependency Injection, DI)。
  • 职责: DependencyInjector 作为一个服务容器，在系统初始化时创建并持有一些共享的、无状态的服务，例如 VehicleStateProvider（车辆状态提供器）。
  • 目的: 避免了每个控制器都需要单独订阅和管理车辆状态，简化了代码结构。同时，它使得单元测试变得极其容易——测试时只需“注入”一个模拟的 (Mock) VehicleStateProvider 即可。

5.2 轨迹分析器 (TrajectoryAnalyzer)

  • 文件: lon_controller.cc, mpc_controller.cc, lat_controller.cc 等。
  • 职责: 这是一个工具类，它封装了来自规划的 ADCTrajectory 消息，并提供了大量便捷的查询功能。
  • 核心功能:
    • QueryNearestPointByPosition: 根据车辆的(x, y)坐标，在轨迹上找到最近的匹配点。
    • QueryNearestPointByAbsoluteTime: 根据绝对时间戳，查询轨迹在特定时刻的目标点。
    • QueryNearestPointByRelativeTime: 根据相对时间（例如“未来 1.5 秒”）查询轨迹上的预瞄点。
    • ToTrajectoryFrame: 将车辆的全局坐标 (x, y, heading) 转换为Frenet 坐标 (s: 沿轨迹距离, l: 横向偏移)。

5.3 传递函数 (transfer_function.cc)

  • 文件: lat_controller.cc (策略 C), transfer_function.cc。
  • 职责: 提供了 first_order_transfer_function 和 second_order_transfer_function 的 C++ 实现。
  • 核心功能:
    • 它使用Tustin 变换 (tustin变换) 作为离散化方法。
    • 这使得控制工程师能够将在 MATLAB 等工具中设计的经典连续域 (s-domain) 传递函数（用于滤波器或补偿器）无缝转换为可在微控制器上周期性执行的离散域 (z-domain) 差分方程。

6. 架构实现 (B)：子模块流水线

当 FLAGS_use_control_submodules 为 true 时，系统启用“流水线”架构。此架构将控制流程解耦为三个独立运行的子模块，职责清晰，逻辑内聚。

6.1 第 1 站：预处理器 (preprocessor_submodule.cc)

这是流水线的入口，负责所有数据的汇集与验证。

  • 核心职责: 订阅 LocalView 消息，进行数据检查，并发布一个“干净”的 Preprocessor 消息，供下游的控制器使用。
  • 关键逻辑:
    • 数据验证: 调用 CheckInput 和 CheckTimestamp 方法，确保所有上游数据（定位、底盘、规划）均有效且未超时。
    • Estop 处理: 检查 local_view.trajectory().estop().is_estop()，如果规划模块发出了 estop（紧急停止）请求，将设置错误状态。
    • Pad 消息: 检查来自 Pad（手柄）的 RESET 动作，用于清除 estop_ 标志。

6.2 第 2 站：控制器 (controller_submodules)

这是流水线的“大脑”，完全专注于执行核心控制算法。根据配置，系统会加载两个实现中的一个。

  • mpc_controller_submodule.cc:
    • 职责: 实现横纵一体控制。
    • 流程: 订阅 Preprocessor 消息，将其传递给内部唯一的 mpc_controller_ 实例进行计算。
  • lat_lon_controller_submodule.cc:
    • 职责: 实现横纵解耦控制。
    • 流程: 订阅 Preprocessor 消息，依次调用内部的 lateral_controller_ (横向) 和 longitudinal_controller_ (纵向)，将计算结果组合到 ControlCommand 中。
  • 共同点: 两个模块都会将计算结果发布到 FLAGS_control_core_command_topic 主题。

6.3 第 3 站：后处理器 (postprocessor_submodule.cc)

这是流水线的出口，负责最终的安全冗余和指令发布。

  • 核心职责: 订阅来自第 2 站的 control_core_command，执行最后一道安全检查，并发布最终的 ControlCommand。
  • 关键逻辑:
    • Estop 覆盖: 它会检查 control_core_command->header().status().error_code()。
    • 如果状态不是 OK（意味着预处理器或控制器中触发了 estop），它将覆盖算法的计算结果，强制设置一个“软刹车”指令 (control_command.set_brake(control_common_conf_.soft_estop_brake())）。
    • 发布: 将最终安全处理过的指令发布到 FLAGS_control_command_topic，供 CAN 总线驱动程序使用。

7. 架构实现 (A)：整体式逻辑 (control_component.cc)

当 FLAGS_use_control_submodules 为 false 时，control_component.cc 将独立执行所有逻辑。它除了调用 controller_agent（已在第 3 节中描述）执行算法外，还包含了自动驾驶启用、指令后处理和力矩转换等关键功能。

7.1 自动驾驶启用逻辑 (Engage Logic)

  • 职责: 处理驾驶员的意图，决定自动驾驶功能是否激活。
  • 关键逻辑:
    • 人工触发: 代码通过 LeCentButtn (方向盘左中按钮) 的状态变化来切换 ctrl_cmd_on（自动驾驶请求）布尔值。
    • 系统使能: 最终的控制启用标志 ctrl_en（Control Enable）必须同时满足三个条件：
      1. ctrl_cmd_on 为 true (驾驶员已按下按钮请求)。
      2. trajectory_msg != nullptr (规划模块已发送有效轨迹)。
      3. cdd_available 为 true (底盘（IPB）报告其“协调减速”功能可用，代表车辆准备就绪)。
    • Estop 重置: 一旦 ctrl_en 变为 true，系统会自动清除 estop_（紧急停止）标志，使车辆可以重新启动。

7.2 指令后处理 (Post-Processing)

  • 职责: 将算法输出的“原始”物理指令（如加速度、转向角）平滑化，以提升乘坐舒适性并符合执行器限制。
  • 纵向 (Acceleration) 平滑:
    • 使用 acc_up_rate_table_ 和 acc_down_rate_table_ 查找表，根据当前车速和上一帧的加速度，查询允许的最大加速度变化率（Jerk）。
    • 使用 rina::common::math::Clamp 函数，确保当前帧的加速度指令 final_acc_out 被限制在 [final_acc_out_pre + acc_dwn_rate_lkup_, final_acc_out_pre + acc_up_rate_lkup_] 范围内。这有效防止了车辆的“蹿动”和“点头”。
  • 横向 (Steering) 平滑:
    • 速率限制: 使用 angle_req_rate_lim_（转向角速率限制）对原始指令 eps_cmd_raw_fnl 进行 Clamp 操作。
    • 滤波: 或者，使用 angle_req_filter_coff_（滤波系数）对指令进行一阶低通滤波。
    • 变道特定逻辑: 当检测到 is_change_lane_path (正在变道) 时，系统会启用一套独立的滤波系数 change_lane_angle_req_filter_coff_，这通常是为了在变道初期让转向动作更加柔和。

7.3 核心：加速度到扭矩转换 (Accel-to-Torque)

这是 control_component 中最复杂的物理模型实现。它负责将纵向控制器（如 Lon-PID 或 MPC）输出的物理量（例如“期望加速度 $a_{\text{target}}$”）转换为执行器（电机/发动机）能理解的控制量（例如“期望扭矩 $T_{\text{target}}$”）。

这是一个经典的前馈 + 反馈 (Feedforward + Feedback) 控制结构。

  1. 坡度估算 (Slope Estimation):

    • 系统通过对比两种加速度来估算道路坡度 Estimated_Road_Slope：
      • $a_{\text{measured}}$: 车辆惯导（IMU）实际测量的纵向加速度 ego_accel。
      • $a_{\text{kinematic}}$: 通过车速变化（ego_spd_history）运动学计算出的加速度 ego_accel_dvdt。
    • 坡度角 Road_Slope $\approx \arcsin(\frac{a_{\text{measured}} - a_{\text{kinematic}}}{g})$。

  2. 前馈 (Feedforward) - 物理模型:

    • 系统计算车辆克服所有物理阻力并达到目标加速度所需的理论总牵引力 $F_{\text{resist}}$。
    • $F_{\text{resist}} = F_{\text{air}} + F_{\text{rolling}} + F_{\text{slope}} + F_{\text{inertia}}$
    • 其中：
      • Force_Air: 空气阻力（与速度的平方成正比）。
      • Force_Rolling: 滚动阻力（与车重和坡道法向分力相关）。
      • Force_Slope: 坡道阻力（车重在坡道上的分力）。
      • Force_Inertia: 惯性力（m \cdot a_{\text{target}}$，即 final_acc_out`）。

  3. **反馈 (Feedback) - PI 补偿**: * 物理模型总是不完美的。系统会计算**加速度误差 Error\_Ax** = final\_acc\_out (目标) - ego\_accel (实际)。 * 一个 PI 控制器（Force\_P 和 Force\_I）运行在此误差上，以补偿模型的任何不精确性。

  4. **最终扭矩 (Final Torque)**: * **总力** = $F_{\text{resist}}$ (前馈) + $F_{\text{PI}}$ (反馈)。 * Torque\_Target\_Out = (总力 $\cdot$ 轮胎半径) / (变速箱传动比 $\cdot$ 效率)。

7.4 驻车/起步状态机 (Standstill Logic)

* **职责**: 管理车辆在接近 0 速时的平稳停止和起步，防止“蠕动”或“溜车”。 * **关键逻辑**: * 实现了一个简单的**状态机** (Last\_sandstill\_req)。 * **进入驻车**: 如果车速低于阈值（FLAGS\_standstill\_req\_ego\_speed\_threshold）且目标加速度为负（刹车），在计时器（standstill\_timewindow）到期后，设置 standstill\_req = 1。 * **请求起步**: 如果 standstill\_req = 1，且目标加速度高于起步阈值（FLAGS\_go\_req\_final\_acc\_threshold），在计时器（drive\_off\_timewindow）到期后，设置 standstill\_req = 0 (同时 go\_req 将变为 1)。 * go\_req (起步请求) 和 standstill\_req (驻车请求) 信号被发送给车辆的 VLC (Vehicle Longitudinal Control) 系统。

8. 总结

rina::control 是一个高度成熟且复杂的控制系统框架。

* **架构灵活**: 它通过 FLAGS\_use\_control\_submodules 标志，同时支持“整体式”和“流水线式”两种软件架构，兼顾了低延迟和高解耦的需求。 * **算法多样**: 它通过“控制器工厂”模式，实现了 PID、LQR 和 MPC 三种主流控制算法的“可插拔”能力，允许针对不同场景或车型灵活选用。 * **逻辑完备**: 它不仅包含了核心算法，还实现了包括驾驶员交互、指令平滑、物理模型（力矩转换）、状态机（驻车管理）和安全冗余在内的完整闭环逻辑。

该框架的设计展示了清晰的职责分离（验证、算法、安全）、现代控制理论的应用（LQR/MPC）、经典控制的深度实践（级联PID/传递函数）以及对车辆动力学的深刻理解（加速度-扭矩模型）。

9. 关键设计原则与总结

通过对 rina::control 模块的深入分析，我们可以提炼出该框架所遵循的几项关键软件工程与控制理论原则。这些原则共同构建了一个功能强大、易于扩展且鲁棒的控制系统。

9.1 原则一：职责分离与架构解耦 (Separation of Concerns)

系统设计严格遵循职责分离原则，体现在两个层面：

  1. **架构解耦**: “流水线”架构（架构 B）是该原则的最佳体现。它将一个复杂的控制任务清晰地分解为三个独立的阶段：**数据验证** (preprocessor\_submodule)、**算法计算** (lat\_lon\_controller\_submodule, mpc\_controller\_submodule) 和**安全冗余** (postprocessor\_submodule)。这使得每个模块都可以独立开发、测试和升级。
  2. **算法解耦**: “横纵解耦”策略（策略 B/C）将车辆控制分解为两个正交的问题：lon\_controller 只关心纵向（前后）运动，而 lat\_controller 只关心横向（左右）运动。这大大降低了算法的设计和调参复杂度。

9.2 原则二：抽象化与工厂模式 (Abstraction & Factory Pattern)

* **文件**: controller\_agent.cc

系统通过 ControllerAgent 类（在“整体式”架构中）和 Controller 基类（未在文件中提供，但隐含存在）实现了一个**工厂模式**。

ControllerAgent 并不关心“具体”是哪个控制器在工作。它只通过 RegisterControllers 方法注册算法实例，并通过一个通用的 ComputeControlCommand 接口来调用它们。这种抽象设计使得添加一个新的控制算法（例如，未来可能加入的 SlidingModeController）变得非常简单，而无需修改任何上层调用逻辑（如 control\_component）。

9.3 原则三：高度可配置性 (High Configurability)

* **文件**: control\_component.cc, mpc\_controller.cc, lat\_controller\_lqr.cc 等

整个框架是**配置驱动**的。开发者无需重新编译代码即可彻底改变系统的行为：

  1. **架构切换**: 通过 FLAGS\_use\_control\_submodules 标志，可以一键切换“整体式”与“流水线式”架构。
  2. **算法切换**: 通过 control\_conf->active\_controllers() 列表，可以选择加载 MPC\_CONTROLLER（策略 A），或是加载 LAT\_CONTROLLER + LON\_CONTROLLER（策略 B/C）。
  3. **参数标定**: 所有的控制参数，如 PID 增益、LQR 代价矩阵 matrix\_q\_、MPC 权重，以及后处理中的平滑系数，都是从 .conf 配置文件中加载的。

9.4 原则四：混合控制 (Hybrid Control)

* **文件**: control\_component.cc, lon\_controller.cc, lat\_controller\_lqr.cc

系统完美融合了**前馈 (Feedforward)** 和**反馈 (Feedback)** 两种控制思想，这是实现高性能控制的基石。

* **反馈 (Feedback)**: * **职责**: 补偿**不可预测的误差**和**外部扰动**（如侧风、路面颠簸）。 * **实现**: PID、LQR 和 MPC 算法的核心都是反馈控制（基于 station\_error, lateral\_error 等）。 * **前馈 (Feedforward)**: * **职责**: 补偿**可预测的**、**基于物理模型**的力，使反馈控制器可以更“轻松”地工作。 * **实现**: 1. **力矩转换模型**: 在 control\_component 中，Force\_Air (风阻), Force\_Rolling (滚阻), Force\_Slope (坡阻) 都是纯粹的前馈项。 2. **曲率前馈**: 在 lat\_controller\_lqr 中，steer\_angle\_feedforward 基于路径曲率和车速计算得出，用于提前转动方向盘以应对弯道。 3. **加速度前馈**: 在 lon\_controller 中，preview\_acceleration\_reference 作为前馈项被直接加到 PID 的输出上。

9.5 原则五：基于模型的设计 (Model-Based Design)

* **文件**: mpc\_controller.cc, lat\_controller\_lqr.cc, lat\_controller.cc

该框架深度依赖于车辆动力学模型和数学模型。

  1. **车辆动力学模型**: LQR 和 MPC 算法均建立在一个**二自由度单车模型**之上（由 matrix\_a\_ 和 matrix\_b\_ 描述）。控制器的性能直接取决于该模型对真实车辆的逼近程度。
  2. **Simulink 集成**: lat\_controller.cc 通过集成自动生成的 C 代码 (ads\_control\_)，展示了**MBD (Model-Based Design)** 的开发流程。算法在 Simulink 中进行图形化设计、仿真和验证，然后一键生成可部署的 C/C++ 代码。
  3. **经典控制理论**: lat\_controller.cc 和 transfer\_function.cc 对**传递函数**和 **Tustin 变换**的应用，表明工程师在频域内进行了滤波器和补偿器的设计。

9.6 总结

rina::control 是一个高度工程化的自动驾驶控制系统。它不是一个单一算法的简单实现，而是一个**灵活的、可配置的、多策略的“算法容器”**。

它通过**架构解耦**（流水线）来保证软件质量和可维护性，通过**算法抽象**（控制器工厂）来保证可扩展性，并通过**混合控制**（前馈+反馈）和**基于模型的设计**（LQR/MPC/Simulink）来保证核心的控制性能、舒适性和鲁棒性。

10. 关键问题与解答 (Q&A)

Q1 (架构广度)：请梳理一下这份代码的整体架构。

**[回答]**

“这份代码实现了一个高度模块化、可配置的控制系统。它在顶层设计上支持**两种执行架构**，通过 FLAGS\_use\_control\_submodules 标志切换。

  1. **第一种是“整体式”架构**。所有逻辑，从数据检查、算法调用到后处理（如力矩转换），都在 ControlComponent 的 Proc() 函数中执行。
  2. **第二种是“子模块流水线”架构**。它将流程拆分为三个解耦的节点： * PreprocessorSubmodule 负责数据验证。 * ControllerSubmodule (如 MpcControllerSubmodule 或 LatLonControllerSubmodule) 负责核心算法计算。 * PostprocessorSubmodule 负责最终的安全检查和指令发布，例如处理 estop。

在**算法层**，该框架通过 ControllerAgent 或 ControllerSubmodule 充当“算法容器”，可以**灵活加载**至少三种不同的控制策略：

* **策略 A**：mpc\_controller (横纵统一的 MPC 控制)。 * **策略 B**：lon\_controller (纵向 PID) + lat\_controller\_lqr (横向 LQR)。 * **策略 C**：lon\_controller (纵向 PID) + lat\_controller (基于 Simulink Coder 生成的 PID)。

这种设计在架构上实现了高解耦，在算法上实现了高灵活性。”

Q2 (算法深度 - Lon)：lon\_controller.cc 是如何实现纵向控制的？

**[回答]**

“lon\_controller 实现了一个非常经典的**级联 PID（Cascade PID）**控制策略。它包含两个闭环：

  1. **外环（位置环）**：station\_pid\_controller\_。它的输入是 station\_error（即车辆在轨迹上的位置误差，是超前还是落后）。它的输出是一个**补偿速度 speed\_offset**。
  2. **内环（速度环）**：speed\_pid\_controller\_。它的输入是总的速度误差，即 (目标速度 + 补偿速度) - 当前速度。它的输出是**期望的加速度 acceleration\_cmd\_closeloop**。

最终的加速度指令还叠加了**前馈项**（preview\_acceleration\_reference，来自规划轨迹）和**补偿项**（slope\_offset\_compensation，坡度补偿）。

最后，这个期望的物理加速度会通过一个 2D 标定表 control\_interpolation\_，转换为执行器能理解的油门（Throttle）和刹车（Brake）百分比指令。”

Q3 (算法深度 - Lat)：lat\_controller\_lqr.cc 和 mpc\_controller.cc 在控制转向时有什么核心区别？

**[回答]**

“它们代表了两种不同层次的控制思想。

**lat\_controller\_lqr (LQR)** 是一种**最优反馈控制**。

* 它将问题视为一个**状态调节**问题，目标是“在当前时刻，施加多大的控制量（转向），才能最快地消除当前的状态误差（如横向误差、朝向误差）”。 * 它通过 SolveLQRProblem 实时求解代数黎卡提方程，得到一个**最优增益矩阵 K**。 * 它的核心公式是 u = -Kx，即**反馈转向角 steer\_angle\_feedback = -(matrix\_k\_ \* matrix\_state\_)**。它不“向前看”，只专注于最优地消除当前误差。

**mpc\_controller (MPC)** 是一种**最优预测控制**。

* 它将问题视为一个**轨迹跟踪**问题，目标是“我应该执行**一串什么样**的控制序列（转向和加速），才能使我**未来 N 秒**的行驶轨迹与规划轨迹最吻合”。 * 它使用车辆模型**向前预测**（horizon\_ 步长），并通过 MpcOsqp 求解器 找到这个**最优控制序列**。 * 最重要的是，MPC 能在优化时**显式地处理物理约束**，比如 upper\_bound << wheel\_single\_direction\_max\_degree\_，这是 LQR 难以做到的。

**总结一下**：LQR 是一种高性能的**反馈调节器**（Error Regulator），而 MPC 是一种更强大的**预测跟踪器**（Trajectory Tracker），它能提前处理未来的路径变化（如曲率）和物理约束。”

Q4 (实现细节)：在 control\_component.cc 中，驻车（Standstill）和起步（go\_req）逻辑是如何实现的？

**[回答]**

“这个模块 通过一个**基于时间窗的状态机**来管理驻车和起步，以确保平顺性。

  1. **进入驻车 (Standstill)**： * 条件是：如果车辆**上一帧不是驻车状态** (Last\_sandstill\_req == 0)，且**车速低于阈值**（如 FLAGS\_standstill\_req\_ego\_speed\_threshold），并且**目标加速度为负**（即需要刹车）。 * 系统会启动一个计时器 standstill\_timewindow。当计时器超过阈值（FLAGS\_standstill\_req\_time\_window\_threshold）后，系统才正式将 standstill\_req（驻车请求）置为 1。

  2. **请求起步 (Go Request)**： * 条件是：如果车辆**上一帧是驻车状态** (Last\_sandstill\_req == 1)，并且**目标加速度大于起步阈值**（FLAGS\_go\_req\_final\_acc\_threshold，即驾驶员或规划希望车辆启动）。 * 系统会启动另一个计时器 drive\_off\_timewindow。当计时器超过阈值（FLAGS\_go\_req\_time\_window\_threshold）后，系统会将 standstill\_req 置为 0，此时 go\_req = !standstill\_req 将变为 1。

这种基于时间窗的设计可以有效过滤掉高频的启停指令，防止车辆在“停未停”的临界状态下（例如堵车蠕动）产生顿挫。”

Q5 (架构权衡)：为什么要同时支持“整体式”和“子模块流水线”两种架构？

**[回答]**

“这体现了在不同工程阶段对**性能**和**可维护性**的权衡。

* **“整体式”架构** 将所有逻辑放在一个组件（甚至一个 Proc 函数）中执行。它的**优点是性能最高、延迟最低**，因为所有数据都在内存中直接传递，没有消息通信的开销。这可能非常适合资源受限的嵌入式硬件或对延迟极其敏感的场景。 * **“子模块流水线”架构** 遵循“职责分离”原则，将系统解耦。它的**优点是可维护性、可测试性和团队协作效率最高**。例如： * 负责数据验证的团队可以独立开发 preprocessor\_submodule。 * 负责算法的团队可以独立开发 lat\_lon\_controller\_submodule。 * 负责安全的团队可以独立开发 postprocessor\_submodule。

同时支持两种架构，使得团队可以在**开发和调试阶段**使用“流水线”架构以提高效率和鲁棒性，而在**最终部署阶段**可以（如果需要的话）切换到“整体式”架构以追求极致的性能。”

Q6 (模型细节)：control\_component.cc 中的“加速度到扭矩”转换模型有什么意义？

**[回答]**

“这个模型 是连接**高层控制算法**（输出物理量：加速度）和**底层执行器**（接收控制量：扭矩）的关键桥梁，它是一个**基于物理模型的前馈 + 反馈**系统。

  1. **为什么需要它？** * lon\_controller 或 mpc\_controller 输出的“期望加速度 $a_{\text{target}}$”是一个**物理目标**，而不是一个**控制指令**。 * 车辆在不同工况下（如上坡、下坡、高速、低速）达到**相同**的加速度，所需的**扭矩**是截然不同的。 * 如果没有这个模型，单纯用一个 PID 去跟踪加速度误差，会导致 PID 的积分项需要吸收所有（风阻、坡阻、滚阻）的非线性变化，导致调参困难且响应迟钝。

  2. **它如何工作？** * **前馈 (Feedforward)**: 它首先基于物理模型（Force\_Air, Force\_Rolling, Force\_Slope, Force\_Inertia） 计算出一个**理论上**达到 $a_{\text{target}}$ 所需的“基础扭矩”。这解决了模型中所有**已知**的非线性问题。 * **反馈 (Feedback)**: 然后，它使用一个 PI 控制器（基于 Error\_Ax = $a_{\text{target}}$ - $a_{\text{actual}}$） 来补偿所有未知的扰动或模型的不精确性（例如，估算的坡度不准、风向突变等）。

通过这种方式，它极大地简化了上层控制器的任务，使其只需专注于运动学规划，而无需关心底层的动力学细节。”

本文作者：战斗包子
本文链接：https://paipai121.github.io/2025/10/30/工作/控制算法/
版权声明：本文采用 CC BY-NC-SA 3.0 CN 协议进行许可
工作体验