【一看就会】Autoware.universe的控制部分梳理——mpc横向跟踪控制数据流【一】

不断学习加努力

已于 2025-01-13 17:24:37 修改

阅读量1.1k

点赞数 25

文章标签：自动驾驶算法

于 2025-01-10 16:18:34 首次发布

本文链接：https://blog.csdn.net/weixin_48386130/article/details/145053700

版权

文章目录

前言
一、路径跟踪部分
总结

前言

第四篇还没写完，就是跟着第三篇把剩余的几个模块源码给注释了一下。

先写第五部分，把控制这部分执行的顺序和数据流给写清楚。

一、路径跟踪部分

在启动了autoware_trajectory_follower_node中，controller_node.cpp中会不停的sub其他节点，主要是planning那边给的目标路径，定位给的车辆位置，车辆反馈的速度和转角等信息。

然后便会调用回调函数：callbackTimerControl()

在函数中，会根据提前在参数文件中选择的算法类型进行调用，并得到算法计算得出的控制量，我们这部分主要讲横向控制，具体调用如下：const auto lat_out = lateral_controller_->run(*input_data);

这句话就调用了mpc_lateral_controller.cpp中的run()函数，让我们到这个函数下。

在这个函数下，会有控制指令的输出：createCtrlCmdMsg()

输出的量为：ctrl_cmd

ctrl_cmd的在run函数中创建，然后在m_mpc->calculateMPC()函数中输入，进行处理。

让我们进入mpc.cpp文件的calculateMPC()函数。
这是整个mpc最核心的函数。
其中输入有：
当前车轮转角：current_steer
当前车辆位姿：current_kinematics
要发布的控制量：ctrl_cmd
要发布的预测路径：predicted_trajectory
要发布的调试值：debug_values

在这个函数中，首先根据planning给的目标路径m_reference_trajectory和当前速度位置速度，计算除了带有速度的目标路径。

然后在下面会进行控制指令ctrl_cmd的计算：
ctrl_cmd.steering_tire_angle = static_cast(u_filtered);
ctrl_cmd.steering_tire_rotation_rate = static_cast(calcDesiredSteeringRate(
mpc_matrix, x0_delayed, Uex, u_filtered, current_steer.steering_tire_angle, prediction_dt));

也就是要输出的车轮转角和角速度。

我们关注转角。
有两个中间量：u_filtered和u_saturated，是进行限幅和低通滤波，不去管它，我们继续往里面看。

Uex(0)是mpc的计算输出量。

计算是在 const auto [success_opt, Uex] = executeOptimization(
mpc_matrix, x0_delayed, prediction_dt, mpc_resampled_ref_trajectory,
current_kinematics.twist.twist.linear.x);

让我们继续看executeOptimization函数，这个是通过二次求解器求解mpc输出量的函数。

函数的输入有：
MPC 的预测方程矩阵：m
延迟补偿：x0
MPC 的预测时间步长：prediction_dt
目标路径：traj
当前车辆的纵向速度：current_velocity

输出有两个，前者是否优化成功，后者为Uex控制量。

计算流程就是先构建代价函数，然后放进qp求解器中求解：
m_qpsolver_ptr->solve(H, f.transpose(), A, lb, ub, lbA, ubA, Uex)

qp求解器的目的就是为了求解出可以使目标函数最小的控制量Uex。
如何求解的先不多说。
我们着重于这个二次规划问题的构建。

目标函数的标准形式：
在这里插入图片描述目标就是找到使代价函数最小的控制输入 U。

其中H和f这两个量就很重要。

代码中写了它俩的构建：

  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);

qp求解器中不涉及其他函数的调用。

这部分的构建有兴趣的可以研究一下，我这篇因为主要是看数据流，所以不在关注。

构架中用到了输入的参数m，那我们就回到calculateMPC函数中找m的生成过程： const auto mpc_matrix = generateMPCMatrix(mpc_resampled_ref_trajectory, prediction_dt);

generateMPCMatrix函数是用于生成预测方程矩阵和代价函数矩阵的核心函数。

输入参数：
参考轨迹，包含路径点、速度、曲率等信息：reference_trajectory
MPC 的预测时间步长：prediction_dt

输出：

    返回一个 MPCMatrix 结构体，包含以下矩阵：

        Aex：状态转移矩阵。

        Bex：控制矩阵。

        Wex：扰动矩阵。

        Cex：输出矩阵。

        Qex：状态误差的权重矩阵。

        R1ex：控制输入误差的权重矩阵。

        R2ex：控制输入变化的权重矩阵。

        Uref_ex：参考控制输入。

我们进入函数看。

众所周知mpc是根据最小化预测轨迹和目标路径的差值，然后通过滚动优化，来找到最优控制量的。
mpc_matrix就是预测方程。

矩阵的运算和初始化就不多解释了。
继续关注数据流的情况。

generateMPCMatrix函数调用了：
m_vehicle_model_ptr->getDimX()获取车辆模型的状态维度
m_vehicle_model_ptr->getDimU()获取车辆模型的控制输入维度
m_vehicle_model_ptr->getDimY()获取车辆模型的输出维度

这三个在创建模型的车辆模型的时候就已经设置好了。

m_vehicle_model_ptr->setVelocity(ref_vx);设置车辆模型的当前速度。ref_vx：参考速度（来自参考轨迹）。
m_vehicle_model_ptr->setCurvature(ref_k);设置车辆模型的当前曲率。ref_k：参考曲率（来自参考轨迹）。
这俩直接从参考轨迹来的。

m_vehicle_model_ptr->setCurvature(ref_smooth_k);设置车辆模型的平滑曲率。
这个直接返回的ref_smooth_k的值。

m_vehicle_model_ptr->calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT);计算离散状态矩阵。根据当前车辆模型的状态（速度、曲率等），计算离散状态矩阵。
m_vehicle_model_ptr->calculateReferenceInput(Uref);计算参考控制输入（前馈控制）。根据当前车辆模型的状态（速度、曲率等），计算参考控制输入。
这个是主要的功能。

还有几个计算权重的，先不管了。

m_vehicle_model_ptr的构建在mpc_lateral_controller.cpp中，具体实现则在vehicle文件夹下，下面有三种模型可供选择。

我们来看其中较为简单的，自行车运动学模型：vehicle_model_bicycle_kinematics.cpp

开始的构造函数：
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; // 设置转向时间常数
}

里面的3,1,2就是m_vehicle_model_ptr->getDimX()这三个要的状态纬度。

再看里面calculateDiscreteMatrix(Ad, Bd, Cd, Wd, DT)这个函数。就是离散矩阵计算，没什么说的。

看calculateReferenceInput(Uref)。根据轴距和曲率计算车轮转向角，这个就是自行车模型的核心了：
在这里插入图片描述