【一看就会】Autoware.universe的控制部分源码梳理【二】

标签：const mpc universe Autoware traj 源码 param double steer

提示：文章写完后，目录可以自动生成，如何生成可参考右边的帮助文档

文章目录

前言
一、autoware_mpc_lateral_controller文件架构
二、mpc_lateral_controller.cpp
- 1.mpc_lateral_controller.cpp源码注释
- 2.mpc_lateral_controller.cpp解释
三、mpc.cpp
- 1.mpc.cpp代码注释
四、vehicle_model
五、qp_solver
- 1.qp_solver_osqp.cpp
- 2.qp_solver_unconstraint_fast.cpp
六、mpc_trajectory.cpp
七、mpc_utils.cpp
八、lowpass_filter.cpp
九、steering_predictor.cpp
十、steering_offset
总结

前言

上一篇讲完纵向速度控制了，这一篇讲横向速度控制的mpc。

一、autoware_mpc_lateral_controller文件架构

这个文件夹里面的文件很多。
mpc原理倒是不难，但是实现起来不容易，我们先将这些文件给梳理清楚。

autoware_mpc_lateral_controller文件夹中有6个cpp文件，还有三个文件夹。
分别是：

以下是对 `autoware_mpc_lateral_controller` 文件夹中这些文件的一个可能的解释：


1. **lowpass_filter.cpp**：
    - 低通滤波器的实现。
    - 低通滤波器通常用于信号处理，在车辆控制中可能用于对某些传感器信号（如速度、位置、角度等）进行滤波，以去除高频噪声。例如，对车辆的侧向速度或转向角的测量值进行滤波，使信号更加平滑，避免因传感器噪声导致的控制不稳定。

2. **mpc.cpp**：
    - 实现模型预测控制（Model Predictive Control，MPC）算法的主要文件。
    - MPC 是一种先进的控制算法，它基于系统的动态模型，在每个控制周期内求解一个有限时间范围内的优化问题，以找到最优控制序列。在车辆控制中，可能会根据车辆的状态（位置、速度、姿态等）、参考轨迹以及车辆动力学模型，预测未来一段时间内的系统行为，并优化控制输入（如转向角、加速度等），使车辆尽可能地跟随参考轨迹。

3. **mpc_lateral_controller.cpp**：
    - 横向控制器的实现文件，使用 MPC 算法进行车辆的横向控制。
    - 它会调用 `mpc.cpp` 中的 MPC 算法核心，并结合车辆的横向动力学模型，根据车辆的当前状态和期望的横向轨迹（如期望的路径或车道中心线），计算出最优的转向角控制输入，使车辆能够稳定地保持在期望的横向位置。

4. **mpc_trajectory.cpp**：
    - 负责处理 MPC 所需的轨迹信息。
    - 包括存储和管理参考轨迹，将参考轨迹转换为 MPC 算法所需的格式，例如将离散的路径点转换为多项式曲线或其他函数形式，以便 MPC 算法使用。它可能还会对轨迹进行预处理，如平滑、插值或根据车辆当前位置截取合适的轨迹段。

5. **mpc_utils.cpp**：
    - 包含一些 MPC 算法的辅助函数或工具函数。
    - 这些函数可能包括矩阵运算、约束的生成、目标函数的构建等。例如，在 MPC 求解过程中需要构建优化问题，可能会涉及到矩阵的构建、求逆、乘法等操作，这些操作可能会封装在 `mpc_utils.cpp` 中。


6. **steering_predictor**：
    - 可能用于预测车辆的转向行为。
    - 基于车辆的动力学模型和当前状态，预测未来一段时间内的转向角变化，为 MPC 算法提供更多的信息，或者用于评估当前控制输入的效果，以便进行更好的控制决策。

7. **qp_solver**文件夹：
    - 二次规划（Quadratic Programming，QP）求解器的实现或接口文件。
    - 由于 MPC 问题通常可以转化为一个二次规划问题，该文件提供了求解二次规划问题的能力，求解器将接收 MPC 问题的目标函数和约束条件（以矩阵和向量的形式），并计算出最优解。

8. **steering_offset**文件夹：
    - 转向偏移的处理。
    - 车辆的转向系统可能存在机械偏移，该文件可能包含用于校准或补偿转向偏移的功能。例如，根据车辆的校准数据或传感器反馈，对计算出的转向角进行偏移补偿，以确保实际转向与期望转向相符。


9. **vehicle_model**文件夹：
    - 包含车辆的动力学模型。
    - 车辆的动力学模型是 MPC 算法的基础，可能是一个简单的运动学模型（如阿克曼转向模型）或更复杂的动力学模型（考虑车辆的质量、惯性、轮胎力等）。这个文件会提供车辆状态更新方程，描述车辆如何根据控制输入（如转向角和加速度）和当前状态（如速度、位置、姿态）随时间变化。


**调用关系**：
- 当需要进行车辆的横向控制时，首先会调用 `mpc_lateral_controller.cpp`，它会从 `mpc_trajectory.cpp` 获取参考轨迹，从传感器或其他模块获取车辆的当前状态信息。
- `mpc_lateral_controller.cpp` 会使用 `vehicle_model` 中的车辆动力学模型，结合 `mpc.cpp` 中的 MPC 算法，构建 MPC 优化问题。
- 在构建 MPC 优化问题时，可能会调用 `mpc_utils.cpp` 中的辅助函数。
- 构建好的优化问题将传递给 `qp_solver` 进行求解，得到最优的控制输入（如转向角）。
- 得到的控制输入可能会经过 `steering_offset` 进行偏移补偿，最终输出到车辆的执行机构。

其中主文件为mpc_lateral_controller.cpp，我们继续往下说。

二、mpc_lateral_controller.cpp

1.mpc_lateral_controller.cpp源码注释

// Copyright 2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/mpc_lateral_controller.hpp"

#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_osqp.hpp"
#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_unconstraint_fast.hpp"
#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"
#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"
#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"
#include "autoware_vehicle_info_utils/vehicle_info_utils.hpp"
#include "tf2/utils.h"
#include "tf2_ros/create_timer_ros.h"

#include <algorithm>
#include <deque>
#include <limits>
#include <memory>
#include <string>
#include <utility>
#include <vector>

namespace autoware::motion::control::mpc_lateral_controller
{

MpcLateralController::MpcLateralController(
  rclcpp::Node & node, std::shared_ptr<diagnostic_updater::Updater> diag_updater)
: clock_(node.get_clock()), logger_(node.get_logger().get_child("lateral_controller"))
{
    // 定义 lambda 函数，用于从节点获取 int 类型的参数
    const auto dp_int = [&](const std::string & s) { return node.declare_parameter<int>(s); };
    // 定义 lambda 函数，用于从节点获取 bool 类型的参数
    const auto dp_bool = [&](const std::string & s) { return node.declare_parameter<bool>(s); };
    // 定义 lambda 函数，用于从节点获取 double 类型的参数
    const auto dp_double = [&](const std::string & s) { return node.declare_parameter<double>(s); };

    // 存储诊断更新器的指针
    diag_updater_ = diag_updater;

    // 创建一个 MPC 对象的唯一指针
    m_mpc = std::make_unique<MPC>(node);

    // 从节点获取控制周期并设置 MPC 的控制周期
    m_mpc->m_ctrl_period = node.get_parameter("ctrl_period").as_double();

    // 轨迹过滤参数
    auto & p_filt = m_trajectory_filtering_param;
    // 是否启用路径平滑
    p_filt.enable_path_smoothing = dp_bool("enable_path_smoothing");
    // 路径平滑的移动平均点数
    p_filt.path_filter_moving_ave_num = dp_int("path_filter_moving_ave_num");
    // 轨迹曲率平滑的点数
    p_filt.curvature_smoothing_num_traj = dp_int("curvature_smoothing_num_traj");
    // 参考转向曲率平滑的点数
    p_filt.curvature_smoothing_num_ref_steer = dp_int("curvature_smoothing_num_ref_steer");
    // 轨迹重采样的距离
    p_filt.traj_resample_dist = dp_double("traj_resample_dist");
    // 是否为最终偏航控制扩展轨迹
    p_filt.extend_trajectory_for_end_yaw_control = dp_bool("extend_trajectory_for_end_yaw_control");

    // MPC 允许的位置误差
    m_mpc->m_admissible_position_error = dp_double("admissible_position_error");
    // MPC 允许的偏航误差（弧度）
    m_mpc->m_admissible_yaw_error_rad = dp_double("admissible_yaw_error_rad");
    // 是否使用转向预测
    m_mpc->m_use_steer_prediction = dp_bool("use_steer_prediction");
    // 车辆模型的转向时间常数
    m_mpc->m_param.steer_tau = dp_double("vehicle_model_steer_tau");

    // 停止状态参数
    // 进入停止状态的自车速度阈值
    m_stop_state_entry_ego_speed = dp_double("stop_state_entry_ego_speed");
    // 进入停止状态的目标速度阈值
    m_stop_state_entry_target_speed = dp_double("stop_state_entry_target_speed");
    // 收敛的转向角度阈值（弧度）
    m_converged_steer_rad = dp_double("converged_steer_rad");
    // 是否保持转向控制直到收敛
    m_keep_steer_control_until_converged = dp_bool("keep_steer_control_until_converged");
    // 新轨迹的持续时间（秒）
    m_new_traj_duration_time = dp_double("new_traj_duration_time");            
    // 新轨迹的结束距离（米）
    m_new_traj_end_dist = dp_double("new_traj_end_dist");                      
    // MPC 收敛阈值（弧度/秒）
    m_mpc_converged_threshold_rps = dp_double("mpc_converged_threshold_rps");  

    // MPC 参数
    // 获取车辆信息
    const auto vehicle_info = autoware::vehicle_info_utils::VehicleInfoUtils(node).getVehicleInfo();
    // 车辆的轴距
    const double wheelbase = vehicle_info.wheel_base_m;
    constexpr double deg2rad = static_cast<double>(M_PI) / 180.0;
    // 车辆的最大转向角限制
    m_mpc->m_steer_lim = vehicle_info.max_steer_angle_rad;

    // 根据曲率的转向速率限制
    const auto steer_rate_lim_dps_list_by_curvature =
        node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_curvature");
    const auto curvature_list_for_steer_rate_lim =
        node.declare_parameter<std::vector<double>>("curvature_list_for_steer_rate_lim");
    for (size_t i = 0; i < steer_rate_lim_dps_list_by_curvature.size(); ++i) {
        // 将曲率映射到转向速率限制
        m_mpc->m_steer_rate_lim_map_by_curvature.emplace_back(
            curvature_list_for_steer_rate_lim.at(i),
            steer_rate_lim_dps_list_by_curvature.at(i) * deg2rad);
    }

    // 根据速度的转向速率限制
    const auto steer_rate_lim_dps_list_by_velocity =
        node.declare_parameter<std::vector<double>>("steer_rate_lim_dps_list_by_velocity");
    const auto velocity_list_for_steer_rate_lim =
        node.declare_parameter<std::vector<double>>("velocity_list_for_steer_rate_lim");
    for (size_t i = 0; i < steer_rate_lim_dps_list_by_velocity.size(); ++i) {
        // 将速度映射到转向速率限制
        m_mpc->m_steer_rate_lim_map_by_velocity.emplace_back(
            velocity_list_for_steer_rate_lim.at(i), steer_rate_lim_dps_list_by_velocity.at(i) * deg2rad);
    }

    // 车辆模型设置
    // 根据轴距、转向限制、转向时间常数和节点创建车辆模型
    auto vehicle_model_ptr =
        createVehicleModel(wheelbase, m_mpc->m_steer_lim, m_mpc->m_param.steer_tau, node);
    // 将车辆模型设置到 MPC 对象中
    m_mpc->setVehicleModel(vehicle_model_ptr);

    // QP 求解器设置
    // 根据节点创建 QP 求解器接口
    auto qpsolver_ptr = createQPSolverInterface(node);
    // 将 QP 求解器设置到 MPC 对象中
    m_mpc->setQPSolver(qpsolver_ptr);

    // 延迟补偿
    {
        // 从参数中获取输入延迟
        const double delay_tmp = dp_double("input_delay");
        // 计算延迟步数，基于控制周期
        const double delay_step = std::round(delay_tmp / m_mpc->m_ctrl_period);
        // 将输入延迟调整为控制周期的倍数
        m_mpc->m_param.input_delay = delay_step * m_mpc->m_ctrl_period;
        // 初始化输入缓冲区，用于延迟补偿
        m_mpc->m_input_buffer = std::deque<double>(static_cast<size_t>(delay_step), 0.0);
    }

    // 转向偏移补偿
    // 是否启用自动转向偏移消除
    enable_auto_steering_offset_removal_ =
        dp_bool("steering_offset.enable_auto_steering_offset_removal");
    // 创建转向偏移估计器
    steering_offset_ = createSteerOffsetEstimator(wheelbase, node);

    // 初始化低通滤波器
    {
        // 转向低通滤波器的截止频率
        const double steering_lpf_cutoff_hz = dp_double("steering_lpf_cutoff_hz");
        // 误差导数低通滤波器的截止频率
        const double error_deriv_lpf_cutoff_hz = dp_double("error_deriv_lpf_cutoff_hz");
        m_mpc->initializeLowPassFilters(steering_lpf_cutoff_hz, error_deriv_lpf_cutoff_hz);
    }

    // 自车最近索引搜索
    const auto check_and_get_param = [&](const auto & param) {
        return node.has_parameter(param)? node.get_parameter(param).as_double() : dp_double(param);
    };
    // 自车最近距离的阈值
    m_ego_nearest_dist_threshold = check_and_get_param("ego_nearest_dist_threshold");
    // 自车最近偏航的阈值
    m_ego_nearest_yaw_threshold = check_and_get_param("ego_nearest_yaw_threshold");
    // 将阈值设置到 MPC 对象中
    m_mpc->ego_nearest_dist_threshold = m_ego_nearest_dist_threshold;
    m_mpc->ego_nearest_yaw_threshold = m_ego_nearest_yaw_threshold;

    // 是否使用延迟初始状态
    m_mpc->m_use_delayed_initial_state = dp_bool("use_delayed_initial_state");

    // 是否发布调试轨迹
    m_mpc->m_publish_debug_trajectories = dp_bool("publish_debug_trajectories");

    // 创建发布者用于发布预测轨迹、调试值和转向偏移
    m_pub_predicted_traj = node.create_publisher<Trajectory>("~/output/predicted_trajectory", 1);
    m_pub_debug_values =
        node.create_publisher<Float32MultiArrayStamped>("~/output/lateral_diagnostic", 1);
    m_pub_steer_offset = node.create_publisher<Float32Stamped>("~/output/estimated_steer_offset", 1);

    // 声明 MPC 参数
    declareMPCparameters(node);

    // 参数更新回调设置
    using std::placeholders::_1;
    m_set_param_res =
        node.add_on_set_parameters_callback(std::bind(&MpcLateralController::paramCallback, this, _1));

    // 初始化 MPC 的转向预测器
    m_mpc->initializeSteeringPredictor();

    // 为 MPC 设置日志记录器
    m_mpc->setLogger(logger_);
    // 为 MPC 设置时钟
    m_mpc->setClock(clock_);

    // 配置诊断
    setupDiag();
}

2.mpc_lateral_controller.cpp解释

1.使用MPC进行横向控制，构建了 m_mpc 指针，然后进行mpc的调用。
2.调用mpc_trajectory.cpp，管理目标路径
3.调用vehicle_model文件，管理车辆模型
4.调用qp_solver文件，构建求解器。

三、mpc.cpp

先不说注释的效果怎么样，deepseek至少能将一篇800多行的代码完整的给注释下来。

1.mpc.cpp代码注释

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/mpc.hpp"

#include "autoware/interpolation/linear_interpolation.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/mpc_lateral_controller/mpc_utils.hpp"
#include "autoware/universe_utils/math/unit_conversion.hpp"
#include "rclcpp/rclcpp.hpp"

#include <algorithm>
#include <limits>

namespace autoware::motion::control::mpc_lateral_controller
{
using autoware::universe_utils::calcDistance2d;
using autoware::universe_utils::normalizeRadian;
using autoware::universe_utils::rad2deg;

// MPC构造函数，初始化发布器
MPC::MPC(rclcpp::Node & node)
{
  // 创建用于发布预测轨迹的发布器
  m_debug_frenet_predicted_trajectory_pub = node.create_publisher<Trajectory>(
    "~/debug/predicted_trajectory_in_frenet_coordinate", rclcpp::QoS(1));
  // 创建用于发布重采样参考轨迹的发布器
  m_debug_resampled_reference_trajectory_pub =
    node.create_publisher<Trajectory>("~/debug/resampled_reference_trajectory", rclcpp::QoS(1));
}

// MPC核心计算函数
bool MPC::calculateMPC(
  const SteeringReport & current_steer, const Odometry & current_kinematics, Lateral & ctrl_cmd,
  Trajectory & predicted_trajectory, Float32MultiArrayStamped & diagnostic)
{
  // 由于参考轨迹没有考虑当前车辆的速度，因此需要根据纵向动力学计算轨迹速度
  const auto reference_trajectory =
    applyVelocityDynamicsFilter(m_reference_trajectory, current_kinematics);

  // 获取必要的数据
  const auto [success_data, mpc_data] =
    getData(reference_trajectory, current_steer, current_kinematics);
  if (!success_data) {
    return fail_warn_throttle("获取MPC数据失败，停止MPC。");
  }

  // 计算误差动力学的初始状态
  const auto x0 = getInitialState(mpc_data);

  // 对初始状态进行时间延迟补偿
  const auto [success_delay, x0_delayed] =
    updateStateForDelayCompensation(reference_trajectory, mpc_data.nearest_time, x0);
  if (!success_delay) {
    return fail_warn_throttle("延迟补偿失败，停止MPC。");
  }

  // 使用MPC采样时间重采样参考轨迹
  const double mpc_start_time = mpc_data.nearest_time + m_param.input_delay;
  const double prediction_dt =
    getPredictionDeltaTime(mpc_start_time, reference_trajectory, current_kinematics);

  const auto [success_resample, mpc_resampled_ref_trajectory] =
    resampleMPCTrajectoryByTime(mpc_start_time, prediction_dt, reference_trajectory);
  if (!success_resample) {
    return fail_warn_throttle("轨迹重采样失败，停止MPC。");
  }

  // 生成MPC矩阵：预测方程 Xec = Aex * x0 + Bex * Uex + Wex
  const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);

