车辆MPC轨迹跟踪控制，跟踪双移线轨迹 无人驾驶车辆模型预测控制第五章 根据文献及第五章代码进行了修改调试，已经调试好以下车速的MPC轨迹跟踪代码： 车速：36km/h、60km/h、72km/h（原本代码会失稳，新添加了质心侧偏角约束后可运行） 1.代码包含： 修改好车体、轮胎、工况、联合仿真配置的CarSim的cpar文件，看输出仿真曲线查看实时动画； 修改好的MPC代码，添加了注释，仿真轨迹跟踪图形实时绘制，进行了代码分块便于学习； 修改了simulink模型，可以实时观测误差曲线，输出各个变量到工作空间； 新增加了轨迹曲线对比与误差分析代码，可在仿真之后分析轨迹跟踪的误差曲线，误差最大值，误差均方根值； 2.可远程指导运行，支持有偿，《无人驾驶车辆模型预测控制》课本PDF； 3.软件版本:CarSim2020 matlab2019b及以下 本宝贝只是36km/h的其他车速程序需要另外单独联系，因为车速不同，mpc控制参数不同。
最近在研究无人驾驶车辆模型预测控制的第五章内容，针对车辆MPC轨迹跟踪控制实现双移线轨迹跟踪这个点，着实花了不少功夫调试代码。今天就来跟大家分享一下成果。
一、调试背景与挑战
原本针对车速36km/h、60km/h、72km/h的MPC轨迹跟踪代码，在运行时会出现失稳的情况。经过一番探索，发现添加质心侧偏角约束能够解决这个问题，使得代码顺利运行。
二、代码构成解析
（一）CarSim的cpar文件
修改后的cpar文件涵盖了车体、轮胎、工况以及联合仿真配置。通过这个文件，我们可以借助CarSim强大的功能来模拟车辆的实际运行状况。比如在车体配置部分，可能会涉及到车辆质量、轴距等参数的设定，像这样：
% 假设设置车辆质量 vehicle_mass = 1500; % 单位：kg % 设置轴距 wheelbase = 2.7; % 单位：m这些参数的合理设置对于车辆动力学模型的准确性至关重要，它们会直接影响到车辆在各种工况下的响应。通过修改这些配置，我们可以更好地与MPC控制算法相匹配，从而实现精准的轨迹跟踪。在运行仿真时，我们能通过输出的仿真曲线以及实时动画，直观地看到车辆的行驶轨迹和状态变化，这对于分析和优化控制算法非常有帮助。
（二）MPC代码
修改后的MPC代码添加了详细注释，方便大家理解每一步的操作。并且对代码进行了分块，更加便于学习。这里以一个简单的MPC预测模型代码片段为例：
% 预测时域 N = 10; % 状态转移矩阵 A = [1 Ts 0 0; 0 1 0 0; 0 0 1 Ts; 0 0 0 1]; % 控制输入矩阵 B = [0.5*Ts^2; Ts; 0.5*Ts^2; Ts]; % 初始化状态 x = [0;0;0;0]; for k = 1:num_steps % 预测状态 for i = 1:N x_pred(:,i) = A * x + B * u_opt(i); x = x_pred(:,i); end % 这里省略了代价函数计算和优化求解部分 % 最终得到优化的控制输入u_opt % 更新当前状态 x = A * x + B * u_opt(1); end这段代码展示了MPC预测模型的基本结构，通过状态转移矩阵A和控制输入矩阵B，在预测时域N内对车辆状态进行预测。其中，Ts是采样时间，在实际应用中需要根据车辆的动态特性合理选择。通过循环预测未来状态，并根据代价函数优化控制输入，使得车辆能够尽量按照期望轨迹行驶。而且，代码还实现了仿真轨迹跟踪图形的实时绘制，让我们能够实时观察控制效果，及时发现问题并调整参数。
（三）Simulink模型
修改后的Simulink模型也有不少亮点。它可以实时观测误差曲线，并且能将各个变量输出到工作空间。在Simulink模型中，我们将MPC控制器与车辆动力学模型等模块连接起来，形成一个完整的闭环控制系统。通过设置相应的示波器模块，我们能够直观地看到车辆实际轨迹与期望轨迹之间的误差随时间的变化情况，例如：
!误差曲线示例
这样我们就能很清楚地了解控制算法在不同时刻的跟踪性能，为进一步优化算法提供依据。同时，将变量输出到工作空间，方便我们在Matlab环境下进行更深入的数据分析和处理。
（四）轨迹曲线对比与误差分析代码
新增加的轨迹曲线对比与误差分析代码是一大特色。在仿真之后，我们可以利用这部分代码来分析轨迹跟踪的误差曲线、误差最大值以及误差均方根值。以计算误差均方根值（RMSE）为例：
% 假设实际轨迹点和期望轨迹点分别存储在x_actual和x_desired中 error = x_actual - x_desired; rmse = sqrt(mean(error.^2));通过这样的计算，我们可以用一个量化的指标来评估MPC控制算法在轨迹跟踪方面的准确性。误差最大值能让我们知道在整个仿真过程中，车辆偏离期望轨迹的最大程度，这对于评估控制算法的鲁棒性非常重要。通过轨迹曲线对比，我们可以直观地看到实际轨迹与期望轨迹的贴合程度，进一步分析控制算法的性能。
三、后续支持
目前本宝贝提供的只是36km/h车速的程序，因为不同车速下MPC控制参数不同，60km/h和72km/h车速的程序需要另外单独联系。同时，我可以提供远程指导运行，当然是支持有偿服务的，还会提供《无人驾驶车辆模型预测控制》课本PDF，方便大家深入学习理论知识。软件版本要求是CarSim2020以及matlab2019b及以下，确保大家在相应环境下能够顺利运行代码。希望这些成果能对研究车辆MPC轨迹跟踪控制的小伙伴们有所帮助。