autoware.ai1.14控制算法学习

Autoware.ai1.14中MPC

---

前言

Mpc_follower该节点为循迹控制主要的处理节点，其输入为：参考轨迹、车辆当前状态、车辆当前位姿。输出为：速度、前轮转角。其主要逻辑为：分别对参考轨迹、车辆当前状态、车辆当前位姿这个三个话题进行处理，并依据当前的位置和参考轨迹点，查找距离本车最近的轨迹点，然后依据最近点的相关信息（X、Y、yaw）计算出横向误差和航向误差，即大地坐标系至Frenet坐标系的转换。之后根据这两个误差进行MPC的具体计算，最终计算出前轮转角（rad），并却与最近点的速度.

一、参考轨迹的处理

Topic --/mpc_waypoints 的回调函数MPCFollower::callbackRefPath
该部分的主要内容：1）对初步的轨迹进行重采样，重采样的间隔为traj_resample_dist，重采样得出新的点内部信息均根据原始点的内容一阶线性插值得出。2）是否对曲线进行圆滑处理，enable_path_smoothing。3）利用三角形外接圆的方式求每个点转弯半径进而求每个点的曲率，三角形的三个点的选取为当前点和当前点第前后curvature_smoothing_num。 主体逻辑如下：

二、当前位姿的处理

Topic --/current_pose 的回调函数MPCFollower::callbackPoss
该部分的主要内容：把接收到的车辆当前位姿消息传递赋值给vehicle_status并设置标志变量my_position_ok_的值为true。车辆状态信息包括，车辆的位姿、车辆速度、车当前的轮胎转角。

三、车辆当前状态的处理

Topic --/vehicle_status 的回调函数MPCFollower::callbackVehicleStatus
该部分的主要内容把接收的车辆的当前状态消息传递赋值给vehicle_status并设置标志变量my_steering_ok和my_velocity_ok的值为true，表明成功接收到车辆状态信息。

四、MPC具体的运算过程

定时器的回调函数MPCFollower::timerCallback
该部分的主要内容：检查车辆模型(vehicle_model_type)和求解器(qp_solver_type)是否定义，参考路径、车辆速度、车辆位置、轮胎转角是否满足要求，有问题则会发布制动的命令。没有问题就进入MPCFollower::calculateMPC进行计算，每ctrl_period(0.03s)进行一次运算。如下是calculateMPC运算流程：

calculateMPC主要内容为：取读相关参数预测时域(mpc_prediction_horizon)、采样时间(mpc_prediction_sampling_time)、当前车辆横摆角等。接着在重采样的轨迹上的查找最近点，再寻找第二近点。随后根据这两个点的信息，进行一阶线性插值得到真正的最近点(该最近点为：本车位置点向这两个点连线做垂线的垂足的点)。

在查找最近点之后，根据最近X Y YAW分别计算出横摆角误差和横向误差公式如下：

在查找出最近点之后会根据设定的预测步长(mpc_prediction_horizon)和预测步长的采样时间(mpc_prediction_sampling_time)推算出未来每一步对应的曲率(k)和速度(v)，然后依据车辆运动学(因本次循迹采用了运动学，只对运动学进行分析)，计算每一步对应的离散的系统矩阵Ad、输入矩阵Bd、输出矩阵Cd、Wd。基于运动学的状态方程如下（包括离散化）：




最总求解出输入量Ux，取出该向量的第一个值进行输出。该部分求解在qp_solver_unconstr_fast。取出的第一值的量为参考的前轮转角（rad），并最近点的车速封装在Twist中发布出去，具体函数为publishControlCommands函数中。

总结

visio思维导图
https://download.csdn.net/download/qq_42027654/48188301