  // 求解优化问题
  const auto [success_opt, Uex] = executeOptimization(
    mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
    current_kinematics.twist.twist.linear.x);
  if (!success_opt) {
    return fail_warn_throttle("优化失败，停止MPC。");
  }

  // 对输入进行限幅和低通滤波
  const double u_saturated = std::clamp(Uex(0), -m_steer_lim, m_steer_lim);
  const double u_filtered = m_lpf_steering_cmd.filter(u_saturated);

  // 设置控制命令
  ctrl_cmd.steering_tire_angle = static_cast<float>(u_filtered);
  ctrl_cmd.steering_tire_rotation_rate = static_cast<float>(calcDesiredSteeringRate(
    mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));

  // 保存控制命令用于转向预测
  m_steering_predictor->storeSteerCmd(u_filtered);

  // 将输入保存到缓冲区以进行延迟补偿
  m_input_buffer.push_back(ctrl_cmd.steering_tire_angle);
  m_input_buffer.pop_front();

  // 保存前一次输入用于MPC速率限制
  m_raw_steer_cmd_pprev = m_raw_steer_cmd_prev;
  m_raw_steer_cmd_prev = Uex(0);

  /* 计算预测轨迹 */
  Eigen::VectorXd initial_state = m_use_delayed_initial_state ? x0_delayed : x0;
  predicted_trajectory = calculatePredictedTrajectory(
    mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "world");

  // 发布不同坐标系下的预测轨迹用于调试
  if (m_publish_debug_trajectories) {
    // 计算并发布Frenet坐标系下的预测轨迹
    auto predicted_trajectory_frenet = calculatePredictedTrajectory(
      mpc_matrix, initial_state, Uex, mpc_resampled_ref_trajectory, prediction_dt, "frenet");
    predicted_trajectory_frenet.header.stamp = m_clock->now();
    predicted_trajectory_frenet.header.frame_id = "map";
    m_debug_frenet_predicted_trajectory_pub->publish(predicted_trajectory_frenet);
  }

  // 准备诊断消息
  diagnostic =
    generateDiagData(reference_trajectory, mpc_data, mpc_matrix, ctrl_cmd, Uex, current_kinematics);

  return true;
}

// 生成诊断数据
Float32MultiArrayStamped MPC::generateDiagData(
  const MPCTrajectory & reference_trajectory, const MPCData & mpc_data,
  const MPCMatrix & mpc_matrix, const Lateral & ctrl_cmd, const VectorXd & Uex,
  const Odometry & current_kinematics) const
{
  Float32MultiArrayStamped diagnostic;

  // 准备诊断消息
  const double nearest_k = reference_trajectory.k.at(mpc_data.nearest_idx);
  const double nearest_smooth_k = reference_trajectory.smooth_k.at(mpc_data.nearest_idx);
  const double wb = m_vehicle_model_ptr->getWheelbase();
  const double current_velocity = current_kinematics.twist.twist.linear.x;
  const double wz_predicted = current_velocity * std::tan(mpc_data.predicted_steer) / wb;
  const double wz_measured = current_velocity * std::tan(mpc_data.steer) / wb;
  const double wz_command = current_velocity * std::tan(ctrl_cmd.steering_tire_angle) / wb;
  const int iteration_num = m_qpsolver_ptr->getTakenIter();
  const double runtime = m_qpsolver_ptr->getRunTime();
  const double objective_value = m_qpsolver_ptr->getObjVal();

  typedef decltype(diagnostic.data)::value_type DiagnosticValueType;
  const auto append_diag = [&](const auto & val) -> void {
    diagnostic.data.push_back(static_cast<DiagnosticValueType>(val));
  };
  append_diag(ctrl_cmd.steering_tire_angle);      // [0] 最终转向命令（MPC + 低通滤波）
  append_diag(Uex(0));                            // [1] MPC计算结果
  append_diag(mpc_matrix.Uref_ex(0));             // [2] 前馈转向值
  append_diag(std::atan(nearest_smooth_k * wb));  // [3] 原始前馈转向值
  append_diag(mpc_data.steer);                    // [4] 当前转向角
  append_diag(mpc_data.lateral_err);              // [5] 横向误差
  append_diag(tf2::getYaw(current_kinematics.pose.pose.orientation));  // [6] 当前姿态的偏航角
  append_diag(tf2::getYaw(mpc_data.nearest_pose.orientation));         // [7] 最近点的偏航角
  append_diag(mpc_data.yaw_err);                                       // [8] 偏航误差
  append_diag(reference_trajectory.vx.at(mpc_data.nearest_idx));       // [9] 参考速度
  append_diag(current_velocity);                                       // [10] 测量速度
  append_diag(wz_command);                           // [11] 转向命令产生的角速度
  append_diag(wz_measured);                          // [12] 测量转向产生的角速度
  append_diag(current_velocity * nearest_smooth_k);  // [13] 路径曲率产生的角速度
  append_diag(nearest_smooth_k);          // [14] 最近路径曲率（用于前馈）
  append_diag(nearest_k);                 // [15] 最近路径曲率（未平滑）
  append_diag(mpc_data.predicted_steer);  // [16] 预测转向
  append_diag(wz_predicted);              // [17] 预测转向产生的角速度
  append_diag(iteration_num);             // [18] 迭代次数
  append_diag(runtime);                   // [19] 最新问题的运行时间
  append_diag(objective_value);           // [20] 最新问题的目标值

  return diagnostic;
}

// 设置参考轨迹
void MPC::setReferenceTrajectory(
  const Trajectory & trajectory_msg, const TrajectoryFilteringParam & param,
  const Odometry & current_kinematics)
{
  const size_t nearest_seg_idx =
    autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
      trajectory_msg.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
      ego_nearest_yaw_threshold);
  const double ego_offset_to_segment = autoware::motion_utils::calcLongitudinalOffsetToSegment(
    trajectory_msg.points, nearest_seg_idx, current_kinematics.pose.pose.position);

  const auto mpc_traj_raw = MPCUtils::convertToMPCTrajectory(trajectory_msg);

  // 重采样
  const auto [success_resample, mpc_traj_resampled] = MPCUtils::resampleMPCTrajectoryByDistance(
    mpc_traj_raw, param.traj_resample_dist, nearest_seg_idx, ego_offset_to_segment);
  if (!success_resample) {
    warn_throttle("[setReferenceTrajectory] 按距离重采样时样条错误");
    return;
  }

  const auto is_forward_shift =
    autoware::motion_utils::isDrivingForward(mpc_traj_resampled.toTrajectoryPoints());

  // 如果驾驶方向未知，使用之前的值
  m_is_forward_shift = is_forward_shift ? is_forward_shift.value() : m_is_forward_shift;

  // 路径平滑
  MPCTrajectory mpc_traj_smoothed = mpc_traj_resampled;  // 平滑滤波后的轨迹
  const int mpc_traj_resampled_size = static_cast<int>(mpc_traj_resampled.size());
  if (
    param.enable_path_smoothing && mpc_traj_resampled_size > 2 * param.path_filter_moving_ave_num) {
    using MoveAverageFilter::filt_vector;
    if (
      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.x) ||
      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.y) ||
      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.yaw) ||
      !filt_vector(param.path_filter_moving_ave_num, mpc_traj_smoothed.vx)) {
      RCLCPP_DEBUG(m_logger, "路径回调：滤波错误，停止滤波。");
      mpc_traj_smoothed = mpc_traj_resampled;
    }
  }

  /*
   * 扩展终点
   * 注意：当前的MPC没有很好地考虑路径终点的姿态角。通过沿姿态方向扩展路径终点，MPC可以更好地考虑姿态角，从而提高控制性能。
   * 如果轨迹已经很好地考虑了终点姿态角，则不需要此功能。
   */
  if (param.extend_trajectory_for_end_yaw_control) {
    MPCUtils::extendTrajectoryInYawDirection(
      mpc_traj_raw.yaw.back(), param.traj_resample_dist, m_is_forward_shift, mpc_traj_smoothed);
  }

  // 计算偏航角
  MPCUtils::calcTrajectoryYawFromXY(mpc_traj_smoothed, m_is_forward_shift);
  MPCUtils::convertEulerAngleToMonotonic(mpc_traj_smoothed.yaw);

  // 计算曲率
  MPCUtils::calcTrajectoryCurvature(
    param.curvature_smoothing_num_traj, param.curvature_smoothing_num_ref_steer, mpc_traj_smoothed);

  // 在终点停止速度
  mpc_traj_smoothed.vx.back() = 0.0;

  // 在末尾添加一个额外的时间点，以使MPC稳定
  auto last_point = mpc_traj_smoothed.back();
  last_point.relative_time += 100.0;  // 额外时间以防止由于时间过短导致MPC计算失败
  last_point.vx = 0.0;                // 在终点停止速度
  mpc_traj_smoothed.push_back(last_point);

  if (!mpc_traj_smoothed.size()) {
    RCLCPP_DEBUG(m_logger, "路径回调：轨迹大小不符合预期。");
    return;
  }

  m_reference_trajectory = mpc_traj_smoothed;
}

// 重置前一次结果
void MPC::resetPrevResult(const SteeringReport & current_steer)
{
  // 考虑限制。前一次值超过限制会破坏优化约束并导致优化失败。
  const float steer_lim_f = static_cast<float>(m_steer_lim);
  m_raw_steer_cmd_prev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
  m_raw_steer_cmd_pprev = std::clamp(current_steer.steering_tire_angle, -steer_lim_f, steer_lim_f);
}

// 获取MPC数据
std::pair<bool, MPCData> MPC::getData(
  const MPCTrajectory & traj, const SteeringReport & current_steer,
  const Odometry & current_kinematics)
{
  const auto current_pose = current_kinematics.pose.pose;

  MPCData data;
  if (!MPCUtils::calcNearestPoseInterp(
        traj, current_pose, &(data.nearest_pose), &(data.nearest_idx), &(data.nearest_time),
        ego_nearest_dist_threshold, ego_nearest_yaw_threshold)) {
    warn_throttle("calculateMPC: 计算最近点时出错，停止MPC。");
    return {false, MPCData{}};
  }

  // 获取数据
  data.steer = static_cast<double>(current_steer.steering_tire_angle);
  data.lateral_err = MPCUtils::calcLateralError(current_pose, data.nearest_pose);
  data.yaw_err = normalizeRadian(
    tf2::getYaw(current_pose.orientation) - tf2::getYaw(data.nearest_pose.orientation));

  // 获取预测转向
  data.predicted_steer = m_steering_predictor->calcSteerPrediction();

  // 检查误差限制
  const double dist_err = calcDistance2d(current_pose, data.nearest_pose);
  if (dist_err > m_admissible_position_error) {
    warn_throttle("位置误差过大：%fm > %fm", dist_err, m_admissible_position_error);
    return {false, MPCData{}};
  }

  // 检查偏航误差限制
  if (std::fabs(data.yaw_err) > m_admissible_yaw_error_rad) {
    warn_throttle("偏航误差过大：%f > %f", data.yaw_err, m_admissible_yaw_error_rad);
    return {false, MPCData{}};
  }

  // 检查轨迹时间长度
  const double max_prediction_time =
    m_param.min_prediction_length / static_cast<double>(m_param.prediction_horizon - 1);
  auto end_time = data.nearest_time + m_param.input_delay + m_ctrl_period + max_prediction_time;
  if (end_time > traj.relative_time.back()) {
    warn_throttle("路径太短，无法进行预测。");
    return {false, MPCData{}};
  }
  return {true, data};
}

// 按时间重采样MPC轨迹
std::pair<bool, MPCTrajectory> MPC::resampleMPCTrajectoryByTime(
  const double ts, const double prediction_dt, const MPCTrajectory & input) const
{
  MPCTrajectory output;
  std::vector<double> mpc_time_v;
  for (double i = 0; i < static_cast<double>(m_param.prediction_horizon); ++i) {
    mpc_time_v.push_back(ts + i * prediction_dt);
  }
  if (!MPCUtils::linearInterpMPCTrajectory(input.relative_time, input, mpc_time_v, output)) {
    warn_throttle("calculateMPC: MPC重采样错误，停止MPC计算。检查代码！");
    return {false, {}};
  }
  // 发布重采样参考轨迹用于调试
  if (m_publish_debug_trajectories) {
    auto converted_output = MPCUtils::convertToAutowareTrajectory(output);
    converted_output.header.stamp = m_clock->now();
    converted_output.header.frame_id = "map";
    m_debug_resampled_reference_trajectory_pub->publish(converted_output);
  }
  return {true, output};
}

// 获取初始状态
VectorXd MPC::getInitialState(const MPCData & data)
{
  const int DIM_X = m_vehicle_model_ptr->getDimX();
  VectorXd x0 = VectorXd::Zero(DIM_X);

  const auto & lat_err = data.lateral_err;
  const auto & steer = m_use_steer_prediction ? data.predicted_steer : data.steer;
  const auto & yaw_err = data.yaw_err;

  const auto vehicle_model = m_vehicle_model_ptr->modelName();
  if (vehicle_model == "kinematics") {
    x0 << lat_err, yaw_err, steer;
  } else if (vehicle_model == "kinematics_no_delay") {
    x0 << lat_err, yaw_err;
  } else if (vehicle_model == "dynamics") {
    double dlat = (lat_err - m_lateral_error_prev) / m_ctrl_period;
    double dyaw = (yaw_err - m_yaw_error_prev) / m_ctrl_period;
    m_lateral_error_prev = lat_err;
    m_yaw_error_prev = yaw_err;
    dlat = m_lpf_lateral_error.filter(dlat);
    dyaw = m_lpf_yaw_error.filter(dyaw);
    x0 << lat_err, dlat, yaw_err, dyaw;
    RCLCPP_DEBUG(m_logger, "(低通滤波前) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
    RCLCPP_DEBUG(m_logger, "(低通滤波后) dot_lat_err = %f, dot_yaw_err = %f", dlat, dyaw);
  } else {
    RCLCPP_ERROR(m_logger, "vehicle_model_type 未定义");
  }
  return x0;
}

// 更新状态以进行延迟补偿
std::pair<bool, VectorXd> MPC::updateStateForDelayCompensation(
  const MPCTrajectory & traj, const double & start_time, const VectorXd & x0_orig)
{
  const int DIM_X = m_vehicle_model_ptr->getDimX();
  const int DIM_U = m_vehicle_model_ptr->getDimU();
  const int DIM_Y = m_vehicle_model_ptr->getDimY();

  MatrixXd Ad(DIM_X, DIM_X);
  MatrixXd Bd(DIM_X, DIM_U);
  MatrixXd Wd(DIM_X, 1);
  MatrixXd Cd(DIM_Y, DIM_X);

  const double sign_vx = m_is_forward_shift ? 1 : -1;

  MatrixXd x_curr = x0_orig;
  double mpc_curr_time = start_time;
  for (size_t i = 0; i < m_input_buffer.size(); ++i) {
    double k, v = 0.0;
    try {
      // 注意：当车辆倒车时，曲率的符号应反转。
      k = autoware::interpolation::lerp(traj.relative_time, traj.k, mpc_curr_time) * sign_vx;
      v = autoware::interpolation::lerp(traj.relative_time, traj.vx, mpc_curr_time);
    } catch (const std::exception & e) {
      RCLCPP_ERROR(m_logger, "延迟补偿时MPC重采样失败，停止MPC：%s", e.what());
      return {false, {}};
    }

    // 获取离散状态矩阵A, B, C, W
    m_vehicle_model_ptr->setVelocity(v);
    m_vehicle_model_ptr->setCurvature(k);
    m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, m_ctrl_period);
    MatrixXd ud = MatrixXd::Zero(DIM_U, 1);
    ud(0, 0) = m_input_buffer.at(i);  // 用于转向输入延迟
    x_curr = Ad * x_curr + Bd * ud + Wd;
    mpc_curr_time += m_ctrl_period;
  }
  return {true, x_curr};
}

// 应用速度动力学滤波器
MPCTrajectory MPC::applyVelocityDynamicsFilter(
  const MPCTrajectory & input, const Odometry & current_kinematics) const
{
  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
  if (autoware_traj.points.empty()) {
    return input;
  }

  const size_t nearest_seg_idx =
    autoware::motion_utils::findFirstNearestSegmentIndexWithSoftConstraints(
      autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
      ego_nearest_yaw_threshold);

  MPCTrajectory output = input;
  MPCUtils::dynamicSmoothingVelocity(
    nearest_seg_idx, current_kinematics.twist.twist.linear.x, m_param.acceleration_limit,
    m_param.velocity_time_constant, output);

  auto last_point = output.back();
  last_point.relative_time += 100.0;  // 额外时间以防止由于时间过短导致MPC计算失败
  last_point.vx = 0.0;                // 在终点停止速度
  output.push_back(last_point);
  return output;
}

/*
 * 预测方程: Xec = Aex * x0 + Bex * Uex + Wex
 * 代价函数: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
 * Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
 */
MPCMatrix MPC::generateMPCMatrix(
  const MPCTrajectory & reference_trajectory, const double prediction_dt)
{
  const int N = m_param.prediction_horizon;
  const double DT = prediction_dt;
  const int DIM_X = m_vehicle_model_ptr->getDimX();
  const int DIM_U = m_vehicle_model_ptr->getDimU();
  const int DIM_Y = m_vehicle_model_ptr->getDimY();

  MPCMatrix m;
  m.Aex = MatrixXd::Zero(DIM_X * N, DIM_X);
  m.Bex = MatrixXd::Zero(DIM_X * N, DIM_U * N);
  m.Wex = MatrixXd::Zero(DIM_X * N, 1);
  m.Cex = MatrixXd::Zero(DIM_Y * N, DIM_X * N);
  m.Qex = MatrixXd::Zero(DIM_Y * N, DIM_Y * N);
  m.R1ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
  m.R2ex = MatrixXd::Zero(DIM_U * N, DIM_U * N);
  m.Uref_ex = MatrixXd::Zero(DIM_U * N, 1);

  // 权重矩阵取决于车辆模型
  MatrixXd Q = MatrixXd::Zero(DIM_Y, DIM_Y);
  MatrixXd R = MatrixXd::Zero(DIM_U, DIM_U);
  MatrixXd Q_adaptive = MatrixXd::Zero(DIM_Y, DIM_Y);
  MatrixXd R_adaptive = MatrixXd::Zero(DIM_U, DIM_U);

  MatrixXd Ad(DIM_X, DIM_X);
  MatrixXd Bd(DIM_X, DIM_U);
  MatrixXd Wd(DIM_X, 1);
  MatrixXd Cd(DIM_Y, DIM_X);
  MatrixXd Uref(DIM_U, 1);

  const double sign_vx = m_is_forward_shift ? 1 : -1;

  // 预测N次动力学
  for (int i = 0; i < N; ++i) {
    const double ref_vx = reference_trajectory.vx.at(i);
    const double ref_vx_squared = ref_vx * ref_vx;

    // 注意：当车辆倒车时，曲率的符号应反转。
    const double ref_k = reference_trajectory.k.at(i) * sign_vx;
    const double ref_smooth_k = reference_trajectory.smooth_k.at(i) * sign_vx;

    // 获取离散状态矩阵A, B, C, W
    m_vehicle_model_ptr->setVelocity(ref_vx);
    m_vehicle_model_ptr->setCurvature(ref_k);
    m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);

    Q = MatrixXd::Zero(DIM_Y, DIM_Y);
    R = MatrixXd::Zero(DIM_U, DIM_U);
    const auto mpc_weight = getWeight(ref_k);
    Q(0, 0) = mpc_weight.lat_error;
    Q(1, 1) = mpc_weight.heading_error;
    R(0, 0) = mpc_weight.steering_input;

    Q_adaptive = Q;
    R_adaptive = R;
    if (i == N - 1) {
      Q_adaptive(0, 0) = m_param.nominal_weight.terminal_lat_error;
      Q_adaptive(1, 1) = m_param.nominal_weight.terminal_heading_error;
    }
    Q_adaptive(1, 1) += ref_vx_squared * mpc_weight.heading_error_squared_vel;
    R_adaptive(0, 0) += ref_vx_squared * mpc_weight.steering_input_squared_vel;

    // 更新MPC矩阵
    int idx_x_i = i * DIM_X;
    int idx_x_i_prev = (i - 1) * DIM_X;
    int idx_u_i = i * DIM_U;
    int idx_y_i = i * DIM_Y;
    if (i == 0) {
      m.Aex.block(0, 0, DIM_X, DIM_X) = Ad;
      m.Bex.block(0, 0, DIM_X, DIM_U) = Bd;
      m.Wex.block(0, 0, DIM_X, 1) = Wd;
    } else {
      m.Aex.block(idx_x_i, 0, DIM_X, DIM_X) = Ad * m.Aex.block(idx_x_i_prev, 0, DIM_X, DIM_X);
      for (int j = 0; j < i; ++j) {
        int idx_u_j = j * DIM_U;
        m.Bex.block(idx_x_i, idx_u_j, DIM_X, DIM_U) =
          Ad * m.Bex.block(idx_x_i_prev, idx_u_j, DIM_X, DIM_U);
      }
      m.Wex.block(idx_x_i, 0, DIM_X, 1) = Ad * m.Wex.block(idx_x_i_prev, 0, DIM_X, 1) + Wd;
    }
    m.Bex.block(idx_x_i, idx_u_i, DIM_X, DIM_U) = Bd;
    m.Cex.block(idx_y_i, idx_x_i, DIM_Y, DIM_X) = Cd;
    m.Qex.block(idx_y_i, idx_y_i, DIM_Y, DIM_Y) = Q_adaptive;
    m.R1ex.block(idx_u_i, idx_u_i, DIM_U, DIM_U) = R_adaptive;

    // 获取参考输入（前馈）
    m_vehicle_model_ptr->setCurvature(ref_smooth_k);
    m_vehicle_model_ptr->calculateReferenceInput(Uref);
    if (std::fabs(Uref(0, 0)) < autoware::universe_utils::deg2rad(m_param.zero_ff_steer_deg)) {
      Uref(0, 0) = 0.0;  // 忽略曲率噪声
    }
    m.Uref_ex.block(i * DIM_U, 0, DIM_U, 1) = Uref;
  }

  // 添加横向加加速度：权重为 (v * {u(i) - u(i-1)} )^2
  for (int i = 0; i < N - 1; ++i) {
    const double ref_vx = reference_trajectory.vx.at(i);
    const double ref_k = reference_trajectory.k.at(i) * sign_vx;
    const double j = ref_vx * ref_vx * getWeight(ref_k).lat_jerk / (DT * DT);
    const Eigen::Matrix2d J = (Eigen::Matrix2d() << j, -j, -j, j).finished();
    m.R2ex.block(i, i, 2, 2) += J;
  }

  addSteerWeightR(prediction_dt, m.R1ex);

  return m;
}

/*
 * 求解二次优化问题。
 * 代价函数: J = Xex' * Qex * Xex + (Uex - Uref)' * R1ex * (Uex - Uref_ex) + Uex' * R2ex * Uex
 *                , Qex = diag([Q,Q,...]), R1ex = diag([R,R,...])
 * 约束矩阵 : lb < U < ub, lbA < A*U < ubA
 * 当前考虑的约束
 *  - 转向限制
 *  - 转向速率限制
 *
 * (1)lb < u < ub && (2)lbA < Au < ubA --> (3)[lb, lbA] < [I, A]u < [ub, ubA]
 * (1)lb < u < ub ...
 * [-u_lim] < [ u0 ] < [u_lim]
 * [-u_lim] < [ u1 ] < [u_lim]
 *              ~~~
 * [-u_lim] < [ uN ] < [u_lim] (*N... DIM_U)
 * (2)lbA < Au < ubA ...
 * [prev_u0 - au_lim*ctp] < [   u0  ] < [prev_u0 + au_lim*ctp] (*ctp ... ctrl_period)
 * [    -au_lim * dt    ] < [u1 - u0] < [     au_lim * dt    ]
 * [    -au_lim * dt    ] < [u2 - u1] < [     au_lim * dt    ]
 *                            ~~~
 * [    -au_lim * dt    ] < [uN-uN-1] < [     au_lim * dt    ] (*N... DIM_U)
 */
std::pair<bool, VectorXd> MPC::executeOptimization(
  const MPCMatrix & m, const VectorXd & x0, const double prediction_dt, const MPCTrajectory & traj,
  const double current_velocity)
{
  VectorXd Uex;

  if (!isValid(m)) {
    warn_throttle("模型矩阵无效，停止MPC。");
    return {false, {}};
  }

  const int DIM_U_N = m_param.prediction_horizon * m_vehicle_model_ptr->getDimU();

  // 代价函数: 1/2 * Uex' * H * Uex + f' * Uex,  H = B' * C' * Q * C * B + R
  const MatrixXd CB = m.Cex * m.Bex;
  const MatrixXd QCB = m.Qex * CB;
  // MatrixXd H = CB.transpose() * QCB + m.R1ex + m.R2ex; // 此计算较耗时，寻找更好的方法。  //NOLINT
  MatrixXd H = MatrixXd::Zero(DIM_U_N, DIM_U_N);
  H.triangularView<Eigen::Upper>() = CB.transpose() * QCB;
  H.triangularView<Eigen::Upper>() += m.R1ex + m.R2ex;
  H.triangularView<Eigen::Lower>() = H.transpose();
  MatrixXd f = (m.Cex * (m.Aex * x0 + m.Wex)).transpose() * QCB - m.Uref_ex.transpose() * m.R1ex;
  addSteerWeightF(prediction_dt, f);

  MatrixXd A = MatrixXd::Identity(DIM_U_N, DIM_U_N);
  for (int i = 1; i < DIM_U_N; i++) {
    A(i, i - 1) = -1.0;
  }

  // 转向角限制
  VectorXd lb = VectorXd::Constant(DIM_U_N, -m_steer_lim);  // 最小转向角
  VectorXd ub = VectorXd::Constant(DIM_U_N, m_steer_lim);   // 最大转向角

  // 转向角速率限制
  VectorXd steer_rate_limits = calcSteerRateLimitOnTrajectory(traj, current_velocity);
  VectorXd ubA = steer_rate_limits * prediction_dt;
  VectorXd lbA = -steer_rate_limits * prediction_dt;
  ubA(0) = m_raw_steer_cmd_prev + steer_rate_limits(0) * m_ctrl_period;
  lbA(0) = m_raw_steer_cmd_prev - steer_rate_limits(0) * m_ctrl_period;

  auto t_start = std::chrono::system_clock::now();
  bool solve_result = m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex);
  auto t_end = std::chrono::system_clock::now();
  if (!solve_result) {
    warn_throttle("QP求解器错误");
    return {false, {}};
  }

  {
    auto t = std::chrono::duration_cast<std::chrono::milliseconds>(t_end - t_start).count();
    RCLCPP_DEBUG(m_logger, "QP求解器计算时间 = %ld [ms]", t);
  }

  if (Uex.array().isNaN().any()) {
    warn_throttle("模型Uex包含NaN，停止MPC。");
    return {false, {}};
  }
  return {true, Uex};
}

// 添加转向权重R
void MPC::addSteerWeightR(const double prediction_dt, MatrixXd & R) const
{
  const int N = m_param.prediction_horizon;
  const double DT = prediction_dt;

  // 添加转向速率：权重为 (u(i) - u(i-1) / dt )^2
  {
    const double steer_rate_r = m_param.nominal_weight.steer_rate / (DT * DT);
    const Eigen::Matrix2d D = steer_rate_r * (Eigen::Matrix2d() << 1.0, -1.0, -1.0, 1.0).finished();
    for (int i = 0; i < N - 1; ++i) {
      R.block(i, i, 2, 2) += D;
    }
    if (N > 1) {
      // 转向速率 i = 0
      R(0, 0) += m_param.nominal_weight.steer_rate / (m_ctrl_period * m_ctrl_period);
    }
  }

  // 添加转向加速度：权重为 { (u(i+1) - 2*u(i) + u(i-1)) / dt^2 }^2
  {
    const double w = m_param.nominal_weight.steer_acc;
    const double steer_acc_r = w / std::pow(DT, 4);
    const double steer_acc_r_cp1 = w / (std::pow(DT, 3) * m_ctrl_period);
    const double steer_acc_r_cp2 = w / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2));
    const double steer_acc_r_cp4 = w / std::pow(m_ctrl_period, 4);
    const Eigen::Matrix3d D =
      steer_acc_r *
      (Eigen::Matrix3d() << 1.0, -2.0, 1.0, -2.0, 4.0, -2.0, 1.0, -2.0, 1.0).finished();
    for (int i = 1; i < N - 1; ++i) {
      R.block(i - 1, i - 1, 3, 3) += D;
    }
    if (N > 1) {
      // 转向加速度 i = 1
      R(0, 0) += steer_acc_r * 1.0 + steer_acc_r_cp2 * 1.0 + steer_acc_r_cp1 * 2.0;
      R(1, 0) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
      R(0, 1) += steer_acc_r * -1.0 + steer_acc_r_cp1 * -1.0;
      R(1, 1) += steer_acc_r * 1.0;
      // 转向加速度 i = 0
      R(0, 0) += steer_acc_r_cp4 * 1.0;
    }
  }
}

// 添加转向权重F
void MPC::addSteerWeightF(const double prediction_dt, MatrixXd & f) const
{
  if (f.rows() < 2) {
    return;
  }

  const double DT = prediction_dt;

  // 转向速率 i = 0
  f(0, 0) += -2.0 * m_param.nominal_weight.steer_rate / (std::pow(DT, 2)) * 0.5;

  // const double steer_acc_r = m_param.weight_steer_acc / std::pow(DT, 4);
  const double steer_acc_r_cp1 =
    m_param.nominal_weight.steer_acc / (std::pow(DT, 3) * m_ctrl_period);
  const double steer_acc_r_cp2 =
    m_param.nominal_weight.steer_acc / (std::pow(DT, 2) * std::pow(m_ctrl_period, 2));
  const double steer_acc_r_cp4 = m_param.nominal_weight.steer_acc / std::pow(m_ctrl_period, 4);

  // 转向加速度 i = 0
  f(0, 0) += ((-2.0 * m_raw_steer_cmd_prev + m_raw_steer_cmd_pprev) * steer_acc_r_cp4) * 0.5;

  // 转向加速度 i = 1
  f(0, 0) += (-2.0 * m_raw_steer_cmd_prev * (steer_acc_r_cp1 + steer_acc_r_cp2)) * 0.5;
  f(0, 1) += (2.0 * m_raw_steer_cmd_prev * steer_acc_r_cp1) * 0.5;
}

// 获取预测时间步长
double MPC::getPredictionDeltaTime(
  const double start_time, const MPCTrajectory & input, const Odometry & current_kinematics) const
{
  // 计算从当前姿态向前min_prediction_length的时间
  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(input);
  const size_t nearest_idx = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
    autoware_traj.points, current_kinematics.pose.pose, ego_nearest_dist_threshold,
    ego_nearest_yaw_threshold);
  double sum_dist = 0;
  const double target_time = [&]() {
    const double t_ext = 100.0;  // 额外时间以防止由于时间过短导致MPC计算失败
    for (size_t i = nearest_idx + 1; i < input.relative_time.size(); i++) {
      const double segment_dist = MPCUtils::calcDistance2d(input, i, i - 1);
      sum_dist += segment_dist;
      if (m_param.min_prediction_length < sum_dist) {
        const double prev_sum_dist = sum_dist - segment_dist;
        const double ratio = (m_param.min_prediction_length - prev_sum_dist) / segment_dist;
        const double relative_time_at_i = i == input.relative_time.size() - 1
                                            ? input.relative_time.at(i) - t_ext
                                            : input.relative_time.at(i);
        return input.relative_time.at(i - 1) +
               (relative_time_at_i - input.relative_time.at(i - 1)) * ratio;
      }
    }
    return input.relative_time.back() - t_ext;
  }();

  // 计算min_prediction_length的时间步长
  const double dt =
    (target_time - start_time) / static_cast<double>(m_param.prediction_horizon - 1);

  return std::max(dt, m_param.prediction_dt);
}

// 计算期望转向速率
double MPC::calcDesiredSteeringRate(
  const MPCMatrix & mpc_matrix, const MatrixXd & x0, const MatrixXd & Uex, const double u_filtered,
  const float current_steer, const double predict_dt) const
{
  if (m_vehicle_model_ptr->modelName() != "kinematics") {
    // 尚未支持，使用旧实现。
    return (u_filtered - current_steer) / predict_dt;
  }

  // 计算预测状态以获取转向运动
  const auto & m = mpc_matrix;
  const MatrixXd Xex = m.Aex * x0 + m.Bex * Uex + m.Wex;

  const size_t STEER_IDX = 2;  // 对于运动学模型

  const auto steer_0 = x0(STEER_IDX, 0);
  const auto steer_1 = Xex(STEER_IDX, 0);

  const auto steer_rate = (steer_1 - steer_0) / predict_dt;

  return steer_rate;
}

// 计算轨迹上的转向速率限制
VectorXd MPC::calcSteerRateLimitOnTrajectory(
  const MPCTrajectory & trajectory, const double current_velocity) const
{
  const auto interp = [&](const auto & steer_rate_limit_map, const auto & current) {
    std::vector<double> reference, limits;
    for (const auto & p : steer_rate_limit_map) {
      reference.push_back(p.first);
      limits.push_back(p.second);
    }

    // 如果速度超出参考范围，应用零阶保持。
    if (current <= reference.front()) {
      return limits.front();
    }
    if (current >= reference.back()) {
      return limits.back();
    }

    // 应用线性插值
    for (size_t i = 0; i < reference.size() - 1; ++i) {
      if (reference.at(i) <= current && current <= reference.at(i + 1)) {
        auto ratio =
          (current - reference.at(i)) / std::max(reference.at(i + 1) - reference.at(i), 1.0e-5);
        ratio = std::clamp(ratio, 0.0, 1.0);
        const auto interp = limits.at(i) + ratio * (limits.at(i + 1) - limits.at(i));
        return interp;
      }
    }

    std::cerr << "MPC::calcSteerRateLimitOnTrajectory() 插值逻辑错误。命令滤波器未工作。请检查代码。"
              << std::endl;
    return reference.back();
  };

  // 当车辆停止时，使用较大的转向速率限制以进行干转向。
  constexpr double steer_rate_lim = 100.0;
  const bool is_vehicle_stopped = std::fabs(current_velocity) < 0.01;
  if (is_vehicle_stopped) {
    return steer_rate_lim * VectorXd::Ones(m_param.prediction_horizon);
  }

  // 计算转向速率限制
  VectorXd steer_rate_limits = VectorXd::Zero(m_param.prediction_horizon);
  for (int i = 0; i < m_param.prediction_horizon; ++i) {
    const auto limit_by_curvature = interp(m_steer_rate_lim_map_by_curvature, trajectory.k.at(i));
    const auto limit_by_velocity = interp(m_steer_rate_lim_map_by_velocity, trajectory.vx.at(i));
    steer_rate_limits(i) = std::min(limit_by_curvature, limit_by_velocity);
  }

  return steer_rate_limits;
}

// 计算预测轨迹
Trajectory MPC::calculatePredictedTrajectory(
  const MPCMatrix & mpc_matrix, const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, const double dt, const std::string & coordinate) const
{
  MPCTrajectory predicted_mpc_trajectory;

  if (coordinate == "world") {
    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInWorldCoordinate(
      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
      dt);
  } else if (coordinate == "frenet") {
    predicted_mpc_trajectory = m_vehicle_model_ptr->calculatePredictedTrajectoryInFrenetCoordinate(
      mpc_matrix.Aex, mpc_matrix.Bex, mpc_matrix.Cex, mpc_matrix.Wex, x0, Uex, reference_trajectory,
      dt);
  } else {
    throw std::invalid_argument("指定的坐标系无效。使用 'world' 或 'frenet'。");
  }

  // 不要超过参考轨迹
  const auto predicted_length = MPCUtils::calcMPCTrajectoryArcLength(reference_trajectory);
  const auto clipped_trajectory =
    MPCUtils::clipTrajectoryByLength(predicted_mpc_trajectory, predicted_length);

  const auto predicted_trajectory = MPCUtils::convertToAutowareTrajectory(clipped_trajectory);

  return predicted_trajectory;
}

// 检查MPC矩阵是否有效
bool MPC::isValid(const MPCMatrix & m) const
{
  if (
    m.Aex.array().isNaN().any() || m.Bex.array().isNaN().any() || m.Cex.array().isNaN().any() ||
    m.Wex.array().isNaN().any() || m.Qex.array().isNaN().any() || m.R1ex.array().isNaN().any() ||
    m.R2ex.array().isNaN().any() || m.Uref_ex.array().isNaN().any()) {
    return false;
  }

  if (
    m.Aex.array().isInf().any() || m.Bex.array().isInf().any() || m.Cex.array().isInf().any() ||
    m.Wex.array().isInf().any() || m.Qex.array().isInf().any() || m.R1ex.array().isInf().any() ||
    m.R2ex.array().isInf().any() || m.Uref_ex.array().isInf().any()) {
    return false;
  }

  return true;
}
}  // namespace autoware::motion::control::mpc_lateral_controller

deepseek还多了一段整理，我觉得不错，直接复制在下面：

主要功能模块的中文注释：

    MPC构造函数：初始化发布器，用于发布调试信息。

    calculateMPC：MPC的核心计算函数，负责计算控制命令和预测轨迹。

    generateDiagData：生成诊断数据，用于调试和监控。

    setReferenceTrajectory：设置参考轨迹，并进行重采样和平滑处理。

    resetPrevResult：重置前一次的结果，用于初始化或重置状态。

    getData：获取MPC计算所需的数据，包括当前状态和参考轨迹。

    resampleMPCTrajectoryByTime：按时间重采样MPC轨迹。

    getInitialState：获取误差动力学的初始状态。

    updateStateForDelayCompensation：更新状态以进行延迟补偿。

    applyVelocityDynamicsFilter：应用速度动力学滤波器。

    generateMPCMatrix：生成MPC矩阵，用于预测和控制。

    executeOptimization：执行二次优化，求解最优控制输入。

    addSteerWeightR：添加转向权重到R矩阵。

    addSteerWeightF：添加转向权重到f向量。

    getPredictionDeltaTime：获取预测时间步长。

    calcDesiredSteeringRate：计算期望的转向速率。

    calcSteerRateLimitOnTrajectory：计算轨迹上的转向速率限制。

    calculatePredictedTrajectory：计算预测轨迹。

    isValid：检查MPC矩阵是否有效。

四、vehicle_model

mpc算法中，车辆模型是很重要的一点，它决定了预测控制的精度和真实性。
在vehicle_model文件夹中，有四个cpp文件：
vehicle_model_bicycle_dynamics.cpp：带有动力学的自行车模型（Bicycle Dynamics Model）
vehicle_model_bicycle_kinematics.cpp：自行车运动学模型（Bicycle Kinematics Model）
vehicle_model_bicycle_kinematics_no_delay.cpp：无延迟的自行车运动学模型（Bicycle Kinematics No Delay Model），忽略了转向系统的延迟。
vehicle_model_interface.cpp：定义了车辆模型的通用接口，所有模型都继承自该类。

自行车模型可以看我之前写的博客。

1.vehicle_model_bicycle_dynamics.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_dynamics.hpp"

#include <algorithm>

namespace autoware::motion::control::mpc_lateral_controller
{
// 构造函数，初始化动力学自行车模型的参数
DynamicsBicycleModel::DynamicsBicycleModel(
  const double wheelbase, const double mass_fl, const double mass_fr, const double mass_rl,
  const double mass_rr, const double cf, const double cr)
: VehicleModelInterface(/* dim_x */ 4, /* dim_u */ 1, /* dim_y */ 2, wheelbase)  // 状态维度4，输入维度1，输出维度2
{
  // 计算前后轴质量
  const double mass_front = mass_fl + mass_fr;
  const double mass_rear = mass_rl + mass_rr;

  // 计算总质量
  m_mass = mass_front + mass_rear;

  // 计算前后轴到质心的距离
  m_lf = m_wheelbase * (1.0 - mass_front / m_mass);  // 前轴到质心的距离
  m_lr = m_wheelbase * (1.0 - mass_rear / m_mass);   // 后轴到质心的距离

  // 计算绕Z轴的转动惯量
  m_iz = m_lf * m_lf * mass_front + m_lr * m_lr * mass_rear;

  // 设置前后轮的侧偏刚度
  m_cf = cf;  // 前轮侧偏刚度
  m_cr = cr;  // 后轮侧偏刚度
}

// 计算离散状态空间矩阵
void DynamicsBicycleModel::calculateDiscreteMatrix(
  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
  const double dt)
{
  /*
   * 离散状态空间方程：
   * x[k+1] = a_d * x[k] + b_d * u + w_d
   */

  // 确保速度不为零，避免除以零错误
  const double vel = std::max(m_velocity, 0.01);

  // 初始化连续时间状态矩阵 A
  a_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_x);
  a_d(0, 1) = 1.0;  // 横向速度
  a_d(1, 1) = -(m_cf + m_cr) / (m_mass * vel);  // 横向加速度
  a_d(1, 2) = (m_cf + m_cr) / m_mass;  // 横向加速度对偏航角的影响
  a_d(1, 3) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel);  // 横向加速度对偏航角速度的影响
  a_d(2, 3) = 1.0;  // 偏航角速度
  a_d(3, 1) = (m_lr * m_cr - m_lf * m_cf) / (m_iz * vel);  // 偏航角加速度对横向速度的影响
  a_d(3, 2) = (m_lf * m_cf - m_lr * m_cr) / m_iz;  // 偏航角加速度对偏航角的影响
  a_d(3, 3) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel);  // 偏航角加速度对偏航角速度的影响

  // 使用双线性离散化方法计算离散状态矩阵
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
  Eigen::MatrixXd a_d_inverse = (I - dt * 0.5 * a_d).inverse();
  a_d = a_d_inverse * (I + dt * 0.5 * a_d);  // 双线性离散化

  // 初始化输入矩阵 B
  b_d = Eigen::MatrixXd::Zero(m_dim_x, m_dim_u);
  b_d(0, 0) = 0.0;  // 横向位置不受输入影响
  b_d(1, 0) = m_cf / m_mass;  // 输入对横向加速度的影响
  b_d(2, 0) = 0.0;  // 偏航角不受输入影响
  b_d(3, 0) = m_lf * m_cf / m_iz;  // 输入对偏航角加速度的影响

  // 初始化扰动矩阵 W
  w_d = Eigen::MatrixXd::Zero(m_dim_x, 1);
  w_d(0, 0) = 0.0;  // 横向位置无扰动
  w_d(1, 0) = (m_lr * m_cr - m_lf * m_cf) / (m_mass * vel) - vel;  // 横向加速度扰动
  w_d(2, 0) = 0.0;  // 偏航角无扰动
  w_d(3, 0) = -(m_lf * m_lf * m_cf + m_lr * m_lr * m_cr) / (m_iz * vel);  // 偏航角加速度扰动

  // 离散化输入矩阵 B 和扰动矩阵 W
  b_d = (a_d_inverse * dt) * b_d;
  w_d = (a_d_inverse * dt * m_curvature * vel) * w_d;

  // 初始化输出矩阵 C
  c_d = Eigen::MatrixXd::Zero(m_dim_y, m_dim_x);
  c_d(0, 0) = 1.0;  // 输出横向位置
  c_d(1, 2) = 1.0;  // 输出偏航角
}

// 计算参考输入（前馈控制）
void DynamicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
{
  const double vel = std::max(m_velocity, 0.01);  // 确保速度不为零
  const double Kv =
    m_lr * m_mass / (2 * m_cf * m_wheelbase) - m_lf * m_mass / (2 * m_cr * m_wheelbase);  // 转向增益
  u_ref(0, 0) = m_wheelbase * m_curvature + Kv * vel * vel * m_curvature;  // 参考转向角
}

// 在世界坐标系中计算预测轨迹（未实现）
MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate(
  const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const
{
  // 输出错误信息：动力学模型不支持在世界坐标系中计算预测轨迹
  RCLCPP_ERROR(
    rclcpp::get_logger("control.trajectory_follower.lateral_controller"),
    "Predicted trajectory calculation in world coordinate is not supported in dynamic model. "
    "Calculate in the Frenet coordinate instead.");
  return calculatePredictedTrajectoryInFrenetCoordinate(
    a_d, b_d, c_d, w_d, x0, Uex, reference_trajectory, dt);
}

// 在 Frenet 坐标系中计算预测轨迹
MPCTrajectory DynamicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate(
  const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const
{
  // 状态变量：x = [e, de, th, dth]
  // e      : 横向误差
  // de     : 横向误差的导数
  // th     : 偏航角误差
  // dth    : 偏航角误差的导数
  // steer  : 转向角（输入）

  // 计算预测状态
  Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d;
  MPCTrajectory mpc_predicted_trajectory;
  const auto DIM_X = getDimX();
  const auto & t = reference_trajectory;

  // 遍历参考轨迹，计算预测轨迹
  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
    const auto lateral_error = Xex(i * DIM_X);  // 横向误差
    const auto yaw_error = Xex(i * DIM_X + 2);  // 偏航角误差
    const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error;  // 世界坐标系中的 x 坐标
    const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error;  // 世界坐标系中的 y 坐标
    const auto yaw = t.yaw.at(i) + yaw_error;  // 世界坐标系中的偏航角
    mpc_predicted_trajectory.push_back(
      x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i));
  }
  return mpc_predicted_trajectory;
}
}  // namespace autoware::motion::control::mpc_lateral_controller

2.vehicle_model_bicycle_kinematics.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics.hpp"

#include <cmath>

namespace autoware::motion::control::mpc_lateral_controller
{
// 构造函数，初始化自行车运动学模型
KinematicsBicycleModel::KinematicsBicycleModel(
  const double wheelbase, const double steer_lim, const double steer_tau)
: VehicleModelInterface(/* dim_x */ 3, /* dim_u */ 1, /* dim_y */ 2, wheelbase)
{
  m_steer_lim = steer_lim;  // 设置转向角限制
  m_steer_tau = steer_tau;  // 设置转向时间常数
}

// 计算离散状态空间矩阵
void KinematicsBicycleModel::calculateDiscreteMatrix(
  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
  const double dt)
{
  auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };  // 符号函数

  /* 在参考转向角 delta_r 附近线性化 */
  double delta_r = atan(m_wheelbase * m_curvature);  // 计算参考转向角
  if (std::abs(delta_r) >= m_steer_lim) {
    delta_r = m_steer_lim * static_cast<double>(sign(delta_r));  // 限制转向角在范围内
  }
  double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));  // 计算 cos^2(delta_r) 的倒数
  double velocity = m_velocity;  // 获取当前速度
  if (std::abs(m_velocity) < 1e-04) {
    velocity = 1e-04 * (m_velocity >= 0 ? 1 : -1);  // 避免速度为零
  }

  // 离散状态矩阵 A_d
  a_d << 0.0, velocity, 0.0, 0.0, 0.0, velocity / m_wheelbase * cos_delta_r_squared_inv, 0.0, 0.0,
    -1.0 / m_steer_tau;

  // 离散输入矩阵 B_d
  b_d << 0.0, 0.0, 1.0 / m_steer_tau;

  // 输出矩阵 C_d
  c_d << 1.0, 0.0, 0.0, 0.0, 1.0, 0.0;

  // 扰动矩阵 W_d
  w_d << 0.0,
    -velocity * m_curvature +
      velocity / m_wheelbase * (tan(delta_r) - delta_r * cos_delta_r_squared_inv),
    0.0;

  // 使用双线性变换对连续系统进行离散化
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);  // 单位矩阵
  const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse();  // 计算 (I - dt/2 * A_d)^-1
  a_d = i_dt2a_inv * (I + dt * 0.5 * a_d);  // 更新 A_d
  b_d = i_dt2a_inv * b_d * dt;  // 更新 B_d
  w_d = i_dt2a_inv * w_d * dt;  // 更新 W_d
}

// 计算参考输入（转向角）
void KinematicsBicycleModel::calculateReferenceInput(Eigen::MatrixXd & u_ref)
{
  u_ref(0, 0) = std::atan(m_wheelbase * m_curvature);  // 根据曲率计算参考转向角
}

// 在世界坐标系中计算预测轨迹
MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInWorldCoordinate(
  [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, const double dt) const
{
  // 在世界坐标系中计算预测轨迹，因为 Frenet 坐标系中存在建模误差
  // 相对坐标系 x = [横向误差, 偏航误差, 转向角]
  // 世界坐标系 x = [x, y, 偏航角, 转向角]

  const auto & t = reference_trajectory;

  // 在世界坐标系中创建初始状态
  Eigen::VectorXd state_w = [&]() {
    Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
    const auto lateral_error_0 = x0(0);  // 初始横向误差
    const auto yaw_error_0 = x0(1);  // 初始偏航误差
    state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // 世界坐标系 x
    state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // 世界坐标系 y
    state(2, 0) = t.yaw.at(0) + yaw_error_0;  // 世界坐标系偏航角
    return state;
  }();

  // 更新世界坐标系中的状态
  const auto updateState = [&](
                             const Eigen::VectorXd & state_w, const double & input, const double dt,
                             const double velocity) {
    const auto yaw = state_w(2);  // 当前偏航角

    Eigen::VectorXd dstate = Eigen::VectorXd::Zero(4);
    dstate(0) = velocity * std::cos(yaw);  // x 方向速度
    dstate(1) = velocity * std::sin(yaw);  // y 方向速度
    dstate(2) = velocity * std::tan(input) / m_wheelbase;  // 偏航角变化率

    // 注意：不要直接返回 "state_w + dstate * dt"，因为 Eigen 的惰性求值会导致问题
    const Eigen::VectorXd next_state = state_w + dstate * dt;  // 计算下一时刻状态
    return next_state;
  };

  MPCTrajectory mpc_predicted_trajectory;  // 预测轨迹
  const auto DIM_U = getDimU();  // 输入维度

  // 遍历参考轨迹，更新状态并生成预测轨迹
  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
    state_w = updateState(state_w, Uex(i * DIM_U, 0), dt, t.vx.at(i));  // 更新状态
    mpc_predicted_trajectory.push_back(
      state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i),
      t.relative_time.at(i));  // 将状态添加到预测轨迹中
  }
  return mpc_predicted_trajectory;
}

// 在 Frenet 坐标系中计算预测轨迹
MPCTrajectory KinematicsBicycleModel::calculatePredictedTrajectoryInFrenetCoordinate(
  const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const
{
  // 相对坐标系 x = [横向误差, 偏航误差, 转向角]

  Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d;  // 计算预测状态
  MPCTrajectory mpc_predicted_trajectory;  // 预测轨迹
  const auto DIM_X = getDimX();  // 状态维度
  const auto & t = reference_trajectory;

  // 遍历参考轨迹，生成预测轨迹
  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
    const auto lateral_error = Xex(i * DIM_X);  // 横向误差
    const auto yaw_error = Xex(i * DIM_X + 1);  // 偏航误差
    const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error;  // 世界坐标系 x
    const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error;  // 世界坐标系 y
    const auto yaw = t.yaw.at(i) + yaw_error;  // 世界坐标系偏航角
    mpc_predicted_trajectory.push_back(
      x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i));  // 添加到预测轨迹
  }
  return mpc_predicted_trajectory;
}
}  // namespace autoware::motion::control::mpc_lateral_controller

3.vehicle_model_bicycle_kinematics_no_delay.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_bicycle_kinematics_no_delay.hpp"

#include <cmath>

namespace autoware::motion::control::mpc_lateral_controller
{
// 构造函数，初始化无延迟运动学自行车模型的参数
KinematicsBicycleModelNoDelay::KinematicsBicycleModelNoDelay(
  const double wheelbase, const double steer_lim)
: VehicleModelInterface(/* dim_x */ 2, /* dim_u */ 1, /* dim_y */ 2, wheelbase)  // 状态维度2，输入维度1，输出维度2
{
  m_steer_lim = steer_lim;  // 设置转向角限制
}

// 计算离散状态空间矩阵
void KinematicsBicycleModelNoDelay::calculateDiscreteMatrix(
  Eigen::MatrixXd & a_d, Eigen::MatrixXd & b_d, Eigen::MatrixXd & c_d, Eigen::MatrixXd & w_d,
  const double dt)
{
  // 定义一个符号函数
  auto sign = [](double x) { return (x > 0.0) - (x < 0.0); };

  /* 在参考转向角 delta_r 附近线性化 */
  double delta_r = atan(m_wheelbase * m_curvature);  // 参考转向角
  if (std::abs(delta_r) >= m_steer_lim) {
    delta_r = m_steer_lim * static_cast<double>(sign(delta_r));  // 限制转向角范围
  }
  double cos_delta_r_squared_inv = 1 / (cos(delta_r) * cos(delta_r));  // 1 / cos^2(delta_r)

  // 设置连续时间状态矩阵 A
  a_d << 0.0, m_velocity, 0.0, 0.0;

  // 设置输入矩阵 B
  b_d << 0.0, m_velocity / m_wheelbase * cos_delta_r_squared_inv;

  // 设置输出矩阵 C
  c_d << 1.0, 0.0, 0.0, 1.0;

  // 设置扰动矩阵 W
  w_d << 0.0, -m_velocity / m_wheelbase * delta_r * cos_delta_r_squared_inv;

  // 使用双线性离散化方法对状态矩阵 A 和输入矩阵 B 进行离散化
  Eigen::MatrixXd I = Eigen::MatrixXd::Identity(m_dim_x, m_dim_x);
  const Eigen::MatrixXd i_dt2a_inv = (I - dt * 0.5 * a_d).inverse();
  a_d = i_dt2a_inv * (I + dt * 0.5 * a_d);
  b_d = i_dt2a_inv * b_d * dt;
  w_d = i_dt2a_inv * w_d * dt;
}

// 计算参考输入（前馈控制）
void KinematicsBicycleModelNoDelay::calculateReferenceInput(Eigen::MatrixXd & u_ref)
{
  u_ref(0, 0) = std::atan(m_wheelbase * m_curvature);  // 参考转向角 = arctan(wheelbase * curvature)
}

// 在世界坐标系中计算预测轨迹
MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInWorldCoordinate(
  [[maybe_unused]] const Eigen::MatrixXd & a_d, [[maybe_unused]] const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, [[maybe_unused]] const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, const double dt) const
{
  // 由于 Frenet 坐标系中存在建模误差，因此在世界坐标系中计算预测轨迹
  // 相对坐标系 x = [横向误差, 偏航角误差]
  // 世界坐标系 x = [x, y, yaw]

  const auto & t = reference_trajectory;

  // 创建世界坐标系中的初始状态
  Eigen::VectorXd state_w = [&]() {
    Eigen::VectorXd state = Eigen::VectorXd::Zero(3);
    const auto lateral_error_0 = x0(0);  // 初始横向误差
    const auto yaw_error_0 = x0(1);      // 初始偏航角误差
    state(0, 0) = t.x.at(0) - std::sin(t.yaw.at(0)) * lateral_error_0;  // 世界坐标系中的 x
    state(1, 0) = t.y.at(0) + std::cos(t.yaw.at(0)) * lateral_error_0;  // 世界坐标系中的 y
    state(2, 0) = t.yaw.at(0) + yaw_error_0;                            // 世界坐标系中的 yaw
    return state;
  }();

  // 更新世界坐标系中的状态
  const auto updateState = [&](
                             const Eigen::VectorXd & state_w, const Eigen::MatrixXd & input,
                             const double dt, const double velocity) {
    const auto yaw = state_w(2);  // 当前偏航角
    const auto steer = input(0);  // 当前转向角

    Eigen::VectorXd dstate = Eigen::VectorXd::Zero(3);
    dstate(0) = velocity * std::cos(yaw);  // x 方向速度
    dstate(1) = velocity * std::sin(yaw);  // y 方向速度
    dstate(2) = velocity * std::tan(steer) / m_wheelbase;  // 偏航角速度

    // 计算下一时刻的状态
    const Eigen::VectorXd next_state = state_w + dstate * dt;
    return next_state;
  };

  // 计算预测轨迹
  MPCTrajectory mpc_predicted_trajectory;
  const auto DIM_U = getDimU();
  for (size_t i = 0; i < t.size(); ++i) {
    state_w = updateState(state_w, Uex.block(i * DIM_U, 0, DIM_U, 1), dt, t.vx.at(i));  // 更新状态
    mpc_predicted_trajectory.push_back(
      state_w(0), state_w(1), t.z.at(i), state_w(2), t.vx.at(i), t.k.at(i), t.smooth_k.at(i),
      t.relative_time.at(i));  // 将状态添加到预测轨迹中
  }

  return mpc_predicted_trajectory;
}

// 在 Frenet 坐标系中计算预测轨迹
MPCTrajectory KinematicsBicycleModelNoDelay::calculatePredictedTrajectoryInFrenetCoordinate(
  const Eigen::MatrixXd & a_d, const Eigen::MatrixXd & b_d,
  [[maybe_unused]] const Eigen::MatrixXd & c_d, const Eigen::MatrixXd & w_d,
  const Eigen::MatrixXd & x0, const Eigen::MatrixXd & Uex,
  const MPCTrajectory & reference_trajectory, [[maybe_unused]] const double dt) const
{
  // 相对坐标系 x = [横向误差, 偏航角误差]

  // 计算预测状态
  Eigen::VectorXd Xex = a_d * x0 + b_d * Uex + w_d;
  MPCTrajectory mpc_predicted_trajectory;
  const auto DIM_X = getDimX();
  const auto & t = reference_trajectory;

  // 遍历参考轨迹，计算预测轨迹
  for (size_t i = 0; i < reference_trajectory.size(); ++i) {
    const auto lateral_error = Xex(i * DIM_X);  // 横向误差
    const auto yaw_error = Xex(i * DIM_X + 1);  // 偏航角误差
    const auto x = t.x.at(i) - std::sin(t.yaw.at(i)) * lateral_error;  // 世界坐标系中的 x
    const auto y = t.y.at(i) + std::cos(t.yaw.at(i)) * lateral_error;  // 世界坐标系中的 y
    const auto yaw = t.yaw.at(i) + yaw_error;  // 世界坐标系中的 yaw
    mpc_predicted_trajectory.push_back(
      x, y, t.z.at(i), yaw, t.vx.at(i), t.k.at(i), t.smooth_k.at(i), t.relative_time.at(i));  // 添加到预测轨迹
  }
  return mpc_predicted_trajectory;
}
}  // namespace autoware::motion::control::mpc_lateral_controller

4.vehicle_model_interface.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/vehicle_model/vehicle_model_interface.hpp"

namespace autoware::motion::control::mpc_lateral_controller
{
// 构造函数，初始化车辆模型接口
VehicleModelInterface::VehicleModelInterface(int dim_x, int dim_u, int dim_y, double wheelbase)
: m_dim_x(dim_x), m_dim_u(dim_u), m_dim_y(dim_y), m_wheelbase(wheelbase)
{
}

// 获取状态向量的维度
int VehicleModelInterface::getDimX() const
{
  return m_dim_x;
}

// 获取输入向量的维度
int VehicleModelInterface::getDimU() const
{
  return m_dim_u;
}

// 获取输出向量的维度
int VehicleModelInterface::getDimY() const
{
  return m_dim_y;
}

// 获取车辆轴距
double VehicleModelInterface::getWheelbase() const
{
  return m_wheelbase;
}

// 设置车辆速度
void VehicleModelInterface::setVelocity(const double velocity)
{
  m_velocity = velocity;
}

// 设置路径曲率
void VehicleModelInterface::setCurvature(const double curvature)
{
  m_curvature = curvature;
}
}  // namespace autoware::motion::control::mpc_lateral_controller

五、qp_solver

二次规划（Quadratic Programming，QP）求解器的接口文件。
这部分就是俩接口，没啥讲的。
二次求解器我没整理过，等之后单独整理一版。
qp_solver文件夹下面有两个cpp文件：
qp_solver_osqp：这是一个基于 OSQP（Operator Splitting Quadratic Program）的二次规划（QP）求解器。OSQP 是一个高效的、开源的二次规划求解器，专门用于求解凸二次规划问题。具体实现在autoware/osqp_interface/osqp_interface.cpp
qp_solver_unconstraint_fast：这是一个用于求解无约束二次规划问题的快速求解器。

求解器这部分先看看代码吧，之后我再单独出一篇说明一下如何修改。

1.qp_solver_osqp.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_
#define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_

#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"  // QP 求解器接口
#include "autoware/osqp_interface/osqp_interface.hpp"  // OSQP 接口
#include "rclcpp/rclcpp.hpp"  // ROS 2 的日志功能

namespace autoware::motion::control::mpc_lateral_controller
{

/// 使用 OSQP 库求解 QP 问题的类
class QPSolverOSQP : public QPSolverInterface
{
public:
  /**
   * @brief 构造函数
   * @param logger ROS 2 日志记录器
   */
  explicit QPSolverOSQP(const rclcpp::Logger & logger);

  /**
   * @brief 析构函数
   */
  virtual ~QPSolverOSQP() = default;

  /**
   * @brief 求解 QP 问题：最小化目标函数 j = u' * h_mat * u + f_vec' * u，无约束
   * @param [in] h_mat 目标函数中的参数矩阵
   * @param [in] f_vec 目标函数中的参数向量
   * @param [in] a 约束条件矩阵 lb_a < a*u < ub_a（此处未使用）
   * @param [in] lb 约束条件向量 lb < U < ub（此处未使用）
   * @param [in] ub 约束条件向量 lb < U < ub（此处未使用）
   * @param [in] lb_a 约束条件向量 lb_a < a*u < ub_a（此处未使用）
   * @param [in] ub_a 约束条件向量 lb_a < a*u < ub_a（此处未使用）
   * @param [out] u 最优解向量
   * @return 如果问题成功求解，返回 true
   */
  bool solve(
    const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
    const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
    const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;

  /**
   * @brief 获取求解器迭代次数
   * @return 迭代次数
   */
  int64_t getTakenIter() const override { return osqpsolver_.getTakenIter(); }

  /**
   * @brief 获取求解器运行时间
   * @return 运行时间（秒）
   */
  double getRunTime() const override { return osqpsolver_.getRunTime(); }

  /**
   * @brief 获取目标函数的最优值
   * @return 目标函数的最优值
   */
  double getObjVal() const override { return osqpsolver_.getObjVal(); }

private:
  autoware::osqp_interface::OSQPInterface osqpsolver_;  // OSQP 求解器接口
  rclcpp::Logger logger_;  // ROS 2 日志记录器
};
}  // namespace autoware::motion::control::mpc_lateral_controller
#endif  // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_OSQP_HPP_

2.qp_solver_unconstraint_fast.cpp

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#ifndef AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_
#define AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_

#include "autoware/mpc_lateral_controller/qp_solver/qp_solver_interface.hpp"  // QP 求解器接口

namespace autoware::motion::control::mpc_lateral_controller
{

/**
 * 使用最小二乘法求解 QP 问题的类
 * 基于 Eigen 的标准 Cholesky 分解（LLT）实现
 */
class QPSolverEigenLeastSquareLLT : public QPSolverInterface
{
public:
  /**
   * @brief 构造函数
   */
  QPSolverEigenLeastSquareLLT();

  /**
   * @brief 析构函数
   */
  ~QPSolverEigenLeastSquareLLT() = default;

  /**
   * @brief 求解 QP 问题：最小化目标函数 j = u' * h_mat * u + f_vec' * u，无约束
   * @param [in] h_mat 目标函数中的参数矩阵
   * @param [in] f_vec 目标函数中的参数向量
   * @param [in] a 约束条件矩阵 lb_a < a*u < ub_a（此处未使用）
   * @param [in] lb 约束条件向量 lb < U < ub（此处未使用）
   * @param [in] ub 约束条件向量 lb < U < ub（此处未使用）
   * @param [in] lb_a 约束条件向量 lb_a < a*u < ub_a（此处未使用）
   * @param [in] ub_a 约束条件向量 lb_a < a*u < ub_a（此处未使用）
   * @param [out] u 最优解向量
   * @return 如果问题成功求解，返回 true
   */
  bool solve(
    const Eigen::MatrixXd & h_mat, const Eigen::MatrixXd & f_vec, const Eigen::MatrixXd & a,
    const Eigen::VectorXd & lb, const Eigen::VectorXd & ub, const Eigen::VectorXd & lb_a,
    const Eigen::VectorXd & ub_a, Eigen::VectorXd & u) override;

  /**
   * @brief 获取求解器迭代次数
   * @return 迭代次数（固定为 1，因为 LLT 分解是直接求解）
   */
  int64_t getTakenIter() const override { return 1; };

  // TODO(someone): 计算运行时间和目标函数值，否则基类默认返回 0
  // double getRunTime() const override { return 0.0; }
  // double getObjVal() const override { return 0.0; }

private:
  // 无私有成员变量
};
}  // namespace autoware::motion::control::mpc_lateral_controller
#endif  // AUTOWARE__MPC_LATERAL_CONTROLLER__QP_SOLVER__QP_SOLVER_UNCONSTRAINT_FAST_HPP_

六、mpc_trajectory.cpp

这个是处理目标路径的文件，将规划给的目标路径转换为 MPC 算法所需的格式。也没啥讲的。

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/mpc_trajectory.hpp"

namespace autoware::motion::control::mpc_lateral_controller
{
/**
 * @brief 向轨迹中添加一个点
 * @param xp x 坐标
 * @param yp y 坐标
 * @param zp z 坐标
 * @param yawp 偏航角
 * @param vxp 速度
 * @param kp 曲率
 * @param smooth_kp 平滑曲率
 * @param tp 相对时间
 */
void MPCTrajectory::push_back(
  const double & xp, const double & yp, const double & zp, const double & yawp, const double & vxp,
  const double & kp, const double & smooth_kp, const double & tp)
{
  x.push_back(xp);  // 添加 x 坐标
  y.push_back(yp);  // 添加 y 坐标
  z.push_back(zp);  // 添加 z 坐标
  yaw.push_back(yawp);  // 添加偏航角
  vx.push_back(vxp);  // 添加速度
  k.push_back(kp);  // 添加曲率
  smooth_k.push_back(smooth_kp);  // 添加平滑曲率
  relative_time.push_back(tp);  // 添加相对时间
}

/**
 * @brief 向轨迹中添加一个点
 * @param p 轨迹点对象
 */
void MPCTrajectory::push_back(const MPCTrajectoryPoint & p)
{
  x.push_back(p.x);  // 添加 x 坐标
  y.push_back(p.y);  // 添加 y 坐标
  z.push_back(p.z);  // 添加 z 坐标
  yaw.push_back(p.yaw);  // 添加偏航角
  vx.push_back(p.vx);  // 添加速度
  k.push_back(p.k);  // 添加曲率
  smooth_k.push_back(p.smooth_k);  // 添加平滑曲率
  relative_time.push_back(p.relative_time);  // 添加相对时间
}

/**
 * @brief 获取轨迹的最后一个点
 * @return 最后一个轨迹点
 */
MPCTrajectoryPoint MPCTrajectory::back()
{
  MPCTrajectoryPoint p;

  p.x = x.back();  // 获取最后一个点的 x 坐标
  p.y = y.back();  // 获取最后一个点的 y 坐标
  p.z = z.back();  // 获取最后一个点的 z 坐标
  p.yaw = yaw.back();  // 获取最后一个点的偏航角
  p.vx = vx.back();  // 获取最后一个点的速度
  p.k = k.back();  // 获取最后一个点的曲率
  p.smooth_k = smooth_k.back();  // 获取最后一个点的平滑曲率
  p.relative_time = relative_time.back();  // 获取最后一个点的相对时间

  return p;
}

/**
 * @brief 获取轨迹中指定索引的点
 * @param i 索引
 * @return 指定索引的轨迹点
 */
MPCTrajectoryPoint MPCTrajectory::at(const size_t i) const
{
  MPCTrajectoryPoint p;

  p.x = x.at(i);  // 获取第 i 个点的 x 坐标
  p.y = y.at(i);  // 获取第 i 个点的 y 坐标
  p.z = z.at(i);  // 获取第 i 个点的 z 坐标
  p.yaw = yaw.at(i);  // 获取第 i 个点的偏航角
  p.vx = vx.at(i);  // 获取第 i 个点的速度
  p.k = k.at(i);  // 获取第 i 个点的曲率
  p.smooth_k = smooth_k.at(i);  // 获取第 i 个点的平滑曲率
  p.relative_time = relative_time.at(i);  // 获取第 i 个点的相对时间

  return p;
}

/**
 * @brief 清空轨迹中的所有点
 */
void MPCTrajectory::clear()
{
  x.clear();  // 清空 x 坐标
  y.clear();  // 清空 y 坐标
  z.clear();  // 清空 z 坐标
  yaw.clear();  // 清空偏航角
  vx.clear();  // 清空速度
  k.clear();  // 清空曲率
  smooth_k.clear();  // 清空平滑曲率
  relative_time.clear();  // 清空相对时间
}

/**
 * @brief 获取轨迹中点的数量
 * @return 轨迹中点的数量
 */
size_t MPCTrajectory::size() const
{
  // 检查所有向量的长度是否一致
  if (
    x.size() == y.size() && x.size() == z.size() && x.size() == yaw.size() &&
    x.size() == vx.size() && x.size() == k.size() && x.size() == smooth_k.size() &&
    x.size() == relative_time.size()) {
    return x.size();  // 返回轨迹点的数量
  } else {
    std::cerr << "[MPC trajectory] trajectory size is inappropriate" << std::endl;  // 输出错误信息
    return 0;  // 返回 0 表示轨迹数据不一致
  }
}
}  // namespace autoware::motion::control::mpc_lateral_controller

七、mpc_utils.cpp

一些 MPC 算法的辅助函数或工具函数。计算曲率之类的。

以下是添加了详细汉语注释的代码：

```cpp
// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/mpc_utils.hpp"

#include "autoware/interpolation/linear_interpolation.hpp"
#include "autoware/interpolation/spline_interpolation.hpp"
#include "autoware/motion_utils/trajectory/trajectory.hpp"
#include "autoware/universe_utils/geometry/geometry.hpp"
#include "autoware/universe_utils/math/normalization.hpp"

#include <algorithm>
#include <limits>
#include <string>
#include <vector>

namespace autoware::motion::control::mpc_lateral_controller
{
namespace
{
/**
 * @brief 计算目标点在前后两点连线上的纵向偏移量
 * @param p_front 前点
 * @param p_back 后点
 * @param p_target 目标点
 * @return 纵向偏移量
 */
double calcLongitudinalOffset(
  const geometry_msgs::msg::Point & p_front, const geometry_msgs::msg::Point & p_back,
  const geometry_msgs::msg::Point & p_target)
{
  const Eigen::Vector3d segment_vec{p_back.x - p_front.x, p_back.y - p_front.y, 0};  // 前后点向量
  const Eigen::Vector3d target_vec{p_target.x - p_front.x, p_target.y - p_front.y, 0};  // 目标点向量
  return segment_vec.dot(target_vec) / segment_vec.norm();  // 计算投影长度
}
}  // namespace

namespace MPCUtils
{
using autoware::universe_utils::calcDistance2d;
using autoware::universe_utils::createQuaternionFromYaw;
using autoware::universe_utils::normalizeRadian;

/**
 * @brief 计算轨迹中两个点之间的二维距离
 * @param trajectory 轨迹
 * @param idx1 第一个点的索引
 * @param idx2 第二个点的索引
 * @return 两点之间的距离
 */
double calcDistance2d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
{
  const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);  // x 方向差值
  const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2);  // y 方向差值
  return std::hypot(dx, dy);  // 计算欧几里得距离
}

/**
 * @brief 计算轨迹中两个点之间的三维距离
 * @param trajectory 轨迹
 * @param idx1 第一个点的索引
 * @param idx2 第二个点的索引
 * @return 两点之间的距离
 */
double calcDistance3d(const MPCTrajectory & trajectory, const size_t idx1, const size_t idx2)
{
  const double dx = trajectory.x.at(idx1) - trajectory.x.at(idx2);  // x 方向差值
  const double dy = trajectory.y.at(idx1) - trajectory.y.at(idx2);  // y 方向差值
  const double dz = trajectory.z.at(idx1) - trajectory.z.at(idx2);  // z 方向差值
  return std::hypot(dx, dy, dz);  // 计算欧几里得距离
}

/**
 * @brief 将欧拉角转换为单调递增的角度序列
 * @param angle_vector 角度向量
 */
void convertEulerAngleToMonotonic(std::vector<double> & angle_vector)
{
  for (uint i = 1; i < angle_vector.size(); ++i) {
    const double da = angle_vector.at(i) - angle_vector.at(i - 1);  // 角度差值
    angle_vector.at(i) = angle_vector.at(i - 1) + normalizeRadian(da);  // 归一化并累加
  }
}

/**
 * @brief 计算横向误差
 * @param ego_pose 自车位姿
 * @param ref_pose 参考位姿
 * @return 横向误差
 */
double calcLateralError(const Pose & ego_pose, const Pose & ref_pose)
{
  const double err_x = ego_pose.position.x - ref_pose.position.x;  // x 方向误差
  const double err_y = ego_pose.position.y - ref_pose.position.y;  // y 方向误差
  const double ref_yaw = tf2::getYaw(ref_pose.orientation);  // 参考位姿的偏航角
  const double lat_err = -std::sin(ref_yaw) * err_x + std::cos(ref_yaw) * err_y;  // 计算横向误差
  return lat_err;
}

/**
 * @brief 计算轨迹的弧长
 * @param trajectory 轨迹
 * @param arc_length 输出的弧长向量
 */
void calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory, std::vector<double> & arc_length)
{
  double dist = 0.0;
  arc_length.clear();
  arc_length.push_back(dist);  // 初始弧长为 0
  for (uint i = 1; i < trajectory.size(); ++i) {
    dist += calcDistance2d(trajectory, i, i - 1);  // 累加两点之间的距离
    arc_length.push_back(dist);  // 存储当前弧长
  }
}

/**
 * @brief 计算轨迹的总弧长
 * @param trajectory 轨迹
 * @return 总弧长
 */
double calcMPCTrajectoryArcLength(const MPCTrajectory & trajectory)
{
  double length = 0.0;
  for (size_t i = 1; i < trajectory.size(); ++i) {
    length += calcDistance2d(trajectory, i, i - 1);  // 累加两点之间的距离
  }
  return length;
}

/**
 * @brief 按距离重新采样轨迹
 * @param input 输入轨迹
 * @param resample_interval_dist 重采样间隔距离
 * @param nearest_seg_idx 最近段的索引
 * @param ego_offset_to_segment 自车到最近段的偏移量
 * @return 重采样后的轨迹
 */
std::pair<bool, MPCTrajectory> resampleMPCTrajectoryByDistance(
  const MPCTrajectory & input, const double resample_interval_dist, const size_t nearest_seg_idx,
  const double ego_offset_to_segment)
{
  MPCTrajectory output;

  if (input.empty()) {
    return {true, output};  // 如果输入轨迹为空，直接返回
  }
  std::vector<double> input_arclength;
  calcMPCTrajectoryArcLength(input, input_arclength);  // 计算输入轨迹的弧长

  if (input_arclength.empty()) {
    return {false, output};  // 如果弧长计算失败，返回失败
  }

  std::vector<double> output_arclength;
  // 从当前位置向前和向后分别重采样
  for (double s = std::clamp(
         input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0,
         input_arclength.back() - 1e-6);
       0 <= s; s -= resample_interval_dist) {
    output_arclength.push_back(s);  // 向后采样
  }
  std::reverse(output_arclength.begin(), output_arclength.end());  // 反转以保持顺序
  for (double s = std::max(input_arclength.at(nearest_seg_idx) + ego_offset_to_segment, 0.0) +
                  resample_interval_dist;
       s < input_arclength.back(); s += resample_interval_dist) {
    output_arclength.push_back(s);  // 向前采样
  }

  std::vector<double> input_yaw = input.yaw;
  convertEulerAngleToMonotonic(input_yaw);  // 将偏航角转换为单调递增序列

  // 使用线性插值和样条插值进行重采样
  const auto lerp_arc_length = [&](const auto & input_value) {
    return autoware::interpolation::lerp(input_arclength, input_value, output_arclength);
  };
  const auto spline_arc_length = [&](const auto & input_value) {
    return autoware::interpolation::spline(input_arclength, input_value, output_arclength);
  };

  output.x = spline_arc_length(input.x);  // 重采样 x 坐标
  output.y = spline_arc_length(input.y);  // 重采样 y 坐标
  output.z = spline_arc_length(input.z);  // 重采样 z 坐标
  output.yaw = spline_arc_length(input_yaw);  // 重采样偏航角
  output.vx = lerp_arc_length(input.vx);  // 重采样速度（必须线性插值）
  output.k = spline_arc_length(input.k);  // 重采样曲率
  output.smooth_k = spline_arc_length(input.smooth_k);  // 重采样平滑曲率
  output.relative_time = lerp_arc_length(input.relative_time);  // 重采样相对时间（必须线性插值）

  return {true, output};  // 返回重采样后的轨迹
}

/**
 * @brief 线性插值轨迹
 * @param in_index 输入索引
 * @param in_traj 输入轨迹
 * @param out_index 输出索引
 * @param out_traj 输出轨迹
 * @return 是否成功
 */
bool linearInterpMPCTrajectory(
  const std::vector<double> & in_index, const MPCTrajectory & in_traj,
  const std::vector<double> & out_index, MPCTrajectory & out_traj)
{
  if (in_traj.empty()) {
    out_traj = in_traj;  // 如果输入轨迹为空，直接返回
    return true;
  }

  std::vector<double> in_traj_yaw = in_traj.yaw;
  convertEulerAngleToMonotonic(in_traj_yaw);  // 将偏航角转换为单调递增序列

  // 使用线性插值进行重采样
  const auto lerp_arc_length = [&](const auto & input_value) {
    return autoware::interpolation::lerp(in_index, input_value, out_index);
  };

  try {
    out_traj.x = lerp_arc_length(in_traj.x);  // 重采样 x 坐标
    out_traj.y = lerp_arc_length(in_traj.y);  // 重采样 y 坐标
    out_traj.z = lerp_arc_length(in_traj.z);  // 重采样 z 坐标
    out_traj.yaw = lerp_arc_length(in_traj.yaw);  // 重采样偏航角
    out_traj.vx = lerp_arc_length(in_traj.vx);  // 重采样速度
    out_traj.k = lerp_arc_length(in_traj.k);  // 重采样曲率
    out_traj.smooth_k = lerp_arc_length(in_traj.smooth_k);  // 重采样平滑曲率
    out_traj.relative_time = lerp_arc_length(in_traj.relative_time);  // 重采样相对时间
  } catch (const std::exception & e) {
    std::cerr << "linearInterpMPCTrajectory error!: " << e.what() << std::endl;  // 输出错误信息
  }

  if (out_traj.empty()) {
    std::cerr << "[mpc util] linear interpolation error" << std::endl;  // 输出错误信息
    return false;
  }

  return true;  // 返回成功
}

/**
 * @brief 从 x, y 坐标计算轨迹的偏航角
 * @param traj 轨迹
 * @param is_forward_shift 是否向前偏移
 */
void calcTrajectoryYawFromXY(MPCTrajectory & traj, const bool is_forward_shift)
{
  if (traj.yaw.size() < 3) {  // 至少需要 3 个点来计算偏航角
    return;
  }
  if (traj.yaw.size() != traj.vx.size()) {
    RCLCPP_ERROR(rclcpp::get_logger("mpc_utils"), "trajectory size has no consistency.");
    return;
  }

  // 插值偏航角
  for (int i = 1; i < static_cast<int>(traj.yaw.size()) - 1; ++i) {
    const double dx = traj.x.at(i + 1) - traj.x.at(i - 1);  // x 方向差值
    const double dy = traj.y.at(i + 1) - traj.y.at(i - 1);  // y 方向差值
    traj.yaw.at(i) = is_forward_shift ? std::atan2(dy, dx) : std::atan2(dy, dx) + M_PI;  // 计算偏航角
  }
  if (traj.yaw.size() > 1) {
    traj.yaw.at(0) = traj.yaw.at(1);  // 第一个点的偏航角与第二个点相同
    traj.yaw.back() = traj.yaw.at(traj.yaw.size() - 2);  // 最后一个点的偏航角与倒数第二个点相同
  }
}

/**
 * @brief 计算轨迹的曲率
 * @param curvature_smoothing_num_traj 轨迹曲率平滑点数
 * @param curvature_smoothing_num_ref_steer 参考转向曲率平滑点数
 * @param traj 轨迹
 */
void calcTrajectoryCurvature(
  const int curvature_smoothing_num_traj, const int curvature_smoothing_num_ref_steer,
  MPCTrajectory & traj)
{
  traj.k = calcTrajectoryCurvature(curvature_smoothing_num_traj, traj);  // 计算轨迹曲率
  traj.smooth_k = calcTrajectoryCurvature(curvature_smoothing_num_ref_steer, traj);  // 计算平滑曲率
}

/**
 * @brief 计算轨迹的曲率
 * @param curvature_smoothing_num 曲率平滑点数
 * @param traj 轨迹
 * @return 曲率向量
 */
std::vector<double> calcTrajectoryCurvature(
  const int curvature_smoothing_num, const MPCTrajectory & traj)
{
  std::vector<double> curvature_vec(traj.x.size());

  /* 通过三点拟合圆计算曲率 */
  geometry_msgs::msg::Point p1, p2, p3;
  const int max_smoothing_num =
    static_cast<int>(std::floor(0.5 * (static_cast<double>(traj.x.size() - 1))));
  const size_t L = static_cast<size_t>(std::min(curvature_smoothing_num, max_smoothing_num));
  for (size_t i = L; i < traj.x.size() - L; ++i) {
    const size_t curr_idx = i;
    const size_t prev_idx = curr_idx - L;
    const size_t next_idx = curr_idx + L;
    p1.x = traj.x.at(prev_idx);
    p2.x = traj.x.at(curr_idx);
    p3.x = traj.x.at(next_idx);
    p1.y = traj.y.at(prev_idx);
    p2.y = traj.y.at(curr_idx);
    p3.y = traj.y.at(next_idx);
    try {
      curvature_vec.at(curr_idx) = autoware::universe_utils::calcCurvature(p1, p2, p3);  // 计算曲率
    } catch (...) {
      std::cerr << "[MPC] 2 points are too close to calculate curvature." << std::endl;  // 输出错误信息
      curvature_vec.at(curr_idx) = 0.0;  // 曲率设为 0
    }
  }

  /* 第一个和最后一个曲率从相邻点复制 */
  for (size_t i = 0; i < std::min(L, traj.x.size()); ++i) {
    curvature_vec.at(i) = curvature_vec.at(std::min(L, traj.x.size() - 1));
    curvature_vec.at(traj.x.size() - i - 1) =
      curvature_vec.at(std::max(traj.x.size() - L - 1, size_t(0)));
  }
  return curvature_vec;
}

/**
 * @brief 将 Trajectory 转换为 MPCTrajectory
 * @param input 输入轨迹
 * @return 转换后的 MPCTrajectory
 */
MPCTrajectory convertToMPCTrajectory(const Trajectory & input)
{
  MPCTrajectory output;
  for (const TrajectoryPoint & p : input.points) {
    const double x = p.pose.position.x;  // x 坐标
    const double y = p.pose.position.y;  // y 坐标
    const double z = p.pose.position.z;  // z 坐标
    const double yaw = tf2::getYaw(p.pose.orientation);  // 偏航角
    const double vx = p.longitudinal_velocity_mps;  // 速度
    const double k = 0.0;  // 曲率
    const double t = 0.0;  // 时间
    output.push_back(x, y, z, yaw, vx, k, k, t);  // 添加到 MPCTrajectory
  }
  calcMPCTrajectoryTime(output);  // 计算轨迹时间
  return output;
}

/**
 * @brief 将 MPCTrajectory 转换为 Autoware Trajectory
 * @param input 输入 MPCTrajectory
 * @return 转换后的 Autoware Trajectory
 */
Trajectory convertToAutowareTrajectory(const MPCTrajectory & input)
{
  Trajectory output;
  TrajectoryPoint p;
  for (size_t i = 0; i < input.size(); ++i) {
    p.pose.position.x = input.x.at(i);  // x 坐标
    p.pose.position.y = input.y.at(i);  // y 坐标
    p.pose.position.z = input.z.at(i);  // z 坐标
    p.pose.orientation = autoware::universe_utils::createQuaternionFromYaw(input.yaw.at(i));  // 偏航角
    p.longitudinal_velocity_mps =
      static_cast<decltype(p.longitudinal_velocity_mps)>(input.vx.at(i));  // 速度
    output.points.push_back(p);  // 添加到 Autoware Trajectory
    if (output.points.size() == output.points.max_size()) {
      break;  // 如果达到最大点数，停止添加
    }
  }
  return output;
}

/**
 * @brief 计算 MPCTrajectory 的时间
 * @param traj 轨迹
 * @return 是否成功
 */
bool calcMPCTrajectoryTime(MPCTrajectory & traj)
{
  constexpr auto min_dt = 1.0e-4;  // 最小时间间隔，避免时间重复
  double t = 0.0;
  traj.relative_time.clear();
  traj.relative_time.push_back(t);  // 初始时间为 0
  for (size_t i = 0; i < traj.x.size() - 1; ++i) {
    const double dist = calcDistance3d(traj, i, i + 1);  // 计算两点之间的距离
    const double v = std::max(std::fabs(traj.vx.at(i)), 0.1);  // 计算速度
    t += std::max(dist / v, min_dt);  // 计算时间间隔
    traj.relative_time.push_back(t);  // 存储当前时间
  }
  return true;
}

/**
 * @brief 动态平滑速度
 * @param start_seg_idx 起始段索引
 * @param start_vel 起始速度
 * @param acc_lim 加速度限制
 * @param tau 时间常数
 * @param traj 轨迹
 */
void dynamicSmoothingVelocity(
  const size_t start_seg_idx, const double start_vel, const double acc_lim, const double tau,
  MPCTrajectory & traj)
{
  double curr_v = start_vel;
  // 设置起始段的速度
  traj.vx.at(start_seg_idx) = start_vel;
  if (1 < traj.vx.size()) {
    traj.vx.at(start_seg_idx + 1) = start_vel;
  }

  // 动态平滑速度
  for (size_t i = start_seg_idx + 2; i < traj.size(); ++i) {
    const double ds = calcDistance2d(traj, i, i - 1);  // 计算两点之间的距离
    const double dt = ds / std::max(std::fabs(curr_v), std::numeric_limits<double>::epsilon());  // 计算时间间隔
    const double a = tau / std::max(tau + dt, std::numeric_limits<double>::epsilon());  // 计算平滑系数
    const double updated_v = a * curr_v + (1.0 - a) * traj.vx.at(i);  // 更新速度
    const double dv = std::max(-acc_lim * dt, std::min(acc_lim * dt, updated_v - curr_v));  // 限制速度变化
    curr_v = curr_v + dv;  // 更新当前速度
    traj.vx.at(i) = curr_v;  // 存储更新后的速度
  }
  calcMPCTrajectoryTime(traj);  // 重新计算轨迹时间
}

/**
 * @brief 计算最近位姿的插值
 * @param traj 轨迹
 * @param self_pose 自车位姿
 * @param nearest_pose 最近位姿
 * @param nearest_index 最近索引
 * @param nearest_time 最近时间
 * @param max_dist 最大距离
 * @param max_yaw 最大偏航角
 * @return 是否成功
 */
bool calcNearestPoseInterp(
  const MPCTrajectory & traj, const Pose & self_pose, Pose * nearest_pose, size_t * nearest_index,
  double * nearest_time, const double max_dist, const double max_yaw)
{
  if (traj.empty() || !nearest_pose || !nearest_index || !nearest_time) {
    return false;  // 如果输入无效，返回失败
  }

  const auto autoware_traj = convertToAutowareTrajectory(traj);  // 转换为 Autoware Trajectory
  if (autoware_traj.points.empty()) {
    const auto logger = rclcpp::get_logger("mpc_util");
    auto clock = rclcpp::Clock(RCL_ROS_TIME);
    RCLCPP_WARN_THROTTLE(logger, clock, 5000, "[calcNearestPoseInterp] input trajectory is empty");
    return false;  // 如果轨迹为空，返回失败
  }

  *nearest_index = autoware::motion_utils::findFirstNearestIndexWithSoftConstraints(
    autoware_traj.points, self_pose, max_dist, max_yaw);  // 找到最近点的索引
  const size_t traj_size = traj.size();

  if (traj.size() == 1) {
    nearest_pose->position.x = traj.x.at(*nearest_index);  // 最近点的 x 坐标
    nearest_pose->position.y = traj.y.at(*nearest_index);  // 最近点的 y 坐标
    nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));  // 最近点的偏航角
    *nearest_time = traj.relative_time.at(*nearest_index);  // 最近点的时间
    return true;
  }

  /* 获取第二个最近点的索引 */
  const auto [prev, next] = [&]() -> std::pair<size_t, size_t> {
    if (*nearest_index == 0) {
      return std::make_pair(0, 1);  // 如果最近点是第一个点，返回第一个和第二个点
    }
    if (*nearest_index == traj_size - 1) {
      return std::make_pair(traj_size - 2, traj_size - 1);  // 如果最近点是最后一个点，返回倒数第二个和最后一个点
    }

    geometry_msgs::msg::Point nearest_traj_point;
    nearest_traj_point.x = traj.x.at(*nearest_index);
    nearest_traj_point.y = traj.y.at(*nearest_index);
    geometry_msgs::msg::Point next_nearest_traj_point;
    next_nearest_traj_point.x = traj.x.at(*nearest_index + 1);
    next_nearest_traj_point.y = traj.y.at(*nearest_index + 1);

    const double signed_length =
      calcLongitudinalOffset(nearest_traj_point, next_nearest_traj_point, self_pose.position);  // 计算纵向偏移
    if (signed_length <= 0) {
      return std::make_pair(*nearest_index - 1, *nearest_index);  // 如果偏移为负，返回前一个点和最近点
    }
    return std::make_pair(*nearest_index, *nearest_index + 1);  // 否则返回最近点和下一个点
  }();

  geometry_msgs::msg::Point next_traj_point;
  next_traj_point.x = traj.x.at(next);
  next_traj_point.y = traj.y.at(next);
  geometry_msgs::msg::Point prev_traj_point;
  prev_traj_point.x = traj.x.at(prev);
  prev_traj_point.y = traj.y.at(prev);
  const double traj_seg_length =
    autoware::universe_utils::calcDistance2d(prev_traj_point, next_traj_point);  // 计算两点之间的距离
  /* 如果两点距离太近 */
  if (traj_seg_length < 1.0E-5) {
    nearest_pose->position.x = traj.x.at(*nearest_index);  // 最近点的 x 坐标
    nearest_pose->position.y = traj.y.at(*nearest_index);  // 最近点的 y 坐标
    nearest_pose->orientation = createQuaternionFromYaw(traj.yaw.at(*nearest_index));  // 最近点的偏航角
    *nearest_time = traj.relative_time.at(*nearest_index);  // 最近点的时间
    return true;
  }

  /* 线性插值 */
  const double ratio = std::clamp(
    calcLongitudinalOffset(prev_traj_point, next_traj_point, self_pose.position) / traj_seg_length,
    0.0, 1.0);  // 计算插值比例
  nearest_pose->position.x = (1 - ratio) * traj.x.at(prev) + ratio * traj.x.at(next);  // 插值 x 坐标
  nearest_pose->position.y = (1 - ratio) * traj.y.at(prev) + ratio * traj.y.at(next);  // 插值 y 坐标
  const double tmp_yaw_err = normalizeRadian(traj.yaw.at(prev) - traj.yaw.at(next));  // 计算偏航角误差
  const double nearest_yaw = normalizeRadian(traj.yaw.at(next) + (1 - ratio) * tmp_yaw_err);  // 插值偏航角
  nearest_pose->orientation = createQuaternionFromYaw(nearest_yaw);  // 设置偏航角
  *nearest_time = (1 - ratio) * traj.relative_time.at(prev) + ratio * traj.relative_time.at(next);  // 插值时间
  return true;
}

/**
 * @brief 计算停止距离
 * @param current_trajectory 当前轨迹
 * @param origin 起始点索引
 * @return 停止距离
 */
double calcStopDistance(const Trajectory & current_trajectory, const int origin)
{
  constexpr float zero_velocity = std::numeric_limits<float>::epsilon();  // 零速度阈值
  const float origin_velocity =
    current_trajectory.points.at(static_cast<size_t>(origin)).longitudinal_velocity_mps;  // 起始点速度
  double stop_dist = 0.0;

  // 向前搜索
  if (std::fabs(origin_velocity) > zero_velocity) {
    for (int i = origin + 1; i < static_cast<int>(current_trajectory.points.size()) - 1; ++i) {
      const auto & p0 = current_trajectory.points.at(i);
      const auto & p1 = current_trajectory.points.at(i - 1);
      stop_dist += calcDistance2d(p0, p1);  // 累加距离
      if (std::fabs(p0.longitudinal_velocity_mps) < zero_velocity) {
        break;  // 如果速度为零，停止搜索
      }
    }
    return stop_dist;
  }

  // 向后搜索
  for (int i = origin - 1; 0 < i; --i) {
    const auto & p0 = current_trajectory.points.at(i);
    const auto & p1 = current_trajectory.points.at(i + 1);
    if (std::fabs(p0.longitudinal_velocity_mps) > zero_velocity) {
      break;  // 如果速度不为零，停止搜索
    }
    stop_dist -= calcDistance2d(p0, p1);  // 累加距离
  }
  return stop_dist;
}

/**
 * @brief 在偏航方向上扩展轨迹
 * @param yaw 偏航角
 * @param interval 间隔
 * @param is_forward_shift 是否向前偏移
 * @param traj 轨迹
 */
void extendTrajectoryInYawDirection(
  const double yaw, const double interval, const bool is_forward_shift, MPCTrajectory & traj)
{
  // 设置终点偏航角
  traj.yaw.back() = yaw;

  // 获取终点位姿
  const auto autoware_traj = MPCUtils::convertToAutowareTrajectory(traj);
  auto extended_pose = autoware_traj.points.back().pose;

  constexpr double extend_dist = 10.0;  // 扩展距离
  constexpr double extend_vel = 10.0;  // 扩展速度
  const double x_offset = is_forward_shift ? interval : -interval;  // x 方向偏移
  const double dt = interval / extend_vel;  // 时间间隔
  const size_t num_extended_point = static_cast<size_t>(extend_dist / interval);  // 扩展点数
  for (size_t i = 0; i < num_extended_point; ++i) {
    extended_pose = autoware::universe_utils::calcOffsetPose(extended_pose, x_offset, 0.0, 0.0);  // 计算偏移位姿
    traj.push_back(
      extended_pose.position.x, extended_pose.position.y, extended_pose.position.z, traj.yaw.back(),
      extend_vel, traj.k.back(), traj.smooth_k.back(), traj.relative_time.back() + dt);  // 添加扩展点
  }
}

/**
 * @brief 按长度裁剪轨迹
 * @param trajectory 轨迹
 * @param length 长度
 * @return 裁剪后的轨迹
 */
MPCTrajectory clipTrajectoryByLength(const MPCTrajectory & trajectory, const double length)
{
  MPCTrajectory clipped_trajectory;
  clipped_trajectory.push_back(trajectory.at(0));  // 添加第一个点

  double current_length = 0.0;
  for (size_t i = 1; i < trajectory.size(); ++i) {
    current_length += calcDistance3d(trajectory, i, i - 1);  // 累加距离
    if (current_length > length) {
      break;  // 如果超过长度，停止添加
    }
    clipped_trajectory.push_back(trajectory.at(i));  // 添加当前点
  }

  return clipped_trajectory;  // 返回裁剪后的轨迹
}

}  // namespace MPCUtils
}  // namespace autoware::motion::control::mpc_lateral_controller

### 代码功能总结

1. **轨迹处理**：
   - 提供了对 MPC 轨迹的各种操作，包括计算距离、插值、平滑、曲率计算等。
   - 支持轨迹的重采样、扩展和裁剪。

2. **数学工具**：
   - 提供了计算偏航角、曲率、距离等数学工具函数。

3. **轨迹转换**：
   - 支持将 Autoware 的 `Trajectory` 转换为 MPC 的 `MPCTrajectory`，以及反向转换。

4. **动态平滑**：
   - 提供了动态平滑速度的功能，确保速度变化平滑且符合加速度限制。

5. **最近点计算**：
   - 提供了计算自车与轨迹最近点的功能，支持插值计算。

---

如果有进一步的问题或需要更详细的解释，欢迎继续讨论！

八、lowpass_filter.cpp

在mpc.cpp中，用到了低通滤波器对输入进行了滤波。

// Copyright 2018-2021 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/lowpass_filter.hpp"

#include <vector>

namespace autoware::motion::control::mpc_lateral_controller
{
/**
 * @brief Butterworth 二阶低通滤波器的构造函数
 * @param dt 采样时间间隔
 * @param f_cutoff_hz 截止频率（Hz）
 */
Butterworth2dFilter::Butterworth2dFilter(double dt, double f_cutoff_hz)
{
  initialize(dt, f_cutoff_hz);  // 初始化滤波器
}

/**
 * @brief Butterworth 二阶低通滤波器的析构函数
 */
Butterworth2dFilter::~Butterworth2dFilter()
{
}

/**
 * @brief 初始化 Butterworth 二阶低通滤波器
 * @param dt 采样时间间隔
 * @param f_cutoff_hz 截止频率（Hz）
 */
void Butterworth2dFilter::initialize(const double & dt, const double & f_cutoff_hz)
{
  m_y1 = 0.0;  // 上一次的输出
  m_y2 = 0.0;  // 上上次的输出
  m_u2 = 0.0;  // 上一次的输入
  m_u1 = 0.0;  // 上上次的输入

  // 使用双线性变换设计二阶 Butterworth 低通滤波器
  const double f_sampling_hz = 1.0 / dt;  // 采样频率
  const double xi = 1.0 / std::tan(M_PI * f_cutoff_hz / f_sampling_hz);  // 预扭曲频率
  const double xi_2 = xi * xi;  // xi 的平方
  const double q = std::sqrt(2);  // 二阶 Butterworth 滤波器的常数
  m_b0 = 1.0 / (1.0 + q * xi + xi_2);  // 滤波器系数 b0
  m_b1 = 2.0 * m_b0;  // 滤波器系数 b1
  m_b2 = m_b0;  // 滤波器系数 b2
  m_a1 = 2.0 * (xi_2 - 1.0) * m_b0;  // 滤波器系数 a1
  m_a2 = -(1.0 - q * xi + xi_2) * m_b0;  // 滤波器系数 a2
}

/**
 * @brief 对输入信号进行滤波
 * @param u0 当前输入信号
 * @return 滤波后的输出信号
 */
double Butterworth2dFilter::filter(const double & u0)
{
  // 使用差分方程计算滤波后的输出
  double y0 = m_b2 * m_u2 + m_b1 * m_u1 + m_b0 * u0 + m_a2 * m_y2 + m_a1 * m_y1;
  m_y2 = m_y1;  // 更新上上次的输出
  m_y1 = y0;  // 更新上一次的输出
  m_u2 = m_u1;  // 更新上上次的输入
  m_u1 = u0;  // 更新上一次的输入
  return y0;  // 返回滤波后的输出
}

/**
 * @brief 对向量中的信号进行滤波
 * @param t 输入信号向量
 * @param u 滤波后的输出信号向量
 */
void Butterworth2dFilter::filt_vector(const std::vector<double> & t, std::vector<double> & u) const
{
  u.resize(t.size());  // 调整输出向量的大小
  double y1 = t.at(0);  // 上一次的输出
  double y2 = t.at(0);  // 上上次的输出
  double u2 = t.at(0);  // 上上次的输入
  double u1 = t.at(0);  // 上一次的输入
  for (size_t i = 0; i < t.size(); ++i) {
    const double u0 = t.at(i);  // 当前输入信号
    const double y0 = m_b2 * u2 + m_b1 * u1 + m_b0 * u0 + m_a2 * y2 + m_a1 * y1;  // 计算滤波后的输出
    y2 = y1;  // 更新上上次的输出
    y1 = y0;  // 更新上一次的输出
    u2 = u1;  // 更新上上次的输入
    u1 = u0;  // 更新上一次的输入
    u.at(i) = y0;  // 存储滤波后的输出
  }
}

/**
 * @brief 对向量中的信号进行双向滤波（前向和后向）
 * @param t 输入信号向量
 * @param u 滤波后的输出信号向量
 */
void Butterworth2dFilter::filtfilt_vector(
  const std::vector<double> & t, std::vector<double> & u) const
{
  std::vector<double> t_fwd(t);  // 前向滤波结果
  std::vector<double> t_rev(t);  // 后向滤波结果

  // 前向滤波
  filt_vector(t, t_fwd);

  // 后向滤波
  std::reverse(t_rev.begin(), t_rev.end());  // 反转输入信号
  filt_vector(t, t_rev);  // 对反转后的信号进行滤波
  std::reverse(t_rev.begin(), t_rev.end());  // 反转滤波结果

  // 合并前向和后向滤波结果
  u.clear();
  for (size_t i = 0; i < t.size(); ++i) {
    u.push_back((t_fwd[i] + t_rev[i]) * 0.5);  // 取前向和后向滤波结果的平均值
  }
}

/**
 * @brief 获取滤波器的系数
 * @param coeffs 存储滤波器系数的向量
 */
void Butterworth2dFilter::getCoefficients(std::vector<double> & coeffs) const
{
  coeffs.clear();
  coeffs.push_back(m_a1);  // 系数 a1
  coeffs.push_back(m_a2);  // 系数 a2
  coeffs.push_back(m_b0);  // 系数 b0
  coeffs.push_back(m_b1);  // 系数 b1
  coeffs.push_back(m_b2);  // 系数 b2
}

namespace MoveAverageFilter
{
/**
 * @brief 对向量中的信号进行移动平均滤波
 * @param num 移动平均的窗口大小
 * @param u 输入信号向量
 * @return 是否成功
 */
bool filt_vector(const int num, std::vector<double> & u)
{
  if (static_cast<int>(u.size()) < num) {
    return false;  // 如果输入信号长度小于窗口大小，返回失败
  }
  std::vector<double> filtered_u(u);  // 存储滤波后的信号
  for (int i = 0; i < static_cast<int>(u.size()); ++i) {
    double tmp = 0.0;  // 临时变量，用于累加窗口内的信号
    int num_tmp = 0;  // 实际的窗口大小
    double count = 0;  // 窗口内的信号数量
    if ((i - num < 0) || (i + num > static_cast<int>(u.size()) - 1)) {
      num_tmp = std::min(i, static_cast<int>(u.size()) - i - 1);  // 调整窗口大小
    } else {
      num_tmp = num;  // 使用指定的窗口大小
    }

    for (int j = -num_tmp; j <= num_tmp; ++j) {
      int index = i + j;  // 计算窗口内的索引
      if (index >= 0 && index < static_cast<int>(u.size())) {
        tmp += u[static_cast<size_t>(index)];  // 累加窗口内的信号
        ++count;  // 增加信号数量
      }
    }
    filtered_u[static_cast<size_t>(i)] = tmp / count;  // 计算移动平均值
  }
  u = filtered_u;  // 更新输入信号
  return true;  // 返回成功
}
}  // namespace MoveAverageFilter
}  // namespace autoware::motion::control::mpc_lateral_controller

九、steering_predictor.cpp

在mpc.cpp中调用了这部分，进行转向预测。也没啥说的。

// Copyright 2023 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/steering_predictor.hpp"

namespace autoware::motion::control::mpc_lateral_controller
{

/**
 * @brief 构造函数，初始化转向预测器
 * @param steer_tau 转向系统的时间常数
 * @param steer_delay 转向系统的延迟时间
 */
SteeringPredictor::SteeringPredictor(const double steer_tau, const double steer_delay)
: m_steer_tau(steer_tau), m_input_delay(steer_delay)
{
}

/**
 * @brief 计算转向预测值
 * @return 预测的转向角度
 */
double SteeringPredictor::calcSteerPrediction()
{
  auto t_start = m_time_prev;  // 上一次计算的时间
  auto t_end = m_clock->now();  // 当前时间
  m_time_prev = t_end;  // 更新上一次计算的时间

  const double duration = (t_end - t_start).seconds();  // 计算时间间隔
  const double time_constant = m_steer_tau;  // 转向系统的时间常数

  // 计算初始响应（指数衰减部分）
  const double initial_response = std::exp(-duration / time_constant) * m_steer_prediction_prev;

  // 如果控制命令数量不足，直接返回初始响应
  if (m_ctrl_cmd_vec.size() <= 2) {
    setPrevResult(initial_response);  // 设置上一次的预测结果
    return initial_response;
  }

  // 计算预测的转向角度（初始响应 + 控制命令的贡献）
  const auto predicted_steering = initial_response + getSteerCmdSum(t_start, t_end, time_constant);
  setPrevResult(predicted_steering);  // 设置上一次的预测结果

  return predicted_steering;
}

/**
 * @brief 存储转向控制命令
 * @param steer 转向角度
 */
void SteeringPredictor::storeSteerCmd(const double steer)
{
  // 计算延迟后的时间
  const auto time_delayed = m_clock->now() + rclcpp::Duration::from_seconds(m_input_delay);
  Lateral cmd;
  cmd.stamp = time_delayed;  // 设置时间戳
  cmd.steering_tire_angle = static_cast<float>(steer);  // 设置转向角度

  // 存储控制命令
  m_ctrl_cmd_vec.emplace_back(cmd);

  // 如果控制命令数量不足，直接返回
  if (m_ctrl_cmd_vec.size() <= 2) {
    return;
  }

  // 删除过期的控制命令
  constexpr double store_time = 0.3;  // 存储时间阈值
  if ((time_delayed - m_ctrl_cmd_vec.at(1).stamp).seconds() > m_input_delay + store_time) {
    m_ctrl_cmd_vec.erase(m_ctrl_cmd_vec.begin());  // 删除最早的控制命令
  }
}

/**
 * @brief 计算控制命令的贡献
 * @param t_start 起始时间
 * @param t_end 结束时间
 * @param time_constant 时间常数
 * @return 控制命令的贡献
 */
double SteeringPredictor::getSteerCmdSum(
  const rclcpp::Time & t_start, const rclcpp::Time & t_end, const double time_constant) const
{
  // 如果控制命令数量不足，返回 0
  if (m_ctrl_cmd_vec.size() <= 2) {
    return 0.0;
  }

  // 找到第一个有效控制命令的索引
  size_t idx = 1;
  while (t_start > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
    if ((idx + 1) >= m_ctrl_cmd_vec.size()) {
      return 0.0;  // 如果没有找到有效命令，返回 0
    }
    ++idx;
  }

  // 计算控制命令的贡献
  double steer_sum = 0.0;
  auto t = t_start;
  while (t_end > rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp)) {
    const double duration = (rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp) - t).seconds();  // 计算时间间隔
    t = rclcpp::Time(m_ctrl_cmd_vec.at(idx).stamp);  // 更新时间
    steer_sum += (1 - std::exp(-duration / time_constant)) *
                 static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);  // 累加贡献
    ++idx;
    if (idx >= m_ctrl_cmd_vec.size()) {
      break;  // 如果超出范围，退出循环
    }
  }

  // 计算最后一个时间段的贡献
  const double duration = (t_end - t).seconds();
  steer_sum += (1 - std::exp(-duration / time_constant)) *
               static_cast<double>(m_ctrl_cmd_vec.at(idx - 1).steering_tire_angle);

  return steer_sum;
}

/**
 * @brief 设置上一次的预测结果
 * @param steering 预测的转向角度
 */
void SteeringPredictor::setPrevResult(const double & steering)
{
  m_steer_prediction_prev = steering;  // 更新上一次的预测结果
}

}  // namespace autoware::motion::control::mpc_lateral_controller

十、steering_offset

转向偏移量是指车辆的实际转向角度与理论转向角度之间的差值。理论转向角度是根据车辆的运动学模型（如速度、角速度和轴距）计算得出的理想转向角度。

// Copyright 2018-2023 The Autoware Foundation
//
// Licensed under the Apache License, Version 2.0 (the "License");
// you may not use this file except in compliance with the License.
// You may obtain a copy of the License at
//
//     http://www.apache.org/licenses/LICENSE-2.0
//
// Unless required by applicable law or agreed to in writing, software
// distributed under the License is distributed on an "AS IS" BASIS,
// WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
// See the License for the specific language governing permissions and
// limitations under the License.

#include "autoware/mpc_lateral_controller/steering_offset/steering_offset.hpp"

#include <algorithm>
#include <iostream>
#include <numeric>

/**
 * @brief 构造函数，初始化转向偏移估计器
 * @param wheelbase 车辆轴距
 * @param average_num 用于计算平均偏移的样本数量
 * @param vel_thres 更新偏移的速度阈值
 * @param steer_thres 更新偏移的转向角度阈值
 * @param offset_limit 偏移量的限制范围
 */
SteeringOffsetEstimator::SteeringOffsetEstimator(
  double wheelbase, double average_num, double vel_thres, double steer_thres, double offset_limit)
: wheelbase_(wheelbase),  // 初始化轴距
  average_num_(average_num),  // 初始化样本数量
  update_vel_threshold_(vel_thres),  // 初始化速度阈值
  update_steer_threshold_(steer_thres),  // 初始化转向角度阈值
  offset_limit_(offset_limit),  // 初始化偏移量限制
  steering_offset_storage_(average_num, 0.0)  // 初始化偏移量存储容器
{
}

/**
 * @brief 更新转向偏移估计
 * @param twist 车辆的速度和角速度
 * @param steering 当前的转向角度
 */
void SteeringOffsetEstimator::updateOffset(
  const geometry_msgs::msg::Twist & twist, const double steering)
{
  // 判断是否满足更新偏移的条件
  const bool update_offset =
    (std::abs(twist.linear.x) > update_vel_threshold_ &&  // 速度大于阈值
     std::abs(steering) < update_steer_threshold_);  // 转向角度小于阈值

  if (!update_offset) return;  // 如果不满足条件，直接返回

  // 计算理论转向角度
  const auto steer_angvel = std::atan2(twist.angular.z * wheelbase_, twist.linear.x);
  // 计算转向偏移量
  const auto steer_offset = steering - steer_angvel;
  // 将偏移量存入存储容器
  steering_offset_storage_.push_back(steer_offset);
  // 如果存储的偏移量数量超过样本数量，移除最早的偏移量
  if (steering_offset_storage_.size() > average_num_) {
    steering_offset_storage_.pop_front();
  }

  // 计算平均偏移量
  steering_offset_ =
    std::accumulate(std::begin(steering_offset_storage_), std::end(steering_offset_storage_), 0.0) /
    std::size(steering_offset_storage_);
}

/**
 * @brief 获取当前的转向偏移量
 * @return 转向偏移量（限制在指定范围内）
 */
double SteeringOffsetEstimator::getOffset() const
{
  // 返回限制在 [-offset_limit_, offset_limit_] 范围内的偏移量
  return std::clamp(steering_offset_, -offset_limit_, offset_limit_);
}

总结

这部分讲了横向跟踪中mpc的相关内容，理解起来不难，就是各个部分很繁琐。

标签：const,mpc,universe,Autoware,traj,源码,param,double,steer
From： https://blog.csdn.net/weixin_48386130/article/details/144915804